#!/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)]
APSStandardSetup
set removeXrayBPMs 1
set ignoreLock 0
set simulator 0
set autoUBOP 1
set verbose 0
set args $argv
APSParseArguments {removeXrayBPMs ignoreLock autoUBOP simulator verbose}

# catch {exec mplayer -quiet /home/helios/SR/Music/ipanema.mp3 -volume 50 > /dev/null &} musicPID

set steerStatus ""
set runControlPV SRID:AutoSteeringRC
if [catch {exec cavget -list=$runControlPV.RUN -pend=10} running] {
    puts stderr "Error reading $runControlPV $result"
    exit 1
}
if {$running} {
    APSAlertBox .autost -name "AutoSteering running" -errorMessage "Another auto steering is running, can not start!"
    exit 1
}

if $simulator {
    set removeXrayBPMs 0
}

APSApplication . -name SRBeamlineSteeringServer -overview "Start SR Beamline steering server."
APSScrolledStatus .status -parent .userFrame -width 150 -height 40 \
        -textVariable steerStatus

proc SetBeamSteerStatus {text} {
    global steerStatus
    set steerStatus "[exec date] $text"
    update
}


APSButton .start -parent .userFrame -text "Start" -command "StartAutoSteering"
APSButton .abort -parent .userFrame -text "Abort" -command "Abort"
APSButton .reset -parent .userFrame -text "Reset" -command "ResetBLSteeringStatus" \
    -contextHelp "Press this button, if the steering request PVs are stuck. Need to use this for example when a steering is aborted by some software error."

APSDisableButton .userFrame.start.button
proc SaveSteerResultToOps {args} {
    eval after 1 SaveSteerResultToOps2 $args
}

proc SaveSteerResultToOps2 {args} {
    set sector ""
    set source ID
    # values of side are "" us ds
    set side ""
    APSParseArguments {review sector side source}

    set sectorf [format %02d $sector]
    global $source${sectorf}${side}BLSaveSteerReq
    if ![set $source${sectorf}${side}BLSaveSteerReq] {
        return
    }
    SetBeamSteerStatus "Save $source${sectorf}${side} steering to operation ..."
    if [catch {exec cavget -list=$source${sectorf}${side}:BLbpmSnapshot -pend=10} snapshot] {
        return -code error "Error in obtaining orbit setpoint snapshot filename of $source${sectorf}${side} : $snapshot"
    }
    if ![string length $snapshot] {
        SetBeamSteerStatus "snapshot is empty."
        return
    }
    SetBeamSteerStatus "Installing $snapshot to new SR UBOP..."
    if [catch {APSSCRInstallPreferredFile -system SR -choice "User beam operator preferred" -filename $snapshot } result] {
        return -code error "Error installing SR UBOP for $source${sectorf}${side} steering: $result"
    }
    global $source${sectorf}${side}BLSteerStatus errorCode
# shouldn't this be done to the BLSteeringErrMsg instead?
    set $source${sectorf}${side}BLSteerStatus "Steering was saved to ops."
    set varList [list $source${sectorf}${side}BLSteerStatus]
    if [pv putw $varList] {
        puts stderr "Error setting status and error: $errorCode"
    }
    SetBeamSteerStatus "done."
}

proc ResetBLSteeringStatus {args} {
    PingRunControl
    SetBeamSteerStatus "Wait for BLSteerStatus=0..."
    catch {exec caput SRID:SteeringLockBI 0 >& /dev/null}
    catch {exec cavput -list=ID,BM -range=beg=1,end=35,format=%02ld -list=:BLSteerStatus=0 -pend=5 -blunder >& /dev/null}
    catch {exec cavput -list=ID -range=beg=1,end=35,format=%02ld -list=us,ds -list=:BLSteerStatus=0 -pend=5 -blunder >& /dev/null}

    PingRunControl
    SetBeamSteerStatus "Wait for BLSteerStart=0..."
    catch {exec cavput -list=ID,BM -range=beg=1,end=35,format=%02ld -list=:BLSteeringStart=0 -pend=5 -blunder >& /dev/null}
    catch {exec cavput -list=ID -range=beg=1,end=35,format=%02ld -list=us,ds -list=:BLSteeringStart=0 -pend=5 -blunder >& /dev/null}

    PingRunControl
    SetBeamSteerStatus "Wait for RequestSteering=0..."
    catch {exec cavput -list=ID,BM -range=beg=1,end=35,format=%02ld -list=:RequestSteering=0 -pend=5 -blunder >& /dev/null}
    catch {exec cavput -list=ID -range=beg=1,end=35,format=%02ld -list=us,ds -list=:RequestSteering=0 -pend=5 -blunder >& /dev/null}
    SetBeamSteerStatus "Done clearing PVs."

}

proc Abort {args} {
    global runControlPV
    if [catch {exec cavput -list=${runControlPV}.ABRT=1 -pend=30} result] {
        return -code error "Error aborting: $result"
    }
}

