#!/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 SFoffset 0
set SDoffset 0
set installSF 0
set installSD 0
set rootname test
set waitTime 20
set stopTime 100
#default wait 60 seconds (1 minute)
set args $argv
APSParseArguments {SFoffset SDoffset rootname installSF installSD waitTime stopTime}

#if ![string length $args] {
#    puts stderr "updownSFSDRamp -SFoffset <delta-I> -SDoffset <detla-I> -installSF <1|0> -installSD <1|0> -rootname <string> -waitTime <seconds>"
#    exit 0
#}

set install 0
if {$installSF || $installSD} {
    set install 1
}


set rampDir /home/helios/oagData/booster/ramps/IRamp

proc UpDownRef {args} {
    set digOffset ""
    set Ioffset ""
    set magnet ""
    set rootname ""
    APSParseArguments {digOffset Ioffset magnet rootname}
    global rampDir stopTime
    if [catch {exec cavput -list=B:${magnet}bcontrollawRC.SUSP=1 -pend=10} result] {
        return -code error "Error suspending $magnet bcontrol: $result"
    }
    switch $magnet {
        SF {
            set delayPV It:Ddg9chan2.DLY
        }
        SD {
            set delayPV It:Ddg9chan3.DLY
        }
    }

    if [catch {exec cavget -list=$delayPV -pend=10 } delay] {
        return -code error "Error reading delay: $delay"
    }
    puts $delay
    if [catch {exec sddsprocess $rampDir/safety/${magnet}-UIref.afg100 \
                 "-redefine=col,RampSetpoint,RampTime $stopTime > ? RampSetpoint : RampSetpoint $Ioffset + $ " \
                 "-redefine=par,afgTrigger,$delay" \
                 $rootname.ramp.${magnet}-U } result] {
        return -code error "Error processing safety: $result"
    }
    set gain [exec sdds2stream $rampDir/safety/${magnet}-UIref.afg100  -par=afgGain]
    #set delay [exec sdds2stream $rampDir/safety/${magnet}-UIref.afg100  -par=afgTrigger]
    
    puts "loading $magnet new ramp..."
    if [catch {exec rampload $rootname.ramp.${magnet}-U 
        exec cavput -list=B:${magnet}-U:CurrentRT.GAIN=$gain,$delayPV=$delay } result] {
        return -code error "Error loading $magnet Iref: $result"
    }
    
}

proc SaveCurrentAFGandLoad {args} {
    set magnet ""
    set rootname ""
    APSParseArguments {magnet}
    if [catch {exec sddswget -pv=B:${magnet}-U:CurrentWF $rootname.${magnet}-U.current
        exec sddsprocess $rootname.${magnet}-U.current -reprint=par,WaveformPV,B:${magnet}-U:IRampRefWF -nowarnings} result] {
        puts stderr "Error reading ${magnet}-U curentWF: $result"
        exit 1
    }
    switch $magnet {
        SF {
            set delayPV It:Ddg9chan2.DLY
        }
        SD {
            set delayPV It:Ddg9chan3.DLY
        }
    }
    
    if [catch {exec cavget -list=B:${magnet}-U:CurrentRT.GAIN,$delayPV -printErrors} valList] {
        puts stderr "Error reading afg gain: $gainList"
        exit 1
    }
    set gain [lindex $valList 0]
    set delay [lindex $valList 1]
    
    puts "loading ${magnet}-U current waveform, afg gain and delay into reference..."
    if [catch {exec sddswput $rootname.${magnet}-U.current
        exec cavput -list=B:${magnet}-U:RefAfgGain=$gain,B:${magnet}-U:RefTriggerDelay=$delay } result] {
        return -code error "Error loading current waveform, afg gain and delay into reference ioc: $result"
    }
}

proc setStatusText {text} {
    puts stderr "$text"
}


