#!/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 APSUBeamlineSteeringServer -overview "Start APSU 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 ReturnWithError {text args} {
    set sector ""
    set source ""
    set side ""
    set steerType ""
    APSParseArguments {sector source side steerType}
    global BLSteer
    set sectorf [format %02d $sector]
    SetBeamSteerStatus $text
    set BLSteer(S${sectorf}$source${side}-${steerType}:Request) 0
    update
    pv putw BLSteer(S${sectorf}$source${side}-${steerType}:Request)
    
    return
}

proc ResetBLSteeringStatus {args} {
    PingRunControl
    SetBeamSteerStatus "Wait for SteerStatus=0..."
    catch {exec cavput -list=S -range=beg=1,end=35,format=%02ld -list=ID,BM -list=-Steering: -list=X,Xp,Y,Yp \
		-list=_StatusM=0 -pend=5 -blunder >& /dev/null}

    PingRunControl
    
    catch {exec cavput -list=S -range=beg=1,end=35,format=%02ld -list=ID,BM -list=-Steering: -list=X,Xp,Y,Yp \
		-list=:SetValueC=0 -pend=5 -blunder >& /dev/null}

    PingRunControl
    
    SetBeamSteerStatus "Wait for RequestSteering=0..."
    catch {exec cavput -list=S -range=beg=1,end=35,format=%02ld -list=ID,BM  -list=-Xp,-Yp -list=:RequestSteeringC=0  \
		-pend=5 -blunder >& /dev/null}

    PingRunControl
    SetBeamSteerStatus "Done clearing PVs."
}

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

    pv unlink $monVarList
    set monVarList ""
}

proc ApplySteering {args} {
    eval after 1 ApplySteering2 $args
}