set stepSizeLimit 1
proc ReturnWithError {args} {
    set text ""
    set sector ""
    set source ID
    set side ""
    APSParseArguments {sector text source side}

    set sectorf [format %02d $sector]
    global $source${sectorf}${side}BLSteerStatus $source${sectorf}${side}BLSteeringErrMsg errorCode  SRIDSteerLock $source${sectorf}${side}BLSteeringStart
    #Add momentary setting of alarms inside the procedure "ReturnWithError"
    set $source${sectorf}${side}BLSteerStatus 5
    if [pv putw $source${sectorf}${side}BLSteerStatus] {
        puts stderr "Error setting status: $errorCode"
    }
    after 1000
    set $source${sectorf}${side}BLSteeringErrMsg "$text"
    set $source${sectorf}${side}BLSteerStatus 3
    set $source${sectorf}${side}BLSteeringStart 0
    set SRIDSteerLock 0
    set varList [list $source${sectorf}${side}BLSteerStatus  $source${sectorf}${side}BLSteeringErrMsg $source${sectorf}${side}BLSteeringStart  SRIDSteerLock]
    SetBeamSteerStatus "$text"
    if [pv putw $varList] {
        puts stderr "Error setting status and error: $errorCode"
    }
    #log error message
    if [catch {exec  logMessage  -sourceId=steeringError \
                   -tag=Instance ${source}${sector}$side -tag=Error "$text"  \
               } result ] {
        APSAlertBox .alert -errorMessage "SteeringServer: Error with logMessage: $result"
        return
    }
    #  APSRunControlExit
}

proc ApplySetpoints {args} {
    eval after 1 ApplySetpoints2 $args
}

