#!/bin/sh
# \
exec oagwish "$0" "$@"

set auto_path [linsert $auto_path 0  /usr/local/oag/apps/lib/$env(HOST_ARCH)]
set auto_path [linsert $auto_path 0  /usr/local/oag/lib_patch/$env(HOST_ARCH)]

set CVSRevisionAuthor "\Revision 1.01 $ \$Author: abrill $"
set appName "FBC Utility"

# Rev 1.01 - added ability to translate non-4x4 matrix to FBC pv format for loading, added
#           option to use custom IRM file and pick working directory. AB



APSApplication . -name $appName -version $CVSRevisionAuthor \
    -overview { Utility to configure FBC }

set rmDirPath /home/helios/oagData/sr/rtfeedback/lattices/default
set rmDir h.2016-0216.00
set workDir [exec pwd]
set plane X
set steps 20
set interval 1.0
set hpFreq 1.0
set Kp 0
set Ki 0.001
set Kd 0
set waitTime 3
set fileSource elegant
#disable doing bad things during testing...
set dryRun 0


set mainStatus "FBC Testing Utility"
APSScrolledStatus .status -parent .userFrame -width 75 \
    -height 8 -textVariable mainStatus
set mainStatus "The working directory requires the following file to transfer orbit: \n   FBC_Scalars_XY.mon"
set mainStatus "1. Enter Inverse Response Matrix path and directory"
set mainStatus "2. Select plane"
set mainStatus "3. Select actions"

#set up scrolled status window
proc SetMainStatus {text args} {
    global mainStatus
    set code ""
    APSParseArguments {code}
    set mainStatus "[clock format [clock seconds] -format %H:%M:%S] $text"
    switch $code {
        error -
        warning {
            bell
        }
        default {}
    }
    update      
}    

#set horizontal/vertical variables based on plane
proc setPlane {args} {
    global plane irmPV rmDir corrLabel rmDirPath fileSource irmFile custIRM
    set rmDirRoot [string range $rmDir 2 end]
    switch ${plane} {
	X {
	    set irmPV S27FB:FBC:H:IrmArrayM
	    set rmDir h.${rmDirRoot}
	    set corrLabel H
	}
	Y {
	    set irmPV S27FB:FBC:V:IrmArrayM
	    set rmDir v.${rmDirRoot}
	    set corrLabel V
	}
	default {
	    SetMainStatus "Error:  Plane must be X or Y"
	    return error
	}
    }
    if { $fileSource == "elegant"} {set irmFile ${rmDirPath}/${rmDir}/irm} else {set irmFile $custIRM}
#    if { ![file exists $irmFile] } { 
#	    SetMainStatus "$irmFile does not exist. Enter valid IRM location to continue."
#	    return error
#    }
}

