#!/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)]
APSDebugPath

wm geometry . +10+10

set args $argv
set sectorList ""
for {set b 1} {$b<5} {incr b} {
    for {set c 0} {$c<10} {incr c} {
        set sector B${b}C$c
        lappend sectorList $sector
    }
}
proc ReadBoosterSteeringConfig {args} {
    global sectorList
    eval global $sectorList
    
    set configFile /home/helios/oagData/booster/localSteering/steeringConfig.sdds
  
    foreach plane {h v} {
        set configFile /home/helios/oagData/booster/localSteering/${plane}steeringConfig.sdds
        foreach nm {Sector BPMName Corr1 Corr2 Corr3 Corr4 PollutedBPMs} {
            set ${nm}List [exec sdds2stream -col=$nm $configFile]
        }
        set sectorList $SectorList
        set index 0
        foreach sector $SectorList {
            set ${sector}($plane.BPMName) [lindex $BPMNameList $index]
            set ${sector}($plane.corr1) [lindex $Corr1List $index]
            set ${sector}($plane.corr2) [lindex $Corr2List $index]
            set ${sector}($plane.corr3) [lindex $Corr3List $index]
            set ${sector}($plane.corr4) [lindex $Corr4List $index]
            set ${sector}($plane.pollutedBPMs) [lindex $PollutedBPMsList $index]
            
            incr index
        }
    }
}

proc StartMeasurement {args} {
    global sectorList outputDir measFile region abort OAGGlobal
    
    cd $outputDir
    set twissFile $OAGGlobal(BoosterLatticesDirectory)/default/refMatrices/booster.twi
    
    SetStatus "load reference ramp ..."
    if [catch {exec ramploadnew /home/helios/oagData/booster/ramps/correctors/lattices/default/HVCorr.ramp } result] {
        SetStatus "Error loading reference ramp: $result"
        return
    }
    after 5000
    foreach Sector $sectorList {
        global $Sector 
        if $abort {
            set abort 0
            SetStatus "aborted"
            return
        }
       # TogglePulsedMagnetEnables -location GuntoBoosterExt
        if [set ${Sector}(checked)] {
            SetStatus "wait 5 seconds..."
            after 5000
            SetStatus "Measure response for $Sector region $region..."
            if [catch {MeasureSectoreResponse -Sector $Sector -region $region} result] {
                SetStatus "Error1: $result"
                return
            }
            set ${Sector}(checked) 0
        }
    }
}

proc ProcessData {args} {
    set before ""
    set sector ""
    set after ""
    set region ""
    set plane ""
    APSParseArguments {before after sector region plane}

    global outputDir
    cd $outputDir
    switch $plane {
        h {
            set coord x
        }
        v {
            set coord y
        }
    }
    if [catch {exec sddsprocess $before -process=B*region${region}*$coord,ave,%sAve -pipe=out \
                 | sddscollapse -pipe \
                 | sddscollect -pipe=in $before.ave -col=suffix=:region${region}Avg:${coord}Ave,col=AveBefore 
        exec sddsprocess $after -process=B*region${region}*$coord,ave,%sAve -pipe=out \
                 | sddscollapse -pipe \
                 | sddscollect -pipe=in $after.ave -col=suffix=:region${region}Avg:${coord}Ave,col=AveAfter } result] {
        return -code error "ProcessData1: $result"
    }
    if [catch {exec sddsxref $before.ave $after.ave -pipe=out -match=Rootname -take=AveAfter \
                 | sddsprocess -pipe "-define=col,OrbitChange,AveAfter AveBefore -" \
                 -print=par,Sector,$sector \
                 | sddsconvert -pipe=in -rename=col,Rootname=BPMName $plane.$sector.region$region.resp } result] {
        return -code error "ProcessData2: $result"
    }
    exec sddsplot -col=BPMName,OrbitChange -topline=@Sector $plane.$sector.region$region.resp &
}

APSApplication . -name Booster3CorrResponse -version 1 \
  -overview {This application allows steering configuration of booster.  You may choose the lattice, which monitors and correctors to use, and restrict your choices to only good components.}
set Status ""
APSScrolledStatus .status -parent .userFrame -width 60 \
  -textVariable Status 


proc SetStatus {text} {
    global Status
    set Status $text
    update
}