proc ApplySetpoints2 {args} {
    set sector ""
    set review 0
    set source ID
    # values of side are "" us ds
    set side ""
    APSParseArguments {review sector side source}
    
    set sectorf [format %02d $sector]
    global P0Sector FFWaveformFile  errorCode stepSizeLimit SRIDSteerLock removeXrayBPMs ignoreLock simulator
    global ID${sectorf}${side}BLSteeringStart ID${sector}${side}VarList autoUBOP
    global BM${sectorf}BLSteeringStart BM${sector}VarList
    set varList [set ${source}${sector}${side}VarList]
    eval global $varList

# why the varList1 ?   
#    set varList1  [set BM${sector}VarList]
#    eval global $varList1
  
    if ![string length $sector] {
        SetBeamSteerStatus "AppySetpoints: No sector variable specified in IDSteeringDialog or equivalent"
        return
    }
    if !$ignoreLock {
        #if [catch {APScavget -list=SRID:SteeringLockBI -pend=10 } lock] {
        #    SetBeamSteerStatus "Error reading SRID:SteeringLockBI: $lock"
        #    catch {ReturnWithError -text "lock reading error" -sector $sector  -source $source -side $side}
        #    return
        #}
        #set SRIDSteerLock $lock
        #if [pv getw SRIDSteerLock 30] {
        #    SetBeamSteerStatus "Error reading SRID:SteeringLockBI: $errorCode  - $SRIDSteerLock"
        #    catch {ReturnWithError -text "lock reading error" -sector $sector  -source $source -side $side}
        #    return
        #}
        SetBeamSteerStatus "Global steering lock status: $SRIDSteerLock ; start sector $sector${side} steering: [set ${source}${sectorf}${side}BLSteeringStart] "
        if $SRIDSteerLock {
            SetBeamSteerStatus "Steering is locked by another sector, steering request for $sector${side} is not processed and ignored."
            return
        }
    }
    PingRunControl
    
   # if [pv getw ${source}${sectorf}${side}BLSteeringStart 10] {
   #     SetBeamSteerStatus "Error reading ${source}${sectorf}${side}:BLSteeringStart: $errorCode"
   #     catch {ReturnWithError -text "pv reading error" -sector $sector -source $source -side ${side} }
    #    return
   # }
    if ![set $source${sectorf}${side}BLSteeringStart] {
        return
    }
    SetBeamSteerStatus "Start steering for $source $sector${side} ..."
    SetBeamSteerStatus "Reading xp yp request of $sector${side} ..."
    set varList [list $source${sectorf}${side}BLSteeringXp  $source${sectorf}${side}BLSteeringYp]
    if [pv getw $varList 30] {
        catch {ReturnWithError -sector $sector  -source $source -side ${side} -text "Error in reading xp and yp" }
        return
    }
    set steerXp [expr [set $source${sectorf}${side}BLSteeringXp]/1000.0]
    set steerYp [expr [set $source${sectorf}${side}BLSteeringYp]/1000.0]
    SetBeamSteerStatus "Steering $source $sector${side}: $steerXp $steerYp"
    if {$steerXp==0 && $steerYp==0} {
        set $source${$sectorf}${side}BLSteeringMsg "Xp and Yp deltas are zero, no steering."
        SetBeamSteerStatus "Xp and Yp deltas are zero, no steering."
        if [pv putw $source${sectorf}${side}BLSteeringMsg] {
            catch {ReturnWithError -text "Error setting status" -sector $sector -side ${side} -source $source}
        }
        return
    }
    #check if XRDP is running, lock steeering if XRDP is running, otherwise do not lock
    if !$simulator {
        if [catch  {APSCheckControllawRunning -config h.defaultXRDP} hrunning] {
            catch {ReturnWithError -text "Error check hXRDP" -sector $sector -side ${side} -source $source}
            return
        }
        if [catch  {APSCheckControllawRunning -config v.defaultXRDP} vrunning] {
            catch {ReturnWithError -text "Error check vXRDP" -sector $sector -side ${side} -source $source}
            return
        }
        if {!$ignoreLock && ($hrunning || $vrunning)} {
            #lock steering if XRDP is running
            set SRIDSteerLock 1
            if [pv putw SRIDSteerLock] {
                catch {ReturnWithError -sector $sector -side ${side}  -source $source -text "Error in setting steering lock." }
                return
            }
        }
    }
    PingRunControl
    if $removeXrayBPMs {
        SetBeamSteerStatus "Check and remove sector $sector xbpms if they are in orbit correction..."
        # Here source is expected to be only the BM and ID, no CU side specified.
        if [catch {APSSRRemoveXrayBPMFromOrbitCorrection -sector $sector  -restart 1 -source $source -side $side} result] {
            SetBeamSteerStatus "Error in removing sector $sector xray bpms from orbit correction: $result"
            catch {ReturnWithError -text "Error removing xbpm" -sector $sector -side ${side}   -source $source}
            return
        }
        SetBeamSteerStatus "$result"
        PingRunControl
    }
    
    #BM does not need to check BPLD limits. I don't think CUs need to check BPLDs either.
    
    if {!$simulator && [concat ${source} ${side}]=="ID"} {
        SetBeamSteerStatus "Check BPLD alarm limits for sector $sector..."
        set ID${sectorf}BLSteeringMsg "Checking BPLD alarms limits"
        if [pv putw ID${sectorf}BLSteeringMsg] {
            catch {ReturnWithError -text "Error setting status" -sector $sector -side ${side} -source $source }
            return
        }
        
        if [catch {CompareSetpointsWithBPLDLimit -sector $sector -side ${side} -steerXp $steerXp -steerYp $steerYp} result] {
            SetBeamSteerStatus "Error in checking BPLD limit: $result"
            catch {ReturnWithError -text "Error checking BPLD limit" -sector $sector -side ${side} -source $source}
            PingRunControl
            return
        } else {
            SetBeamSteerStatus "The setpoints of $sector bpms are within the BPLD minor alarm limits."
        }
    }
    PingRunControl
    
    if $P0Sector($sector) {
        set digit 0
    } else {
        set digit 1
    }
    set Sn $sector
    set Sn1 [exec rpnl "$Sn 1 + 40 > pop ? 40 - : \$"]
    global R11 R12 R33 R34
    if {$source=="BM"} {
        set bpm1 S${Sn}B:P4
        set bpm2 S${Sn}B:P3
        set bpmList [list $bpm1 $bpm2]
    } elseif {$source=="ID"} {
        switch $side {
            "" {
                set bpm1 S${Sn}B:P${digit}
                set bpm2 S${Sn1}A:P${digit}
                set bpmList [list $bpm1 $bpm2]
            } 
            us {
                set bpm1 S${Sn}B:P0
                set bpmList $bpm1 
            }
            ds {
                set bpm1 S${Sn}C:P0
		set bpm2 S${Sn1}A:P0
                set bpmList [list $bpm1 $bpm2]
            }
        }
    }
    switch $source {
        BM {
            set namedSource $source
        }
        ID {
            switch $side {
                "" {set namedSource $source}
                us {set namedSource CUUSP0}
                ds {set namedSource CUDSP0}
            }
        }
        default {
            set namedSource $source 
        }
    }
    set controllawDir  /home/helios/oagData/sr/localSteering/lattices/default/${namedSource}s/[format %02ld ${sector}]$namedSource
    
    SetBeamSteerStatus "Starting steering sector $sector${side} ..."
    set $source${sectorf}${side}BLSteerStatus 2
    if [pv putw $source${sectorf}${side}BLSteerStatus] {
        puts stderr "Error setting steer status: $errorCode"
        catch {ReturnWithError -text "Error setting status" -sector $sector -side ${side} -source $source} 
        return
    }
    SetBeamSteerStatus "Steer $source $sector $side xp=$steerXp yp=$steerYp"
    foreach pl {h v} coord {x y}  Coord {X Y} {
        set deltap [set steer${Coord}p]
        SetBeamSteerStatus "$source $sector $pl $deltap"
        if {$deltap==0} {
            SetBeamSteerStatus "No setpoint changes needed for $pl plane of sector $sector."
            continue
        }
        switch $source {
            BM {
                switch $coord {
                    x {
                        set deltaBP4 [expr $R12(BP4) * $deltap]
                        set deltaBP3 [expr $R12(BP3) * $deltap]
                    }
                    y {
                        set deltaBP4 [expr $R34(BP4) * $deltap]
                        set deltaBP3 [expr $R34(BP3) * $deltap]
                    }
                }
                set deltaList [list $deltaBP4 $deltaBP3]
            }
            ID {
                switch $side {
                    "" {
                        if {$coord=="x"} {
                            set deltaBP [expr -$R12($sector) * $deltap]
                            set deltaAP [expr $R12($sector) * $deltap]
                        } else {
                            set deltaBP [expr -$R34($sector) * $deltap]
                            set deltaAP [expr $R34($sector) * $deltap]
                        }
                        set deltaList [list $deltaBP $deltaAP]
                    }
                    us {
                        if {$coord=="x"} {
                            set deltaBP [expr -$R12(us) * $deltap]
                        } else {
                            set deltaBP [expr -$R34(us) * $deltap]
                        }
                        set deltaList $deltaBP
                    }
                    ds {
                        if {$coord=="x"} {
                            set deltaCP [expr $R12(dsc0) * $deltap]
                            set deltaAP [expr $R12(ds) * $deltap]
                        } else {
                            set deltaCP [expr $R34(dsc0) * $deltap]
                            set deltaAP [expr $R34(ds) * $deltap]
                        }
                        set deltaList [list $deltaCP $deltaAP]
                    }
                }
            }
            default {
                set $source${sectorf}${side}BLSteerStatus 2
                if [pv putw $source${sectorf}${side}BLSteerStatus] {
                    puts stderr "Error setting steer status: $errorCode"
                    catch {ReturnWithError -text "Error setting status" -sector $sector -side ${side} -source $source} 
                    return
                }
                return
            }
        }
        if !$review  {
            # skip id steering of CU
            if {$side==""} {
                SetBeamSteerStatus "change corr range for sector $sector..."
                #set $source${sectorf}${side}BLSteeringMsg  "Change corrector rangeErrors..."
                #if [pv putw $source${sectorf}${side}BLSteeringMsg] {
                #    catch {ReturnWithError -sector $sector  -side ${side} -source $source -text "Error in set status." }
                #    return
                #}
                if [catch {exec sddscasr -restore $controllawDir/$pl.corr.range } result] {
                    catch {ReturnWithError -sector $sector  -side ${side} -source $source -text "Error change corr range." }
                    return
                }
            }
        }
        PingRunControl
        SetBeamSteerStatus "Start sector $sector${side} $pl plane steering..."
        set $source${sectorf}${side}BLSteeringMsg "Start steering $pl plane"
        if [pv putw $source${sectorf}${side}BLSteeringMsg] {
            catch {ReturnWithError -sector $sector  -side ${side} -source $source -text "Error set status." }
            return
        }
        PingRunControl
        SetBeamSteerStatus "Apply setpoints for sector $sector$side..."
        if [catch {APSSRApplySteeringSetpoints -sector $sector  -source $namedSource -plane $pl \
                       -xp [expr $steerXp*1000.0] -yp [expr $steerYp*1000] \
                       -bpmList $bpmList -deltaList $deltaList \
                       -FFWaveform $FFWaveformFile($pl) \
                       -review $review -dryRun 0 \
                       -statusCallback "" \
                       -stepSizeLimit $stepSizeLimit \
                       -refMatrix $controllawDir/${pl}.steering.rm } result] {
            SetBeamSteerStatus "Error in applying setpoints for sector $sector${side}:  $result"
            if !$review {
                if [catch {exec sddscasr -restore $controllawDir/$pl.corr.range.orig } result] {
                    SetBeamSteerStatus "Error restoring corrector rangeErrors: $result"
                }
            }
            catch {ReturnWithError -sector $sector -side $side -source $source-text "Error apply setpoints." }
            return
        }
        SetBeamSteerStatus "Steering completed for $pl plane."
        PingRunControl
        set $source${sectorf}${side}BLSteeringMsg "steered $pl."
        if [pv putw $source${sectorf}${side}BLSteeringMsg] {
            puts stderr "Error in setting ID $sector steering status message: $errorCode"
        }
        PingRunControl
        if !$review {
            SetBeamSteerStatus "Restore corr range errors after steering ($pl plane)..."
            set $source${sectorf}${side}BLSteeringMsg "restore corr rangeErrors"
            if [pv putw $source${sectorf}${side}BLSteeringMsg] {
                puts stderr "Error in setting ID $sector${side} steering status message: $errorCode"
            }
            set corrList [exec sddsquery -col $controllawDir/${pl}.steering.rm | grep S]
            if [catch {exec sddsmakedataset -pipe=out -col=CorrName,type=string \
                           -data=[join $corrList ,] \
                           | sddsprocess -pipe -edit=col,ControlName,CorrName,ei/:DacAI/ \
                           | sddscasr -pipe -save -pend=30 \
                           | sddsprocess -pipe -reedit=col,ControlName,%/DacAI/RangeErrorCALC.B/ \
                           | sddscasr -pipe=in -restore } result] {
                SetBeamSteerStatus "Error in transfer DacAI: $result"
                
                if [catch {exec sddscasr -restore $controllawDir/$pl.corr.range.orig } result] {
                    put stderr  "Error restoring corrector rangeErrors: $result"
                }
                catch {ReturnWithError -sector $sector -side ${side}  -source $source -text "Error in transfer DacAI." } 
                return
            }
            PingRunControl
            if [catch {exec sddscasr -restore $controllawDir/$pl.corr.range.orig } result] {
                puts "Error in restoring original corrector range error: $result"
                catch {ReturnWithError -sector $sector -side ${side}  -source $source -text "Error in restore corr range." }
                return 
            }
        }
        PingRunControl
    }
    SetBeamSteerStatus "$source sector $sector${side} steering done."
    if {0} {
        #the accumulator was updated in libary code, do not need do it here.
        set varList [list $source${sectorf}${side}XpActualAccum $source${sectorf}${side}YpActualAccum]
        if [pv getw $varList 20] {
            catch {ReturnWithError -sector $sector  -source $source -side $side -text "Error in reading sector $sector accumulator pvs." }
            return
        }
        
        set $source${sectorf}${side}XpActualAccum [expr [set $source${sectorf}${side}XpActualAccum] + $steerXp]
        set $source${sectorf}${side}YpActualAccum [expr [set $source${sectorf}${side}YpActualAccum] + $steerYp]
        SetBeamSteerStatus "Updating accumulators for sector $sector..."
        if [pv putw $varList] {
            catch {ReturnWithError -sector $sector -side $side -source $source -text "Error in update sector $sector accumulator." } 
            return
        }
    }
    
    SetBeamSteerStatus "Wrap up steering for sector $sector${side}."
    set $source${sectorf}${side}BLSteerStatus 3
    set $source${sectorf}${side}BLSteeringStart 0
    set SRIDSteerLock 0
    set $source${sectorf}${side}BLSteeringMsg "Steering completed."
    set $source${sectorf}${side}BLSteeringErrMsg ""
    set varList [list $source${sectorf}${side}BLSteerStatus $source${sectorf}${side}BLSteeringStart $source${sectorf}${side}BLSteeringMsg SRIDSteerLock $source${sectorf}${side}BLSteeringErrMsg]
    if [pv putw $varList] {
        puts "Error setting steering done: $errorCode."
    }
    PingRunControl
    # save SR. For now use the smaller special files in order to make this quick.
    if [catch {APSSRSaveSteeringToOps -installUBOP $autoUBOP \
                 -description "Auto steering save: $source${sectorf}${side} deltaXp: [set $source${sectorf}${side}BLSteeringXp] deltaYp: [set $source${sectorf}${side}BLSteeringYp]" \
                 -statusCallabck SetBeamSteerStatus } snapshot] {
        SetStatuts "Error in SCR save: $snapshot"
        return
    }
    
    if [catch {exec cavput -list=$source${sectorf}${side}:BLbpmSnapshot=$snapshot  -pend=20} result] {
        return -code error "Error setting snapshot filename for $source${sectorf}${side}: $result"
    }
    return
}