proc standardizeIRMfile {args} {
    #convert irm file to 4x4 version for FBC
    global irmPV rmDir rmDirPath workDir plane corrLabel irmFile
    #configure array to store IRM values
    setPlane
    if { ![file exists $irmFile] } { 
	    SetMainStatus "$irmFile does not exist. Enter valid IRM location to continue."
	    return error
    }

    for {set i 0} {$i < 35} {incr i} { 
	set val($i) 0
    }
    set bpmList {S27A:P0 S27B:P0 S28A:P0 S28B:P0}
    if {$corrLabel=="V"} {set corrList {S27A:V3 S27B:V4 S28A:V3 S28B:V4}} else { set corrList {S27A:H3 S27B:H4 S28A:H3 S28B:H4}}
    set fileCorrs [exec sddsprintout $irmFile -col=ControlName "-spreadsheet=delimiter= ,noLabels" -noTitle]
    set fileBPMs [exec sddsquery $irmFile -col ]
    set fbpmList [split $fileBPMs "\n"]
    set fcorrList [split $fileCorrs "\n"]
    for {set i 0} {$i < [llength $fcorrList]} {incr i} {
	lset fcorrList $i [string trimleft [lindex $fcorrList $i]]
    }
    set firstCorr [lindex $fcorrList 0]
    set irmVals [exec sddsbreak -pipe=out $irmFile -rowlimit=1 | sddstranspose -pipe \
	| sddsconvert -pipe -delete=col,OldColumnNames -rename=col,${firstCorr}=Waveform \
	| sddscombine -pipe -merge -overWrite \
	| sddsprintout -pipe=in -col=Waveform -noTitle -noLabels]
    set valList [split $irmVals "\n"]
    #shift for missing BPM(s)
    set missingBPM 0
    foreach index {0 1 2 3} {
	if { ([lsearch $fbpmList [lindex $bpmList $index]] < 0) } { 
       #some calculation to shift rows here
	    if {$missingBPM} { 
	        SetMainStatus "multiple missing bpms"
	        return 1
            }
	    set missingBPM 1    
            set valList1 [linsert $valList [expr $index +9] 0]
            set valList2 [linsert $valList1 [expr $index + 6] 0]
            set valList3 [linsert $valList2 [expr $index + 3] 0]
            set valList [linsert $valList3 $index 0]
	}
    }

    set rep [llength $valList]

    for {set i 0} {$i < $rep} {incr i} {
	set val($i) [lindex $valList  $i]
    }

    foreach index {3 2 1 0} {
	if { ([lsearch $fcorrList [lindex $corrList $index]] < 0) } { 
	    set start [expr $index * 4]
            set stop [expr $start + 4]
            for {set i $start} {$i < $stop } {incr i} {
       	        set val([expr $i + 12]) $val([expr $i + 8])
       	        set val([expr $i + 8]) $val([expr $i + 4])
                set val([expr $i + 4]) $val($i)
                set val($i) 0
            }
        }
    } 
    for {set i 0} {$i < 16} {incr i} {
	lappend valueList $val($i)
    }
    set valueList [join $valueList ,]
    set irmPVtemp [string range $irmPV 0 end-1]
    set irmPVwrite ${irmPVtemp}C
    #generate single column file for sddswput
    exec sddsmakedataset $workDir/irm_FBC_Load_${plane}.sdds -column=Index,type=short -data=0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 \
	-column=Waveform,type=double -data=$valueList -ascii \
	-param=WaveformPV,type=string -data=$irmPVwrite 
    #generate 4x4 file
    exec sddsbreak -pipe=out $workDir/irm_FBC_Load_$plane.sdds -rowlimit=4 \
	| sddsconvert -pipe -delete=col,Index \
	| sddstranspose -pipe \
	| sddsconvert -pipe \
	-delete=col,OldColumnNames \
	-rename=col,Column000=S27A:P0 \
	-rename=col,Column001=S27B:P0 \
	-rename=col,Column002=S28A:P0 \
	-rename=col,Column003=S28B:P0 \
	| sddscombine -pipe=in $workDir/irm_standardized_FBC_$plane.sdds -merge -overWrite 

}

