# 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
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 $CCW

  array set ch1_gains {
  24000 { 0.35 { 10 10 }  { 10 10 }  { 2.33 2.33 }  } 
  21000 { 0.40 { 10 15 }  { 10 13 }  { 2.33 13   }  } 
  18000 { 0.45 { 10 10 }  { 10 10 }  { 2.33 2.33 }  } 
  15000 { 0.55 { 10 10 }  { 10 10 }  { 2.33 2.33 }  } 
  12000 { 0.65 { 12 12 }  { 10 10 }  { 10 10     }  } 
  9000  { 0.95 { 10 10 }  { 10 10 }  { 10 10     }  } 
  6000  { 1.35 { 10 10 }  { 10 10 }  { 10 10     }  } 
  3600  { 2.31 { 5 5   }  { 5 5   }  { 5 5       }  } 
  3000  { 2.75 { 5 5   }  { 5 5   }  { 2.33 2.33 }  } 
  }

  array set ch2_gains {
  24000 { 0.35 { 15 15 }  { 13 13 }  { 13 13     }  } 
  21000 { 0.40 { 15 15 }  { 13 13 }  { 13 13     }  } 
  18000 { 0.45 { 15 15 }  { 13 13 }  { 13 13     }  } 
  15000 { 0.55 { 15 15 }  { 13 13 }  { 13 13     }  } 
  12000 { 0.65 { 10 10 }  { 10 10 }  { 2.33 2.33 }  } 
  9000  { 0.95 { 10 10 }  { 10 10 }  { 2.33 2.33 }  } 
  6000  { 1.35 { 10 10 }  { 10 10 }  { 2.33 2.33 }  } 
  3000  { 2.75 { 5 5   }  { 5 5   }  { 2 2       }  } 
  }

  array set ch3_gains {
  24000 { 0.35 { 15 10 }  { 13 10 }  { 13 2.33   }  } 
  21000 { 0.40 { 15 10 }  { 13 10 }  { 13 2.33   }  } 
  18000 { 0.45 { 15 10 }  { 13 10 }  { 13 2.33   }  } 
  15000 { 0.55 { 10 10 }  { 10 10 }  { 2.33 2.33 }  } 
  12000 { 0.65 { 10 10 }  { 10 10 }  { 2.33 2.33 }  } 
  9000  { 0.95 { 10 10 }  { 10 10 }  { 2.33 2.33 }  } 
  6000  { 1.35 { 10 10 }  { 10 10 }  { 2.33 2.33 }  } 
  3000  { 2.75 { 5 5   }  { 5 5   }  { 2 2       }  } 
  }

  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
  } {
    lappend speedMult [expr double($mult)]
  }

  ##
  # Echeck_SpeedMult: Checks that speed is an allowed multiple of the reference signal
  # refFreq: Reference signal frequency (Hz)
  # speed: Requested chopper speed (RPM)
  proc Echeck_SpeedMult {refFreq speed} {
    variable speedMult
    set refRPM [expr 60 * $refFreq]
    foreach mult $speedMult {
      if { [expr $mult * $refRPM] == $speed} {return}
    }
    return -code error "Speed ($speed) is not an allowed multiple of the reference signal"
  }


  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 getVetoWin {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 }
      }
      foreach sp [lsort -integer -decreasing [array names $gainTable]] {
        if {$sp <= $speed} {
          return [lindex [set ${gainTable}($sp)] 0]
        }
      }
      error "Failed when looking up veto window for speed $speed"
    } 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
  }


  ##
  # setSpeed
  # ch: Chopper number
  # refFreq: The reference frequency in Hz
  # dir: Chopper direction CW or CCW
  # speed: The chopper speed in RPM
  proc setSpeed {ch refFreq dir speed} {
    set catch_status [ catch {
      Echeck_SpeedMult $refFreq $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
  }

  ##
  # Implement speed and phase motor objects
  proc SGetPos {hpath node addr name} {
    global SCode
    variable CW
    variable CCW
    variable POSDIR

    set chpath ${hpath}/$node/rotation_speed
    set speed [hval $chpath]
    broadcast [info level 0]: chpath=$chpath, speed = $speed
    if {[hval $hpath/$node/motdir] == $POSDIR} {
    return $speed
    } else {
      return -$speed
    }
  }

  proc SRun {hpath node addr name target} {
    global SCode
    variable CW
    variable CCW

#   broadcast [info level 0]
    if {$target >= 0} {
      set dir $CW
    } else {
      set dir $CCW
    }
    set speed [expr abs($target)]
    getVetoWin $addr $speed
    
    broadcast TODO: hset $hpath/$node/control/set_rotspeed $speed
#   hset $hpath/$node/control/set_rotspeed $speed
    return $SCode(OKOK)
  }

  proc SStatus {hpath node addr name} {
    global SCode

    return $SCode(HWIdle)
  }

  proc SHalt {hpath node addr name} {
    global SCode
  }

  proc SGetError {hpath node addr name} {
    global SCode
    return "[info level 0]: TODO error message"
  }

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

    return $SCode(MOTOK)
  }
  # Implementation of phase motor interface
  proc PhGetPos {hpath node addr name} {
    global SCode
    return 0
  }

  proc PhRun {hpath node addr name target} {
    global SCode
    
    return $SCode(OKOK)
  }

  proc PhStatus {hpath node addr name} {
    global SCode

    return $SCode(HWIdle)
  }

  proc PhHalt {hpath node addr name} {
    global SCode
  }

  proc PhGetError {hpath node addr name} {
    global SCode
    return "[info level 0]: TODO error message"
  }

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

    return $SCode(MOTOK)
  }
}
# TEST
global SCode
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
}
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"
    }
  }
}

