# This program starts or kills Bcontrol depending on what the user wants and 
# if Bcontrol is running or not.  It also allows enabling or disabling "slope" and/or "zero" 
# options in Bcontrol.

proc APSMpBoosterPSStartKillBControlInfo {} {

    return "For more detailed information please refer to OPSDoc_Boo_PEM003 under required reading.

This procedure is used to \"Start\" or \"Kill\" the Bcontrol program and enable \"Slope\" and \"Zero\" correction for selected power supplies.  Bcontrol is used to correct the measured ramp current slope and ramp current zero crossing to the reference values for a given lattice.  The operator selects either \"Start\" or \"Kill\".  If \"Kill\" is selected the program is terminated and nothing further is required.  If \"Start\" is selected, the operator must select the supply(s), and whether to enable or disable either the slope or zero or both.
"

}

proc APSMpBoosterPSStartKillBControlDialog {frame} {

    APSSetVarAndUpdate status "Select booster power supplies..."

    APSFrame .bconFrame -parent $frame -label "Bcontrol Startup\nand Control"

    APSRadioButtonFrame .bconStartKillFrame -parent ${frame}.bconFrame.frame \
	-label "" -buttonList {Start Kill} -valueList {start kill} \
	-variable APSMpBoosterPSStartKillBControl(action1) -packOption

    APSRadioButtonFrame .bconEnableDisableFrame -parent ${frame}.bconFrame.frame \
	-label "" -buttonList {Enable Disable} -valueList {enable disable} \
	-variable APSMpBoosterPSStartKillBControl(action2) -packOption "-side right"

    APSCheckButtonFrame .bconSlopeZeroFrame -parent ${frame}.bconFrame.frame \
        -buttonList {Slope Zero} -allNone 1 -label "" -packOption "-side right" \
	-variableList {APSMpBoosterPSStartKillBControl(slope) \
			   APSMpBoosterPSStartKillBControl(zero)}
    
    APSCheckButtonFrame .magnetSelectButton -parent $frame -label "Select magnet\npower supplies" \
      -variableList {APSMpBoosterPSStartKillBControl(BM)    \
                       APSMpBoosterPSStartKillBControl(QF)  \
                       APSMpBoosterPSStartKillBControl(QD)  \
                       APSMpBoosterPSStartKillBControl(SF)  \
                       APSMpBoosterPSStartKillBControl(SD)} \
      -buttonList {BM QF QD SF SD} \
      -allNone 1 \
      -packOption {-side top}

    return
}

