# Andrew Kerrigan
# andrewk@ansto.gov.au / u4673036@anu.edu.au
#
# Driver to control the pressure inside the 12T magnet using a pfeiffer pressure gauge as a pressure readout and the needle valve on the magnet as a pressure/flow control
#
# PID parameters:
# setPoint/control Kp       Proportionate control constant
# setPoint/control Ki       Integral control constant
# setPoint/control Kd       Derivative control constant
# setPoint/control period   Period of control loop in seconds
#
# Kp is in units of %/mbar, Ki and Kd follow from that.
#
# Usage:
#   add_mercury name IP_mercury Port_mercury IP_pfeiffer Port_pfeiffer filename
#       IP_mercury      IP address of the mercury iTC on the moxabox
#       Port_mercury    Port of the mercury iTC on the moxabox
#       IP_pfeiffer     IP address of the pfeiffer on the moxabox
#       Port_pfeiffer   Port of the pfeiffer on the moxabox
#       filename        Name and location of the log file
#
# Example:
#   add_mercury mercury 137.157.202.78 4004 137.157.202.78 4002 /home/nbi/AndrewKSICS/MercuryPLoop.txt
#
# The commands to read and set the needle pressure are specific to the setup on the mercury

namespace eval ::scobj::mercurycontrol {
    proc getValue {id type signal nextState path} {
        if [hpropexists [sct] geterror] {
            hdelprop [sct] geterror
        }
        sct send "READ:DEV:DB4.G1:AUX:SIG:OPEN"
        return $nextState
    }

