# Table Source Document: 998-0234-004 Rev A.doc
# Speed (RPM) {veto {KpCW KpCCW} {KiCW KiCCW} {KthCW KthCCW}
# NOTE: You can use the motor direction register value to lookup the gains because motdir=0 is CW and motdir=1 is CCW
# TEST
global SCode ECode fcRPM fcPHASE
array set SCode {
  OKOK       1
  HWIdle     2
  HWBusy     3
  HWFault    4
  HWPosFault 5
  HWCrash    6
  NOMEMORY   7
  HWNoBeam   8
  HWPause    9
  HWWarn     10
  HWRedo     11
}
array set ECode {
  MOTREDO -1
  MOTFAIL  0
  MOTOK    1
}
set fcRPM 0
set fcPHASE 1

proc handle_exception {status message args} {
  switch $status {
    0 {
      # TCL_OK, This is raised when you just drop out of the
      # bottom of a 'catch' command.
      return -code ok
    }
    1 {
      # TCL_ERROR
      return -code error "([info level -1]) $message: $args"
    }
    2 {
      # TCL_RETURN
      return -code return "$message"
    }
    3 {
      # TCL_BREAK
      return -code break
    }
    4 {
      # TCL_CONTINUE
      return -code continue
    }
    default {
    # Propogate user defined return codes with message
      return -code $status "$message"
    }
  }
}

namespace eval ::chopper {
  variable ch1_gains
  variable ch2_gains
  variable ch3_gains
  variable speedMult
  variable CW 0 CCW 1
  variable PROPGAIN 1 INTGAIN 2 PHGAIN 3
  variable POSDIR
  set POSDIR $CW

  array set ch1_gains {
  24000  {  350 { 10   10  }  { 10   10  }  {  2.33   2.33 }  }
  21000  {  400 { 10   15  }  { 10   13  }  {  2.33  13    }  }
  18000  {  450 { 10   10  }  { 10   10  }  {  2.33   2.33 }  }
  15000  {  550 { 10   10  }  { 10   10  }  {  2.33   2.33 }  }
  12000  {  650 { 12   12  }  { 10   10  }  { 10     10    }  }
   9000  {  950 { 10   10  }  { 10   10  }  { 10     10    }  }
   6000  { 1350 { 10   10  }  { 10   10  }  { 10     10    }  }
   3600  { 2310 {  5    5  }  {  5    5  }  {  5      5    }  }
   3000  { 2750 {  5    5  }  {  5    5  }  {  2.33   2.33 }  }
  }

  array set ch2_gains {
  24000  {  350 { 15   15  }  { 13   13  }  { 13     13    }  }
  21000  {  400 { 15   15  }  { 13   13  }  { 13     13    }  }
  18000  {  450 { 15   15  }  { 13   13  }  { 13     13    }  }
  15000  {  550 { 15   15  }  { 13   13  }  { 13     13    }  }
  12000  {  650 { 10   10  }  { 10   10  }  {  2.33   2.33 }  }
   9000  {  950 { 10   10  }  { 10   10  }  {  2.33   2.33 }  }
   6000  { 1350 { 10   10  }  { 10   10  }  {  2.33   2.33 }  }
   3000  { 2750 {  5    5  }  {  5    5  }  {  2      2    }  }
  }

  array set ch3_gains {
  24000  {  350 { 15   10  }  { 13   10  }  { 13     2.33 }  }
  21000  {  400 { 15   10  }  { 13   10  }  { 13     2.33 }  }
  18000  {  450 { 15   10  }  { 13   10  }  { 13     2.33 }  }
  15000  {  550 { 10   10  }  { 10   10  }  {  2.33  2.33 }  }
  12000  {  650 { 10   10  }  { 10   10  }  {  2.33  2.33 }  }
   9000  {  950 { 10   10  }  { 10   10  }  {  2.33  2.33 }  }
   6000  { 1350 { 10   10  }  { 10   10  }  {  2.33  2.33 }  }
   3000  { 2750 {  5    5  }  {  5    5  }  {  2     2    }  }
  }