proc APSBoosterApplySteeringsetpoints {args} {
    global OAGGlobal
    set region ""
    set sector ""
    set x 0
    set y 0
    set plane h
    set review 0
    set stepSizeLimit 0.1
    set statusCallback ""
    set dryRun 0
    set bpmx ""
    set bpmy ""
    APSParseArguments {region sector x y plane review stepSizeLimit statusCallback dryRun bpmx bpmy}

    set tmpRoot /tmp/[APSTmpString]
    set zeroDispBPMs {B1C0P1 B1C0P2  B2C9P2 B3C0P1 B3C0P2 B4C9P2}
    exec sddsmakedataset $tmpRoot.zeroDisp -col=BPMName,type=string -data=[join $zeroDispBPMs ,]
    switch $plane {
        H -
        h {
            set delta $x
            set coord x
            set plane h
            set bpm $bpmx
        }
        V -
        v {
            set delta $y
            set coord y
            set plane v
            set bpm $bpmy
        }
        default {
            return -code error "APSBoosterApplySteeringsetpoints1: Invalid plane provided."
        }
    }
    set twissFile $OAGGlobal(BoosterLatticesDirectory)/default/refMatrices/booster.twi
    set controllawDir  /home/helios/oagData/booster/localSteering/lattices/default/$sector
    
    if [catch {exec sddsmakedataset $tmpRoot.$plane.setpoint -col=BPMName,type=string \
                 -data=$bpm \
                 -col=delta,type=double -data=$delta } result] {
        return -code error "APSBoosterApplySteeringsetpoints2: $result"
    }


    if [catch {exec sddsquery -col $controllawDir/irm.$coord -sddsOutput=$tmpRoot.$plane.bpm
        exec sddsprocess $tmpRoot.$plane.bpm -pipe=out -match=col,Name=B*C*P* \
                 | sddsxref -pipe $tmpRoot.$plane.setpoint -fillIn -match=Name=BPMName -take=delta -nowarnings \
                 | sddsconvert -pipe=in $tmpRoot.$plane.bpm1 -retain=col,Name,delta } result] {
        return -code error "APSBoosterApplySteeringsetpoints3: $result"
    }
    
    #get the corrector delta changes
    if [catch {exec sddsmatrixmult $controllawDir/irm.$coord $tmpRoot.$plane.bpm1 -pipe=out \
                 | sddsxref -pipe $controllawDir/irm.$coord -nowarnings -take=ControlName \
                 | tee $tmpRoot.$plane.corr \
                 | sddsprocess -pipe=in "-redefine=col,absChange,delta abs" -process=absChange,max,MaxChange \
                 $tmpRoot.$plane.corr1 } result] {
        return -code error "APSBoosterApplySteeringsetpoints4: $result"
    }
    
    if [catch {exec sddsxref $tmpRoot.$plane.corr $twissFile -nowarnings -pipe=out \
                 -match=ControlName=ElementName -take=eta$coord \
                 | sddsprocess -pipe "-define=col,corrEta,delta eta$coord *" \
                 | sddsprocess -pipe -process=corrEta,sum,corrEtaSum \
                 | sdds2stream -pipe=in -par=corrEtaSum } corrEta] {
        return -code error "APSBoosterApplySteeringsetpoints4a: $corrEta"
    }

    #get the in-bound bpm setpoint changes
    set refMatrix $controllawDir/$plane.steering.rm
    
    set bpmList [exec sdds2stream -col=BPMName $controllawDir/$plane.steering.rm]
   
    if [catch {exec sddsmatrixmult  $refMatrix  $tmpRoot.$plane.corr  -pipe=out \
                 | sddsxref -pipe $refMatrix -take=BPMName \
                 | sddsprocess -pipe=in $tmpRoot.bpm$plane \
                  "-redefine=col,delta,delta,units=mm" } result] {
        return -code error "APSBoosterApplySteeringsetpoints5: $result"
    }
    set bpmList [exec sdds2stream -col=BPMName $tmpRoot.bpm$plane]
    set index [lsearch -exact $bpmList $bpm]
    if [catch {exec sddsprocess $tmpRoot.bpm$plane "-define=par,SteerPos,$index" "-define=par,SteerRequest,$delta" -nowarnings } result] {
        return -code error "APSBoosterApplySteeringsetpoints5a: $result"
    }
    
    if [catch {exec sddsselect $tmpRoot.bpm$plane $tmpRoot.zeroDisp -match=BPMName -invert -pipe=out \
                 | sddsprocess -pipe -process=delta,sum,deltaSum \
                 | sdds2stream -pipe=in -par=deltaSum} bpmDelta] {
        return -code error "APSBoosterApplySteeringsetpoints5b: $bpmDelta"
    }
    if $review {
        exec sddsplot -par=SteerPos,SteerRequest -grap=sym,scale=2,fill,sub=1 -leg=spec=SteerRequest $tmpRoot.bpm$plane \
          -col=BPMName,delta  -grap=sym,conn,scale=2,fill -axes=x "-topline=corrEtaSum [format %.3e $corrEta]; bpm delta sum [format %.3e $bpmDelta]" \
          "-title=Sector $sector $plane plane expected bpm setpoint changes" $tmpRoot.bpm$plane  &
        after 1000
        set files [glob $tmpRoot.*]
       # APSAddToTmpFileList -ID steering -fileList $files
        return
    }
    
    set deltaList [exec sdds2stream -col=delta $tmpRoot.bpm$plane]
    set bpmList [exec sdds2stream -col=BPMName $tmpRoot.bpm$plane]
    set maxChange [exec sdds2stream -par=MaxChange $tmpRoot.$plane.corr1]
    set steps [format %.0f [expr $maxChange/$stepSizeLimit + 1]]
    
    set corrList [exec sdds2stream -col=ControlName $tmpRoot.$plane.corr]
    set corrChangeList [exec sdds2stream -col=delta $tmpRoot.$plane.corr]
    
   # set steps 1
    set putcomm ""
    foreach bpm $bpmList delta $deltaList {
        set pv ${bpm}:region$region:$coord:SetpointAO
        set value [expr $delta /($steps*1.0)]
        lappend putcomm $pv=$value
    }
    set putcomm1 ""
    set ampList ""
    foreach corr $corrList delta $corrChangeList {
       # lappend putcomm1 B:$corr:CorrectionAI=[expr $delta/($steps*1.0)]
        lappend ampList [expr $delta/($steps*1.0)]
    }
    for {set i 0} {$i<$steps} {incr i} {
        if [string length $statusCallback] {
            eval $statusCallback {"Step $i ..."}
        }
        if !$dryRun {
            SetStatus "step $i : $putcomm; $ampList [join $corrList ,]"
            if [catch {exec cavput -list=[join $putcomm ,] -delta -pend=30} result] {
                return -code error "APSBoosterApplySteeringsetpoints6: Error in setting bpm  setpoints: $result"
            }
            #if [catch {exec cavput -list=[join $putcomm1 ,] -delta -pend=30 } result] {
            #    return -code error "APSBoosterApplySteeringsetpoints7: Error in setting corrector setpoints: $result"
            #}
            if [catch {exec makeboosterbump -correctorList=[join $corrList ,] -amplitude=[join $ampList ,] -regions=$region -outRampFile=$tmpRoot.ramp.$i } result] {
                return -code error "APSBoosterApplySteeringsetpoints8: Error in setting corrector ramps: $result"
            }
        }
    }
    return
}

