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

# $Log: not supported by cvs2svn $
# Revision 1.16  1999/02/16 15:56:44  borland
# Added restore feature for P2 offsets.
# Added ability to set P0 and P1 offsets and setpoints such that the
# readbacks don't change.
#
# Revision 1.15  1999/01/16 17:15:21  emery
# When the punch down or zero command is sent to the
# [HV][12] correctors, the H1 correctors in
# S34/35 (from a protected list) are protected from change.
#
# Revision 1.14  1998/10/18 20:59:04  borland
# Uses new data directory and has P0 support.
#
# Revision 1.13  1997/10/20 18:23:24  emery
# Changed the directory for controllaw matrices.
#
# Revision 1.12  1997/10/17 05:02:20  emery
# Added verbose option to sddscontrollaw
#
# Revision 1.11  1997/07/24 23:09:34  emery
# By default only the light sources will have the sector button
# enabled. The command line option allowAllSectors
# may be used to allow steering in all sectors.
#
# Revision 1.10  1996/12/05 20:09:54  borland
# Fixed name of status callback procedure for IOC averaging setup.
#
# Revision 1.9  1996/12/05 15:46:13  borland
# Added punch-down capability for H2/V2 correctors.
#
# Revision 1.8  1996/10/08 17:59:20  borland
# Removed 'Mp' from APSMpSRSetIOCAveraging.
#
# Revision 1.7  1996/09/17 11:51:57  emery
# Added 5 second wait time for averaging to complete.
#
# Revision 1.6  1996/09/17 11:29:49  emery
# Use IOC bpm averaging on the P1's to determine new offsets for P1's.
#
# Revision 1.5  1996/09/12 18:35:39  emery
# Changed controllaw data directories to
# /home/helios/oagData/controllaw/ID/P1 for P1 steering and
# /home/helios/oagData/controllaw/ID/P2 for P2 steering and
#
# Revision 1.4  1996/09/12 06:07:24  emery
# Removed the "ID" part of the button labels to make the main
# window narrower, and to make a more consitent labeling
# across SR BPM tcl interfaces.
#
# Revision 1.3  1996/05/06 19:16:34  borland
# Set tcl_precision to 6 to avoid ridiculously long displays of setpoint
# values.
#
# Revision 1.2  1996/05/05  08:29:42  emery
# Added a callback to the APSExecLog of the sddscontrnollaw so that
# the user will be notified when the sddscontrollaw is completed.
#
# Revision 1.1  1996/05/05  06:34:09  emery
# First commit of SRIDP2Steering, an interface for steering
# the SR beam at the P2 bpms.
#

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

set args $argv
set adjustOnly 0
set allowTransfer 1
set allowAllSectors 0
APSStrictParseArguments {adjustOnly allowTransfer allowAllSectors}

APSApplication . -name SRIDP2Steering \
  -overview "SRIDP2Steering provides convenience controls for steering at the ID P2's using sddscontrollaw."

set tcl_precision 6

set IDSteeringStatus Ready.
APSScrolledStatus .status -parent .userFrame -width 60 \
        -textVariable IDSteeringStatus 

proc SetIDSteeringStatus {text} {
    global IDSteeringStatus
    set IDSteeringStatus $text
    update
    bell
}

