# TODO Check if requested tilt-angle is within range

##
# @file
# The velocity selector control is split into two objects,
# 1. velsel_poller: This object polls the velocity selector to get its
#    current state.  The first time that it gets valid state info it will
#    register the read and write parameters for the velocity_selector object
#    and create nvs_speed and nvs_lambda drivable adapters.
# 2. velocity_selector: This object manages a set of status nodes which
#    correspond to the state parameters read by the velsel_poller object.
#    It also provides commands to set the speed, wavelength and angle for the velocity
#    selector and provides drivable interfaces for the speed and wavelength.
#
#  You can drive the velocity selector speed via the driveable object called nvs_speed
#  You can drive the wavelength via the driveable object called nvs_lambda

# NOTE Doesn't provide the power loss command. Do we need it?

# Test by adding the following to barebones.tcl
# InstallHdb
# source config/velocity_selector/xsct_velsel.tcl
# hfactory /velocity_selector link velocity_selector

# The velocity selector doesn't close client connections
# if the connection is broken.  It only closes the connection
# when a client logs off with "#SES#bye", NOTE bye must be lowercase.

namespace eval ::scobj::velocity_selector {
  variable UID
  variable PWD
  variable sim_mode
  variable paramindex
  variable paramtype
  variable pollrate 7

  #from NVSOptions.cpp nha
  # m_dTwistAngle degrees
  # m_dTwistAngle m
  # m_iMaxSpeed   rpm
  variable m_dTwistAngle 
  variable m_dLength     
  variable m_iMaxSpeed   
  variable rBeamCenter   
  variable VNeutron      
  variable blocked_speeds

  set sim_mode [SplitReply [velsel_simulation]]

  proc AngleSpeedToWavelength {angle VsVarSpeed} {
    variable m_dTwistAngle
    variable m_dLength    
    variable m_iMaxSpeed  
    variable rBeamCenter  
    variable VNeutron     

    if {$VsVarSpeed < 3100} {
      return -code error "Minimum speed is 3100 rpm"
    }

    set lambda0 [expr ($m_dTwistAngle*60.0*$VNeutron)/(360.0*$m_dLength*$m_iMaxSpeed)]
    set pi [expr acos(-1)]
  #	set pi = 3.14159265358979;
    
    set A [expr (2.0 * $rBeamCenter * $pi) / (60.0 * $VNeutron)]
    set angle_rad [expr ($angle * $pi) / 180.0]
    set lambda1 [expr ( tan($angle_rad)+($A * $m_iMaxSpeed * $lambda0) ) / ((-($A*$A) * $m_iMaxSpeed * $VsVarSpeed * $lambda0 * tan($angle_rad) )+($A * $VsVarSpeed))]

    return [format "%#.5g" $lambda1]
  }

  proc WavelengthToSpeed {angle lambda1} {
    variable m_dTwistAngle
    variable m_dLength    
    variable m_iMaxSpeed  
    variable rBeamCenter  
    variable VNeutron     

    if {$lambda1 < 4.6125} {
      return -code error "Minimum wavelength is 4.6125 Angstrom"
    }

    set lambda0 [expr ($m_dTwistAngle*60.0*$VNeutron)/(360.0*$m_dLength*$m_iMaxSpeed)]
    set pi [expr acos(-1)]
  #	set pi = 3.14159265358979;
    
    set A [expr (2.0 * $rBeamCenter * $pi) / (60.0 * $VNeutron)]
    set angle_rad [expr ($angle * $pi) / 180.0]
    set VsVarSpeed [expr ( tan($angle_rad)+($A * $m_iMaxSpeed * $lambda0) ) / ((-($A*$A) * $m_iMaxSpeed * $lambda1 * $lambda0 * tan($angle_rad) )+($A * $lambda1))]

    return [expr round($VsVarSpeed)]
  }


    array set paramindex {
      state    0
      rspeed   1
      aspeed   2
      sspeed   3
      aveto    4
      ploss    5
      splos    6
      ttang    7
      rtemp    8
      wflow    9
      winlt   10
      woutt   11
      vacum   12
      wvalv   13
      vvalv   14
      vibrt   15
      bcuun   16
    }
    array set paramtype {
      state    text
      rspeed   float
      aspeed   float
      sspeed   float
      aveto    text
      ploss    float
      splos    float
      ttang    float
      rtemp    float
      wflow    float
      winlt    float
      woutt    float
      vacum    float
      wvalv    text
      vvalv    text
      vibrt    float
      bcuun    float
    }
  MakeSICSObj velsel_poller SCT_OBJECT
  MakeSICSObj velocity_selector SCT_OBJECT
  sicslist setatt velocity_selector klass NXvelocity_selector
  sicslist setatt velocity_selector long_name velocity_selector

proc sendUID {user} {
   sct send $user
   return rdPwdChallenge
}

proc rdPwdChallenge {} {
  set challenge [sct result]
  return sndPwd
}
proc sndPwd {pwd} {
  sct send $pwd
  return rdPwdAck
}
proc rdPwdAck {} {
   set ack [sct result]
  return idle
}
##
# @brief Request a state report from the velocity selector
  proc getStatus {} {
    sct send "#SOS#STATE "
    return rdState
  }