proc APSMpBoosterPSStartKillBControl {args} {
    
    set BM 0
    set QF 0
    set QD 0
    set SF 0
    set SD 0
    set slope 0
    set zero 0
    set action1 start
    set action2 enable
    
    APSStrictParseArguments {BM QF QD SF SD slope zero action1 action2}

    APSMpStep init -promptDialog APSMpBoosterPSStartKillBControlDialog -vars {BM QF QD SF SD action1 action2 slope zero}
    
    set magnetList {BM QF QD SF SD}

    APSMpStep "$action1 Bcontrol" -desc "Start or Kill Bcontrol"
    if {[catch {APScavget -list=B:PSBcontrolRC.RUN -pendIoTime=20} bcontrolstate]} {
	APSMpReturn error "APSMpBoosterPSStartKillBControl: $bcontrolstate"
    }
    if {[catch {APScavget -list=B:PSBcontrolRC.HOST -pendIoTime=20} host]} {
	APSMpReturn error "APSMpBoosterPSStartKillBControl: $host"
    }
    if {$action1=="start" && $bcontrolstate=="1"} {
	APSMpStep "Bcontrol is already running on $host."
    } elseif {$action1=="start" && $bcontrolstate=="0"} {
	if {[catch {APScavget -list=B:PSBcontrolRC.ABRT -pendIoTime=20} abortState]} {
	    APSMpReturn error "APSMpBoosterPSStartKillBControl: $abortState"
	}
	if {[catch {APScavget -list=B:PSBcontrolRC.MSG -pendIoTime=20} msgState]} {
	    APSMpReturn error "APSMpBoosterPSStartKillBControl: $msgState"
	}
puts $msgState
	if {($abortState=="Run") && ($msgState != {"IOC rebooted"})} {
	    if {[catch {APScavput -list=B:PSBcontrolRC.ABRT=1 -pendIoTime=20} results]} {
                if {[APSPEMTKDialog .warningDialog -text "APSMpBoosterPSStartKillBControl: Channel access problem while aborting Bcontrol RunControl Status\n$results"] == 1} {
                    APSMpReturn error "APSMpBoosterPSStartKillBControl: $results"
                }
	    }
	    set waiting 1
	    set waitLimit 0
	    while {$waiting} {
		update
		if {$waitLimit > 20} {
                    if {[APSPEMTKDialog .warningDialog -text "APSMpBoosterPSStartKillBControl: Timed out waiting to abort Bcontrol RunControl"] == 1} {
                        APSMpReturn error "APSMpBoosterPSStartKillBControl: Timed out waiting to abort Bcontrol RunControl"
                    }
		}
                APSWaitWithUpdate -waitSeconds 2
		if {[catch {APScavget -list=B:PSBcontrolRC.MSG -noQuotes} results]} {
		    APSMpReturn error "APSMpBoosterPSStartKillBControl: $results"
		}
		if {[string compare "** Aborted **" $results]==0} {
		    set waiting 0
		}
		if {[string compare "" $results]==0} {
		    set waiting 0
		}
		incr waitLimit
	    }
	    if {[catch {APScavput -list=B:PSBcontrolRC.CLR=1 -pendIoTime=20} results]} {
                if {[APSPEMTKDialog .warningDialog -text "APSMpBoosterPSStartKillBControl: Channel access problem while clearing Bcontrol RunControl Status\n$results"] == 1} {
                    APSMpReturn error "APSMpBoosterPSStartKillBControl: $results"
                }
	    }
            APSWaitWithUpdate -waitSeconds 2
	}
	if {$abortState=="Abort"} {
	    if {[catch {APScavput -list=B:PSBcontrolRC.CLR=1 -pendIoTime=20} results]} {
                if {[APSPEMTKDialog .warningDialog -text "APSMpBoosterPSStartKillBControl: Channel access problem while clearing Bcontrol RunControl Status\n$results"] == 1} {
                    APSMpReturn error "APSMpBoosterPSStartKillBControl: $results"
                }
	    }
            APSWaitWithUpdate -waitSeconds 2
	}	    
	if {[catch {exec sddsbcontrol >& /dev/null &} results]} {
            if {[APSPEMTKDialog .warningDialog -text "APSMpBoosterPSStartKillBControl: Problem occurred while running Bcontrol\n$results"] == 1} {
                APSMpReturn error "APSMpBoosterPSStartKillBControl: $results"
            }
	}
        APSWaitWithUpdate -waitSeconds 2
	if {[catch {APScavget -list=B:PSBcontrolRC.HOST -pendIoTime=20} host]} {
	    APSMpReturn error "APSMpBoosterPSStartKillBControl: $host"
	}
	APSMpStep "Bcontrol started on $host."
    } elseif {$action1=="kill" && $bcontrolstate=="0"} {
	APSMpStep "Bcontrol not running."
    } elseif {$action1=="kill" && $bcontrolstate=="1"} {
	if {[catch {APScavput -list=B:PSBcontrolRC.ABRT=1 -pendIoTime=20} results]} {
            if {[APSPEMTKDialog .warningDialog -text "APSMpBoosterPSStartKillBControl: Channel access problem while aborting Bcontrol RunControl Status\n$results"] == 1} {
                APSMpReturn error "APSMpBoosterPSStartKillBControl: $results"
            }
	}
	APSMpStep "Bcontrol killed on $host."
    }

    APSMpStep "$action2 slopes and zeros" -desc "Enable or Disable slopes and zeros"
    set slopeCommands ""
    set zeroCommands ""
    if {[string compare $action1 start]==0} {
	foreach magnet $magnetList {
	    if {[set $magnet]} {
		if {[catch {APScavget -list=B:${magnet}:SlopeStatMBBO -numerical} slopestate]} {
		    APSMpReturn error "APSMpBoosterPSStartKillBControl: $slopestate"
		}
		if {[catch {APScavget -list=B:${magnet}:ZeroStatMBBO -numerical} zerostate]} {
		    APSMpReturn error "APSMpBoosterPSStartKillBControl: $zerostate"
		}
		if {$slopestate == "?"} {
		    APSMpReturn error "APSMpBoosterPSStartKillBControl: Problem reading B:${magnet}:SlopeStatMBBO"
		}
		if {$zerostate == "?"} {
		    APSMpReturn error "APSMpBoosterPSStartKillBControl: Problem reading B:${magnet}:ZeroStatMBBO"
		}
		if {$slope} {
		    if {($action2=="enable" && !$slopestate) || \
			    ($action2=="disable" && $slopestate)} {
			lappend slopeCommands "B:${magnet}:SlopePushMBBO=1" ","
		    }
		}
		if {$zero} {
		    if {($action2=="enable" && !$zerostate) || \
			    ($action2=="disable" && $zerostate)} {
			lappend zeroCommands "B:${magnet}:ZeroPushMBBO=1" ","
		    }
		}
	    }
	}
    }

    if {$action2=="enable"} {
        set mode "enabling"
    } else {
        set mode "disabling"
    }
    if {[string length $slopeCommands]} {
        set finalSlopeCommand [cavputMakeCommand $slopeCommands]
        if {[catch {eval exec $finalSlopeCommand -numerical} results]} {
            if {[APSPEMTKDialog .warningDialog -text "APSMpBoosterPSStartKillBControl: Channel access problem while $mode slopes\n$results"] == 1} {
                APSMpReturn error "APSMpBoosterPSStartKillBControl: $results"
            }
	}
    }
    if {[string length $zeroCommands]} {
        set finalZeroCommand [cavputMakeCommand $zeroCommands]
        if {[catch {eval exec $finalZeroCommand -numerical} results]} {
            if {[APSPEMTKDialog .warningDialog -text "APSMpBoosterPSStartKillBControl: Channel access problem while $mode zeros\n$results"] == 1} {
                APSMpReturn error "APSMpBoosterPSStartKillBControl: $results"
            }
	}
    }		    
    APSMpReturn ok
}