proc MakeSectorsWidget {widget args} {
    global sector allowAllSectors

    set parent ""
    APSParseArguments {parent}

    set w $parent$widget
    APSFrame $widget -parent $parent \
      -label "Storage Ring ID selection for steering the BP2 and AP2 bpms." \
      -contextHelp {ID selection frame} 

    if !$allowAllSectors {
        set sectorList [APSGetSDDSColumn -column Sector \
                          -fileName /home/helios/oagData/sr/IDs/sectors.sdds]
    }
    
    for {set quad 1} {$quad<5} {incr quad} {
        set start [expr ($quad-1)*10+1]
        set end   [expr $start+9]
        set buttonList {}
        set valueList {}
        APSFrame .sector$quad -parent $w.frame 
        $w.frame.sector$quad.frame configure -relief flat
        for {set sector $start} {$sector<=$end} {incr sector} {
            set cbLabel $sector
            if {$sector<10} {set cbLabel "0$sector"}
            APSButton .sector$sector -parent $w.frame.sector$quad.frame \
              -text "$cbLabel" \
              -command "IDSteeringDialog -sector $sector" \
              -contextHelp "Brings up dialog box for ID$cbLabel steering."
            if !$allowAllSectors {
                if [expr -1 == [lsearch $sectorList $sector]] {
                    APSDisableButton $w.frame.sector$quad.frame.sector$sector.button
                }
            }
        }
    }
}

proc MakeOptionWidget {widget args} {
    global sector tolerance timeLimit statusCallback punchDelta

    set parent ""
    APSParseArguments {parent}

    set w $parent$widget
    APSFrame $widget -parent $parent -label "Options" \
        -contextHelp "Options in IO steering."
    
    APSFrame .parameters -parent $w.frame  -packOption  {-side top}
    set w $parent$widget.frame.parameters
    $w.frame configure -relief flat

    APSFrame .misc -parent $w.frame -packOption  {-side left}
    $w.frame.misc.frame configure -relief flat
    APSLabeledEntry .timeLimit -parent $w.frame.misc.frame \
      -label "Time limit (sec)" \
      -textVariable timeLimit  -width 8 \
      -contextHelp "Time allowed for sddscontrollaw to steer the beam to the desired setpoints."
    APSLabeledEntry .punchDelta \
      -parent $w.frame.misc.frame \
      -label "Punchdown delta" \
      -textVariable punchDelta -width 8 \
      -contextHelp "Enter value for the punchdown delta.  This is the amount by which the \[HV\]\[12\] correctors (except for those in sectors 34/35) are reduced each time the punch-down button is pressed."
    
}