proc MeasureSectoreResponse {args} {
    set Sector ""
    set region ""
    APSParseArguments {Sector region}
    
    global outputDir OAGGlobal
    
    cd $outputDir
    set twissFile $OAGGlobal(BoosterLatticesDirectory)/default/refMatrices/booster.twi
    global $Sector
    set controllawDir  /home/helios/oagData/booster/localSteering/lattices/default/$Sector
  
    foreach plane {h v} {
        if {$Sector=="B2C9" && $plane=="v"} {
            continue
        }
        set bpm [set ${Sector}($plane.BPMName)]
        switch $plane {
            h {
                set coord x
                set measFile  /home/helios/oagData/booster/responseMeasFiles/measH.sdds
            }
            v {
                set coord y
                set measFile  /home/helios/oagData/booster/responseMeasFiles/measV.sdds
            }
        }
        SetStatus "Create $bpm setpoint change of 1mm file h.$Sector.$bpm.setpoint"
        if [catch {exec sddsmakedataset $plane.$Sector.$bpm.setpoint -col=BPMName,type=string \
                     -data=$bpm \
                     -col=delta,type=double -data=1.0 } result] {
            return -code error "Error1: $result"
        }
        if [catch {exec sddsquery -col $controllawDir/irm.$coord -sddsOutput=$plane.$Sector.bpm
            exec sddsprocess $plane.$Sector.bpm -pipe=out -match=col,Name=B*C*P* \
                     | sddsxref -pipe $plane.$Sector.$bpm.setpoint -fillIn -match=Name=BPMName -take=delta -nowarnings \
                     | sddsconvert -pipe=in $plane.$Sector.$bpm.delta -retain=col,Name,delta } result] {
                    return -code error "Error2: $result"
        }
        SetStatus "Compute corrector delta..."
        if [catch { exec sddsmatrixmult $controllawDir/irm.$coord $plane.$Sector.$bpm.delta -pipe=out \
                      | sddsxref -pipe=in $controllawDir/irm.$coord -nowarnings -take=ControlName \
                      $plane.$Sector.corr.delta \
                  } result] {
            return -code error "Error3: $result"
        }
    }
    SetStatus "Measure orbit before changing corrector..."
    foreach plane {h v} {
        if {$Sector=="B2C9" && $plane=="v"} {
            continue
        }
        switch $plane {
            h {
                set coord x
                set measFile  /home/helios/oagData/booster/responseMeasFiles/measH.sdds
            }
            v {
                set coord y
                set measFile  /home/helios/oagData/booster/responseMeasFiles/measV.sdds
            }
        }
        if [catch {exec sddsmonitor $measFile $plane.$Sector.orbit.before -erase -steps=10 -interval=1 } result] {
            return -code error "Error read orbit1: $result"
        }
    }
    if [catch {exec cavget -list=B:OrbitControllaw -list=X,Y -list=SDDS.RUN -pend=20 } runs] {
        return -code error "Error reading orbit correction control pv: $runs"
    }
    set hRunning [lindex $runs 0]
    set vRunning [lindex $runs 1]
    SetStatus "Make corrector bump..."
    foreach plane {h v} coord {X Y} {
        if {$Sector=="B2C9" && $plane=="v"} {
            continue
        }
        if [set ${plane}Running] {
            #pause orbit correction
            if [catch {exec cavput -list=B:OrbitControllaw${coord}SDDS.SUSP=1 -pend=10 } result] {
                return -code error "Error suspend $plane orbit correction: $result"
            }
        }
        set corrList [exec sdds2stream -col=ControlName $plane.$Sector.corr.delta]
        set deltaList [exec sdds2stream -col=delta $plane.$Sector.corr.delta]
        if [catch {exec makeboosterbump -correctorList=[join $corrList ,] -amplitude=[join $deltaList ,] \
                     -regions=$region -outRampFile=$plane.$Sector.ramp} result] {
            return -code error "APSBoosterApplySteeringsetpoints8: Error in setting corrector ramps: $result"
        }
    }
    
    
    SetStatus "Waiting 5 seconds ..."
    after 5000
    SetStatus "Taking orbit after after corrector bump..."
    foreach plane {h v} {
        if {$Sector=="B2C9" && $plane=="v"} {
            continue
        }
        switch $plane {
            h {
                set coord x
                set measFile  /home/helios/oagData/booster/responseMeasFiles/measH.sdds
            }
            v {
                set coord y
                set measFile  /home/helios/oagData/booster/responseMeasFiles/measV.sdds
            }
        }
        if [catch {exec sddsmonitor $measFile $plane.$Sector.orbit.after -erase -steps=10 -interval=1 } result] {
            return -code error "Error read orbit1: $result"
        }
    }
    
    if [catch {exec ramploadnew /home/helios/oagData/booster/ramps/correctors/lattices/default/HVCorr.ramp } result] {
        return -code error "Error loading reference ramp: $result"
    }
    after 2000
    foreach plane {h v} coord {X Y} {
        if [set ${plane}Running] {
            #resume orbit correction
            
            if [catch {exec cavput -list=B:OrbitControllaw${coord}SDDS.SUSP=0 -pend=10 } result] {
                return -code error "Error resume $plane orbit correction: $result"
            }
        }
    }
    if {$hRunning || $vRunning} {
        after 5000
    }
    SetStatus "processing orbit change..."
    foreach plane {h v} {
        
        if {$Sector=="B2C9" && $plane=="v"} {
            continue
        }
        if [catch {ProcessData -before $plane.$Sector.orbit.before -after $plane.$Sector.orbit.after -sector $Sector \
                     -plane $plane \
                     -region $region } result] {
            return -code error $result
        }
    }
    SetStatus "done."
}

