##
# @file
# Implements astrium disk_chopper driver for platypus

# Test by adding the following to barebones.tcl
# InstallHdb
# source config/chopper/sct_chopper.tcl
# hfactory /chopper link disk_chopper

# The chopper 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.
## State reports for each chopper are requested with the STATE n command
## #SOS#STATE 1:
## #SOS#ACCEPT CH= 1# State= Asynchron.#ASPEED= 1200#RSPEED= 1200#APHASE= 999.99#RPHASE= 0#AVETO = 0#DIR   =  CW#MONIT = ok#FLOWR = 2.9#WTEMP = 15.7#MTEMP = 19.2#MVIBR = 0.0#MVACU = 0.0036#DATE  = 29/03/2011#TIME  = 8:11:54 AM#

namespace eval ::scobj::chopper {
  variable UID
  variable PWD
  variable sim_mode
  variable paramindex
  variable paramtype
  variable pollrate 900


  set sim_mode [SplitReply [chopper_simulation]]
  foreach {
    param  index type  units   } {
    state  0     text  @none
    aspeed 1     float rpm
    rspeed 2     float rpm
    aphase 3     float degrees
    rphase 4     float degrees
    aveto  5     text  @none
    dir    6     text  @none
    monit  7     text  @none
    flowr  8     float @none
    wtemp  9     float C
    mtemp  10    float C
    mvibr  11    float @none
    mvacu  12    float mbar
    date   13    text  @none
    time   14    text  @none
  } {
    set paramindex($param) $index
    set paramtype($param)  $type
    set paramunits($param) $units
  }

  MakeSICSObj disk_chopper SCT_OBJECT user int
  sicslist setatt disk_chopper klass NXdisk_chopper
  sicslist setatt disk_chopper long_name disk_chopper

proc sendUID {user} {
   sct send "user:$user"
   return RDPWDCHALLENGE
}

proc rdPwdChallenge {} {
  set challenge [sct result]
  switch -glob -- $challenge {
    "#SES#Fill in your password*" { return SENDPWD }
    "#SES#You are not a valid user, try again*" {return SENDUID}
    default {return -code error "Unhandled reply to [info level 0]: $challenge"}
  }
}
proc sndPwd {pwd} {
  sct send "password:$pwd"
  return RDPWDACK
}
proc rdPwdAck {} {
   set ack [sct result]
  switch -glob -- $ack {
    "#SES#Hello*" {
      sct chopper 1
      return NXTCHOPPER
    }
    "#SES#Fill in your password*" { return SENDPWD }
    "#SES#You are not a valid user, try again*" {return SENDUID}
    default {return -code error "Unhandled reply to [info level 0]: $ack"}
  }
}
##
# @brief Request a state report from the chopper
  proc sndStateReq {root} {
      set curChopper [sct chopper]
      if {$curChopper == 1} {
        hset $root/device_error ""
        sct update 0
      }
    sct send "#SOS#STATE [sct chopper]:"
    return RDSTATE
  }

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