proc IDSteeringDialog {args} {
    global BP2x BP2y AP2x AP2y
    set sector ""
    APSParseArguments {sector}
    if ![string length $sector] {
        APSAlertBox [APSUniqueName .] -errorMessage \
          "No sector variable specified in IDSteeringDialog"
        return 1
    }

    set dialogFrame .dialogID$sector.userFrame
    APSDialogBox .dialogID$sector  \
      -name "ID$sector Dialog" \
      -contextHelp "Dialog box for steering in ID$sector."

    set Sn $sector
    set Sn1 [exec rpnl "$Sn 1 + 40 > pop ? 40 - : \$"]
    if {[pv linkw \
           [list BP2x($sector) BP2y($sector) AP2x($sector) AP2y($sector)] \
           [list S${Sn}B:P2:ms:x:SetpointAO S${Sn}B:P2:ms:y:SetpointAO \
              S${Sn1}A:P2:ms:x:SetpointAO S${Sn1}A:P2:ms:y:SetpointAO ]] != 0} {
        APSAlertBox .alert -errorMessage "$errorCode"
        exit
    }
    if {[pv umon BP2x($sector)] != 0} {
        APSAlertBox .alert -errorMessage "$errorCode"
        exit
    }
    if {[pv umon BP2y($sector)] != 0} {
        APSAlertBox .alert -errorMessage "$errorCode"
        exit
    }
    if {[pv umon AP2x($sector)] != 0} {
        APSAlertBox .alert -errorMessage "$errorCode"
        exit
    }
    if {[pv umon AP2y($sector)] != 0} {
        APSAlertBox .alert -errorMessage "$errorCode"
        exit
    }
    
    APSFrame .parameters -parent $dialogFrame
    $dialogFrame.parameters.frame configure -relief flat
    APSFrame .setpoints -parent $dialogFrame.parameters.frame \
      -label "Setpoints for bpms" \
      -contextHelp "Enter setpoints for the four bpm readbacks of ID$sector"
    $dialogFrame.parameters.frame.setpoints.frame configure -relief flat
    APSLabeledEntry .bP2x \
      -parent $dialogFrame.parameters.frame.setpoints.frame \
      -label "S${Sn}B:P2:x (mm)" \
      -textVariable BP2x($sector) \
      -contextHelp "Enter value for S${Sn}B:P2:ms:x:SetpointAO"
    APSLabeledEntry .bP2y \
      -parent $dialogFrame.parameters.frame.setpoints.frame \
      -label "S${Sn}B:P2:y (mm)" \
      -textVariable BP2y($sector) \
      -contextHelp "Enter value for S${Sn}B:P2:ms:y:SetpointAO"
    APSLabeledEntry .aP2x \
      -parent $dialogFrame.parameters.frame.setpoints.frame \
      -label "S${Sn1}A:P2:x (mm)" \
      -textVariable AP2x($sector) \
      -contextHelp "Enter value for S${Sn1}A:P2:ms:x:SetpointAO"
    APSLabeledEntry .aP2y \
      -parent $dialogFrame.parameters.frame.setpoints.frame \
      -label "S${Sn1}A:P2:y (mm)" \
      -textVariable AP2y($sector) \
      -contextHelp "Enter value for S${Sn1}A:P2:ms:y:SetpointAO"

    APSFrame .row1 -parent ${dialogFrame} -label "" -relief flat -packOption "-anchor w"
    
    APSButton .start -parent ${dialogFrame}.row1.frame -text START  -command "StartControllaw -sector $sector" -contextHelp  "Applies setpoints and starts sddscontrollaw on ID$sector. Note that the cancel button does not cancel the sddscontrollaw."

    APSFrame .row2 -parent ${dialogFrame} -label "" -relief flat -packOption "-anchor w"

    APSButton .zeroSetpoints -parent ${dialogFrame}.row2.frame -text "ZERO P2 SETPOINTS" \
    -command "ZeroP2Setpoints -sector $sector" -contextHelp "Zeroes the setpoints for the P2s of the ID straight section.  Can be used as part of offset measurement for P1 and P0 BPMs." 

    APSButton .restoreSetpoints -parent ${dialogFrame}.row2.frame -text "RESTORE P2 SETPOINTS" -command "RestoreP2Setpoints -sector $sector" -contextHelp "Restores the P2 setpoints for this ID setpoints to their values prior to being zeroed." 

    APSFrame .row3 -parent ${dialogFrame} -label "" -relief flat -packOption "-anchor w"

    APSButton .punchDown -parent ${dialogFrame}.row3.frame -text "PUNCH DOWN CORRECTORS"  -command "PunchDownCorrectors -sector $sector" -contextHelp  "Does one step of punching down the correctors within the P2's of the ID straight setion. This step can be done repeatedly while the orbit is being corrected.\n\nThe H1's in sectors 34/35 are excluded from this operation."

    APSButton .zeroCorrectors -parent ${dialogFrame}.row3.frame -text "ZERO CORRECTORS"  -command "ZeroCorrectors -sector $sector" -contextHelp  "Zeroes the correctors within the P2's of the ID straight setion. This step can be done while the orbit is being corrected. \n\nThe H1's in sectors 34/35 are excluded from this operation."

    APSFrame .row4 -parent ${dialogFrame} -label "" -relief flat -packOption "-anchor w"

    APSButton .setP1Offset -parent ${dialogFrame}.row4.frame -text "SET P0/P1 OFFSETS/SETPOINTS"  -command "SetP1Offset -sector $sector" -contextHelp  "Reads the P0/P1 bpm values, and transfers them to the associated offset PV.\nAlso adjusts the setpoints for the P0/P1 bpms so that the error readback is unchanged.\nThis operation is meaningful after the orbit was corrected at the P2's while all correctors between the P2's are off."

    APSEnableButton .dialogID$sector.buttonRow.ok.button
    
}