proc StartADT {args} {
    global region
    
    exec adt -geometry +30+0 -f /home/helios/oagData/ADTFiles/booster/booster.bpmError.pv-region$region.pv -d &
}

set mainDir /home/helios/oagData/booster/localSteering/lattices/default
set measFile /home/helios/oagData/booster/responseMeasFiles/measH.sdds
ReadBoosterSteeringConfig 
set statusCallback SetStatus
set outputDir [APSGoToDailyDirectory -subdirectory booster3CorrResponse]
APSLabeledEntry .dir -parent .userFrame -label "Output directory:" -textVariable outputDir -width 75

set varList ""
foreach sector $sectorList {
    set ${sector}(checked) 1
    lappend varList ${sector}(checked)
}

APSCheckButtonFrame .sectors -parent .userFrame -label "Sector:" -buttonList $sectorList  \
    -variableList $varList -allNone 1 \
    -orientation horizontal -limitPerRow 10


#MakeSectorFrame .sectors -parent .userFrame
set abort 0
set region 0
APSRadioButtonFrame .region -parent .userFrame -label "Steering time region:" -buttonList {0 1 2 3 4 5 6 7 8 9} \
  -variable region -valueList {0 1 2 3 4 5 6  7 8 9} -orientation horizontal
APSButton .adt -parent .userFrame -text "ADT" -command "StartADT"
#APSButton .start -parent .userFrame -text "Start" -command StartMeasurement
APSButton .meas -parent .userFrame -text "Measure HV Response" -command "StartMeasurement"
#APSButton .vmeas -parent .userFrame -text "Measure V response" -command "StartMeasurement -plane v"
APSButton .abort -parent .userFrame -text "Abort" -command "set abort 1"