  set i 0
  foreach mult {
     1.0/12 1.0/11 1.0/10 1.0/9 1.0/8
     1.0/7  1.0/6  1.0/5  1.0/4 1.0/3
     1.0/2  1      1.5    2     2.5
     3      3.5    4      4.5   5
     5.5    6      6.5    7     7.5
     8      8.5    9      9.5  10
    10.5   11     11.5   12    12.5
    13     13.5   14     14.5  15
    16     17     18     19    20
    21     22     23     24    25
    26     27     28     29    30
  } {
    set speedMult($i) [expr double($mult)] 
    incr i
  }

  proc Echeck_ChSpeed {ch speed} {
    set minSpeed 3000
    array set vetoRange {min 9000 max 11000}

    if {$ch != 1 && $ch != 2 && $ch != 3} {
      return -code error "Chopper number ($ch) must be 1, 2, or 3"
    }
    if { ![string is double $speed] } {
      return -code error "Speed ($speed) is not a valid number"
    } elseif {$speed < $minSpeed} {
      return -code error "Speed ($speed) should be >= $minSpeed"
    } elseif {$speed >= $vetoRange(min) && $speed <= $vetoRange(max)} {
      return -code error "Speed ($speed) is in the vetoed range: $vetoRange(min), $vetoRange(max)"
    }
  }

  proc Echeck_gainDir {gain dir} {
    variable CW
    variable CCW
    variable PROPGAIN
    variable INTGAIN
    variable PHGAIN

    if {$gain != $INTGAIN && $gain != $PROPGAIN && $gain != $PHGAIN} {
      return -code error "gain identifier ($gain) should be $PROPGAIN (PROPGAIN), $INTGAIN (INTGAIN), or $PHGAIN (PHGAIN)"
    }
    if {$dir != $CW && $dir != $CCW} {
      return -code error "direction identifier ($dir) should be $CW {CW} or $CCW {CCW}"
    }
  }

  proc findGainIndex {ch speed} {
    variable ch1_gains
    variable ch2_gains
    variable ch3_gains

  # Check argument and return error otherwise return veto window
    set catch_status [ catch {
      Echeck_ChSpeed $ch $speed
      set speed [expr int($speed)]
      set ch    [expr int($ch)]
      switch $ch {
        1 { set gainTable ch1_gains }
        2 { set gainTable ch2_gains }
        3 { set gainTable ch3_gains }
      }
      set speeds [lsort -integer [array names $gainTable]]
      set lower 0
      set upper [expr [llength $speeds] - 1]
      while {1} {
        if {[expr $upper - $lower] == 1} {
          if { $speed < [lindex $speeds $upper] } {
            return "$gainTable [lindex $speeds $lower]"
          } else {
            return "$gainTable [lindex $speeds $upper]"
          }
        }
        set mid [expr int(($lower + $upper)/2.0)]
        if {$speed < [lindex $speeds $mid]} {
          set upper $mid
        } else {
          set lower $mid
        }
      }
    } message ]
    handle_exception $catch_status $message
  }

  proc getGain {gain ch speed dir} {
    variable ch1_gains
    variable ch2_gains
    variable ch3_gains
    variable CW
    variable CCW
    variable INTGAIN
    variable PROPGAIN
    variable PHGAIN

    set catch_status [ catch {
      Echeck_ChSpeed $ch $speed
      Echeck_gainDir $gain $dir
      set gain [expr int($gain)]
      set dir [expr int($dir)]
      set speed [expr int($speed)]
      set ch    [expr int($ch)]
      switch $ch {
        1 { set gainTable ch1_gains }
        2 { set gainTable ch2_gains }
        3 { set gainTable ch3_gains }
      }
      foreach sp [lsort -integer -decreasing [array names $gainTable]] {
        if {$sp <= $speed} {
          return [lindex [set ${gainTable}($sp)] $gain $dir]
        }
      }
      error "Failed when looking up integral gain for speed $speed"
    } message ]
    handle_exception $catch_status $message
  }


  #TODO Define a run function which looks up parameters from hdb tree and calls the setSpeed function
  #    pDriv->GetPosition = GetTclPos;
  #    pDriv->RunTo = TclRun;
  #    pDriv->GetStatus = TclStat;
  #    pDriv->GetError = TclError;
  #    pDriv->TryAndFixIt = TclFix;
  #    pDriv->SetDriverPar = TclSetPar;
  #    pDriv->GetDriverPar = TclGetPar;
  #    pDriv->Halt = TclHalt;
  #    pDriv->KillPrivate = KillTCL;
  variable chPath "/instrument/fermi_chopper"