proc ApplySteering2 {args} {
    set sector ""
    set review 0
    set source ID
    # values of side are "" us ds
    set side ""
    set steerType Yp
    APSParseArguments {review sector side source steerType}
    
    set sectorf [format %02d $sector]
    global BLSteer
    
    if ![string length $sector] {
        SetBeamSteerStatus "AppySetpoints: No sector variable specified"
        return
    }
    pv getw BLSteer(S${sectorf}$source${side}-${steerType}:Request)
    update
    if ![set BLSteer(S${sectorf}$source${side}-${steerType}:Request)] {
	#SetBeamSteerStatus "AppySetpoints1: No steering request for S${sectorf}$source$side $steerType." 
	return
    }
    PingRunControl
   
    pv getw BLSteer(S$sectorf$source${side}-${steerType}:SetValue)
    set steerDone 0
    
    if !$BLSteer(S$sectorf$source${side}-${steerType}:SetValue) {
	ReturnWithError  "zero steering value for S$sectorf$source$side $steerType, skip"  -sector $sector -source $source -side $side -steerType $steerType
	return
    }
    pv getw BLSteer(S${sectorf}$source${side}-${steerType}:AccuValue)
    update
    set prevVal $BLSteer(S${sectorf}$source${side}-${steerType}:AccuValue)
    SetBeamSteerStatus "run BPLD precheck ..."
    SetBeamSteerStatus "precheck S$sectorf$source$side $steerType  ..."
   
    if [catch {exec checkBPLDLimits -sector $sector -steeringType $source \
		   -steeringUnit $steerType -steeringValue $BLSteer(S$sectorf$source${side}-${steerType}:SetValue) } result] {
	ReturnWithError   "precheck failed: $result; skip steering." \
	    -sector $sector -source $source -side $side -steerType $steerType
	return
    }
    
    PingRunControl
    
    set msg  "start S$sectorf$source$side $steerType steering..."
    SetBeamSteerStatus "start S$sectorf$source$side $steerType steering..."
    set BLSteer(S${sectorf}$source${side}:Msg) $msg
    set BLSteer(S${sectorf}$source${side}-${steerType}:SteerValue) $BLSteer(S${sectorf}$source${side}-${steerType}:SetValue)
    update
    after 1000
    #comment out temporarily
    pv putw BLSteer(S${sectorf}$source${side}-${steerType}:SteerValue)
    pv putw BLSteer(S${sectorf}$source${side}:Msg)
   
    SetBeamSteerStatus "waiting for S$sectorf$source$side $steerType steering..."
    set BLSteer(S${sectorf}$source${side}:Msg) "S$sectorf$source$side $steerType start steering..."
    pv putw BLSteer(S${sectorf}$source${side}:Msg)
    
    SetBeamSteerStatus "waiting for S$sectorf$source$side $steerType steering"
    after 2000
    while {1} {
	PingRunControl
	pv getw BLSteer(S${sectorf}$source${side}-${steerType}:Status)
	update
	if [string compare $BLSteer(S${sectorf}$source${side}-${steerType}:Status) "all good"]==0 {
	    break
	}
	after 1000
    }

     #set request steering value to zero
    set BLSteer(S${sectorf}$source${side}-${steerType}:SetValue) 0
    update
   
    pv putw BLSteer(S${sectorf}$source${side}-${steerType}:SetValue)
    catch {exec caput $BLSteer(S${sectorf}$source${side}-${steerType}:SetValuePV) 0}
    
    set BLSteer(S${sectorf}$source${side}:Msg) "S$sectorf$source$side $steerType steering done."
    pv putw BLSteer(S${sectorf}$source${side}:Msg)
    
    pv getw BLSteer(S${sectorf}$source${side}-${steerType}:AccuValue)
    update
    set currVal $BLSteer(S${sectorf}$source${side}-${steerType}:AccuValue)
    SetBeamSteerStatus "Previous steering total $prevVal, current steering total $currVal"
    set delta [expr $currVal - $prevVal]
    catch {exec cavput -list=S${sectorf}${source}${side}-Steering:${steerType}:RunAccumM=$delta -delta }
    PingRunControl
    
    if [catch {APSSRSaveSteeringToOps  \
		   -description "Auto steering save: $source${sectorf}${side} delta$steerType: $delta" } snapshot] {
	SetBeamSteerStatuts "Error in SCR save: $snapshot"
    }
    PingRunControl
    set BLSteer(S${sectorf}$source${side}:Msg) "ready."
    pv putw BLSteer(S${sectorf}$source${side}:Msg)

    set BLSteer(S${sectorf}$source${side}-${steerType}:Request) 0
    update
    pv putw BLSteer(S${sectorf}$source${side}-${steerType}:Request)
    PingRunControl
    SetBeamSteerStatus "S$sectorf$source$side $steerType steering done."
    return
}

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 {
            return -code error "Aborted"
        }
        RUNCONTROL_TIMEOUT -
        RUNCONTROL_ERROR {
            return -code error "Unable to ping runcontrol record of $runControlPV: $status."
        }
    }
}