proc CompareSetpointsWithBPLDLimit {args} {
    global R11 R12 R33 R34 
    set sector ""
    set steerXp 0
    set steerYp 0
    set debug 0
    APSParseArguments {sector steerXp steerYp debug}
    if ![string length $sector] {
        return -code error "No sector variable specified in IDSteeringDialog"
    }
    if [catch {exec sdds2stream /home/helios/oagData/sr/BPLDs/sectors.sdds -col=Sector} BPLDList] {
        return -code error $BPLDList
    }
    set index [lsearch -exact $BPLDList $sector]
    if {$index<0} {
        #no need to check
        return
        # return -code error "Sector $sector is not included in BPLD sectors.sdds file."
    }
    set Sn $sector
    set Sn1 [exec rpnl "$Sn 1 + 40 > pop ? 40 - : \$"]
    # only the P1 are the BPLDs now
    if [catch {exec cavget -pend=30 -list=S${Sn}B:P1,S${Sn1}A:P1 \
                   -list=:dbpld: \
                   -list=x,y -list=hi,lo -list=AO.VAL -pend=30} tripLimits] {
        return -code error $tripLimits
    }
    foreach {bxUp bxLow byUp byLow axUp axLow ayUp ayLow} $tripLimits {}
    set badList ""
    set digit 1
    if [catch {exec cavget -list=S${Sn}B:,S${Sn1}A: -list=P${digit}:ms: \
                 -list=x,y -list=:GainAO -pend=30 -printErrors} gainList] {
        return -code error "Error in reading gain value of sector $sector pvs."
    }
    if [catch {exec cavget -list=S${Sn}B:,S${Sn1}A: -list=P${digit}:ms: \
                 -list=x,y -list=:SetpointAO -pend=30 -printErrors} setpointList] {
        return -code error "Error in reading setpoint value of sector $sector pvs."
    }
    if [catch {exec cavget -list=S${Sn}B:,S${Sn1}A: -list=P${digit}:ms: \
                 -list=x,y -list=:OffsetAO -pend=30 -printErrors} offsetList] {
        return -code error "Error in reading offset value of sector $sector pvs."
    }
    if $debug {
        puts stdout "tripLimits $tripLimits"
        puts stdout "gain $gainList"
        puts stdout "setpoint $setpointList"
        puts stdout "offset $offsetList"
    }
    foreach {BPxGain BPyGain APxGain APyGain} $gainList {} 
    foreach {BPx BPy APx APy} $setpointList {}
    foreach {BPxOffset BPyOffset APxOffset APyOffset} $offsetList {}

    # changed to use setpoints plus offsets divided by gain, instead of only setpoints 11/20/2013, H. Shang
    # which is important because BPLDs only reads back raw bpm values.
    set deltabx [expr  -$R12(P1) * $steerXp]
    set deltaax [expr  $R12(P1) * $steerXp]
    set deltaby [expr  -$R34(P1) * $steerYp]
    set deltaay [expr  $R34(P1) * $steerYp]

    set bx [expr ($BPx + $deltabx + $BPxOffset) / $BPxGain]
    set by [expr ($BPy + $deltaby + $BPyOffset) / $BPyGain]
    set ax [expr ($APx + $deltaax + $APxOffset) / $APxGain]
    set ay [expr ($APy + $deltaay + $APyOffset) / $APyGain]
    if $debug {
        puts stdout "Compare with limits"
        puts stdout "tripLimits $tripLimits"
        puts stdout "bx=$bx by=$by ax=$ax ay=$ay"
    }
    set badList ""
    set alarmMargin 0.3
    if {$bx<[expr $bxLow+$alarmMargin] || $bx>[expr $bxUp-$alarmMargin]} {
        append badList "The setpoint of S${Sn}B:P${digit}:x is outside of BPLD minor alarm limit.\n"
    }
    if {$by<[expr $byLow+$alarmMargin] || $by>[expr $byUp-$alarmMargin]} {
        append badList "The setpoint of S${Sn}B:P${digit}:y is outside of BPLD minor alarm limit.\n"
    }
    if {$ax<[expr $axLow+$alarmMargin] || $ax>[expr $axUp-$alarmMargin]} {
        append badList "The setpoint of S${Sn1}A:P${digit}:x is outside of BPLD minor alarm limit.\n"
    }
    if {$ay<[expr $ayLow+$alarmMargin] || $ay>[expr $ayUp-$alarmMargin]} {
        append badList "The setpoint of S${Sn1}A:P${digit}:y is outside of BPLD minor alarm limit.\n"
    }


    if {$bx<$bxLow || $bx>$bxUp} {
        append badList "The setpoint of S${Sn}B:P${digit}:x is outside of BPLD trip limit as well.\n"
    }
    if {$by<$byLow || $by>$byUp} {
        append badList "The setpoint of S${Sn}B:P${digit}:y is outside of BPLD trip limit as well.\n"
    }
    if {$ax<$axLow || $ax>$axUp} {
        append badList "The setpoint of S${Sn1}A:P${digit}:x is outside of BPLD trip limit as well.\n"
    }
    if {$ay<$ayLow || $ay>$ayUp} {
        append badList "The setpoint of S${Sn1}A:P${digit}:y is outside of BPLD trip limit as well."
    }
    if [llength $badList] {
        return -code error $badList
    }
}