proc checkIRM {args} {
    global irmPV rmDir rmDirPath workDir plane dryRun irmFile
    SetMainStatus "Comparing currently loaded FBC IRM to ${rmDir}/irm..."
    if { ($dryRun==1) || ([setPlane]=="error") } { return }
    #generate local standardized 4x4 irm file from selected irm
    standardizeIRMfile
    exec sddswget -pipe=out -PVNames=${irmPV} \
	| sddsconvert -pipe -delete=col,Index \
	| sddsbreak -pipe -rowlimit=4 \
	| sddstranspose -pipe \
	| sddsconvert -pipe \
	-delete=col,OldColumnNames \
	-rename=col,Column000=S27A:P0 \
	-rename=col,Column001=S27B:P0 \
	-rename=col,Column002=S28A:P0 \
	-rename=col,Column003=S28B:P0 \
	| sddscombine -pipe=in $workDir/irm_FBC_${plane}.sdds -merge -overWrite

    exec sddsmatrixop $workDir/irm_standardized_FBC_$plane.sdds $workDir/irm_FBC_Elegant_Diff_${plane}.sdds \
	-push=$workDir/irm_FBC_${plane}.sdds -subtract

    #send results to pop up windows
    set calcIRM [exec sddsprintout -noTitle -col $workDir/irm_standardized_FBC_${plane}.sdds]
    set irmDiff [exec sddsprintout -noTitle -col ${workDir}/irm_FBC_Elegant_Diff_${plane}.sdds]
    set printString "Calculated IRM:\n$calcIRM \n\n Difference between loaded and calculated:\n$irmDiff"
    APSExecLog .printirm -width -80 -name "IRM comparison" \
	-unixCommand "echo $printString"
#    APSExecLog .printdiff -width -80 -name "Difference between Elegant calculated and FBC loaded RM" \
#	-unixCommand "sddsprintout -param -col ${workDir}/irm_FBC_Elegant_Diff_${plane}.sdds"
    

#    exec sddsplot -layout=2,2 -graph=symb,sca=2,conn,vary=sub \
#	"-topline=Elegant and FBC IRM" \
#	"-title=IRM Units A/mm" \
#	-col=ControlName,S27A:P0 -leg=spec=Elegant $workDir/irm_standardized_FBC_$plane.sdds\
#	-col=ControlName,S27A:P0 -leg=spec=FBC_Loaded $workDir/irm_FBC_${plane}.sdds -end \
#	-col=ControlName,S27B:P0 -leg=spec=Elegant $workDir/irm_standardized_FBC_$plane.sdds\
#	-col=ControlName,S27B:P0 -leg=spec=FBC_Loaded $workDir/irm_FBC_${plane}.sdds -end \
#	-col=ControlName,S28A:P0 -leg=spec=Elegant $workDir/irm_standardized_FBC_$plane.sdds \
#	-col=ControlName,S28A:P0 -leg=spec=FBC_Loaded $workDir/irm_FBC_${plane}.sdds -end \
#	-col=ControlName,S28B:P0 -leg=spec=Elegant $workDir/irm_standardized_FBC_$plane.sdds\
#	-col=ControlName,S28B:P0 -leg=spec=FBC_Loaded $workDir/irm_FBC_${plane}.sdds &
   SetMainStatus "Done."
}

#dry run procedure, only reads data
proc testCheckIRM {args} {
    global irmPV rmDir rmDirPath workDir dryRun irmFile
    if {[setPlane]=="error" } { return}   
    #file creation format changed 03/04 to include plane in generated files
    APSExecLog .printirm -width 85 -height 16 -name "Elegant Calculated IRM" \
	-unixCommand "sddsprintout -param -col ${rmDirPath}/${rmDir}/irm"
    APSExecLog .printdiff -width 85 -height 11 -name "Difference between Elegant calculated and FBC loaded RM" \
	-unixCommand "sddsprintout -col ${workDir}/irm_FBC_Elegant_Diff.sdds"
    exec sddsplot -layout=2,2 -graph=symb,sca=2,conn,vary=sub \
	"-topline=Elegant and FBC IRM" \
	"-title=IRM Units A/mm" \
	-col=ControlName,S27A:P0 -leg=spec=Elegant ${rmDirPath}/${rmDir}/irm \
	-col=ControlName,S27A:P0 -leg=spec=FBC_Loaded $workDir/irm_FBC.sdds -end \
	-col=ControlName,S27B:P0 -leg=spec=Elegant ${rmDirPath}/${rmDir}/irm \
	-col=ControlName,S27B:P0 -leg=spec=FBC_Loaded $workDir/irm_FBC.sdds -end \
	-col=ControlName,S28A:P0 -leg=spec=Elegant ${rmDirPath}/${rmDir}/irm \
	-col=ControlName,S28A:P0 -leg=spec=FBC_Loaded $workDir/irm_FBC.sdds -end \
	-col=ControlName,S28B:P0 -leg=spec=Elegant ${rmDirPath}/${rmDir}/irm \
	-col=ControlName,S28B:P0 -leg=spec=FBC_Loaded $workDir/irm_FBC.sdds &
}

