#!/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.16 $ \$Author: lemery $"

APSApplication . -name "SCUMeasureResponse" -version $CVSRevisionAuthor \
  -overview "SCUMeasureResponse measured the DC response of the orbit to the SCU correctors. The inverse can also be calculated. The inverse with a reduced gain factor for the H and V plane can be calculated. It is more important for the H plane I think. Matrices for one one corrector can be calculated as well."

set SROrbitStatus "Initializing ..."
APSScrolledStatus .status -parent .userFrame -width 60 \
  -textVariable SROrbitStatus 

proc SetSROrbitStatus {text} {
    global SROrbitStatus
    set SROrbitStatus $text
    update
    bell
}

# run this with SCU is off!
# the orbit generated is too large for the ID4 BPLD.

# no V correctors

set root(h) hResponse
# "End corrector and 1st Integral corrector"
set corrList(h) "ID06ds:PSCorr1Set ID06ds:PSCorr2Set"
set amplitudeList(h) "10.0 2.0"
set coord(h) x
set factor(h) 5

proc MakeInputFrame {widget args} {
    APSStrictParseArguments {parent}
    set w $parent$widget.frame
    APSFrame $widget -parent $parent -label "Input parameters"
    
    # select corrector (plane) and amplitude
    APSLabeledEntry .outputDir -parent $w \
      -label "Output directory:" -textVariable outputDir \
      -contextHelp "Enter a name for the output file directory." -width 50
    APSButton .daily -parent $w.outputDir -packOption "-side top" \
      -text "daily" -size small \
      -command {set outputDir [APSGoToDailyDirectory -subdirectory SCU]}
    APSLabeledEntry .rooth -parent $w -label "Root name of horizontal file: " \
      -textVariable root(h) \
      -contextHelp "Enter the root of the horizontal raw data files and processed output <root><index>."
    APSLabeledEntry .deltah -parent $w -label "Amplitude list for h-plane (A): " \
      -textVariable amplitudeList(h) -width 16 \
      -contextHelp "Enter the desired values for corrector amplitude, one per corrector. (There are two correctors ID06ds:PSCorr1Set ID06ds:PSCorr2Set that we would like to use for SCU1"
    APSLabeledEntry .gainReductionh -parent $w -label "Reduction for gain of second SV for H-plane: " \
      -textVariable factor(h) -width 6 \
      -contextHelp "Enter the desired reduction factor of second SV of the H-plane matrix."
    APSLabeledEntry .bpmPause -parent $w -label "BPM average time (s): " \
      -textVariable bpmPause -width 6 \
      -contextHelp "Enter the time for the averaging of bpms."
    APSLabeledEntry .correctorPause -parent $w -label "Corrector pause time (s): " \
      -textVariable correctorPause -width 6 \
      -contextHelp "Enter the time for the correctors to settle."
    
    APSLabeledEntry .comment -parent $w -label "Comment: " \
      -textVariable comment \
      -width 55 \
      -contextHelp "Enter a comment to be written as a parameter to the output file."
    APSButton .setup -parent $parent \
      -text "Setup"  \
      -packOption "-side left" \
      -command Setup \
      -contextHelp "Sets up data collection,i.e. creates a monitor file of bpms and sets up bpm averaging.\n\nAnd turns off SCU. Very important as orbit created can be larger than BPLD limits."

    # collect orbit before and after
    APSButton .collect -parent $parent \
      -text "Collect Data"  \
      -packOption "-side left" \
      -command {CollectData \
                  -outputDir $outputDir \
                  -bpmPause $bpmPause \
                  -correctorPause $correctorPause \
                  -comment $comment \
                  -abortVariable abortRun } \
      -contextHelp "Collects orbit data for two values of the SCU dipole correctors. Horizontal plane only is run as there are no vertical correctors"
    APSButton .abort -parent $parent \
      -text "Abort"  \
      -packOption "-side left" \
      -command {set abortRun 1; \
                  APSSetVarAndUpdate SROrbitStatus "Abort pending..." } \
      -contextHelp "Aborts scan at next appropriate moment."
    APSButton .plot -parent $parent \
      -text "Plot Response"  \
      -packOption "-side left" \
      -command {PlotResponses -outputDir $outputDir} \
      -contextHelp "Plot responses."
    APSButton .calcInv -parent $parent \
      -text "Calculate Inverses"  \
      -packOption "-side left" \
      -command {CalculateInverses  -outputDir $outputDir}\
      -contextHelp "Calculates inverses using only the good bpms as determined by the bpm status management database. This function is already included in the \"Collect Data\" button."
    return
}

proc Setup {args} {
    global bpmPause monitorDir amplitudeList
    set interval $bpmPause

    if [catch {APSSRSetIOCAveraging -num2Ave [expr $interval * 10] \
                 -filterCoeff 1.0 -enable 1 \
             } result] {
        APSSetVarAndUpdate SROrbitStatus "Setup: $result"        
        return -code error "Setup: $result"
    }

    # setup BPM monitor file.
    if [catch {exec sddsprocess $monitorDir/SROrbit-msAve.vmon \
                 SROrbit-msAve.vmon \
                 -match=col,Suffix=:scdu:ave_sum.VAL,! \
             } result] {
        APSSetVarAndUpdate SROrbitStatus "Setup: $result"        
        return -code error "Setup: $result"
    }
    return
}