proc StartAutoSteering {args} {
    global runControlPV BLSteer pingTimeout monPVList monVarList

    if [catch {APSRunControlInit -pv $runControlPV -timeout [expr $pingTimeout * 1000] \
                 -description "SR Beamline User 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
    }
    if ![string length $monVarList] {
	SetupPVLinks
    }
    SetBeamSteerStatus "Auto steering started."
    
    exec cavput -list=S  -range=begin=1,end=35,format=%02d -list=ID- -list=Xp,Yp -list=:RequestSteeringC=0
    exec cavput -list=S  -range=begin=1,end=35,format=%02d -list=BM-Yp:RequestSteeringC=0
    PingRunControl
    
    SetBeamSteerStatus "Setting up monitors on ID steering PVs"
    foreach sector $BLSteer(IDsectorList) {
        set sectorf [format %02d $sector]
	pv umon BLSteer(S${sectorf}ID-Xp:Request) "ApplySteering -sector $sector -source ID -steerType Xp"
        PingRunControl
	pv umon BLSteer(S${sectorf}ID-Yp:Request) "ApplySteering -sector $sector -source ID -steerType Yp"
        PingRunControl
    }
    
    SetBeamSteerStatus "Setting up monitors on BM steering PVs"
    foreach sector $BLSteer(BMsectorList) {
        set sectorf [format %02d $sector]
	PingRunControl
        pv umon BLSteer(S${sectorf}BM-Yp:Request) "ApplySteering -sector $sector -source BM -steerType Yp"
        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
    }
}

proc SetupPVLinks {args} {
    global BLSteer
    set IDsectorList [exec sdds2stream -col=Sector /home/helios/oagData/sr/IDs/sectors.sdds]
    set BLSteer(IDsectorList) $IDsectorList
    #beamline steering, ID Xp, Yp; BM: Yp; CU steering, Xp,Yp (CU steering not available yet)
   
    foreach sector $IDsectorList {
	set sectorf [format %02d $sector]

	foreach var {Xp Yp} {
	    #steering request PV
	    set BLSteer(S${sectorf}ID-${var}:RequestPV) S${sectorf}ID-${var}:RequestSteeringC
	    
	    #set steering value PV
	    set BLSteer(S${sectorf}ID-${var}:SetValuePV)  S${sectorf}ID-Steering:${var}:SetValueC
	    
	    #steering value PV
	    set BLSteer(S${sectorf}ID-${var}:SteerValuePV) S${sectorf}ID-Steering:${var}_SetValueC

	    #accumulated steering PV
	    set BLSteer(S${sectorf}ID-${var}:AccuValuePV)  S${sectorf}ID-Steering:${var}_AccuValueM

	    #steering status PV
	    set BLSteer(S${sectorf}ID-${var}:StatusPV) S${sectorf}ID-Steering:${var}_StatusM
	}
	#medm steering message PV
	set BLSteer(S${sectorf}ID:MsgPV)  S${sectorf}ID:BLSteeringMsgM
    }
    
    #BM only has Yp steering
    set BMsectorList [exec sdds2stream -col=Sector /home/helios/oagData/sr/BMs/sectors.sdds]
    set BLSteer(BMsectorList) $BMsectorList
    foreach sector $BMsectorList {
	set sectorf [format %02d $sector]
	set BPSteer(S${sectorf}BM.varList) ""
	#steering request PV
	set BLSteer(S${sectorf}BM-Yp:RequestPV) S${sectorf}BM-Yp:RequestSteeringC

	#steering set value PV
	set  BLSteer(S${sectorf}BM-Yp:SetValuePV) S${sectorf}BM-Steering:Yp:SetValueC
	
	#steering value PV
	set BLSteer(S${sectorf}BM-Yp:SteerValuePV) S${sectorf}BM-Steering:Yp_SetValueC
	
	#accumulated steering value PV
	set BLSteer(S${sectorf}BM-Yp:AccuValuePV) S${sectorf}BM-Steering:Yp_AccuValueM

	#steering status PV
	set BLSteer(S${sectorf}BM-Yp:StatusPV)  S${sectorf}BM-Steering:Yp_StatusM
       

	#medm steering message PV
	set  BLSteer(S${sectorf}BM:MsgPV) S${sectorf}BM:BLSteeringMsgM
    }
    global monVarList
    set monVarList ""
    set monPVList ""
    foreach name [array names BLSteer] {
	if [regexp {PV} $name] {
	    lappend monPVList $BLSteer($name)
	    set var [regsub "PV" $name ""]
	    lappend monVarList BLSteer($var)
	}
    }
    global errorCode
    set errorCode ""
    
    if [pv linkw $monVarList $monPVList 30] {
	puts stderr "Error link BM PVs: $monPVList"
	puts stderr "Error link BM pvs: $errorCode"
	exit 1
    }
    SetBeamSteerStatus "Finished setting up connections to steering PVs"
    SetBeamSteerStatus "Press Start now"
}
set monVarList ""
SetupPVLinks

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

APSEnableButton .userFrame.start.button

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

