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

APSApplication . -name "EMWMeasureResponse" -version $CVSRevisionAuthor \
  -overview "EMWMeasureResponse measured the DC response of the orbit to the EMW correctors."

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

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

# chan 1 Upstream By (hcorr1)
# chan 2 Downstream Bx (vcorr2)
# chan 3 Upstream Bx (vcorr1)
# chan 4 Downstream By (hcorr2)
# set the EMA polarity to those that enable the PVs below
set root(v) vResponse
set corrList(v) "ID11b:ai_afg3 ID11b:ai_afg2"
set amplitude(v) 1.0
set coord(v) y

set root(h) hResponse
set corrList(h) "ID11b:ai_afg1 ID11b:ai_afg4"
set amplitude(h) 1.0
set coord(h) x

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 response]}
    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 .rootv -parent $w -label "Root name of vertical file: " \
      -textVariable root(v) \
      -contextHelp "Enter the root of the vertical raw data files and processed output <root><index>."
    APSLabeledEntry .deltah -parent $w -label "Amplitude for h-plane (A): " \
      -textVariable amplitude(h) -width 6 \
      -contextHelp "Enter the desired value for corrector amplitude."
    APSLabeledEntry .deltav -parent $w -label "Amplitude for v-plane (A): " \
      -textVariable amplitude(v) -width 6 \
      -contextHelp "Enter the desired value for corrector amplitude."
    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."

    # 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 EMW dipole correctors. Horizontal plane is run first, then vertical plane."
    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 .calcInv -parent $parent \
      -text "Calculate Inverse"  \
      -packOption "-side left" \
      -command CalculateInverse \
      -contextHelp "Calculates inverse using only the good bpms as determined by the bpm status management database."
    return
}

proc Setup {args} {
    global bpmPause
    set interval $bpmPause monitorDir
    if [catch {APSSRSetIOCAveraging -num2Ave [expr $interval * 10] \
                 -filterCoeff 1.0 -enable 1 \
             } result] {
        APSSetVarAndUpdate SROrbitStatus "Setup: $result"        
        return -code error "Setup: $result"
    }
    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
}

proc CollectData {args} {
    global root corrList amplitude coord monitorDir
    APSParseArguments {outputDir bpmPause correctorPause comment}

    foreach plane {h v} {
        set output [file join $outputDir $root($plane)]
        set corrFileList ""
        foreach corr $corrList($plane) {
            catch {file delete -force -- corr.mon}
            set fid [open corr.mon w]
            puts $fid "SDDS1"
            puts $fid "&column name=ControlName type=string &end"
            puts $fid "&data mode=ascii no_row_counts=1 &end"
            puts $fid "$corr"
            close $fid

            set delta $amplitude($plane)

            #################################
            # +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 [expr (1 + $bpmPause + $correctorPause)] -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
            #################################
            # -delta tweek
            #################################
            APSSetVarAndUpdate SROrbitStatus "cavput -delta=factor=-2 -list=${corr}=$delta"
            exec cavput -delta=factor=-2 -list=${corr}=$delta -pend=5
            set interval  [expr (1 + $bpmPause + $correctorPause)]
            APSSetVarAndUpdate SROrbitStatus "wait for $interval seconds"
            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}
            #################################
            # restore corrector
            #################################
            APSSetVarAndUpdate SROrbitStatus "cavput -delta=factor=1 -list=${corr}=$delta"
            exec cavput -delta=factor=1 -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 / -2.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
        }
        APSSetVarAndUpdate SROrbitStatus "Finished scanning correctors."
        eval exec sddscombine $corrFileList -pipe=out \
          -merge -overWrite \
          | sddsconvert -pipe -retain=col,Corrector,[join [FindGoodBpms -plane $plane] ,] \
          | sddstranspose -pipe=in ${output}
        APSSetVarAndUpdate SROrbitStatus "Response matrix created."
        CalculateInverse -plane $plane -output $output
        APSSetVarAndUpdate SROrbitStatus "Inverse response matrix calculated."
    }
}

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*\[AB\]:P\[12345\] \
                 -filter=col,Nonexistent$Plane,0,0,OkForDCOrbitCorrection$Plane,1,1,& \
                 | sdds2stream -pipe -col=DeviceName \
             } goodBpms] {
        return -code error $goodBpms
    }
    return $goodBpms
}

proc CalculateInverse {args} {
    global outputDir
    exec sddspseudoinverse $outputDir/hResponse -pipe=out \
      | sddsconvert -pipe=in $outputDir/hResponse.inv \
      -rename=col,OldColumnNames=ActuatorName \
      -edit=col,S*,ei/:msAve:x:ErrorCC/
    exec sddspseudoinverse $outputDir/vResponse -pipe=out \
      | sddsconvert -pipe=in $outputDir/vResponse.inv \
      -rename=col,OldColumnNames=ActuatorName \
      -edit=col,S*,ei/:msAve:y:ErrorCC/
    exec sddsconvert $outputDir/vResponse -pipe=out \
      -dele=col,ID11b:ai_afg3 \
      | sddspseudoinverse -pipe \
      | sddsconvert -pipe=in $outputDir/vResponse.vcorr2.inv \
      -rename=col,OldColumnNames=ActuatorName \
      -edit=col,S*,ei/:msAve:y:ErrorCC/
    return
}

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

MakeInputFrame .input -parent .userFrame