# The experiment is changed because the H correctors are unipolar. they
# can't go to negative values with the -2 factor.
proc CollectData {args} {
    global corrList amplitudeList coord monitorDir root
    APSParseArguments {outputDir bpmPause correctorPause comment abortVariable}
    if {$comment == ""} {
        return -code error "CollectData: No values given to comment"
    }
        #0)make sure no orbit correction is running.
        if [catch {exec cavput -list=S:RC:OrbitControlLawXC,S:RC:OrbitControlLawYC,S:rfFreqControlLawRC -list=.SUSP=1} result ] {
            return -code error "StartScan: $result"
        }
        if [catch {exec cavput -list=DP:S: \
                     -list=OrbitControlLawX,OrbitControlLawY \
                     -list=SDDS.SUSP=1 \
                 } result ] {
            return -code error "StartScan: $result"
        }

        # set the RTFB high-pass filter to high values in order to
        # steady the orbit while not causing frequency overlap problems.
        exec cavput -list=SRFB:fhSlAO=5,SRFB:fhAO=1
        exec cavput -list=SRFB:GBL:loadBO=1

    global $abortVariable
    set interval [expr 1 + $bpmPause + $correctorPause]
    foreach plane {h } {
        set corrFileList ""
        set output [file join $outputDir $root($plane)]
        foreach corr $corrList($plane) amplitude $amplitudeList($plane) {
            exec sddsmakedataset corr.mon \
              -column=ControlName,type=string -data=$corr
            set delta $amplitude

            #################################
            # +delta tweek
            #################################
            APSSetVarAndUpdate SROrbitStatus "cavput -delta=factor=1 -list=${corr}=$delta"
            exec cavput -delta=factor=1 -list=${corr}=$delta -pend=5
            # wait for some time for corrector and bpms to settle
            APSSetVarAndUpdate SROrbitStatus "wait for $correctorPause for correctors and $bpmPause seconds for bpms"
            APSWaitWithUpdate -waitSeconds $interval -updateInterval 1
            set monID [open "|sddsvmonitor -erase \
                -rootname=$monitorDir/SROrbit-Rootnames.vmon \
                -suffixes=SROrbit-msAve.vmon \
                ${output}.${corr}.raw \
                -scalars=corr.mon \
                -singleShot=noprompt -steps=2" w]
            puts $monID "y"
            if [catch {flush $monID} result] {
                return -code error "CollectData: Problem with flush $monID command (0)"
            }
            APSWaitWithUpdate -waitSeconds 3 -updateInterval 1
            #################################
            # Can't do -delta tweek because the supply isn't unipolar
            #################################
            APSSetVarAndUpdate SROrbitStatus "cavput -delta=factor=-1 -list=${corr}=$delta"
            exec cavput -delta=factor=-1 -list=${corr}=$delta -pend=5

            # wait for some time for corrector and bpms to settle
            APSSetVarAndUpdate SROrbitStatus "wait for $correctorPause for correctors and $bpmPause seconds for bpms"
            APSWaitWithUpdate -waitSeconds $interval -updateInterval 1
            puts $monID "y"
            if [catch {flush $monID} result] {
                return -code error "CollectData: Problem with flush $monID command (1)"
            }
            APSWaitWithUpdate -waitSeconds 3 -updateInterval 1
            catch {close $monID}
            #################################
            # Don't need to restore corrector
            #################################
            APSSetVarAndUpdate SROrbitStatus "cavput -delta=factor=0 -list=${corr}=$delta"
            exec cavput -delta=factor=0 -list=${corr}=$delta -pend=5

            # Do some processing for each corrector during loop
            exec sddschanges ${output}.${corr}.raw -pipe=out \
              -change=:msAve:? -cop=Index,Rootname \
              | sddsprocess -pipe \
              "-redef=col,%s,ChangeIn%s $delta / -1.0 /,select=ChangeIn:msAve:?,edit=%/ChangeIn//" \
              |  sddsconvert -pipe \
              -retain=col,Rootname,:msAve:$coord($plane) \
              -rename=col,:msAve:$coord($plane)=$coord($plane) \
              | sddstranspose -pipe -newcolumnnames=Rootname \
              | sddsconvert -pipe -dele=col,OldColumnNames \
              | sddsprocess -pipe=in ${output}.${corr}.proc \
              -print=col,Corrector,${corr} \
              -print=para,Comment,[APSMakeSafeQualifierString $comment]
            lappend corrFileList ${output}.${corr}.proc
        }

        eval exec sddscombine $corrFileList -pipe=out \
          -merge -overWrite \
          | sddsconvert -pipe \
          -retain=col,Corrector,[join [FindGoodBpms -plane $plane] ,] \
          | sddstranspose -pipe=in ${output} \
          -oldColumnNames=BPMNames

    }
    CalculateInverses -outputDir $outputDir
    APSSetVarAndUpdate SROrbitStatus "Done."
}