proc StartControllaw {args} {
    global BP2x BP2y AP2x AP2y
    global timeLimit tolerance statusCallback

    set sector ""
    APSParseArguments {sector}
    if {$sector==""} {
        APSAlertBox [APSUniqueName .] -errorMessage \
          "No sector variable specified in IDSteeringDialog"
        return 1
    }

    set Sn $sector
    set Sn1 [exec rpnl "$Sn 1 + 40 > pop ? 40 - : \$"]
    if [ catch { exec cavput -list=S${Sn}B:P2:ms:x:SetpointAO=$BP2x($sector),S${Sn}B:P2:ms:y:SetpointAO=$BP2y($sector),S${Sn1}A:P2:ms:x:SetpointAO=$AP2x($sector),S${Sn1}A:P2:ms:y:SetpointAO=$AP2y($sector) -pendIoTime=10 \
               } result ] {
        APSAlertBox .alert -errorMessage "$result\nSomething wrong with a cavput command. Setpoints may not be asserted."
        return
    }

    set controllawDir /home/helios/oagData/sr/localSteering/lattices/default/P2s/[format %02ld ${sector}]P2
    set irm irm
    set tests tests

    if {$statusCallback!=""} {
        $statusCallback "Starting sddscontrollaw on ID$sector P2 bpms."
    }

    set interval 2.0
    set gain 0.3
    set oldDir [pwd]
    cd $controllawDir
    APSExecLog .controllaw -width 100 \
      -lineLimit 1024 \
      -name "$controllawDir P2 local correction" \
      -unixCommand "sddscontrollaw $irm \
      -test=$tests \
      -gain=$gain -interval=$interval -deltaLimit=value=2 \
      -verbose=1 \
      -steps=[expr int( $timeLimit / $interval)]"  \
      -callback "controllawCallback -sector $sector" \
      -abortCallback "abortControllawCallback  -sector $sector" \
      -cancelCallback "abortControllawCallback  -sector $sector"
    cd $oldDir
    return 0
}

proc ZeroP2Setpoints {args} {
    set sector 0
    APSStrictParseArguments {sector}
    if {[set sectorP1 [expr $sector+1]]>40} {
        set sectorP1 1
    }
    global setpointReturnData
    if [catch {exec cavget -pend=10 -list=S -list=${sector}B,${sectorP1}A \
                 -list=:P2:ms: -list=x,y -list=:SetpointAO -cavputForm \
                 -excludeErrors} setpointReturnData($sector)] {
        return -code error "$setpointReturnData($sector)"
    }
    if [catch {exec cavput -pend=10 -list=S -list=${sector}B,${sectorP1}A \
                 -list=:P2:ms: -list=x,y -list=:SetpointAO=0} result] {
        return -code error "$result"
    }
}

proc RestoreP2Setpoints {args} {
    set sector 0
    APSStrictParseArguments {sector}
    if {[set sectorP1 [expr $sector+1]]>40} {
        set sectorP1 1
    }
    global setpointReturnData
    if [lsearch -exact [array names setpointReturnData] $sector]==-1 {
        return -code error "No setpoint data saved for sector $sector"
    }
    if [catch {exec cavput -list=$setpointReturnData($sector) -pend=10} result] {
        return -code error "$result"
    }
}