  ##
  # @brief Read the current state report from the velocity selector.
  proc rdState {root statuspath} {
    variable paramindex

    set staterep [sct result]
    if {[string match {ASCERR:*} $staterep]} {
       hset $root/device_error $staterep
       return idle
    }
    if {[string match {*#SES#You are not a valid user*} $staterep]} {
       return sendUID
    }
    if {[string match {#SOS#*} $staterep] == 0 } {
      hset $root/device_error $staterep
      return idle
    }
    set status [lrange [split $staterep "#"] 3 end-1]
    set rspeed [lindex $status $paramindex(rspeed) end]
    set aspeed [lindex $status $paramindex(aspeed) end]
    set speedvar [expr 0.2*$rspeed/100]
    if {[hval $root/status] == "busy"} {
      set target [hgetpropval $root/setspeed target]
      if {$rspeed != $target} {
        hset $root/device_error "Resending target speed $target"
        hset $root/setspeed $target"
        return idle
      }
      if {[expr abs($rspeed - $aspeed)] <= $speedvar} {
        hset $root/status "idle"
        statemon stop nvs_speed
        statemon stop nvs_lambda
        if [hgetpropval $root/setspeed driving] {
          hsetprop $root/setspeed driving 0
          hsetprop $root/setLambdaA driving 0
        }
      }
    }
    if {$staterep != [sct oldval]} {
      set state [lindex $status $paramindex(state) end]
      if {$state != [sct oldstate]} {
        if {[string match {*CONTROL*} $state] && [expr abs($rspeed - $aspeed)] > $speedvar} {
#         hset $root/status "busy"
        } elseif {[string match {*CONTROL*} $state]==0 && $aspeed == 0} {
          hset $root/status "idle"
          statemon stop nvs_speed
          statemon stop nvs_lambda
          if [hgetpropval $root/setspeed driving] {
            hsetprop $root/setspeed driving 0
            hsetprop $root/setLambdaA driving 0
          }
        }
        sct oldstate $state
      }
      if {[sct oldval] == "UNKNOWN"} {
        sct_velsel_init $root
      }
      sct oldval $staterep
      sct update $status
      sct utime readtime
    }
    if {[hval $root/device_error] != ""} {
      hset $root/device_error ""
    }
    return idle
  }

##
# @brief This dummy read command forces a transition to a state which
# will update a parameter from the current status.
  proc getpar {nextstate} {
    return $nextstate
  }

  proc noResponse {} {
    sct result
    return idle
  }
##
# @brief Looks up a parameter in the current status and updates the
# parameter node.
# @param statuspath, path to the poller object's status node.
# @param parindex, index of the required parameter
  proc updatepar {statuspath parindex} {
    set data [lindex [hval $statuspath] $parindex end]
    if {$data != [sct oldval]} {
      sct oldval $data
      sct update $data
      sct utime readtime
    }
    return idle
  }

  proc setSpeed {vs_root statuspath nextState} {
    variable paramindex
    set speed [format "%5d" [sct target]] 

    sct send "#SOS#SPEED $speed"
    set angle [lindex [hval $statuspath] $paramindex(ttang) end]
    set lambda [AngleSpeedToWavelength $angle $speed]
    sct target $speed
    hsetprop $vs_root/setLambdaA target $lambda
    hset $vs_root/status "busy"
    statemon start nvs_speed
    statemon start nvs_lambda
    if {[sct writestatus] == "start"} {
    # Called by drive adapter
      hsetprop $vs_root/setspeed driving 1
      hsetprop $vs_root/setLambdaA driving 1
    }
    return $nextState
  }

  proc sendCommand {nextState} {
    set state [string tolower [sct target]] 
      switch $state {
        "idle" {
          sct send "#SOS#IDLE  "
        }
        "brake" {
          sct send "#SOS#BRAKE "
        }
        "init" {
          sct send "#SOS#TTINIT"
        }
        default {
					return idle
        }
      }
    return $nextState
  }


  proc readLambda {statuspath} {
    variable paramindex

    set angle [lindex [hval $statuspath] $paramindex(ttang) end]
    set aspeed [lindex [hval $statuspath] $paramindex(aspeed) end]
    set lambda [AngleSpeedToWavelength $angle $aspeed]
    if {$lambda != [sct oldval]} {
      sct oldval $lambda
      sct update $lambda
      sct utime readtime
    }
    return idle
  }

##
# @brief This will check if turntable operation is allowed
  proc ttableCheck {statuspath nextState} {
    variable paramindex

    set state [lindex [hval $statuspath] $paramindex(state) end]
    set aspeed [lindex [hval $statuspath] $paramindex(aspeed) end]
    if {[string match {*CONTROL*} $state] || $aspeed != 0} {
      error "Not allowed while the velocity selector is running"
    }
    return OK
  }

proc is_Speed_in_blocked_range {speed} {
    variable blocked_speeds
    foreach {min max} $blocked_speeds {
      if {$min <= $speed && $speed <= $max} {
        error "Speed of $speed rpm is within the blocked range of $min to $max rpm"
      }
    }
    return OK
}
proc get_nearest_allowed_speed {speed} {
  variable blocked_speeds
  set speed_ok true

  foreach {min max} $blocked_speeds {
    if {$min <= $speed && $speed <= $max} {
      set speed_ok false
      break
    }
  }
  if {$speed_ok} {
    return $speed
  } else {
    foreach {min max} $blocked_speeds {
      if {$min <= $speed && $speed <= $max} {
        if {$min == -inf} {
          return [expr $max+10]
        }
        if {$max == inf} {
          return [expr $min-10]
        }
        if {[expr $max - $speed] > [expr $speed - $min]} {
          return [expr $min-10]
        } else {
          return [expr $max+10]
        }
      }
    }
  }
}

##
# @brief This will check if the requested speed is allowed
  proc checkBlockedSpeeds {statuspath} { 
	  variable paramindex

    set speed [sct target]
    set ttang [lindex [hval $statuspath] $paramindex(ttang) end]
    if {$ttang > 90} { 
			error "ERROR: You must first initialise the turntable"
    }

    return [is_Speed_in_blocked_range $speed]
  }

##
# @brief This will check if target wavelength is allowed
proc checkBlockedWavelengths {statuspath} {
    variable paramindex

    set lambda [sct target]
    set ttang [lindex [hval $statuspath] $paramindex(ttang) end]
    if {$ttang > 90} { 
			error "ERROR: You must first initialise the turntable"
    }
    set angle [lindex [hval $statuspath] $paramindex(ttang) end]
    set speed [WavelengthToSpeed $angle $lambda]
    return [is_Speed_in_blocked_range $speed]
}
##
# @brief Implement the checkstatus command for the drivable interface
#
# NOTE: The drive adapter initially sets the writestatus to "start" and will
# only call this when writestatus!="start"
# TODO Do we need to handle hardware faults or is the state check in rdstate enough?
  proc drivestatus {} {
    if [sct driving] {
      return busy
    } else {
      return idle
    }
  }

proc halt {root} {
  hsetprop $root/setspeed driving 0
  hsetprop $root/setLambdaA driving 0
  hset $root/status "idle"
  statemon stop nvs_speed
  statemon stop nvs_lambda
  set speed [get_nearest_allowed_speed [hval $root/aspeed]]
  broadcast halt: set speed to $speed
  catch {hset $root/setspeed $speed} msg
  broadcast $msg
  return idle
}

  proc setPar {par nextState} {
    set val [sct target]
    sct send "#SOS#$par $val"
    return $nextState
  }

  proc setLambda {vs_root statuspath nextState} {
    variable paramindex
    set lambda [sct target]

    set angle [lindex [hval $statuspath] $paramindex(ttang) end]
    set speed [WavelengthToSpeed $angle $lambda]
    set fmtspeed [format "%5d" $speed] 
    sct send "#SOS#SPEED $fmtspeed"
    sct target $lambda
    hsetprop $vs_root/setspeed target $speed
    hset $vs_root/status "busy"
    statemon start nvs_speed
    statemon start nvs_lambda
    if {[sct writestatus] == "start"} {
    # Called by drive adapter
      hsetprop $vs_root/setLambdaA driving 1
      hsetprop $vs_root/setspeed driving 1
    }
    return $nextState
  }

# Create Velocity selector control
  set scobjNS ::scobj::velocity_selector
  set statusPath /sics/velsel_poller/status
  set velselPath /sics/velocity_selector

  hfactory $statusPath plain internal text
  hsetprop $statusPath read ${scobjNS}::getStatus
  hsetprop $statusPath rdState ${scobjNS}::rdState $velselPath $statusPath
  hsetprop $statusPath sendUID ${scobjNS}::sendUID $UID
  hsetprop $statusPath rdPwdChallenge ${scobjNS}::rdPwdChallenge
  hsetprop $statusPath sndPwd ${scobjNS}::sndPwd $PWD
  hsetprop $statusPath rdPwdAck ${scobjNS}::rdPwdAck
  hsetprop $statusPath oldval "UNKNOWN"
  hsetprop $statusPath oldstate "UNKNOWN"

# Abstract status info for GumTree
  hfactory $velselPath/status plain spy text
  hset $velselPath/status "UNKNOWN"
  hsetprop $velselPath/status values busy,idle
  hfactory $velselPath/device_error plain spy text
  hset $velselPath/device_error ""
  
# Must be set by user
  hfactory $velselPath/LambdaResFWHM_percent plain user float
  hfactory $velselPath/geometry plain spy none
  hfactory $velselPath/geometry/position plain spy none
  hfactory $velselPath/geometry/position/VelSelPosXmm plain user float
  hfactory $velselPath/geometry/position/VelSelPosYmm plain user float
  hfactory $velselPath/geometry/position/VelSelPosZmm plain user float
  hfactory $velselPath/geometry/position/VelSelCoordScheme plain user text
  
# Get parameters from state report
  foreach par [lsort [array names paramindex]] {
    hfactory $velselPath/$par plain spy $paramtype($par)
    hsetprop $velselPath/$par read ${scobjNS}::getpar rdpar
    hsetprop $velselPath/$par rdpar ${scobjNS}::updatepar $statusPath $paramindex($par)
    hsetprop $velselPath/$par oldval "UNKNOWN"
  }
# Initialise turntable command
  hfactory $velselPath/ttinit plain spy none
  hsetprop $velselPath/ttinit check ${scobjNS}::ttableCheck $statusPath ignore
  hsetprop $velselPath/ttinit write ${scobjNS}::sendCommand ignore
  hsetprop $velselPath/ttinit ignore ${scobjNS}::noResponse
  hsetprop $velselPath/ttinit values init

# Set tilt angle
# TODO Can we set "check" to test if angle is within range then chain to ttableCheck
	hfactory $velselPath/set_ttang plain spy float
  hsetprop $velselPath/set_ttang check ${scobjNS}::ttableCheck $statusPath ignore
  hsetprop $velselPath/set_ttang write ${scobjNS}::setPar TTANGL ignore
  hsetprop $velselPath/set_ttang ignore ${scobjNS}::noResponse


# Get Lambda
  hfactory $velselPath/LambdaA plain spy float
  hsetprop $velselPath/LambdaA read ${scobjNS}::getpar rdpar
  hsetprop $velselPath/LambdaA rdpar ${scobjNS}::readLambda $statusPath
  hsetprop $velselPath/LambdaA oldval "UNKNOWN"
# Set Lambda
  hfactory $velselPath/setLambdaA plain spy float
  hsetprop $velselPath/setLambdaA check ${scobjNS}::checkBlockedWavelengths $statusPath
  hsetprop $velselPath/setLambdaA write ${scobjNS}::setLambda $velselPath $statusPath ignore
  hsetprop $velselPath/setLambdaA ignore ${scobjNS}::noResponse
  hsetprop $velselPath/setLambdaA driving 0
#TODO WARNING remove sicsdev and type if setLambdaA gets a drive addapter
# hsetprop $velselPath/setLambdaA sicsdev "nvs_lambda"
  hsetprop $velselPath/setLambdaA type "drivable"
  hsetprop $velselPath/setLambdaA target 0
  hsetprop $velselPath/setLambdaA writestatus "UNKNOWN"

# Set speed
  hfactory $velselPath/setspeed plain spy int
  hsetprop $velselPath/setspeed check ${scobjNS}::checkBlockedSpeeds $statusPath
  hsetprop $velselPath/setspeed write ${scobjNS}::setSpeed $velselPath $statusPath ignore
  hsetprop $velselPath/setspeed ignore ${scobjNS}::noResponse
  hsetprop $velselPath/setspeed driving 0
  hsetprop $velselPath/setspeed type "drivable"
  hsetprop $velselPath/setspeed target 0
  hsetprop $velselPath/setspeed writestatus "UNKNOWN"

# Stop velocity selector (brake or idle)
  hfactory $velselPath/cmd plain spy text
  hsetprop $velselPath/cmd write ${scobjNS}::sendCommand ignore
  hsetprop $velselPath/cmd ignore ${scobjNS}::noResponse
  hsetprop $velselPath/cmd values brake,idle

#XXX  ::scobj::hinitprops velocity_selector
  ::scobj::set_required_props $velselPath
  hsetprop $velselPath klass NXvelocity_selector
  hsetprop $velselPath privilege spy
  hsetprop $velselPath type part
  hsetprop $velselPath control true
  hsetprop $velselPath data true
  hsetprop $velselPath/geometry klass NXgeometry
  hsetprop $velselPath/geometry privilege spy
  hsetprop $velselPath/geometry type instrument
  hsetprop $velselPath/geometry data true
  hsetprop $velselPath/geometry control true
  hsetprop $velselPath/geometry/position klass NXtranslation
  hsetprop $velselPath/geometry/position privilege spy
  hsetprop $velselPath/geometry/position type instrument
  hsetprop $velselPath/geometry/position data true
  hsetprop $velselPath/geometry/position control true
  foreach {hpath                        klass     control data  nxsave mutable priv alias} {
    LambdaA                             parameter true    true  true   true    user velsel_lambdaa
    LambdaResFWHM_percent               parameter true    true  true   true    spy  velsel_lambdaresfwhm_percent
    rspeed                              parameter true    true  true   true    spy  velsel_rspeed
    aspeed                              parameter true    true  true   true    user velsel_aspeed
    ttang                               parameter true    true  true   true    user velsel_ttang
    ttinit                              parameter true    false false  true    user velsel_ttang
    geometry/position/VelSelPosXmm      parameter true    true  true   false   user VelSelPosXmm
    geometry/position/VelSelPosYmm      parameter true    true  true   false   user VelSelPosYmm
    geometry/position/VelSelPosZmm      parameter true    true  true   false   user VelSelPosZmm
    geometry/position/VelSelCoordScheme parameter true    true  true   false   user VelSelCoordScheme
  } {
    hsetprop $velselPath/$hpath nxalias $alias
    hsetprop $velselPath/$hpath klass $klass
    hsetprop $velselPath/$hpath privilege $priv
    hsetprop $velselPath/$hpath control $control
    hsetprop $velselPath/$hpath data $data
    hsetprop $velselPath/$hpath nxsave $nxsave
    hsetprop $velselPath/$hpath mutable $mutable
    hsetprop $velselPath/$hpath sdsinfo ::nexus::scobj::sdsinfo
  }

  hsetprop $velselPath/setspeed checklimits ${scobjNS}::checkBlockedSpeeds $statusPath
  hsetprop $velselPath/setspeed checkstatus ${scobjNS}::drivestatus
  hsetprop $velselPath/setspeed halt        ${scobjNS}::halt $velselPath

  hsetprop $velselPath/setLambdaA checklimits ${scobjNS}::checkBlockedWavelengths $statusPath
  hsetprop $velselPath/setLambdaA checkstatus ${scobjNS}::drivestatus
  hsetprop $velselPath/setLambdaA halt        ${scobjNS}::halt $velselPath

##
# @brief This is the position of the velocity selector bunker face. It is used
# as the reference for other positions. x=y=z=0.
  hset $velselPath/geometry/position/VelSelPosXmm 0.0
  hset $velselPath/geometry/position/VelSelPosYmm 0.0
  hset $velselPath/geometry/position/VelSelPosZmm 0.0
  hset $velselPath/geometry/position/VelSelCoordScheme "Cartesian"
  


  proc sct_velsel_init {velselPath } {
    variable pollrate
    variable paramindex

    foreach par [lsort [array names paramindex]] {
      sct_velsel poll $velselPath/$par $pollrate
    }
    sct_velsel write $velselPath/ttinit
      sct_velsel write $velselPath/set_ttang
      sct_velsel poll $velselPath/LambdaA $pollrate
      sct_velsel write $velselPath/setLambdaA
      sct_velsel write $velselPath/setspeed
      sct_velsel write $velselPath/cmd
      ansto_makesctdrive nvs_speed $velselPath/setspeed $velselPath/aspeed sct_velsel
      ansto_makesctdrive nvs_lambda $velselPath/setLambdaA $velselPath/LambdaA sct_velsel
  }
  if {$sim_mode == "false"} {
    makesctcontroller sct_velsel astvelsel $velsel_IP:$velsel_port "" 10
    sct_velsel poll $statusPath $pollrate
  }
}