proc loadIRM {args} {
    global irmPV rmDir rmDirPath workDir dryRun plane irmFile
    SetMainStatus "Loading IRM to FBC..."
    if {($dryRun==1) || ([setPlane]=="error")} {return}
    exec sddswget $workDir/irm_FBC_Initial_${plane}.sdds -PVNames=${irmPV}
    standardizeIRMfile

    exec sddsplot -graph=symb,sca=2,conn,vary=sub \
	"-topline=FBC IRM Initial and Loaded Matrix Elements" \
	"-title=IRM Units A/mm" \
	-col=Index,Waveform -leg=spec=Initial $workDir/irm_FBC_Initial_${plane}.sdds \
	-col=Index,Waveform -leg=spec=Loaded $workDir/irm_FBC_Load_${plane}.sdds &
	
    exec sddswput $workDir/irm_FBC_Load_${plane}.sdds -pendIOtime=20
    SetMainStatus "Done."
}

proc transferOrbit {args} {
    global plane workDir steps interval dryRun
    SetMainStatus "Collecting ${steps} samples of ${plane} plane position data to transfer orbit and zero bpm errors..."
    if { $dryRun==1 } {return}
    set monFile /home/helios/oagData/sr/rtfeedback/monitorFiles/FBC_Scalars_XY.mon
    if {![file exists $monFile] } { 
        SetMainStatus "FBC_Scalars_XY.mon missing..."
        return
    }

    exec sddsmonitor $monFile $workDir/FBC_Scalars_XY.sdds -erase -steps=${steps} -interval=${interval}

    set setptXferList [exec sddsprocess -pipe=out $workDir/FBC_Scalars_XY.sdds \
			   -process=S27AP0${plane}:InputValue,average,S27AP0${plane}_Avg \
			   -process=S27BP0${plane}:InputValue,average,S27BP0${plane}_Avg \
			   -process=S28AP0${plane}:InputValue,average,S28AP0${plane}_Avg \
			   -process=S28BP0${plane}:InputValue,average,S28BP0${plane}_Avg \
			   | sddsprocess -pipe \
			   "-redefine=param,%s,%s 1.0e6 /,select=S*_Avg" \
			   | sddsprintout -pipe=in -noTitle -noLabels -param=*_Avg,format=%1.16e]
    exec cavput -list=S27FB:FBC:S27AP0${plane}:SetpointSP=[lindex ${setptXferList} 0],S27FB:FBC:S27BP0${plane}:SetpointSP=[lindex ${setptXferList} 1],S27FB:FBC:S28AP0${plane}:SetpointSP=[lindex ${setptXferList} 2],S27FB:FBC:S28BP0${plane}:SetpointSP=[lindex ${setptXferList} 3]
    SetMainStatus "${plane} plane orbit transferred to FBC, bpm errors zeroed."
}

proc transferCorrSetpoints {args} {
    global plane corrLabel dryRun
    SetMainStatus "Transferring $plane plane fast corrector setpoints..."
    if { $dryRun==1 } {return}    
    switchToOpsMode
    after 500
    set setptXferList [exec cavget -list=S27A:${corrLabel}3:DacAI,S27B:${corrLabel}4:DacAI,S28A:${corrLabel}3:DacAI,S28B:${corrLabel}4:DacAI]
    exec cavput -list=S27FB:FBC:S27A${corrLabel}3:DcSetpointSP=[lindex ${setptXferList} 0],S27FB:FBC:S27B${corrLabel}4:DcSetpointSP=[lindex ${setptXferList} 1],S27FB:FBC:S28A${corrLabel}3:DcSetpointSP=[lindex ${setptXferList} 2],S27FB:FBC:S28B${corrLabel}4:DcSetpointSP=[lindex ${setptXferList} 3]
    SetMainStatus "Transferred ${plane} plane S27 and S28 fast corrector setpoints to FBC."
}

proc transferAll {args} {
    transferOrbit
    transferCorrSetpoints
    switchToMaintMode
}

proc switchToOpsMode {args} {
    global dryRun corrLabel
    SetMainStatus "Setting power supplies to ops mode..."
    if {$dryRun==1} {return}
    #exec cavput -list=S27A:H3:ControlSrcBO=1,S27B:H4:ControlSrcBO=1,S28A:H3:ControlSrcBO=1,S28B:H4:ControlSrcBO=1
    exec cavput -list=S27A:${corrLabel}3,S27B:${corrLabel}4,S28A:${corrLabel}3,S28B:${corrLabel}4 -list=:ControlSrcBO=1
    SetMainStatus "Done."
}

