#!/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
package require math::statistics

##############
# Initialize the run control and set the timeout value.
# If another version is already running then we will get 
# the RUNCONTROL_DENIED response and we exit without 
# printing any error messages.
############## 
proc ConnectToRunControl {args} {
    global rcPV runControlTimeout
    catch {APScavput -list=${rcPV}.CLR=1 -pend=5}
    if {[catch {APSRunControlInit -pv $rcPV \
                  -description "BEFI Protection" \
                  -timeout [expr $runControlTimeout * 1000]} result]} {
        if {$result == "RUNCONTROL_DENIED"} {
            exit
        } else {
            puts stderr "Unable to initialize runcontrol ${rcPV}: $result"
            exit 1
        }
    }
    if {[catch {APSRunControlLogMessage -message "Running" -severity 0} result]} {
        puts stderr "Unable to set the runcontrol log message for ${rcPV}: $result"
        exit 1
    }
    if {[catch {APSRunControlPing} result]} {
        if {($result == "RUNCONTROL_TIMEOUT") || ($result == "RUNCONTROL_ERROR")} {
            puts stderr "Unable to ping ${rcPV}: $result"
            exit 1
        } else {
            exit
        }
    }
}

##############
# We are looking for four main values with the ramp waveform.
# 1. The initial value should be lower than 30
# 2. The number of values that are within the ramp target range as the 
#    ramp is increasing should be at least 1.
# 3. The maximum value should be greater than rampTargetUpper
# 4. The minimum value after the peak should be lower than 30
##############
proc IsDipoleRampAbnormal {args} {
    global waveform rampTargetLower rampTargetUpper minimumCurrent
    foreach item $waveform {
        lappend scaledWaveform [expr $item * .015259021896696]
    }
    set len [llength $scaledWaveform]
    set startValue [lindex $scaledWaveform 0]
    set maxValue [::math::statistics::max $scaledWaveform]
    set maxValueAlmost [expr $maxValue - .00001]
    set minPostValue 99e99
    set withinTargetRange 0
    for {set index 0} {$index < $len} {incr index} {
        set value [lindex $scaledWaveform $index]
        if {($value >= $rampTargetLower) && ($value <= $rampTargetUpper)} {
            incr withinTargetRange
        }
        if {$maxValueAlmost < $value} {
            #We found the index of the maximum value, now look for the minimum value of the remaining points
            set minPostValue [::math::statistics::min [lrange $scaledWaveform $index end]]
            break
        }
    }
    if {($startValue > $minimumCurrent) || ($withinTargetRange < 1) || ($maxValue < $rampTargetUpper) || ($minPostValue > $minimumCurrent)} {
        return 1
    }
    return 0
}

##############
# Disable the BEK Charge if it is currently enabled
##############
proc DisableBEKcharge {args} {
    global BEKchargeGate
    set result [pv getw BEKchargeGate 1]
    if {$result} {
        #Problem reading the PV.
        catch {APSRunControlLogMessage -message "Problem disabling BEK charge" -severity 1}
    } elseif {$BEKchargeGate == "Enabled"} {
        #Disable the BEK charge since it was found to be enanbled
        set BEKchargeGate Disabled
        pv put BEKchargeGate
        catch {APSRunControlLogMessage -message "BEK charge disabled" -severity 0}
    }
}

##############
# Main loop that runs every .5 seconds
##############
proc MainLoop {args} {
    global rcPV storedBeamA storedBeamB swapOutEnabled waveform BEKchargeGate boosterRampTurns turnsTargetLower turnsTargetUpper

    ##### Connect to important PVs
    set result [pv linkw "storedBeamA storedBeamB swapOutEnabled waveform BEKchargeGate boosterRampTurns" \
                  "S34-SBM:tripM S36-SBM:tripM RF-ACIS:BEFI:PermitM B:BM:CurrentWF Mt:Ddg1chan0.GATE Mt:BoosterRampTurnsAO" 1]

    ##### Error out if we failed to connect to the PVs
    if {$result == 1} {
        puts stderr "Problem connecting to one or more of these PVs: S33:topup:TripBi S36:topup:TripBi RF-ACIS:BEFI:PermitM B:BM:sCurrentWF Mt:Ddg1chan0.GATE"
        catch {APSRunControlLogMessage -message "Problem connecting to PVs" -severity 2}
        exit 1
    }

    ##### Ping the run control
    if {[catch {APSRunControlPing} result]} {
        if {($result == "RUNCONTROL_TIMEOUT") || ($result == "RUNCONTROL_ERROR")} {
            puts stderr "Unable to ping ${rcPV}: $result"
            exit 1
        } else {
            exit
        }
    }

    ##### Enter main loop
    while {1} {
        set c1 [clock clicks]

        puts $c1
        
        ##### Main code below
        set result [pv getw "storedBeamA storedBeamB swapOutEnabled waveform boosterRampTurns" 1]
        if {$result} {
            #Disable BEK Charge because we can't read some or all of the PVs
            DisableBEKcharge
        } elseif {($swapOutEnabled == "ON") && (($storedBeamA == "NO BEAM") || ($storedBeamB == "NO BEAM"))} {
            #Disable BEK Charge because swap out is enabled but there is no stored beam
            DisableBEKcharge
        } else {
            set dipoleRampAbnormal [IsDipoleRampAbnormal]
            if {$dipoleRampAbnormal} {
                #Disable BEK Charge because the ramp is abnormal
                DisableBEKcharge
            } elseif {(($storedBeamA == "BEAM") || ($storedBeamB == "BEAM"))} {
                #Check that the number of booster ramp turns is within +/- <~1.9% of 50631 turns, which is its nominal 6 GeV level
                if {($boosterRampTurns < $turnsTargetLower) || ($boosterRampTurns > $turnsTargetUpper)} {
                    DisableBEKcharge
                }
            }
        }
        ##### Main code above

        ##### Ping the run control
        if {[catch {APSRunControlPing} result]} {
            if {($result == "RUNCONTROL_TIMEOUT") || ($result == "RUNCONTROL_ERROR")} {
                puts stderr "Unable to ping ${rcPV}: $result"
                exit 1
            } else {
                exit
            }
        }

        ##### Wait .5 seconds between between executing loop
        ##### This code includes the data processing time in the .5 seconds
        set clicks [expr int(500 - (([clock clicks] - $c1) * 1e-3))]
        if {$clicks > 0} {
            after $clicks
        }
    }
}



set rcPV OAG017RC
set rampTarget 795
set runControlTimeout 5
set rampTargetPercentageRange 3
set minimumCurrent 30
set turnsTarget 50631
set turnsTargetPercentageRange 1.9

set args $argv
if {[APSStrictParseArguments {rampTarget rampTargetPercentageRange minimumCurrent turnsTarget turnsTargetPercentageRange}] == -1} {
    puts stderr "Usage: BEFI_MachineProtection \[-rampTarget 795\] \[-rampTargetPercentageRange 3\] \[-minimumCurrent 30\] \[-turnsTarget 50631\] \[-turnsTargetPercentageRange 1.9\]"
    exit 1
}

set rampTargetLower [expr $rampTarget * (100 - $rampTargetPercentageRange) / 100.0]
set rampTargetUpper [expr $rampTarget * (100 + $rampTargetPercentageRange) / 100.0]
set turnsTargetLower [expr $turnsTarget * (100 - $turnsTargetPercentageRange) / 100.0]
set turnsTargetUpper [expr $turnsTarget * (100 + $turnsTargetPercentageRange) / 100.0]

ConnectToRunControl
MainLoop