set lastPing 0
#use the biggest non-used OAG runcontrol OAG199RC
set pingTimeout 30
proc PingRunControl {args} {
    global runControlPV lastPing pingTimeout
    set sec [clock seconds]
    if {[expr $lastPing + 2] > $sec} {
        return
    } else {
        set lastPing $sec
    }

    catch {APSRunControlPing} status
    switch $status {
        RUNCONTROL_OK {}
        RUNCONTROL_ABORT {
            puts stderr "aborted."
            return -code error "Aborted"
        }
        RUNCONTROL_TIMEOUT -
        RUNCONTROL_ERROR {
            puts stderr "Unable to ping runcontrol record of $runControlPV: $status."
            return -code error "Unable to ping runcontrol record of $runControlPV: $status."
        }
    }
}

# These are the datapool feedforward of RTFB bpm setpoint.
# The /tmp files reside in the server workstation.
proc PrepareFFFiles {args} {
    global FFWaveformFile
    set tmpRoot /tmp/[APSTmpString]
    set waveformDir /home/helios/oagData/sr/orbitControllaw/waveforms
    foreach plane {h v} coord {x y} {
        if [catch {exec sddsprocess $waveformDir/RTFB[string toupper $coord]bpmInfo.sdds \
            -edit=col,ControlName,DeviceName,ei/:${coord}:SelectedMI/ -pipe=out \
            | sddscasr -save -pipe \
            | sddsconvert -pipe=in $tmpRoot.$plane -rename=col,ValueString=BPMName } result] {
            return -code error $result
        }
        set FFWaveformFile($plane) $tmpRoot.$plane
        APSAddToTmpFileList -ID steering -fileList $tmpRoot.$plane
    }
}