proc switchToMaintMode {args} {
    global dryRun corrLabel
    SetMainStatus "Setting power supplies to maintenance mode..."
    if  {$dryRun==1} {return}
    #exec cavput -list=S27A:H3:ControlSrcBO=0,S27B:H4:ControlSrcBO=0,S28A:H3:ControlSrcBO=0,S28B:H4:ControlSrcBO=0
    exec cavput -list=S27A:${corrLabel}3,S27B:${corrLabel}4,S28A:${corrLabel}3,S28B:${corrLabel}4 -list=:ControlSrcBO=0
    SetMainStatus "Done."
}

proc closeLoops {args} {
    global plane corrLabel dryRun Kp Ki Kd hpFreq waitTime
    SetMainStatus "Gently closing loop for $plane plane..."
    if {($dryRun==1) || ([setPlane]=="error")} {return}
    #transfer orbit and setpoints
    transferAll
    #set correctors to maintenance mode
    switchToMaintMode
    #set high pass to 1hz
    exec caput S27FB:FBC:${corrLabel}:HPF:FreqC $hpFreq
    #enable high pass filter
    exec caput S27FB:FBC:${corrLabel}:HPF:EnableC 1
    #set Kp=0,Ki=0.001,Kd=0
    exec cavput -list=S27FB:FBC:${corrLabel}:PID: -list=KpC=${Kp},KiC=${Ki},KdC=${Kd}
    #reset filters
    exec caput S27FB:FBC:${corrLabel}:FiltersResetC 1 
    #wait 3 seconds
    after [expr $waitTime * 1000]
    #close loops
    exec caput S27FB:FBC:${corrLabel}:LoopEnableC 1

    SetMainStatus "$plane plane FBC loop closed."
}

proc fileMode {args} {
    global fileSource
    APSParseArguments {source}
    set fileSource $source
}

proc pickFile {args} {
    set today [clock format [clock seconds] -format %Y/%m/%d ]
    set startDir /home/helios/SR/daily/$today
    set fileName [APSFileSelectDialog .[APSTmpString] -listDir $startDir -width 100 -selectDir 1]
    return $fileName
}

proc settingsFrame {widget args} {
    global rmDirPath rmDirRoot dryRun rmDir custIRM fileSource
    APSStrictParseArguments {parent}
    set w $parent$widget.frame
    set fileSource elegant
    APSFrame $widget -parent $parent -label "Settings"
    set tabList [APSTabFrame .main -parent $w -label "" \
	-labelList [list "Elegant" "custom"] -width 80 -height 50 \
	-commandList {{fileMode -source elegant} {fileMode -source custom}}]
    
    #APSLabeledOutput .lo -parent $w -label "irm source:" -textVariable fileSource
    set irmWidg [lindex $tabList 0]
    set custWidg [lindex $tabList 1]

    APSLabeledEntry .rmp -parent $irmWidg -label "IRM path:" -textVariable rmDirPath -width 65
    APSLabeledEntry .rm -parent $irmWidg -label "IRM dir:" -textVariable rmDir -width 65 \
	-contextHelp {Directory containing Elegant IRM file }
    APSLabeledEntry .cirm -parent $custWidg -label "IRM file:" -textVariable custIRM -width 65
    APSLabeledEntry .ld -parent $w -label "Work Dir:" -textVariable workDir -width 65 \
	-contextHelp {Directory to save generated files }
    APSButton .pk -parent $w.ld -text "Pick" -size small -command {set workDir [pickFile]}
    APSRadioButtonFrame .pl -parent $w -label "Plane:      " -orientation horizontal \
	-buttonList "X Y" -variable plane -valueList "X Y" \
	-commandList { setPlane setPlane }
#    APSRadioButtonFrame .dr -parent $w -label "Dry Run:    " -orientation horizontal \
#	-buttonList "yes no" -variable dryRun -valueList "1 0"
}