  proc getSpeed {ch} {
    variable chPath

    set rSpeed [hval $chPath/$ch/rotation_speed]
    return $rSpeed
  }

  proc findSpeedMult {sprat} {
    variable speedMult

    set lower 0
    set upper [expr [array size speedMult] - 1]
    while {1} {
      if {[expr ($upper - $lower)] == 1} {
        if { [ expr ($sprat - $speedMult($lower)) ] <= [expr ($speedMult($upper) - $sprat)] } {
          return $speedMult($lower)
        } else {
          return $speedMult($upper)
        }
      }
      set mid [expr int( ($upper + $lower) / 2.0)]
      if { $sprat < $speedMult($mid) } {
        set upper $mid
      } else {
        set lower $mid
      }
    }
  }

  proc listAllowedSpeeds {ref_period_50ns} {
    variable speedMult

    set ref_speed_rpm   [ expr { 60.0 / ($ref_period_50ns * 50e-9) } ]
    set len [array size speedMult]
    for {set i 0} {$i < $len} {incr i} {
      lappend speeds [format "%.2f" [expr $ref_speed_rpm * $speedMult($i)]]
    }
    return $speeds
  }

  proc get_setrefSpeedRPM {hpath node addr name {target ""}} {
    if {$target == ""} {
      set ref_period_50ns [ hval $hpath/$node/ref_period ]
      set ref_speed_rpm   [ expr { 60.0 / ($ref_period_50ns * 50e-9) } ]
      return $ref_speed_rpm
    } else {
      set ref_period_50ns [expr {60.0 / ($target * 50e-9)} ]
      hset $hpath/$node/control/set_ref_period $ref_period_50ns
      return $target
    }
  }
  ##
  # Return nearest allowed speed to target speed
  proc abspermittedSpeed {hpath node addr name target} {
    set ref_period_50ns [ hval $hpath/$node/ref_period ]
#   set ref_period_50ns 333333.33333334
    set ref_speed_rpm   [ expr { 60.0 / ($ref_period_50ns * 50e-9) } ]
    set speed [expr abs($target)]
    set sprat [expr {$speed / $ref_speed_rpm}]
    set mult [findSpeedMult $sprat]
    set allowed_speed [expr $mult * $ref_speed_rpm]
    return $allowed_speed
  }

  proc setSpeed {hpath node addr name target} {
    global SCode
    variable CW
    variable CCW
    variable ch1_gains
    variable ch2_gains
    variable ch3_gains

    set currspeed [hval $hpath/$node/rotation_speed]
    set currdir [hval $hpath/$node/motdir]
    if {$target >= 0} {
      if {$currdir != $CW && $currspeed != 0} {
        clientput "ERROR: Tried to change direction when speed != 0. Set motor to idle first."
        return $SCode(HWFault)
      }
      set dir $CW
    } else {
      if {$currdir != $CCW && $currspeed != 0} {
        clientput "ERROR: Tried to change direction when speed != 0. Set motor to idle first."
        return $SCode(HWFault)
      }
      set dir $CCW
    }

    set allowed_speed [abspermittedSpeed $hpath $node $addr $name $target]
    foreach {gainTable index} [findGainIndex $addr $allowed_speed] {}
    set row [array get $gainTable $index]
    set vetowin  [lindex $row 1 0]
    set propGain [lindex $row 1 1 $dir]
    set intGain  [lindex $row 1 2 $dir]
    set phGain   [lindex $row 1 3 $dir]
    
    hset $hpath/$node/control/set_motor_dir $dir
    hset $hpath/$node/control/set_vetowin50 [expr $vetowin/50.0]
    hset $hpath/$node/control/set_prop_gain $propGain
    hset $hpath/$node/control/set_int_gain $intGain
    hset $hpath/$node/control/set_phase_gain $phGain
    hset $hpath/$node/control/set_rotspeed $allowed_speed
    return $SCode(OKOK)
  }

  proc get_setPeriod {hpath node addr name {target ""}} {
    global SCode
    if {$target == ""} {
      set refPeriod [hval $hpath/$node/ref_period]
      return $refPeriod
    } else {
      hset $hpath/$node/control/set_ref_period $target
      return $target
    }
  }