proc ZeroCorrectors {args} {
    global statusCallback
    set protectedH1 {S34A:H1 S34B:H1 S35A:H1 S35B:H1}

    set sector ""
    APSParseArguments {sector}
    if {$sector==""} {
        APSAlertBox [APSUniqueName .] -errorMessage \
          "No sector variable specified in IDSteeringDialog"
        return 1
    }

    set Sn $sector
    set Sn1 [exec rpnl "$Sn 1 + 40 > pop ? 40 - : \$"]
    
    if {$statusCallback!=""} {
        $statusCallback "Zeroing correctors S${Sn}B:\[HV\]\[12\] and S${Sn1}A:\[HV\]\[12\] for ID${sector}."
    }
    
    set converterList ""
    foreach plane {H V} {
        foreach position {1 2} {
            lappend converterList S${Sn}B:${plane}${position} S${Sn1}A:${plane}${position}
        }
    }
    foreach converter $converterList {
        if {-1 < [lsearch $protectedH1 $converter] } {
            set index [lsearch $converterList $converter]
            if {$index > -1} {
                set converterList [lreplace $converterList $index $index]
            }
        }
    }
    if [ catch { exec cavput -list=[join $converterList ","] -list=:CurrentAO=0 -pendIoTime=10 \
               } result ] {     
        APSAlertBox .alert -errorMessage "$result\nSomething wrong with a cavput command. Setpoints may not be asserted."
        return
    }
    return 0
}

proc PunchDownCorrectors {args} {
    global statusCallback punchDelta
    if {$punchDelta<0 } {
        set punchDelta [expr abs($punchDelta)]
    }
    
    set sector ""
    APSParseArguments {sector}
    if {$sector == ""} {
        APSAlertBox [APSUniqueName .] -errorMessage \
          "No sector variable specified in IDSteeringDialog"
        return 1
    }

    set Sn $sector
    set Sn1 [exec rpnl "$Sn 1 + 40 > pop ? 40 - : \$"]

    if {$statusCallback!=""} {
        $statusCallback "Punching-down correctors S${Sn}B:\[HV\]\[12\] and S${Sn1}A:\[HV\]\[12\] for ID${sector}."
    }

    if [ catch {eval exec cavget -list=S${Sn}B,S${Sn1}A -list=:H,:V -list=1,2 -list=:CurrentAO \
                  -pendIOTime=10 -label {{-delim= }}} result] {
        APSAlertBox .alert -errorMessage "$result\nSomething wrong with a cavget command."
        return
    }
    

    array set valueNow $result
    set outputList ""
    set maxAbs 0
    set protectedH1 {S34A:H1 S34B:H1 S35A:H1 S35B:H1}
    foreach elem [array names valueNow] {
        regexp {(.*):CurrentAO} $elem {} elemRoot
        if {-1 < [lsearch $protectedH1 ${elemRoot}] } {
            continue
        }
        set value $valueNow($elem)
        if {$value != 0} {
            set absValue [expr abs($value)]
            if {$absValue < $punchDelta} {
                set newValue 0
            } else {
                set newValue [expr $value - $punchDelta * ($value/$absValue)]
            }
            lappend outputList "$elem=$newValue"
            if {$absValue > $maxAbs} {
                set maxAbs $absValue
            }
        }
    }
    if {$statusCallback!=""} {
        $statusCallback "Maximum corrector strength is $maxAbs A"
    }

    if [llength $outputList] {
        if [catch {exec cavput -list=[join $outputList ,] -pend=30} result] {
            APSAlertBox .alert -errorMessage "$result\nSomething wrong with a cavput command."
            return
        }
    }

}