proc actionFrame {widget args} {
    global rmDir rmDirPath plane
    APSStrictParseArguments {parent}
    set w $parent$widget.frame
    APSFrame $widget -parent $parent -label "Actions" -packOption "-fill x"
    APSFrameGrid .grid -parent $w -xList {x1 x2} -packOption "-fill x"
    set w1 $w.grid.x1
    set w2 $w.grid.x2
    #set w3 $w.grid.x3
    #set w4 $w.grid.y4
    #first box
    #APSButton .test -parent $w1 -text "Test Check IRM" -command {testCheckIRM} -size "small" \
    #	-packOption "-side top"

    APSFrame .irm -parent $w1 -label "Inverse Response Matrix" -packOption "-side top -fill x"
    APSButton .cirm -parent $w1.irm.frame -text "Check IRM" -command {checkIRM} \
	-contextHelp {Compare currently loaded IRM with Elegant calculated IRM for the current plane }
    APSButton .lirm -parent $w1.irm.frame -text "Load IRM" -command {loadIRM} \
	-contextHelp {Load selected Elegant calculated IRM to FBC for selected plane }
    APSFrame .corrSet -parent $w1 -label "Corrector Settings" -packOption "-side bottom -fill x"
    APSButton .om -parent $w1.corrSet.frame -text "Operations" -command {switchToOpsMode} \
	-contextHelp {Set correctors to operations mode }
    APSButton .mm -parent $w1.corrSet.frame -text "Maintenance" -command {switchToMaintMode} \
	-contextHelp {Set correctors to maintenance mode }

    #second box
    APSFrame .tx -parent $w2 -label "Transfer Settings to FBC" -packOption "-fill x"
    APSFrameGrid .grid2 -parent $w2.tx.frame -yList {y1 y2 y3} -packOption "-fill x"
    set ww1 $w2.tx.frame.grid2.y1
    set ww2 $w2.tx.frame.grid2.y2
    set ww3 $w2.tx.frame.grid2.y3
    #grid2 box 1
    APSLabeledEntry .st -parent $ww1 -label "Steps: " -textVariable steps -width 5 \
	-packOption "-side left" \
	-contextHelp {Number of BPM samples to average for setpoint when transferring orbit }
    APSLabeledEntry .int -parent $ww1 -label "    Interval: " -textVariable interval -width 5 \
	-packOption "-side left" \
	-contextHelp {Sample interval when averaging BPM values for setpoint when transferring orbit }
    #grid2 box 2
    APSButton .to -parent $ww2 -text "Transfer Orbit" \
	-command {transferOrbit} -contextHelp {Transfer current bpm readings to FBC BPM setpoints}
    APSButton .tcs -parent $ww2 -text "Transfer Corrector Setpoints" \
	-command {transferCorrSetpoints} -contextHelp {Transfer current corrector readings to FBC setpoints }
    APSButton .ta -parent $ww3 -text "Transfer Orbit and Setpoints" -command {transferAll} \
	-contextHelp {Transfer orbit and corrector setpoints using existing settings }
    #close loops box
    APSFrame .lp -parent $w -label "Close loops" -packOption "-fill x"
    set cl $w.lp.frame
    APSLabeledEntry .hp -parent $cl -label "HP freq(Hz):" -textVariable hpFreq -width 5 -packOption "-side left"
    APSLabeledEntry .kp -parent $cl -label " Kp:" -textVariable Kp -width 5 -packOption "-side left"
    APSLabeledEntry .ki -parent $cl -label " Ki:" -textVariable Ki -width 5 -packOption "-side left"
    APSLabeledEntry .kd -parent $cl -label " Kd:" -textVariable Kd -width 5 -packOption "-side left"
    APSLabeledEntry .wt -parent $cl -label " Wait(sec):" -textVariable waitTime -width 3 \
	-packOption "-side left" 
    APSButton .cl -parent $cl -text "Close Now" -command {closeLoops} \
	-contextHelp {Gently close loops by setting HP freq, enable HP freq, set gain constants, reset filters, wait, and then closing loops }
}

setPlane
settingsFrame .settingsFrame -parent .userFrame
actionFrame .actionFrame -parent .userFrame