proc LoadIRampReference {args} {
    set refFile /home/helios/oagData/booster/ramps/IRamp/current-IRamp/IRef
    set tmpRoot  /tmp/[APSTmpString]
    
    if [catch {exec cavget -list=B:SD-U:PoweredM -pend=10} SDstate] {
        return -code error "Error reading B:SD-U:PoweredM: $SDstate"
    }
    switch $SDstate {
        B:SD {
            set magnetList {BM QF QD SF-U SD}
        }
        B:SD-U {
            set magnetList {BM QF QD SF-U SD-U}
        }
        default {
            setStatusText "Unknown SD status: $SDstate; nothing will be done for SD/SD-U"
            set magnetList {BM QD QF SF-U}
        }
    }
    
    setStatusText "Loading IRamp reference waveform, gain and delays for [join $magnetList]..."
    foreach magnet $magnetList {
        if [catch {exec sddsconvert $refFile -pipe=out \
                     -retain=col,Index,B:${magnet}:CurrentWF -retain=par,${magnet}* \
                     | sddsconvert -pipe -rename=col,B:${magnet}:CurrentWF=Waveform \
                     | sddsprocess -pipe=in -reprint=par,WaveformPV,B:${magnet}:IRampRefWF $tmpRoot.$magnet} result] {
            return -code error $result
        }
        set gain [exec sdds2stream -par=${magnet}AfgGain $tmpRoot.$magnet]
        set delay [exec sdds2stream -par=${magnet}TriggerDelay $tmpRoot.$magnet]
        if [catch {exec cavput -list=B:${magnet}:RefAfgGain=$gain,B:${magnet}:RefTriggerDelay=$delay -pend=20
            exec sddswput $tmpRoot.$magnet -pend=20 } result] {
            return -code error "error in setting reference pvs: $result"
        }
        lappend fileList $tmpRoot.$magnet
    }
    setStatusText "done."
    eval file delete -force $fileList
}