proc SetP1Offset {args} {
    global statusCallback
    set sector ""
    APSParseArguments {sector}
    set msType "msAve"
    if {$sector==""} {
        APSAlertBox [APSUniqueName .] -errorMessage \
          "No sector variable specified in IDSteeringDialog"
        return 1
    }

    set Sn $sector
    set Sn1 [exec rpnl "$Sn 1 + 40 > pop ? 40 - : \$"]

    set num2Ave 50
    set filterCoeff 1
    if { $msType == "msAve" } {
        if {$statusCallback!=""} {
            $statusCallback "Setting up IOC averaging for bpms S${Sn}B:P\[01\] S${Sn1}A:P\[01\]."
        }
        if [catch {APSSRSetIOCAveraging -num2Ave $num2Ave -filterCoeff $filterCoeff \
                     -enable 0 \
                 } result] {
            SetIDSteeringStatus $result
            return
        }
        if [ catch { exec cavput -list=S${Sn}B,S${Sn1}A -list=:P1 \
                       -list=:msAve:AveEnbBO=Enable \
                   } result ] {     
            APSAlertBox .alert -errorMessage "$result\nSomething wrong with a cavput command. Setpoints may not be asserted."
            return
        }
        if [ catch { exec cavput -list=S${Sn}B,S${Sn1}A -list=:P0 \
                       -blunder -list=:msAve:AveEnbBO=Enable \
                   } result ] {     
            APSAlertBox .alert -errorMessage "$result\nSomething wrong with a cavput command. Setpoints may not be asserted."
            return
        }
        after 5000
    }
    set readbacks [exec cavget -list=S${Sn}B,S${Sn1}A -list=:P0,:P1 -list=:${msType}: -list=x,y -label -pendIoTime=10]
    set setpoints [exec cavget -list=S${Sn}B,S${Sn1}A -list=:P0,:P1 -list=:ms: -list=x,y -list=:SetpointAO -label -pendIoTime=10]

    for {set i 0 } {$i < [llength $readbacks]} {incr i 2} {
        set j [expr $i + 1]
        set pvname [lindex $readbacks $i]
        regexp {(.*:.*):.*:(x|y)} $pvname {} bpmname plane
        if [string compare [lindex $readbacks $j] ?]!=0 {
            lappend bpmList ${bpmname}:ms:${plane}:
            lappend offsetPutList ${bpmname}:ms:${plane}:OffsetAO=[lindex $readbacks $j]
            lappend offset1List [lindex $readbacks $j]
        }
    }
    if {$statusCallback!=""} {
        $statusCallback "Transfering S${Sn}B:P\[01\]:ms:\[xy\] and S${Sn1}A:P\[01\]:ms:\[xy\] readbacks to offset value and adjusting setpoints."
    }
    # Read the present offsets 
    if [catch {eval exec cavget -list=[join $bpmList ,] -list=OffsetAO} offset0List] {
        return -code error "$result"
    }
    # Prepare to change setpoints by -(newOffset-oldOffset) 
    foreach offset0 $offset0List offset1 $offset1List bpm $bpmList {
        lappend setpointPutList ${bpm}SetpointAO=[expr $offset0-$offset1]
    }
    # Send delta values for setpoints and new values for offsets.
    if [catch {eval exec cavput -list=[join $setpointPutList ,] -deltaMode -pendIoTime=10
        eval exec cavput -list=[join $offsetPutList ,] -pendIoTime=10} result] {
        return -code error "$result"
    }
}

proc abortControllawCallback {args} {
    global statusCallback

    APSParseArguments {sector}

    if {$statusCallback!=""} {
        $statusCallback "sddscontrollaw on BP2 and AP2 bpms for ID$sector aborted."
    }
    if [ catch {exec logMessage  -sourceId=steeringAudit \
                  -tag=Instance ID${sector} -tag=Action Abort \
              } result ] {
        APSAlertBox .alert -errorMessage "error with logMessage: $result"
        return
    }
    return
}

proc controllawCallback {args} {
    global statusCallback

    APSParseArguments {sector}

    if {$statusCallback!=""} {
        $statusCallback "sddscontrollaw on BP1 and AP1 bpms for ID$sector completed."
    }
    if [ catch {exec logMessage  -sourceId=steeringAudit \
                  -tag=Instance ID${sector} -tag=Action Exit \
              } result ] {
        APSAlertBox .alert -errorMessage "error with logMessage: $result"
        return
    }
    return
}

MakeSectorsWidget .sectors -parent .userFrame
MakeOptionWidget .options -parent .userFrame

set tolerance 0.050
set timeLimit 25
set punchDelta 5.0
set statusCallback SetIDSteeringStatus

