#!/bin/sh  
# \
exec oagtclsh "$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 args $argv
set sector ""
set steeringType ID
set steeringUnit ""
set steeringValue ""
APSStrictParseArguments {sector steeringType steeringUnit steeringValue}

puts [exec date]
if ![string length $sector] {
    puts stderr "checkBPLDLimits -sector <sector> -steeringType <ID|BM> -steeringUnit <x|xp|y|yp> -steeringValue <value>"
    puts stderr "Error: sector not provided!"
    exit 1
}

if ![string length $steeringUnit] {
    switch $steeringType {
	ID {
	    set steerList {Xp Yp}
	}
	BM {
	    set steerList Yp
	}
    }
} else {
    set steerList $steeringUnit
}

proc CheckBPLDLimit {args} {
    #BM only has Yp steering
    set steeringValue ""
    set steeringUnit ""
    set sector ""
    set knobFile ""
    set steeringType ""
    APSStrictParseArguments {steeringValue steeringUnit sector knobFile steeringType}
    set steeringUnit [string tolower $steeringUnit]
    set tmpRoot /tmp/[APSTmpString]
    if [catch {exec sddsprocess $knobFile -reedit=col,ControlName,%/.VAL// -match=par,KnobType=$steeringUnit $tmpRoot.1} result] {
	return -code error "Error1: $result"
    }
    APSAddToTmpFileList -ID bpld -fileList $tmpRoot.1
    set pvList [exec sdds2stream -col=ControlName $tmpRoot.1]
    set weightList [exec sdds2stream -col=Weight $tmpRoot.1]
    set statusPV [exec sdds2stream -par=SetStatusPV $tmpRoot.1]
    set deviceList [exec sdds2stream -col=DeviceName $tmpRoot.1]
    set readbackPV [exec sdds2stream -par=SetRbvPV $tmpRoot.1]
  
    if [catch {exec cavget -list=$readbackPV,$readbackPV.DRVH,$readbackPV.DRVL -pend=10 -printErrors} limits] {
	return -code error "Error reading limits: $limits"
    }
    set accuVal [lindex $limits 0]
    set hiLimit [lindex $limits 1]
    set loLimit [lindex $limits 2]
    set newval [expr $accuVal + $steeringValue]
    if {$newval>$hiLimit || $newval<$loLimit} {
	return -code error "Error: steering request exceeds the steering limit"
    }
    
    set gain [exec sdds2stream -par=Gain $tmpRoot.1]
    if [catch {exec cavget -list=[join $pvList ,] -pend=10 } valList] {
	return -code error "Error reading PV values: $valList"
    }
    if [catch {exec cavget -list=[join $pvList ,] -list=.DRVH -pend=10 } hiList] {
	return -code error "Error reading PV DRVH values: $hiList"
    }
    if [catch {exec cavget -list=[join $pvList ,] -list=.DRVL -pend=10 } loList] {
	return -code error "Error reading PV DRVL values: $loList"
    }
    #check if PV out of range
   
    set configFile /home/helios/oagData/sr/BPLD/BPLD_Config/config.sdds
    if [regexp {x} $steeringUnit] {
	set coord X
	set plane H
	set posLimit 1000
    } else {
	set coord Y
	set plane V
	set posLimit 500
    }
    set angleLimit 250
    foreach pv $pvList weight $weightList  val $valList  hi $hiList lo $loList device $deviceList {
	set newValue [expr $val + $gain*$weight*$steeringValue]
	set NewValue($device) $newValue
	if {$newValue>$hi || $newValue<$lo} {
	    puts stderr "Error: $pv current value $val, new value: $newValue, out-of-range!"
	    #exec cavput -list=$statusPV=3
	    exit 1
	}
	if [regexp "SetpointC" $pv] {
	    #read BPLD BPM value
	    lappend bpmList $device
	    if [catch {exec sddsprocess $configFile $tmpRoot.$pv -match=col,DeviceName=$device } result] {
		return -code error "Error finding $device in config file $configFile: $result"
	    }
	    APSAddToTmpFileList -ID bpld -fileList $tmpRoot.$pv
	    set BPLDsector [exec sdds2stream $tmpRoot.$pv -col=BPLDSector]
	    set BPLDindex [exec sdds2stream $tmpRoot.$pv -col=BPLDIndex]
	    if {$steeringType=="BM"} {
		#check if BPM in BM-BPLD
		if [catch {exec cavget -list=${device}:MPS:bmBPLD:InUse -pend=10 -num -printErrors} inUse] {
		    return -code error "Unable to read InUse of $device: $inUse"
		}
		if !$inUse {
		    puts "$device is not being used for BM-BPLD, skip prechecking."
		    #do not need to check BM-BPLD if not being used for BM-BPLD
		    continue
		}
		#only need to check the BPM limit
		if [catch {exec cavget -list=S[format %02d $BPLDsector]-LMPS:BM_${coord}${BPLDindex}_TH-SP -pend=10} limit] {
		    return -code error "Unable to read BM-BPLD limit: $limit"
		}
		#read bpld bpm value
		if [catch {exec cavget -list=S[format %02d $BPLDsector]-LMPS:BM_TBT_${coord}${BPLDindex}-I \
			       -repeat=number=5,pause=0.1,ave -pend=10} bpmValue] {
		    return -code error "Unable to read BM-BPLD bpm value: $bpmValue"
		}
		set value [expr $bpmValue - $newValue]
		if {[expr abs($value)]>=$limit} {
		    return -code error "Error: $device out-of-BM-BPLD limit!"
		   # exec cavput -list=$statusPV=3
		}
	    } else {
		#ID-BPLD
		#do it later
	    }
	}
    }
    if {$steeringType=="BM"} {
	return
    }
    #check ID BPLD limit
    if {$sector==40} {
	set BPLDSector 1
	set ID ID1
    } else {
	if [expr $sector%2]==1 {
	    set BPLDSector $sector
	    set ID ID2
	} else {
	    set BPLDSector [expr $sector+1]
	    set ID ID1
	}
    }
    if [catch {exec cavget -list=S[format %02d $BPLDSector]-LMPS:BPLD:$ID -list=:BPM-XY -list=1,2 -list=_idx-SP -pend=10} indexList] {
	return -code error "Error reading ID bpm index: $indexList"
    }
    if [catch {exec cavget -list=S[format %02d $BPLDSector]-LMPS:BPLD:$ID -list=:S -list=1,2 -list=-SP  -pend=10 -printErrors} lenList] {
	return -code error "Error reading ID length: $lenList"
    }
    set L1 [expr abs([lindex $lenList 0])]
    set L2 [expr abs([lindex $lenList 1])]
    
    if [catch {exec sddsprocess $configFile -pipe=out -filter=col,BPLDSector,$BPLDSector,$BPLDSector \
		   -filter=col,BPLDIndex,[lindex $indexList 0],[lindex $indexList 0] \
		   | sdds2stream -pipe -col=DeviceName} bpm1] {
	return -code error "Error getting bpm1: $bpm1"
    }
    if [catch {exec sddsprocess $configFile -pipe=out -filter=col,BPLDSector,$BPLDSector,$BPLDSector \
		   -filter=col,BPLDIndex,[lindex $indexList 1],[lindex $indexList 1] \
		   | sdds2stream -pipe -col=DeviceName} bpm2] {
	return -code error "Error getting bpm2: $bpm2"
    }
    
    set IDbpm1 S[format %02d $BPLDSector]-LMPS:BPLD:${ID}_${coord}1-I
    set IDbpm2 S[format %02d $BPLDSector]-LMPS:BPLD:${ID}_${coord}2-I
    if [catch {exec cavget -list=$IDbpm1,$IDbpm2 -repeat=number=5,pause=0.1,ave -pend=10} valList1] {
	return -code error "Error reading ID-BPLD bpm readings: $valList"
    }
    #puts "current values: [join $valList1]"
    set val1 [expr [lindex $valList1 0] - $NewValue($bpm1)]
    set val2 [expr [lindex $valList1 1] - $NewValue($bpm2)]
    #puts "val1: $val1"
    #puts "val2: $val2"
    #compute virtual BPM position and angle
    #Xs=(L1*X2 + L2*X1)/(L1+L2)  Xs'=2*(X2-X1)/(L1+L2)
    set L1 [expr abs($L1)]
    set L2 [expr abs($L2)]
    set Xpos [expr ($L1*$val2+$L2*$val1)/($L1+$L2)]
    set Xangle [expr 2.0*($val2-$val1)/($L1+$L2)]
    puts "future ID bpm1 pos: $val1; bpm2 pos: $val2"
    puts "virtual bpm pos: pos=$Xpos, angle=$Xangle"
    set error 0
    if {[expr abs($Xpos)]>=$posLimit || [expr abs($val1)]>=$posLimit || [expr abs($val2)]>=$posLimit } {
	return -code error "Error: ID-BPLD position out of limit"
    }
    if {[expr abs($Xangle)]>=$angleLimit} {
	return -code error "Error: ID-BPLD angle out of limit"
	incr error
    }
}

set dir /home/helios/oagData/sr/knobs/${steeringType}bump/lattices/default
set sectorf [format %02d $sector]
set knobFile $dir/${steeringType}$sectorf.cokn
foreach steeringUnit $steerList {
    set steeringUnit [string tolower $steeringUnit]
    switch $steeringUnit {
	xp {
	    set steeringUnit Xp
	}
	yp {
	    set steeringUnit Yp
	}
	x {
	    set steeringUnit X
	}
	y {
	    set steeringUnit Y
	}
	default {
	    puts stderr "Invalid steering - $steeringUnit provided!"
	    exit 1
	}
    }
    puts "start steering prechecking for S[format %02d $sector]$steeringType $steeringUnit..."
    if ![string length $steeringValue] {
	#reading for pv
	set pv S[format %02d $sector]${steeringType}-Steering:${steeringUnit}:SetValueC
	if [catch {exec cavget -list=$pv -pend=10 -printError} steerValue] {
	    puts stderr "Error reading reqested steering value: $steeringValue!"
	    exit 1
	}
	set steerpv S[format %02d $sector]${steeringType}-Steering:${steeringUnit}_SetValueC
    } else {
	set steerValue $steeringValue
    }
    if {$steerValue==0} {
	if ![string length $steeringValue] {
	    catch {exec caput S[format %02d $sector]${steeringType}:BLSteeringMsgM "$steeringUnit requested steering is 0, ignore." }
	}
	puts stdout "$steeringUnit requested steering value is 0, ignore."
	continue
    }
    if [catch {CheckBPLDLimit -sector $sector -steeringType $steeringType -steeringUnit $steeringUnit -knobFile $knobFile -steeringValue $steerValue} result] {
	puts stderr $result
	catch {exec caput S[format %02d $sector]${steeringType}:BLSteeringMsgM "$steeringUnit precheck failed, $result"}
	puts stderr [exec date]
	exit 1
    } else {
	puts stdout "precheck passed."
    }
}

puts [exec date]
exit 0