proc GenerateIRampReference {args} {
    #suspend bcontrol
    if [catch {exec cavget -list=B:SD-U:PoweredM -pend=10} SDstate] {
        return -code error "Error reading B:SD-U:PoweredM: $SDstate"
    }
    switch $SDstate {
        B:SD {
            set magnetList {BM QF QD SF-U SD}
        }
        B:SD-U {
            set magnetList {BM QF QD SF-U SD-U}
        }
        default {
            setStatusText "Unknown SD status: $SDstate; nothing will be done for SD/SD-U"
            set magnetList {BM QD QF SF-U}
        }
    }
    if [catch {exec cavget -list=B: -list=[join $magnetList ,] -list=:IRampDelayErrAI -pend=10} delayList] {
        return -code error "Error in reading delay pvs: $delayList"
    }
    set error 0
    foreach magnet $magnetList delay $delayList {
       # if {$magnet=="SF-U" || $magnet=="SD-U"} {
       #     continue
       # }
        if {[expr abs($delay)]>0.2} {
            setStatusText "$magnet delay error is $delay, too big."
            incr error
        }
    }
    if $error {
        setStatusText "generating reference was aborted due to too big delay error."
        return
    }
    #check if the gain setpoint and readback agrees or not
    set gainTolerance 2.0e-4
    if [catch {exec cavget -list=B: -list=[join $magnetList ,] -list=:IRampGainSetAI -pend=10} setgainList] {
        return -code error "Error in reading IRamp gain setpoint: $setgainList"
    }
    if [catch {exec cavget -list=B: -list=[join $magnetList ,] -list=:IRampGainAI -pend=10} gainList] {
        return -code error "Error in reading IRamp gain setpoint: $gainList"
    }
    foreach set $setgainList readback $gainList magnet $magnetList {
      #  if {$magnet=="SF-U" || $magnet=="SD-U"} {
      #      continue
      #  }
        if {[expr abs($readback-$set)]>$gainTolerance} {
            setStatusText "$magnet the difference betain gain setpoint and readback > $gainTolerance, generating reference was aborted.."
            return
        }
    }
    set delayTolerance 2e-2
    if [catch {exec cavget -list=B: -list=[join $magnetList ,] -list=:IRampDelayOffsetAI -pend=10} setdelayList] {
        return -code error "Error in reading IRamp gain setpoint: $setdelayList"
    }
    if [catch {exec cavget -list=B: -list=[join $magnetList ,] -list=:IRampDelayAI -pend=10} delayList] {
        return -code error "Error in reading IRamp gain setpoint: $delayList"
    }
    foreach set $setdelayList readback $delayList magnet $magnetList {
        if {[expr abs($readback-$set)]>$delayTolerance} {
            setStatusText "$magnet the difference between delay setpoint and readback > $delayTolerance, generating reference was aborted.."
            return
        }
    }
    
    setStatusText "suspending bcontrols ..."
    if [catch {exec cavput -list=B: -list=BM,QF,QD,SF,SD  -list=bcontrollawRC.SUSP=1 -pend=30} result] {
        return -code error "Error in suspend bcontrols: $result"
    }
    setStatusText "aborting injection tune controllaw..."
    if [catch {exec cavput -list=B:InjTune:ControllawRC.ABRT=1 -pend=30 } result] {
         return -code error "Error in suspend injection controllaw: $result"
    }
    
    set refDir /home/helios/oagData/booster/ramps/IRamp/current-IRamp
    set lattice [file readlink /home/helios/oagData/booster/ramps/IRamp]
    set oldFile [file readlink $refDir/IRef]
    set newName [APSNextGenerationedName -directory $refDir -name IRef-0000 -newFile 1 -separator -]
    
    setStatusText "creating IRamp reference ..."
    if [catch {exec GetBooRampCurrWF -filename $refDir/$newName } result] {
        return -code error "Error in creating IRamp reference."
    }
    setStatusText "clear gain and delay..."
    if [catch {exec cavput -list=B: -list=[join $magnetList ,] -list=:IRampGainSetAI=1 -pend=10
        exec cavput -list=B: -list=[join $magnetList ,] -list=:IRampDelayOffsetAI=0 -pend=10 } result] {
        return -code error "Error in setting IRamp gain to 1 and delay to 0: $result"
    }
    
    setStatusText "load reference into reference waveform..."
   
    set tmpRoot /tmp/[APSTmpString]
    
    foreach magnet $magnetList {
        if [catch {exec sddsconvert $refDir/$newName -pipe=out \
                     -retain=col,Index,B:${magnet}:CurrentWF -retain=par,${magnet}* \
                     | sddsconvert -pipe -rename=col,B:${magnet}:CurrentWF=Waveform \
                     | sddsprocess -pipe=in -reprint=par,WaveformPV,B:${magnet}:IRampRefWF $tmpRoot.$magnet} result] {
            return -code error $result
        }
        set gain [exec sdds2stream -par=${magnet}AfgGain $tmpRoot.$magnet]
        set delay [exec sdds2stream -par=${magnet}TriggerDelay $tmpRoot.$magnet]
        if [catch {exec cavput -list=B:${magnet}:RefAfgGain=$gain,B:${magnet}:RefTriggerDelay=$delay -pend=20
            exec sddswput $tmpRoot.$magnet -pend=20 } result] {
            return -code error "error in setting reference pvs: $result"
        }
    }
    set oldDir [pwd]
    cd $refDir
    exec rm IRef
    exec ln -s $newName IRef
    cd $oldDir
    file delete -force $tmpRoot.BM $tmpRoot.QF $tmpRoot.QD $tmpRoot.SF $tmpRoot.SD
    setStatusText "do SCR save..."
    if [catch {APSSaveMachine -machine Booster -description "Save QF and QD trigger delay offset after generating IRamp reference." } result] {
        return -code error "Error in saving booster SCR: $result"
    }
    APSMpStep "Reset QF, QD trigger offset to zero..."
    if [catch {exec cavput -list=B: -list=BM,QF,QD,SF,SD -list=:IRampDelayOffsetAI=0 -pend=10} result] {
        APSMpReturn error "Error in reseting QF, QD trigger offset: $result"
    }
    
    after 1000
    setStatusText "resuming bcontrols ...."
    if [catch {exec cavput -list=B: -list=BM,QF,QD,SF,SD  -list=bcontrollawRC.SUSP=0 -pend=30} result] {
        return -code error "Error in resume bcontrols: $result"
    }
    setStatusText "starting booster injection controllaw..."
    puts stderr "Booster injection tune controllaw was aborted, you need to restart it now." 
    
    if [catch {LoadIRampReference} result] {
        return -code error "Error loading reference: $result"
    }
    
    global env
#    if [catch {exec logMessage -sourceId=BRampIRefAudit -tag=User $env(USER) -tag=Host $env(HOST) 
#                 -tag=Lattice $lattice -tag=OldFile $oldFile -tag=NewFile $newName 
#                 } result] {
#        return -code error "Error in logMessage: $result"
#    }
    setStatusText "done."
}
###                 -tag=CurrentWaveformFile $archiveFile } result] {