# global variable P0Sector is used to set the label names
# of the bpms whose setpoints are to be changed.
set linkDir /home/helios/oagData/sr/localSteering/lattices/default/IDs
for {set sector 1} {$sector<41} {incr sector} {
    set id ${linkDir}/[format %02ld $sector]ID
    set idLink [file readlink $id] 
    switch -regexp $idLink  {
        P1 {
        # means that P1 bpms are used for steering
            set P0Sector($sector) 0
            set R11($sector) 1.09
            set R12($sector) 4.29
            set R33($sector) 0.91
            set R34($sector) 3.67
        }
        P0 {
        # means that P0 bpms are used for steering
            set P0Sector($sector) 1
            set R11($sector) 1.0
            set R12($sector) 2.48
            set R33($sector) 1.0
            set R34($sector) 2.48
	    #removed the special case for ID 34, since it is same as others now, previously R12($sector) and R34($sector) were 1.24
        }
    }
}

set R11(P1) 1.09
set R12(P1) 4.29
set R33(P1) 0.91
set R34(P1) 3.67

#updated the coefficents by Louis 11/26/2015
set R12(usc0) 0
set R34(usc0) 0
set R12(dsc0) 0.2558
set R34(dsc0) 0.2558

set R12(us) 2.3297
set R34(us) 2.3297
set R12(ds) 2.6157
set R34(ds) 2.6157