  proc get_setMode {hpath node addr name {target ""}} {
    global SCode
    if {$target == ""} {
      set mode [hval $hpath/$node/mode]
      return $mode
    } else {
      hset $hpath/$node/control/set_mode $target
      return $target
    }
  }

  proc get_setRefDelay {hpath node addr name {target ""}} {
    global SCode
    if {$target == ""} {
      set delay [hval $hpath/$node/ref_delay]
      return $delay
    } else {
      hset $hpath/$node/control/set_ref_delay $target
      return $target
    }
  }

  proc get_setSettleTime {hpath node addr name {settle ""}} {
    global SCode
    if {$settle == ""} {
      return [hgetpropval $hpath/$node settletime]
    } else {
      hsetprop $hpath/$node settletime $settle
      return $settle
    }
  }

  proc imot_SGetpos {hpath node addr name} {
    global SCode
    variable CW
    variable CCW
    variable POSDIR

    set chpath ${hpath}/$node/rotation_speed
    set speed [hval $chpath]
    if {[hval $hpath/$node/motdir] == $POSDIR} {
      return $speed
    } else {
      return -$speed
    }
  }

  proc imot_SRun {hpath node addr name target} {
    global SCode
    set catch_status [ catch {
      if {[hval $hpath/$node/control/device_error] != ""} {
        hset $hpath/$node/control/device_error ""
      }
      # imot_status clears the abort flag, but this only happens if imot_status
      # is being checked because of a "run" or "drive" when a stop is sent.
      hsetprop $hpath/$node abort 0
      set ret [setSpeed $hpath $node $addr $name $target]
      if {$ret != $SCode(OKOK)} {
        return $ret
      }
      hset $hpath/$node/control/start 1
      # Disable idle if set
      if {[hval $hpath/$node/idle_toggle] == 1} {
        hset $hpath/$node/control/idle_toggle 1
      }
      hsetprop $hpath/$node/control timecheck -1
      set readtime [hgetpropval $hpath read_time]
      hsetprop $hpath lastupdate $readtime
      return $SCode(OKOK)
    } message ]
    handle_exception $catch_status $message
  }

  proc imot_Status {hpath node addr name} {
    global SCode
    set catch_status [ catch {
      if [hgetpropval $hpath/$node abort] {
        hsetprop $hpath/$node abort 0
        clientput "ERROR: User requested stop. Aborting operation"
        return $SCode(HWFault)
      }
      set devErr [hval $hpath/$node/control/device_error]
      if {$devErr != ""} {
        clientput "ERROR: Drive request failed.  Aborting operation"
        clientput $devErr
        return $SCode(HWFault)
      }
      if [hpropexists $hpath geterror] {
        clientput "WARNING: [hgetpropval $hpath geterror]"
      }
      set readtime [hgetpropval $hpath read_time]
      set lastupdate [hgetpropval $hpath lastupdate]
      if {$readtime <= $lastupdate} {
        return $SCode(HWBusy)
      }
      hsetprop $hpath lastupdate $readtime
      set up_to_speed [hval $hpath/$node/system_status/up_to_speed]
      set phase_locked [hval $hpath/$node/system_status/phase_locked]
      set timecheck [hgetpropval $hpath/$node/control timecheck]
      set timeout [hgetpropval $hpath/$node settletime]
      if {[hval $hpath/$node/mode] == 0} {
        # RPM mode
        set locked $up_to_speed
      } else {
        # PHASE mode
        set locked $phase_locked
      }
      if {$locked} {
        if {$timecheck == -1} {
          hsetprop $hpath/$node/control timecheck $readtime
          return $SCode(HWBusy)
        } elseif {[expr $readtime - $timecheck] > $timeout} {
          return $SCode(HWIdle)
        }
      } else {
        if {$timecheck != -1} {
          hsetprop $hpath/$node/control timecheck $readtime
        }
        return $SCode(HWBusy)
      }
    } message ]
    handle_exception $catch_status $message
  }

  proc imot_Halt {hpath node addr name} {
    global SCode
    # Don't halt the choppers just because someone sent INT1712 to interrupt a scan.
    if { [GetInt] == "continue" } {
      hset $hpath/$node/control/stop 1
      hsetprop $hpath/$node abort 1
    }
    return $SCode(OKOK)
  }