proc PlotResponses {args} {
    global root
    APSParseArguments {outputDir}

    exec sddsplot3 $outputDir/$root(h) -layout=1,2 \
      -axes=x -grap=sym,conn -sep \
      -col=BPMNames,ID06ds:PSCorr1Set $outputDir/$root(h) \
      "-ylabel=2nd int. hcorr" -enum=int=18 \
      -col=BPMNames,ID06ds:PSCorr2Set $outputDir/$root(h) \
      "-ylabel=1st int. hcorr" -enum=int=18 \
      -col=BPMNames,ID06ds:PSCorr1Set $outputDir/$root(h) \
      "-ylabel=2nd int. hcorr" -match=col,BPMNames=S\[12]\?:P* \
      -col=BPMNames,ID06ds:PSCorr2Set $outputDir/$root(h) \
      "-ylabel=1st int. hcorr" -match=col,BPMNames=S\[12]\?:P* \
      &

}

proc FindGoodBpms {args} {
    set plane ""
    APSParseArguments {plane}
    set Plane [string toupper $plane]
    set statusFile /home/helios/oagData/sr/BPMStatus/config.sdds
    if [catch {exec sddsprocess $statusFile -pipe=out \
                 -match=col,DeviceName=S*\[ABC\]:P\[012345\] \
                 -filter=col,Nonexistent$Plane,0,0,OkForDCOrbitCorrection$Plane,1,1,& \
                 | sdds2stream -pipe -col=DeviceName \
             } goodBpms] {
        return -code error $goodBpms
    }
    return $goodBpms
}

proc CalculateInverses {args} {
    global root factor corrList
    set outputDir ""
    APSParseArguments {outputDir}
    foreach plane {h} coord {x} {
        set output $outputDir/$root($plane)
        # regular inverse
        exec sddspseudoinverse ${output} -pipe=out \
          -sFile=s,matrix -uMatrix=u -vMatrix=v \
          -economy \
          -oldColumnNames=ActuatorName \
          | sddsconvert -pipe=in ${output}.inv \
          -edit=col,S*,ei/:msAve:${coord}:ErrorCC/
        # keep 1 SV
        exec sddspseudoinverse ${output} -pipe=out \
            -oldColumnNames=ActuatorName \
            -largestSingularValues=1 \
            | sddsconvert -pipe=in ${output}.1SV.inv \
            -edit=col,S*,ei/:msAve:${coord}:ErrorCC/

        # reduce gain of 2nd SV
        exec sddsconvert ${output}.inv -pipe=out \
          -dele=col,ActuatorName \
          | sddsquery -pipe=in -col -sddsoutput=${output}.inv.columns
        exec sddstranspose u -pipe=out \
          | sddsconvert -pipe=in ut \
          -dele=col,OldColumnNames
        
        exec sddsprocess  s -pipe=out \
          -clip=2,0,invert \
          | sddspseudoinverse -pipe \
          | sddsconvert -pipe \
          -retain=col,Column* \
          | sddsprocess -pipe=in sinvModified \
          "-redef=col,Column001,Column001 $factor($plane) /"

        # For inverse with specially-edited singular values
        exec sddsconvert v -pipe=out \
          -dele=col,ActuatorName \
          | sddsmatrixop -pipe -verbose \
          -push=sinvModified -multiply -push=ut -multiply \
          -columnNames=filename=${output}.inv.columns,column=Name \
          | sddsxref -pipe=in ${output}.inv ${output}Mod.inv \
          -take=ActuatorName


        # first corrector
        exec sddsconvert  ${output} -pipe=out \
          -retain=col,BPMNames,[lindex $corrList($plane) 0] \
          | sddspseudoinverse -pipe \
          -oldColumnNames=ActuatorName \
          | sddsconvert -pipe=in ${output}\#1.inv \
          -edit=col,S*,ei/:msAve:${coord}:ErrorCC/
        
        # second corrector
        exec sddsconvert  ${output} -pipe=out \
          -retain=col,BPMNames,[lindex $corrList($plane) 1] \
          | sddspseudoinverse -pipe \
          -oldColumnNames=ActuatorName \
          | sddsconvert -pipe=in ${output}\#2.inv \
          -edit=col,S*,ei/:msAve:${coord}:ErrorCC/

        APSSetVarAndUpdate SROrbitStatus "Created files ${output}.inv  ${output}.1SV.inv ${output}Mod.inv ${output}\#1.inv ${output}\#2.inv"
    }
    APSSetVarAndUpdate SROrbitStatus "Choose one for each plane for SCU perturbation measurement."
    return
}

set bpmPause 10
set correctorPause 3
set outputDir .
set monitorDir /home/helios/oagData/sr/sddsmonitorFiles
set SROrbitStatus "Ready."

MakeInputFrame .input -parent .userFrame


# Local Variables:
# mode: tcl
# indent-tabs-mode: nil
# End:
