# TODO Check if requested tilt-angle is within range
# TODO What should be reported for the wavelength if the tilt angle is 999.99
##
# @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)]
  }


  foreach {
    param  index type  units   } {
    state  0     text  @none
    rspeed 1     float rpm
    aspeed 2     float rpm
    sspeed 3     float Hz
    aveto  4     text  @none
    ploss  5     float @none
    splos  6     float @none
    ttang  7     float degrees
    rtemp  8     float degrees
    wflow  9     float @none
    winlt  10    float @none
    woutt  11    float @none
    vacum  12    float @none
    wvalv  13    text  @none
    vvalv  14    text  @none
    vibrt  15    float @none
    bcuun  16    float @none
  } {
    set paramindex($param) $index
    set paramtype($param)  $type
    set paramunits($param) $units
  }

  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 "N#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 {N#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/setLambda 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/setLambda 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 "N#SOS#SPEED $speed"
    set angle [lindex [hval $statuspath] $paramindex(ttang) end]
    set lambda [AngleSpeedToWavelength $angle $speed]
    sct target $speed
    hsetprop $vs_root/setLambda 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/setLambda driving 1
    }
    return $nextState
  }

  proc sendCommand {nextState} {
    set state [string tolower [sct target]] 
      switch $state {
        "idle" {
          sct send "N#SOS#IDLE  "
        }
        "brake" {
          sct send "N#SOS#BRAKE "
        }
        "init" {
          sct send "N#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]
    if {$aspeed >= 800} {
      set lambda [AngleSpeedToWavelength $angle $aspeed]
    } else {
      set lambda 9999
    }
    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/setLambda 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 "N#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 "N#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/setLambda 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"

# Set identifier
  hfactory $velselPath/ID plain spy text
  hset $velselPath/ID $velsel_ID

# 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/VelSelPosX plain user float
  hsetprop $velselPath/geometry/position/VelSelPosX units "mm"
  hfactory $velselPath/geometry/position/VelSelPosY plain user float
  hsetprop $velselPath/geometry/position/VelSelPosY units "mm"
  hfactory $velselPath/geometry/position/VelSelPosZ plain user float
  hsetprop $velselPath/geometry/position/VelSelPosZ units "mm"
  hfactory $velselPath/geometry/position/VelSelCoordScheme plain user text
  
# Setup nodes for state report parameters
  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"
    if {$paramunits($par) != "@none"} {
      hsetprop $velselPath/$par units $paramunits($par)
    }
  }
# 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
  hsetprop $velselPath/set_ttang units "degrees"


# Get Lambda
  hfactory $velselPath/Lambda plain spy float
  hsetprop $velselPath/Lambda read ${scobjNS}::getpar rdpar
  hsetprop $velselPath/Lambda rdpar ${scobjNS}::readLambda $statusPath
  hsetprop $velselPath/Lambda oldval "UNKNOWN"
  hsetprop $velselPath/Lambda units "Angstrom"

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

# 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"
  hsetprop $velselPath/setspeed units "rpm"

# 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
  } {
    Lambda                              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/VelSelPosX        parameter true    true  true   false   user VelSelPosX
    geometry/position/VelSelPosY        parameter true    true  true   false   user VelSelPosY
    geometry/position/VelSelPosZ        parameter true    true  true   false   user VelSelPosZ
    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/setLambda checklimits ${scobjNS}::checkBlockedWavelengths $statusPath
  hsetprop $velselPath/setLambda checkstatus ${scobjNS}::drivestatus
  hsetprop $velselPath/setLambda 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/VelSelPosX 0.0
  hset $velselPath/geometry/position/VelSelPosY 0.0
  hset $velselPath/geometry/position/VelSelPosZ 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/Lambda $pollrate
      sct_velsel write $velselPath/setLambda
      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/setLambda $velselPath/Lambda sct_velsel
  }
  if {$sim_mode == "false"} {
    makesctcontroller sct_velsel astvelsel $velsel_IP:$velsel_port "" 10
    sct_velsel poll $statusPath $pollrate
  }
}