  proc imot_GetError {hpath node addr name} {
    global SCode
    return [hgetpropval $hpath/$node errmsg]
  }

  proc imot_SFixit {hpath node addr name icode fVal} {
    global ECode

    return $ECode(MOTFAIL)
  }
  # Implementation of phase motor interface
  proc imot_PhGetPos {hpath node addr name} {
    set delay [hval $hpath/$node/ref_delay]
    return $delay
  }

  proc imot_PhRun {hpath node addr name target} {
    global SCode
    
    hset $hpath/$node/control/set_ref_delay $target
    hsetprop $hpath/$node/control timecheck -1
    set readtime [hgetpropval $hpath read_time]
    hsetprop $hpath lastupdate $readtime
    return $SCode(OKOK)
  }

  proc imot_PhFixit {hpath node addr name icode fVal} {
    global SCode

    return $SCode(MOTFAIL)
  }
  
  proc get_phase_acc {ch} {
    variable chPath

    set phase_acc [hval $chPath/$ch/phase_acc]
    return $phase_acc
  }

  proc get_phase_rep {ch} {
    variable chPath

    set phase_rep [hval $chPath/$ch/phase_rep]
    return $phase_rep
  }

  proc get_phase_ok {ch} {
    variable chPath

    set phase_ok [hval $chPath/$ch/phase_ok]
    return $phase_ok
  }

  ##
  # @brief Returns the permitted speed as a multiple of the reference period
  # @target 0, Lists permitted speeds as multiples of the reference speed.
  #         speed, Returns nearest permitted speed to the given speed.
  proc permSpd {hpath node addr name target} {
    global SCode

    set ref_period_50ns [ hval $hpath/$node/ref_period ]
    if {$target == 0} {
      return "[listAllowedSpeeds $ref_period_50ns]"
    }
    set allowed_speed [abspermittedSpeed $hpath $node $addr $name $target]
    if {$target > 0} {
      return $allowed_speed
    } else {
      return -$allowed_speed
    }
  }

  proc start {hpath node addr name} {
    global SCode
    hset $hpath/$node/control/start 1
    return $SCode(OKOK)
  }

  # Speed motor stop
  proc stop {hpath node addr name} {
    global SCode
    hset $hpath/$node/control/stop 1
    hsetprop $hpath/$node abort 1
    return $SCode(OKOK)
  }

  proc reset {hpath node addr name} {
    global SCode
    hset $hpath/$node/control/reset 1
    return $SCode(OKOK)
  }

  proc idle {hpath node addr name} {
    global SCode
    hset $hpath/$node/control/idle_toggle 1
    hsetprop $hpath/$node abort 1
    return $SCode(OKOK)
  }
}


proc mkFSpeedMot {fmot hdbroot chnode addr lowlim uplim} {
  clientput [info level 0]
  Motor $fmot tclmot [subst {\
    getpos    "::chopper::imot_SGetpos       $hdbroot $chnode $addr"\
    run       "::chopper::imot_SRun          $hdbroot $chnode $addr"\
    status    "::chopper::imot_Status        $hdbroot $chnode $addr"\
    halt      "::chopper::imot_Halt          $hdbroot $chnode $addr"\
    geterror  "::chopper::imot_GetError      $hdbroot $chnode $addr"\
    fixit     "::chopper::imot_SFixit        $hdbroot $chnode $addr"\
    refspeed  "::chopper::get_setrefSpeedRPM $hdbroot $chnode $addr"\
    refperiod "::chopper::get_setPeriod      $hdbroot $chnode $addr"\
    mode      "::chopper::get_setMode        $hdbroot $chnode $addr"\
    refdelay  "::chopper::get_setRefDelay    $hdbroot $chnode $addr"\
    settle    "::chopper::get_setSettleTime  $hdbroot $chnode $addr"\
    setspeed  "::chopper::setSpeed           $hdbroot $chnode $addr"\
    permspd   "::chopper::permSpd            $hdbroot $chnode $addr"\
    start     "::chopper::start              $hdbroot $chnode $addr"\
    stop      "::chopper::stop               $hdbroot $chnode $addr"\
    idle      "::chopper::idle               $hdbroot $chnode $addr"\
  }]
  hsetprop $hdbroot/$chnode abort 0
  $fmot movecount 5000
  $fmot hardlowerlim $lowlim
  $fmot hardupperlim $uplim
  $fmot softlowerlim $lowlim
  $fmot softupperlim $uplim
  $fmot settle 30
  $fmot precision 1
  $fmot maxretry 0
  sicslist setatt $fmot units rpm
  sicslist setatt $fmot klass fermi_chopper
  sicslist setatt $fmot long_name $fmot
}