proc mkFSpeedMot {fmot hdbroot chnode addr lowlim uplim} {
  Motor $fmot tclmot [params\
    getpos   "::chopper::SGetPos   $hdbroot $chnode $addr"\
    run      "::chopper::SRun      $hdbroot $chnode $addr"\
    status   "::chopper::SStatus   $hdbroot $chnode $addr"\
    halt     "::chopper::SHalt     $hdbroot $chnode $addr"\
    geterror "::chopper::SGetError $hdbroot $chnode $addr"\
    fixit    "::chopper::SFixit    $hdbroot $chnode $addr"
  ]
  $fmot hardlowerlim $lowlim
  $fmot hardupperlim $uplim
  $fmot softlowerlim $lowlim
  $fmot softupperlim $uplim
  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} {
  Motor $fmot tclmot [params\
    getpos   "::chopper::PhGetPos   $hdbroot $chnode $addr"\
    run      "::chopper::PhRun      $hdbroot $chnode $addr"\
    status   "::chopper::PhStatus   $hdbroot $chnode $addr"\
    halt     "::chopper::PhHalt     $hdbroot $chnode $addr"\
    geterror "::chopper::PhGetError $hdbroot $chnode $addr"\
    fixit    "::chopper::PhFixit    $hdbroot $chnode $addr"
  ]
  $fmot hardlowerlim $lowlim
  $fmot hardupperlim $uplim
  $fmot softlowerlim $lowlim
  $fmot softupperlim $uplim
  sicslist setatt $fmot units degrees
  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 sPhmot master maddr slave saddr} {
  set hdbPath [mkChoppers " $master $maddr $slave $saddr"]
  #  Create Master Chopper Speed motor object
  mkFSpeedMot $mSpdmot $hdbPath $master $maddr -24000 24000
  mkFSpeedMot $sSpdmot $hdbPath $slave $saddr -24000 24000
  mkFPhaseMot $sPhmot  $hdbPath $slave $saddr 0 180
}

mkFermiMotors mchs schs schp mch 1 sch 2