proc cavputMakeCommand {commandList} {
    set beforeLastCommaIndex [expr [llength $commandList] - 2]
    set listCommand [lrange $commandList 0 $beforeLastCommaIndex]
    set listCommand [join $listCommand {}]
    set cavputCommand "-list="
    lappend cavputCommand $listCommand
    set cavputCommand [join $cavputCommand {}]
    set finalcavputCommand "cavput"
    lappend finalcavputCommand $cavputCommand
    return $finalcavputCommand
}

#No errors in this procedure are fatal. 
#The PEM will work fine if PVs fail to connect
proc APSMpBoosterPSResetAFGTriggers {args} {
    set BM 0
    set QF 0
    set QD 0
    set SF 0
    set SD 0
    
    APSStrictParseArguments {BM QF QD SF SD}

    catch {
        set zeroSetpointsAndReadbacks [APScavget -list=B:BM:ZeroRefAO,B:BM:RampZeroAI,B:QF:ZeroRefAO,B:QF:RampZeroAI,B:QD:ZeroRefAO,B:QD:RampZeroAI,B:SF:ZeroRefAO,B:SF:RampZeroAI,B:SD:ZeroRefAO,B:SD:RampZeroAI -repeat=number=3,pause=1,average]
        
        foreach "setpoint readback" $zeroSetpointsAndReadbacks {
            lappend zeroValues [expr {$setpoint - $readback}]
            lappend zeroPercentages [expr {abs(1.0 - ($readback / $setpoint))}]
        }

        set triggerValues [APScavget -list=It:Bs:BmVafgTrigAO,It:Bs:QfVafgTrigAO,It:Bs:QdVafgTrigAO,It:Bs:SfVafgTrigAO,It:Bs:SdVafgTrigAO]

        set lockoutPercentage [expr {[APScavget -list=B:BconLockoutAO] / 100.0}]

        foreach zeroValue $zeroValues zeroPercentage $zeroPercentages triggerValue $triggerValues magnet "Bm Qf Qd Sf Sd" mag "BM QF QD SF SD" {
            if {[set $mag]} {
                if {$zeroPercentage >= $lockoutPercentage} {
                    APScavput -list=It:Bs:${magnet}VafgTrigAO=[expr {$triggerValue - $zeroValue}]
                }
            }
        }
    }
}