    proc readValue {path} {
        set data [sct result]
        switch -glob -- $data {
            "ASCERR:*" {
                sct geterror $data
                set nextState idle
            }
            default {
                set val [string range $data 29 34]
                set oldval [hgetpropval ${path}/sensor/value oldval]
                set nextstate idle
                if {$val != $oldval} {
                    sct update $val
                    sct utime readtime
                }
                hsetprop ${path}/sensor/value oldval $val
                if {[string equal "true" [hgetpropval ${path}/setPoint isSet]]} {
                    set nextstate controlValve
                }
                if {[hgetpropval $path/currentPressure logging] = 1} {
                    set floc [hval ${path}/filename]
                    set fn [open $floc a+]
                    set valvepos [hval ${path}/sensor/value]
                    set pressure [hval ${path}/currentPressure]
                    set setpoint [hval ${path}/setPoint]
                    set statustext [hgetpropval ${path}/currentPressure statusText]
                    set curtime [hgetpropval ${path}/sensor/value readtime]
                    puts $fn "$curtime, $pressure, $setpoint, $valvepos, $statusText"
                    close $fn
                }
            }
        return $nextstate
    }

    proc controlValve {path} {
        set currentTime [clock seconds]
        set prevTime [hgetpropval $path/setPoint updateTime]
        set elapsed [expr $currentTime - $prevTime]
        set period [expr [hgetpropval $path/setPoint/control period]]

        set controlling 1
        #30 second update frequency - the needle valve is fairly slow
        if {$elapsed < $period} {
            return idle
        } else {
            hsetprop $path/setPoint updateTime [clock seconds]
        }

        set Kp [hgetpropval $path/setPoint/control Kp]
        set Ki [hgetpropval $path/setPoint/control Ki]
        set Kd [hgetpropval $path/setPoint/control Kd]
        set currentError [hgetpropval $path/setPoint/control currentError]
        hsetprop $path/setPoint/control prevError $currentError
        set currentPressure [expr [hval $path/currentPressure]]
        set currentError [expr [hval $path/setPoint] - $currentPressure]
        set currentPos [expr [hval $path/sensor/value]]
        hsetprop $path/setPoint/control currentError $currentError
        set oldIntControl [hgetpropval $path/setPoint/control integralControl]
        set integralControl [expr $oldIntControl + $Ki * $currentError]
        hsetprop $path/setPoint/control integralControl $integralControl

        set proportionateControl [expr $Kp * $currentError]
        set differentialControl [expr $Kd * $currentError - $Kd * $prevError]
        set newPos [expr $currentPos + $proportionateControl + $integralControl + $differentialControl]

        if {$controlling} {         # Control range is [0.0 - 100.0], the valve won't move if it is set outside this range
            if {$newPos > 100} {
                set outPos 100.0
            } elseif {$newPos < 0} {
                set outPos 0.0
            } else {
                set outPos $newPos
            }
            sct_mercury send "SET:DEV:MB1.T1:TEMP:LOOP:FSET:${outPos}"
        }
        return idle
    }

        proc sendPR1 {par nextState} {
        if [hpropexists [sct] geterror] {
            hdelprop [sct] geterror
        }
                sct send "$par"
                return $nextState
        }

        proc isReady {nextState} {
        set data [sct result]
        switch -glob -- $data {
            "ASCERR:*" {
                sct geterror $data
                set nextState idle
            }
            default {
                if {$data == "\x06"} {
                    sct send "\x05"
                }
            }
        }
                return $nextState
        }

        proc getPR1 {path nextState} {
                set data [sct result]
        switch -glob -- $data {
            "ASCERR:*" {
                sct geterror $data
                set nextState idle
            }
            default {
                set val [string range $data 3 12]
                set status [string range $data 0 0]
                sct update $val
                hsetprop ${path}/currentPressure status $status
                sct utime readtime
                set nextState statusUpdate
            }
                return $nextState
        }
        }

    proc statusUpdate {path} {
        set status [hgetpropval $path/currentPressure status]
        switch $status {
            0 { set text "Sensor OK" }
            1 { set text "Sensor underrange" }
            2 { set text "Sensor overrange" }
            3 { set text "Sensor error" }
            4 { set text "Sensor off" }
            5 { set text "No sensor" }
            6 { set text "Identification error" }
            default { set text "Status unreadable" }
        }
        hsetprop $path/currentPressure statusText $text
        return idle
    }

    proc mk_mercury_itc {name sct_mercury sct_pfeiffer klass filename} {

        makesicsobj $name SCT_OBJECT
        sicslist setatt $name klass $klass
        sicslist setatt $name long_name $name

        set scobj_hpath /sics/$name
        set ns ::scobj::mercurycontrol

        hfactory ${scobj_hpath}/sensor plain user none

        hfactory ${scobj_hpath}/sensor/value plain user float
        hsetprop ${scobj_hpath}/sensor/value read ${ns}::getValue readValue $scobj_hpath
        hsetprop ${scobj_hpath}/sensor/value readValue ${ns}::readValue $scobj_hpath
        hsetprop ${scobj_hpath}/sensor/value controlValve ${ns}::controlValve $scobj_hpath
        hsetprop ${scobj_hpath}/sensor/value oldval -1.0

        hfactory ${scobj_hpath}/setPoint plain user float
        hsetprop ${scobj_hpath}/setPoint isSet "false"
        hsetprop ${scobj_hpath}/setPoint updateTime 0

        hfactory ${scobj_hpath}/setPoint/control plain user none
        hsetprop ${scobj_hpath}/setPoint/control Kp 33.0
        hsetprop ${scobj_hpath}/setPoint/control Ki 0.0
        hsetprop ${scobj_hpath}/setPoint/control Kd 0.0
        hsetprop ${scobj_hpath}/setPoint/control currentError UNKNOWN
        hsetprop ${scobj_hpath}/setPoint/control prevError UNKNOWN
        hsetprop ${scobj_hpath}/setPoint/control integralControl 0.0
        hsetprop ${scobj_hpath}/setPoint/control period 30

                hfactory ${scobj_hpath}/currentPressure plain user float
                hsetprop ${scobj_hpath}/currentPressure read ${ns}::sendPR1 "PR1" isReady
                hsetprop ${scobj_hpath}/currentPressure isReady ${ns}::isReady getPR1
                hsetprop ${scobj_hpath}/currentPressure getPR1 ${ns}::getPR1 $scobj_hpath statusUpdate
                hsetprop ${scobj_hpath}/currentPressure statusUpdate ${ns}::statusUpdate $scobj_hpath
                hsetprop ${scobj_hpath}/currentPressure status 0
        hsetprop ${scobj_hpath}/currentPressure statusText UNKNOWN
        hsetprop ${scobj_hpath}/currentPressure readtime UNKNOWN

        hfactory ${scobj_hpath}/filename plain user text
        hset ${scobj_hpath}/filename $filename
        if {$filename = "noFile"} {
            hsetprop ${scobj_hpath}/currentPressure logging 0
        } else {
            hsetprop ${scobj_hpath}/currentPressure logging 1
            set floc $filename
            set fn [open $floc a+]
            puts $fn "Time (s), Pressure (mbar), Setpoint (mbar), Valve Pos (%), Gauge Status"
            close $fn
        }

                ::scobj::hinitprops $name
            ::scobj::set_required_props $scobj_hpath

        if {[SplitReply [environment_simulation]]=="false"} {
            $sct_mercury poll ${scobj_hpath}/sensor/value 1 read read
            $sct_pfeiffer poll ${scobj_hpath}/currentPressure 1 read read
        }
        }
    namespace export mk_mercury_itc
}
namespace import ::scobj::mercurycontrol::*
proc add_mercury {name IP_mercury port_mercury IP_pfeiffer port_pfeiffer {filename "noFile"}} {
    if {[SplitReply [environment_simulation]]=="false"} {
        makesctcontroller sct_mercury std ${IP_mercury}:$port_mercury
        makesctcontroller sct_pfeiffer std ${IP_pfeiffer}:$port_pfeiffer
    }
        mk_mercury_itc $name sct_mercury sct_pfeiffer environment $filename
}

# add_mercury mercury 137.157.202.78 4004 137.157.202.78 4002 /home/nbi/AndrewKSICS/MercuryPLoop.txt