# New values due to new circumberefence of the ring calculated
# belatedly for Decker distortion. Old values are:
#set R12(BP4) -0.5015
#set R34(BP4) -0.5015
#set R12(BP3) 2.8310
#set R34(BP3) 2.8310
set R12(BP4) -0.50306
set R34(BP4) -0.50306
set R12(BP3) 2.82951
set R34(BP3) 2.82951

proc StartAutoSteering {args} {
    global runControlPV suffixList IDsectorList CUsectorList pingTimeout BMsectorList simulator

# This would have to be checked periocially, and not just at the launch.
    if !$simulator { 
        if [catch {APSSRCheckCorrectorMode -plane h} hCorrMode] {
            return -code error  "Error checking h corrector mode: $hCorrMode"
        }
        if [catch {APSSRCheckCorrectorMode -plane v} vCorrMode] {
            return -code error "Error checking v corrector mode: $vCorrMode"
        }
        if {$hCorrMode!="vector" || $vCorrMode!="vector"} {
            SetBeamSteerStatus "H/V correctors are not in vector mode. Please start DP or XRDP orbit correction before starting auto steering."
            return
        }
    }
    if [catch {APSRunControlInit -pv $runControlPV -timeout [expr $pingTimeout * 1000] \
                 -description "SR ID Auto Steering" } result] {
        SetBeamSteerStatus "Unable to initialize runcontrol SRIDAutoSteering, the runcontrol record may not be cleared, press the clear button on the medm screen to clear the record and press Start again: $result"
        return
    }
    
    SetBeamSteerStatus "Auto steering started."
    
    PingRunControl
    SetBeamSteerStatus "Setting up monitors on ID steering PVs"
    foreach sector $IDsectorList {
        set sectorf [format %02d $sector]
        global ID${sectorf}BLSteeringStart
        pv umon ID${sectorf}BLSteeringStart "ApplySetpoints -sector $sector -review $simulator -source ID"
        PingRunControl
        pv umon ID${sectorf}BLSaveSteerReq "SaveSteerResultToOps -sector $sector -source ID"
        PingRunControl
    }
    SetBeamSteerStatus "Setting up monitors on ID CU steering PVs"
    foreach side {us ds} {
        foreach sector $CUsectorList {
            set sectorf [format %02d $sector]
            global ID${sectorf}${side}BLSteeringStart
            pv umon ID${sectorf}${side}BLSteeringStart "ApplySetpoints -sector $sector -side $side -review $simulator -source ID"
            PingRunControl
            pv umon ID${sectorf}${side}BLSaveSteerReq "SaveSteerResultToOps -sector $sector -side $side -source ID"
            PingRunControl
        }
    }
    
    SetBeamSteerStatus "Setting up monitors on BM steering PVs"
    foreach sector $BMsectorList {
        set sectorf [format %02d $sector]
        global BM${sectorf}BLSteeringStart
        pv umon BM${sectorf}BLSteeringStart "ApplySetpoints -sector $sector -review $simulator -source BM"
        PingRunControl
        pv umon BM${sectorf}BLSaveSteerReq "SaveSteerResultToOps -sector $sector -source BM"
        PingRunControl
    }
    
    SetBeamSteerStatus "Finished setting up monitors and waiting for beamline steering requests."
    while {1} {
        if [catch {PingRunControl} result] {
            SetBeamSteerStatus "Steering aborted: $result"
            break
        }
        APSWaitWithUpdate -waitSeconds 2 -updateInterval 1
    }
}

set BPLDFile /home/helios/oagData/sr/BPLDs/sectors.sdds
sdds load $BPLDFile bpldConfig
set BPLDList [lindex $bpldConfig(Column.Sector) 0]
set IsDigitalList [lindex $bpldConfig(Column.IsDigital) 0]
set Channel2List [lindex $bpldConfig(Column.Channel2) 0]
set Channel1List [lindex $bpldConfig(Column.Channel1) 0]
foreach sector $BPLDList chan1 $Channel1List chan2 $Channel2List {
    set sector1 [expr $sector + 1]
    set firstChannelBPM($sector) S${sector}B:$chan1
    set secondChannelBPM($sector) S${sector1}A:$chan2
}
#PrepareFFFiles
set FFWaveformFile(h) /home/helios/oagData/sr/orbitControllaw/waveforms/FFwaveform.h
set FFWaveformFile(v) /home/helios/oagData/sr/orbitControllaw/waveforms/FFwaveform.v

if [catch {APScavput -list=$runControlPV.CLR=1 -pend=20} result] {
    puts stderr "[exec date] error clear run control $runControlPV record: $result"
    exit
}

set IDsectorList [exec sdds2stream -col=Sector /home/helios/oagData/sr/IDs/sectors.sdds]
set suffixList {BLSteeringXp BLSteeringYp RequestSteering BLSteeringStart \
                  BLSteerStatus BLSteeringErrMsg BLSteeringMsg XpActualAccum YpActualAccum}
#puts stderr "Before linkw SRIDSteerLock SRID:SteeringLockBI.VAL"