proc InstallSafety {args} {
    set rootname ""
    set magnet ""
    APSParseArguments {rootname magnet}
    if ![file exist $rootname.ramp.${magnet}-U] {
        return -code error "$rootname $magnet Error no IRef generated."
    }
    set oldDir [pwd]
    set safetyDir /home/helios/oagData/booster/ramps/IRamp/safety
    
    set newName [APSNextGenerationedName -directory $safetyDir -name ${magnet}-UIref.afg100-0000 -newFile 1 -separator -]
    switch $magnet {
        SF {
            set delayPV It:Ddg9chan2.DLY
        }
        SD {
            set delayPV It:Ddg9chan3.DLY
        }
    }
    
    if [catch {exec cavget -list=B:${magnet}-U:CurrentRT.GAIN,$delayPV -printErrors} valList] {
        puts stderr "Error reading afg gain: $gainList"
        exit 1
    }
    set gain [lindex $valList 0]
    set delay [lindex $valList 1]
    if [catch {exec sddsprocess $rootname.ramp.${magnet}-U \
        -redefine=par,afgGain,$gain -redefine=par,afgTrigger,$delay \
        $safetyDir/$newName } result] {
        return -code error "Error creating new safety: $result"
    }
    cd $safetyDir
    exec rm ${magnet}-UIref.afg100
    exec ln -s $newName ${magnet}-UIref.afg100
    cd $oldDir
}


set magnetList {SF SD}
set currentScale 0.0022888
if !$install {
    if [catch {exec cavget -list=B:SF:SlopeInterceptSUB.K -printErrors } digScale] {
        puts stderr "Error reading SF digital scale and offset: $digScale"
        exit 1
    }
    set SFdigOffset [expr $SFoffset/$currentScale/$digScale]
    set SFIoffset  [expr $SFoffset/91.1]
    if [catch {exec cavget -list=B:SD:SlopeInterceptSUB.K -printErrors} digScale] {
        puts stderr "Error reading SD digital scale and offset: $valList"
        exit 1
    }
    set SDdigOffset [expr $SDoffset/$currentScale/$digScale]
    set SDIoffset [expr $SDoffset/146.4]
    
    foreach magnet $magnetList {
        set Ioffset [set ${magnet}Ioffset]
        if [catch {UpDownRef -magnet $magnet -Ioffset $Ioffset -rootname $rootname} result] {
            puts stderr "Error updown ${magnet}-U: $result"
            exit 1
        }
        
    }
   # exit 0
    puts "Wait for $waitTime seconds..."
    APSWaitWithUpdate -waitSeconds $waitTime
    puts "save current WF and load into reference ioc..."
    foreach magnet $magnetList {
        if [catch {SaveCurrentAFGandLoad -magnet $magnet -rootname $rootname } result] {
            puts stderr "Error save current WF and load into reference ioc: $result"
            exit 1
        }
    }
    
    puts "Resuming bcontrol ..."
    foreach magnet $magnetList {
        if [catch {exec cavput -list=B:${magnet}bcontrollawRC.SUSP=0 -pend=30} result] {
            puts stderr "Error resuming $magnet  bcontrols: $result"
            exit 1
        }
    }
    
    puts "Waiting $waitTime seconds..."
    APSWaitWithUpdate -waitSeconds $waitTime
}

if $installSF {
    if [catch {InstallSafety -rootname $rootname -magnet SF} result] {
        puts stderr "Error install safety: $result"
        exit 1
    }
}
if $installSD {
    if [catch {InstallSafety -rootname $rootname -magnet SD} result] {
        puts stderr "Error install safety: $result"
        exit 1
    }
}
if $install {
    if [catch {GenerateIRampReference} result] {
        puts stderr "Error generating new IRef: $result"
        exit 1
    }
}

exit 0