    set catch_status [ catch {
      set staterep [sct result]
      set curChopper [sct chopper]

      if {[string match {ASCERR:*} $staterep]} {
         sct chopper 1
         sct update -1
         hset $root/device_error $staterep
         error $staterep
      }
      if {[string match {*#SES#*} $staterep]} {
         sct chopper 1
         return SENDUID
      }
      if {[string match {#SOS#*} $staterep] == 0 } {
        sct chopper 1
        sct update -1
        hset $root/device_error $staterep
        error $staterep
      }
      set status [lrange [split $staterep "#"] 3 end-1]

      if {$staterep != [sct _oldval]} {
        set state [lindex $status $paramindex(state) end]
        if {$state != [sct _oldstate]} {
          sct _oldstate $state
        }
        sct _oldval $staterep
        sct staterep $status
        foreach {param index} [array get paramindex] {
          set data [lindex [ split [lindex $status $paramindex($param)] = ] 1]
          if {$param == "time"} {
            # Convert time to 24 hour format
            set data [clock format [clock scan $data] -format %T]
          }
          set data [string trim $data]
          if {$param == "aspeed"} {
            hset $root/ch${curChopper}speed $data
          }
          if {$param == "rphase"} {
            hset $root/ch${curChopper}phase $data
          }
          hset $root/ch$curChopper/$param $data
        }
        sct utime readtime
      }

      if {$curChopper >= 4} {
        sct chopper 1
        sct update 1
        return idle
      } else {
        incr curChopper
        sct chopper $curChopper
        return NXTCHOPPER
      }
    } message ]
    handle_exception $catch_status $message 
  }

# Create chopper control
  set scobjNS ::scobj::chopper
  set chopperPath /sics/disk_chopper

  hsetprop $chopperPath read ${scobjNS}::sndStateReq $chopperPath
  hsetprop $chopperPath NXTCHOPPER ${scobjNS}::sndStateReq $chopperPath
  hsetprop $chopperPath RDSTATE ${scobjNS}::rdState $chopperPath
  hsetprop $chopperPath chopper 1
  hsetprop $chopperPath chopper_ready "UNKNOWN"
  hsetprop $chopperPath SENDUID ${scobjNS}::sendUID $UID
  hsetprop $chopperPath RDPWDCHALLENGE ${scobjNS}::rdPwdChallenge
  hsetprop $chopperPath SENDPWD ${scobjNS}::sndPwd $PWD
  hsetprop $chopperPath RDPWDACK ${scobjNS}::rdPwdAck
  hsetprop $chopperPath _oldval "UNKNOWN"
  hsetprop $chopperPath _oldstate "UNKNOWN"

  hfactory $chopperPath/device_error plain spy text
  hset $chopperPath/device_error ""
  
# Must be set by user
  hfactory $chopperPath/geometry plain spy none
  hfactory $chopperPath/geometry/position plain spy none
  hfactory $chopperPath/geometry/position/ChopperPosX plain user float
  hsetprop $chopperPath/geometry/position/ChopperPosX units "mm"
  hfactory $chopperPath/geometry/position/ChopperPosY plain user float
  hsetprop $chopperPath/geometry/position/ChopperPosY units "mm"
  hfactory $chopperPath/geometry/position/ChopperPosZ plain user float
  hsetprop $chopperPath/geometry/position/ChopperPosZ units "mm"
  hfactory $chopperPath/geometry/position/ChopperCoordScheme plain user text
  
# Setup nodes for state report parameters
  foreach ch {"ch1" "ch2" "ch3" "ch4"} {
    hfactory $chopperPath/$ch plain spy none
    foreach par [lsort [array names paramindex]] {
      hfactory $chopperPath/$ch/$par plain spy $paramtype($par)
      if {$paramunits($par) != "@none"} {
        hsetprop $chopperPath/$ch/$par units $paramunits($par)
      }
    }
  }

# ::scobj::hinitprops chopper
  ::scobj::set_required_props $chopperPath
  hsetprop $chopperPath klass NXdisk_chopper
  hsetprop $chopperPath privilege spy
  hsetprop $chopperPath type part
  hsetprop $chopperPath control true
  hsetprop $chopperPath data true
  hsetprop $chopperPath nxsave true


  proc mkChopperDatPar {chopperPath datPar datSrce} {
    variable paramunits

    hfactory $chopperPath/$datPar plain user float
    hsetprop $chopperPath/$datPar klass parameter
    hsetprop $chopperPath/$datPar control true
    hsetprop $chopperPath/$datPar data true
    hsetprop $chopperPath/$datPar nxsave true
    hsetprop $chopperPath/$datPar mutable true
    hsetprop $chopperPath/$datPar privilege user
    hsetprop $chopperPath/$datPar nxalias $datPar
    hsetprop $chopperPath/$datPar units $paramunits($datSrce)
    hsetprop $chopperPath/$datPar sdsinfo ::nexus::scobj::sdsinfo
  }
  foreach param {ch1speed ch2speed ch3speed ch4speed} {
    mkChopperDatPar $chopperPath $param "rspeed"
  }
  foreach param {ch1phase ch2phase ch3phase ch4phase} {
    mkChopperDatPar $chopperPath $param "rphase"
  }
  if {$sim_mode == "false"} {
    makesctcontroller sct_chopper astvelsel $chopper_IP:$chopper_port "\r" 10
    sct_chopper poll $chopperPath $pollrate
    sct_chopper queue $chopperPath progress read
  }
}

namespace eval ::chopper {
}

proc ::chopper::ready? {} {
  set chPath $::scobj::chopper::chopperPath

  sct_chopper queue $chPath progress read
  wait 1
  set UDstate [hval $chPath]
  set retry 0
  while {$UDstate != 1} {
    if {$UDstate == -1} {
      if {$retry < 3} {
        incr retry
      } else {
        return "NOTREADY: Failed to getstatus update"
      }
    }
    wait 1
    set UDstate [hval $chPath]
  }
  foreach ch {"1" "2" "3" "4"} {
    set stVal [hval $chPath/ch$ch/monit]
    if {$stVal != "ok"} {
      return "NOTREADY: monitor $ch = $stVal"
    }
    set stVal [hval $chPath/ch$ch/state]
    switch $stVal {
      "Brake" {
        return "NOTREADY: Chopper $ch braking"
      }
      "Commutation" {
        return "NOTREADY: Calibrating"
      }
      "Inactive" {
        return "NOTREADY: Inactive, may need calibrating"
      }
      "E-Stop" {
        return "NOTREADY: State is EMERGENCY STOP"
      }
    }
    set aspeed [hval $chPath/ch$ch/aspeed]
    set rspeed [hval $chPath/ch$ch/rspeed]
    if {[expr abs(abs($aspeed) - abs($rspeed))] > 2} {
      return "NOTREADY: Current speed $aspeed != requested $rspeed for chopper $ch"
    }
  }
  foreach ch {"2" "3" "4"} {
    set stVal [hval $chPath/ch$ch/state]
    if { $stVal == "Asynchron."} {
      return "NOTREADY: Chopper $ch is set to Asynchronous"
    }
    set aphase [hval $chPath/ch$ch/aphase]
    if { $aphase == "999.99"} {
      return "NOTREADY: phase=$aphase on CH$ch, press 'Accept' on Astrium program"
    }
    set rphase [hval $chPath/ch$ch/rphase]
    if { [expr abs($aphase - $rphase)] > 0.2 } {
      return "NOTREADY: Current phase $aphase != requested phase $rphase for chopper $ch"
    }
  }
  return "READY"
}

proc ::chopper::get_frequency {} {
  set chPath $::scobj::chopper::chopperPath

  sct_chopper queue $chPath progress read
  wait 1
  set UDstate [hval $chPath]
  set retry 0
  while {$UDstate != 1} {
    if {$UDstate == -1} {
      if {$retry < 3} {
        incr retry
      } else {
        return "NOTREADY: Failed to getstatus update"
      }
    }
    wait 1
    set UDstate [hval $chPath]
  }
  set speed [hval $chPath/ch1speed]
  return [expr $speed/60.0]
}

publish ::chopper::get_frequency user
publish ::chopper::ready? user