proc mkFPhaseMot {fmot hdbroot chnode addr lowlim uplim} {
  clientput [info level 0]
  Motor $fmot tclmot [subst {\
    getpos    "::chopper::imot_PhGetPos      $hdbroot $chnode $addr"\
    run       "::chopper::imot_PhRun         $hdbroot $chnode $addr"\
    status    "::chopper::imot_Status        $hdbroot $chnode $addr"\
    halt      "::chopper::imot_Halt          $hdbroot $chnode $addr"\
    geterror  "::chopper::imot_GetError      $hdbroot $chnode $addr"\
    fixit     "::chopper::imot_PhFixit       $hdbroot $chnode $addr"\
    refspeed  "::chopper::get_setrefSpeedRPM $hdbroot $chnode $addr"\
    refperiod "::chopper::get_setPeriod      $hdbroot $chnode $addr"\
    mode      "::chopper::get_setMode        $hdbroot $chnode $addr"\
    refdelay  "::chopper::get_setRefDelay    $hdbroot $chnode $addr"\
    settle    "::chopper::get_setSettleTime  $hdbroot $chnode $addr"\
    setspeed  "::chopper::setSpeed           $hdbroot $chnode $addr"\
    phase_acc "::chopper::get_phase_acc      $hdbroot $chnode $addr"\
    phase_rep "::chopper::get_phase_rep      $hdbroot $chnode $addr"\
    phase_ok  "::chopper::get_phase_ok       $hdbroot $chnode $addr"\
    start     "::chopper::start              $hdbroot $chnode $addr"\
    stop      "::chopper::stop               $hdbroot $chnode $addr"\
    idle      "::chopper::idle               $hdbroot $chnode $addr"\
  }]
  hsetprop $hdbroot/$chnode abort 0
  $fmot movecount 5000
  $fmot hardlowerlim $lowlim
  $fmot hardupperlim $uplim
  $fmot softlowerlim $lowlim
  $fmot softupperlim $uplim
  $fmot settle 30
  $fmot precision 1
  $fmot maxretry 0
  sicslist setatt $fmot units ns
  sicslist setatt $fmot klass fermi_chopper
  sicslist setatt $fmot long_name $fmot
}

##
# @brief Make the fermichopper driver and speed and phase motors for the master and
# slave choppers.
#
# @param mSpdmot, Name of master chopper speed motor
# @param sSpdmot, Name of slave chopper speed motor
# @param sPhmot, Name of slave chopper phase motor
# @param master, Master chopper hdb node name
# @param maddr, Master chopper modbus address
# @param slave, Slave chopper hdb node name
# @param saddr, Slave chopper modbus address
#proc mkFermiMotors {mSpdmot sSpdmot master maddr slave saddr} {
proc mkFermiMotors { master maddr {slave "EMPTY"} {saddr "EMPTY"} } {
  set mSpdmot ${master}s
  set mPhmot ${master}p
  set sSpdmot ${slave}s
  set sPhmot ${slave}p
  if {$slave != "EMPTY"} {
    # Make master and slave choppers
    set hdbPath [mkChoppers " $master $maddr $slave $saddr"]
    mkFSpeedMot $sSpdmot $hdbPath $slave $saddr -24000 24000
    mkFPhaseMot $sPhmot  $hdbPath $slave $saddr 0 166.6667e6
  } else {
    set hdbPath [mkChoppers " $master $maddr"]
  }
  #  Create Master Chopper Speed motor object
  mkFSpeedMot $mSpdmot $hdbPath $master $maddr -24000 24000
  mkFPhaseMot $mPhmot $hdbPath $master $maddr 0 166.6667e6
}

mkFermiMotors mch 2 sch 3

mchs mode $fcPHASE
schs mode $fcPHASE