if [pv linkw SRIDSteerLock SRID:SteeringLockBI.VAL 30] {
    puts stderr "Error setup link for SRID:SteeringLockBI: $errorCode"
    exit 1
}
if [pv umon SRIDSteerLock] {
    puts stderr "Error monitoring for SRID:SteeringLockBI: $errorCode"
    exit 1
}
#puts $SRIDSteerLock

SetBeamSteerStatus "Setting up connections to ID steering PVs plus other files"
set monVarList ""
foreach sector $IDsectorList {
    # make backup of current corrector range values
    set controllawDir /home/helios/oagData/sr/localSteering/lattices/default/IDs/[format %02ld ${sector}]ID
    foreach plane {h v} {
        file delete -force $controllawDir/${plane}.corr.range.orig
        if [catch {exec sddscasr -save $controllawDir/${plane}.corr.range $controllawDir/${plane}.corr.range.orig } result] {
            puts stderr "Error in reading the corrector range of ID sector $sector: $result"
            exit 1
        }
    }
    set sectorf [format %02d $sector]
    foreach suffix $suffixList {
        lappend ID${sector}PVList ID${sectorf}:$suffix
        lappend ID${sector}VarList ID${sectorf}$suffix
    }
    set varList [set ID${sector}VarList]
    set pvList [set ID${sector}PVList]
    
    if [pv linkw $varList $pvList 30] {
	puts stderr "Error link pvs: $pvList"
        puts stderr "Error link pvs: $errorCode"
        exit 1
    }
    lappend monVarList ID${sector}BLSteeringXp 
    lappend monVarList ID${sector}BLSteeringYp
}

set CUdataDir /home/helios/oagData/sr/XAXSTF
if [catch {exec sddsprocess $CUdataDir/sectors.sdds -pipe=out \
               -filter=col,CUflag,1,1 -filter=col,CUdipoleExists,1,1 \
               | sdds2stream -pipe -col=Sector} CUsectorList] {
    puts stderr "$CUsectorList"
    exit 1
}
SetBeamSteerStatus "Setting up connections to ID CU steering PVs"
foreach side {us ds} {
    foreach sector $CUsectorList {
        set sectorf [format %02d $sector]
        foreach suffix $suffixList {
            lappend ID${sector}${side}PVList ID${sectorf}${side}:$suffix
            lappend ID${sector}${side}VarList ID${sectorf}${side}$suffix
        }
        set varList [set ID${sector}${side}VarList]
        set pvList [set ID${sector}${side}PVList]

#        puts stderr "Sector $sector$side Before linkw $varList"
        if [pv linkw $varList $pvList 30] {
	    puts stderr "Error link pvs: $pvList"
            puts stderr "Error link pvs: $errorCode"
            exit 1
        }
        lappend monVarList ID${sector}${side}BLSteeringXp
        lappend monVarList ID${sector}${side}BLSteeringYp
    }
}

set BMsectorList [exec sdds2stream -col=Sector /home/helios/oagData/sr/BMs/sectors.sdds]

#temporarily removed 24BM due to S24B:HV3 magnets water leak.
set BMsectorList [join [regsub 24 $BMsectorList ""]]
SetBeamSteerStatus "Setting up connections to BM steering PVs plus other files"

foreach sector $BMsectorList {
    # make backup of current corrector range values
    set controllawDir /home/helios/oagData/sr/localSteering/lattices/default/BMs/[format %02ld ${sector}]BM
    foreach plane {h v} {
        file delete -force $controllawDir/${plane}.corr.range.orig
        if [catch {exec sddscasr -save $controllawDir/${plane}.corr.range $controllawDir/${plane}.corr.range.orig } result] {
            puts stderr "Error in reading the corrector range of BM sector $sector: $result"
            exit 1
        }
    }
    set sectorf [format %02d $sector]
    foreach suffix $suffixList {
        lappend BM${sector}PVList BM${sectorf}:$suffix
        lappend BM${sector}VarList BM${sectorf}$suffix
    }
    set varList [set BM${sector}VarList]
    set pvList [set BM${sector}PVList]
 
    if [pv linkw $varList $pvList 30] {
	puts stderr "Error link BM PVs: $pvList"
        puts stderr "Error link BM pvs: $errorCode"
        exit 1
    }
    lappend monVarList BM${sector}BLSteeringXp
    lappend monVarList BM${sector}BLSteeringYp
}
#foreach var $monVarList {
#    if [pv umon $var] {
 #       return -code error "pv umon error: $errorInfo,  $errorCode"
#    }
#}
SetBeamSteerStatus "Finished setting up connections to BM steering PVs"
SetBeamSteerStatus "Press Start now"

exec medm -x -attach -macro RCPV=SRID:AutoSteeringRC ./sr/psApp/APSRunControlSingle.adl &

APSEnableButton .userFrame.start.button
#if [catch {APSRunControlInit -pv $runControlPV -timeout [expr $pingTimeout * 1000] \
#                 -description "SR ID Auto Steering" } result] {
#        SetBeamSteerStatus "Unable to initialize runcontrol SRIDAutoSteering, the runcontrol record may not be cleared, press the clear button on the medm screen to clear the record and restart: $result"
 #       return
  #  }

#PingRunControl
#StartAutoSteering
