# Define procs in ::scobj::xxx namespace
# MakeSICSObj $obj SCT_<class>
#  The MakeSICSObj cmd adds a /sics/$obj node.  NOTE the /sics node is not browsable.


namespace eval ::scobj::robot_pp {
# Environment controllers should have at least the following nodes
# /envcont/setpoint
# /envcont/sensor/value
  proc debug_log {args} {
    set fd [open "../log/robot_pp.log" a]
    puts $fd "[clock format [clock seconds] -format "%T"] $args"
    close $fd
  }

# issue a command to read a register and expect a value response
  proc getValue {tc_root nextState cmd} {
debug_log "getValue $cmd sct=[sct] root=$tc_root nextState=$nextState"
    sct send "$cmd"
    return $nextState
  }

# issue a command with a value in the target property of the variable
  proc setValue {tc_root nextState cmd} {
debug_log "setValue cmd=$cmd sct=[sct] $tc_root"
    set par "[sct target]"
    sct send "$cmd$par"
debug_log "setValue $cmd$par"
    return $nextState
  }

  proc setNam {tc_root nextState cmd} {
    set data [string toupper [sct target]]
    if {$data != [sct oldval]} {
      sct oldval $data
      sct update $data
      sct utime readtime
debug_log "setNam new data for $tc_root [sct] result=$data"
    }
    return $nextState
  }

  proc setRot {tc_root nextState cmd} {
    set data [string toupper [sct target]]
    if {$data != [sct oldval]} {
      sct oldval $data
      sct update $data
      sct utime readtime
debug_log "setRot new data for $tc_root [sct] result=$data"
      if {$data > 0} {
        hset $tc_root/status "busy"
        sct send "ROTATE $data -1"
      } else {
        sct send "STOPROT"
      }
    }
    return $nextState
  }

  proc chkWrite {tc_root} {
    set data [sct result]
debug_log "chkWrite resp=$data sct=[sct] tc_root=$tc_root"
    if {[string equal -nocase -length 7 $data "ASCERR:"]} {
      sct geterror "$data"
    } elseif {[string equal -nocase -length 1 $data "?"]} {
      sct geterror "Error: $data"
    } else {
      set data [sct target]
      if {$data != [sct oldval]} {
        sct oldval $data
        sct update $data
        sct utime readtime
debug_log "chkWrite new data for $tc_root [sct] result=$data"
      }
    }
    return idle
  }

  proc setPoint {tc_root nextState cmd} {
    set catch_status [ catch {
debug_log "setPoint $tc_root $nextState $cmd sct=[sct]"
debug_log "setPoint: sct=[sct] target=[sct target] writestatus=[sct writestatus]"
    sct print "setPoint: sct=[sct] target=[sct target] writestatus=[sct writestatus]"
    set err_msg ""
    if { [hval $tc_root/Control/Pallet_Nam] != "A" && [hval $tc_root/Control/Pallet_Nam] != "B" } {
      set err_msg "Invalid Pallet_ Num: [hval $tc_root/Control/Pallet_Nam]"
    } elseif { [hval $tc_root/Control/Pallet_Idx] < 1 || [hval $tc_root/Control/Pallet_Idx] > 50 } {
      set err_msg "Invalid Pallet_Idx: [hval $tc_root/Control/Pallet_Idx]"
    } 
    if { $err_msg != "" } {
      sct print "error:$err_msg"
      debug_log "error:$err_msg"
      return -code error "$err_msg"
    }
    set par "[sct target]"
    if {$par != [sct oldval]} {
      sct oldval $par
      sct update $par
      sct utime readtime
debug_log "setPoint new data for $tc_root [sct] result=$par"
    }
    if { [sct target] == 1 } {
      if { [string toupper [hval $tc_root/Control/Target_Loc]] == "BEAM" } {
        set cmd  "SampToBeam [hval $tc_root/Control/Pallet_Nam] [hval $tc_root/Control/Pallet_Idx]"
debug_log "sct send $cmd"
        sct send "$cmd"
      } elseif { [string toupper [hval $tc_root/Control/Target_Loc]] == "VACUUM" } {
        set cmd  "SampToVac [hval $tc_root/Control/Pallet_Nam] [hval $tc_root/Control/Pallet_Idx]"
debug_log "sct send $cmd"
        sct send "$cmd"
      } else {
        set err_msg "Invalid Target_Loc: [hval $tc_root/Control/Target_Loc]"
        sct print "error:$err_msg"
        debug_log "error:$err_msg"
        return -code error "$err_msg"
      }
    } elseif { [sct target] == 0 } {
      if { [string toupper [hval $tc_root/Control/Target_Loc]] == "BEAM" } {
        set cmd  "SampRtn [hval $tc_root/Control/Pallet_Nam] [hval $tc_root/Control/Pallet_Idx]"
debug_log "sct send $cmd"
        sct send "$cmd"
      } elseif { [string toupper [hval $tc_root/Control/Target_Loc]] == "VACUUM" } {
        set cmd  "VacRtn [hval $tc_root/Control/Pallet_Nam] [hval $tc_root/Control/Pallet_Idx]"
debug_log "sct send $cmd"
        sct send "$cmd"
      } else {
        set err_msg "Invalid Target_Loc: [hval $tc_root/Control/Target_Loc]"
        sct print "error:$err_msg"
        debug_log "error:$err_msg"
        return -code error "$err_msg"
      }
    } else {
        set err_msg "Invalid DRIVE target: [sct target]"
        sct print "error:$err_msg"
        debug_log "error:$err_msg"
        return -code error "$err_msg"
    }
    hset $tc_root/status "busy"
    sct print "status: busy"
    hsetprop $tc_root/setpoint driving 1
    } catch_message ]
    if {$catch_status != 0} {
      return -code error $catch_message
    }
    sct print "setPoint: [hget $tc_root/drive_state]"
    return $nextState
  }

  proc rdValue {tc_root} {
debug_log "rdValue tc_root=$tc_root sct=[sct]"
    set data [sct result]
    if {[ catch {
debug_log "rdValue $tc_root [sct] result=$data"
    } catch_message ]} {
debug_log "rdValue $tc_root failure"
    }
    if {[string equal -nocase -length 7 $data "ASCERR:"]} {
      sct geterror "$data"
      set nextState idle
    } elseif {[string equal -nocase -length 1 $data "?"]} {
      sct geterror "Error: $data"
      set nextState idle
    } else {
      if { [hpropexists [sct] geterror] } {
        hdelprop [sct] geterror
      }
      if {$data != [sct oldval]} {
debug_log "rdValue new data for $tc_root [sct] was=[sct oldval] now=$data"
        sct oldval $data
        sct update $data
        sct utime readtime
      }
    }
    return idle
  }

# This is the command phase of the state machine that drives the controller.
# For each state, it sends the appropriate command to get values from, or set
# values in the controller in a sequence intended to transition the controller
# between states.
  proc getState {tc_root nextState cmd} {
debug_log "getState $tc_root $nextState $cmd sct=[sct]"
  if {[ catch {
    if { [hpropexists [sct] geterror] } {
      hdelprop [sct] geterror
    }
debug_log "getState sends: $cmd"
    sct send "$cmd"
  } catch_message ]} {
debug_log "getState error: $catch_message"
  }
debug_log "getState returns: $nextState"
    return $nextState
  }

# This is the response phase of the state machine that drives the controller.
# For each state, it reads the appropriate command response from the controller
# and, based on the response and internal variables performs a sequence
# intended to transition the controller between states.

  ##
  # @brief Reads the current robot state and error messages.
  proc rdState {tc_root} {
    debug_log "rdState $tc_root sct=[sct] response=\"[sct result]\""
    set nextState {}
    if {[ catch {
      set data "[sct result]"
      if {[string equal -nocase -length 7 $data "ASCERR:"]} {
        sct geterror "$data"
        set nextState idle
      } elseif {[string equal -nocase -length 1 $data "?"]} {
        sct geterror "Error: $data"
        set nextState idle
      } else {
        set my_driving [SplitReply [hgetprop $tc_root/setpoint driving]]
        if { $my_driving } {
          if { [string toupper [sct result]] == "IDLE" } {
            hset $tc_root/status "idle"
            hsetprop $tc_root/setpoint driving 0
            hset     $tc_root/sensor/value [hval $tc_root/setpoint]
            if {[hval $tc_root/Control/Rotate] > 0} {
              hset $tc_root/Control/Rotate 0
            }
          }
        }
      }
    } catch_message ]} {
    debug_log "rdState error: $catch_message"
    }
    if { "$nextState" == "" } {
      set nextState "idle"
    }
    debug_log "rdState returns: $nextState"
    return $nextState
  }

  proc noResponse {} {
    return idle
  }
  proc wrtValue {wcmd args} {
  }

# check that a target is within allowable limits
  proc check {tc_root} {
    set setpoint [sct target]
    set lolimit [hval $tc_root/lowerlimit]
    set hilimit [hval $tc_root/upperlimit]
    if {$setpoint < $lolimit || $setpoint > $hilimit} {
      error "setpoint violates limits"
    }
    return OK
  }

##
# @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"
  proc drivestatus {tc_root} {
    if {[hval $tc_root/status] == "busy"} {
      return busy
    } elseif {[sct driving]} {
      return busy
    } else {
      return idle
    }
  }

  proc halt {tc_root} {
debug_log "halt $tc_root"
sct print "halt $tc_root"
    set my_driving [SplitReply [hgetprop $tc_root/setpoint driving]]
    hset $tc_root/drive_state "HALT"
    if { $my_driving } {
      hsetprop $tc_root/robot_state my_state "HALT"
      return read
    }
    return idle
  }

##
# @brief createNode() creates a node for the given nodename with the properties and virtual
# function names provided
# @param scobj_hpath    string variable holding the path to the object's base node in sics (/sample/tc1)
# @param sct_controller name of the scriptcontext object (typically sct_xxx_yyy)
# @param cmdGroup       subdirectory (below /sample/tc*/) in which the node is to be created
# @param varName        name of the actual node typically representing one device command
# @param readable       set to 1 if the node represents a query command, 0 if it is not
# @param writable       set to 1 if the node represents a request for a change in settings sent to the device
# @param drivable       if set to 1 it prepares the node to provide a drivable interface
# @param dataType       data type of the node, must be one of none, int, float, text
# @param permission     defines what user group may read/write to this node (is one of spy, user, manager)
# @param rdCmd          actual device query command to be sent to the device
# @param rdFunc         nextState Function to be called after the getValue function, typically rdValue()
# @param wrCmd          actual device write command to be sent to the device
# @param wrFunc         Function to be called to send the wrCmd to the device, typically setValue()
# @param allowedValues  allowed values for the node data - does not permit other
# @param klass          Nexus class name (?)
# @return               OK
proc createNode {scobj_hpath sct_controller cmdGroup varName readable writable\
  drivable dataType permission rdCmd rdFunc wrCmd\
  wrFunc allowedValues klass} {

  set catch_status [ catch {
    set ns "[namespace current]"
    set nodeName "$scobj_hpath/$cmdGroup/$varName"
    if {1 > [string length $cmdGroup]} {
      set nodeName "$scobj_hpath/$varName"
    }
debug_log "Creating node $nodeName"
    hfactory $nodeName plain $permission $dataType
    if {$readable > 0} {
      hsetprop $nodeName read ${ns}::getValue $scobj_hpath $rdFunc $rdCmd
      hsetprop $nodeName $rdFunc ${ns}::$rdFunc $scobj_hpath
      set poll_period 30
      if { $readable >= 0 && $readable <= 9 } {
        set poll_period [lindex [list 0 1 2 3 4 5 10 15 20 30] $readable]
      }
debug_log "Registering node $nodeName for poll at $poll_period seconds"
      $sct_controller poll $nodeName $poll_period
    }
    if {$writable == 1 && "$wrFunc" != ""} {
      set pos [string first "." "$wrFunc"]
      if { $pos > 0 } {
        set parts [split "$wrFunc" "."]
        if { [llength $parts] >= 2 } {
          set func_name [lindex $parts 0]
          set next_state [lindex $parts 1]
          hsetprop $nodeName write ${ns}::$func_name $scobj_hpath $next_state $wrCmd
          hsetprop $nodeName $next_state ${ns}::$next_state $scobj_hpath
        }
      } else {
        hsetprop $nodeName write ${ns}::$wrFunc $scobj_hpath noResponse $wrCmd
        hsetprop $nodeName noResponse ${ns}::noResponse
      }
      hsetprop $nodeName writestatus UNKNOWN
debug_log "Registering node $nodeName for write callback"
      $sct_controller write $nodeName
    }
    switch -exact $dataType {
      "none"  { }
      "int"   { hsetprop $nodeName oldval -1 }
      "float" { hsetprop $nodeName oldval -1.0 }
      default { hsetprop $nodeName oldval UNKNOWN }
    }
    if {1 < [string length $allowedValues]} {
      hsetprop $nodeName values $allowedValues
    }
    #        Drive adapter interface
    if {$drivable == 1} {
      hsetprop $nodeName check ${ns}::check $scobj_hpath
      hsetprop $nodeName driving 0
      hsetprop $nodeName checklimits ${ns}::check $scobj_hpath
      hsetprop $nodeName checkstatus ${ns}::drivestatus $scobj_hpath
      hsetprop $nodeName halt ${ns}::halt $scobj_hpath
    }
  } message ]
  if {$catch_status != 0} {
    return -code error "in createNode $message"
  }
  return OK
}

  proc mk_sct_robot_pp {sct_controller klass tempobj} {
    set catch_status [ catch {
      set ns "[namespace current]"

      MakeSICSObj $tempobj SCT_OBJECT
      sicslist setatt $tempobj klass $klass
      sicslist setatt $tempobj long_name $tempobj

      set scobj_hpath /sics/$tempobj

      set deviceCommand {\
        {}       setpoint        0 1 1 float user     {}         {rdValue}      {}                 {setPoint.chkWrite} {}\
        sensor   value           0 0 0 float internal {}         {rdValue}      {}                 {}         {}\
        Display  State           1 0 0 text  internal {STATE}    {rdValue}      {}                 {}         {}\
        Display  Status          1 0 0 text  internal {STATUS}   {rdValue}      {}                 {}         {}\
        Display  Location        1 0 0 text  internal {WHERE}    {rdValue}      {}                 {}         {}\
        Control  Pallet_Nam      0 1 0 text  user     {}         {}             {}                 {setNam}   {A,B}\
        Control  Pallet_Idx      0 1 0 int   user     {}         {}             {}                 {}         {}\
        Control  Target_Loc      0 1 0 Text  user     {}         {}             {}                 {setNam}   {Beam,Vacuum}\
        Control  Rotate          0 1 0 int   user     {}         {}             {}                 {setRot}   {}\
      }

      hfactory $scobj_hpath/sensor plain spy none
      hfactory $scobj_hpath/Display plain spy none
      hfactory $scobj_hpath/Control plain spy none

      foreach {cmdGroup varName readable writable drivable dataType permission rdCmd rdFunc wrCmd wrFunc allowedValues} $deviceCommand {
        createNode $scobj_hpath $sct_controller $cmdGroup $varName $readable $writable $drivable $dataType $permission $rdCmd $rdFunc $wrCmd $wrFunc $allowedValues $klass
      }

      hsetprop $scobj_hpath/sensor/value lowerlimit -12
      hsetprop $scobj_hpath/sensor/value upperlimit 12
      hsetprop $scobj_hpath/sensor/value units "T"

      hfactory $scobj_hpath/status plain spy text
      hset     $scobj_hpath/status "idle"
      hsetprop $scobj_hpath/status values busy,idle

      hfactory $scobj_hpath/robot_state plain spy text
      hsetprop $scobj_hpath/robot_state read ${ns}::getState $scobj_hpath rdState "STATE"
      hsetprop $scobj_hpath/robot_state rdState ${ns}::rdState $scobj_hpath
      hsetprop $scobj_hpath/robot_state oldval "UNKNOWN"
      hsetprop $scobj_hpath/robot_state my_state "UNKNOWN"
      hsetprop $scobj_hpath/robot_state my_status "UNKNOWN"
      hsetprop $scobj_hpath/robot_state my_version "UNKNOWN"

      hfactory $scobj_hpath/drive_state plain mugger text
      hset     $scobj_hpath/drive_state "UNKNOWN"

      hfactory $scobj_hpath/remote_ctrl plain spy text
      hset     $scobj_hpath/remote_ctrl "UNKNOWN"

      hfactory $scobj_hpath/robot_lasterror plain user text
      hset     $scobj_hpath/robot_lasterror ""

      hfactory $scobj_hpath/lowerlimit plain mugger float
      hsetprop $scobj_hpath/lowerlimit units "T"
      hset     $scobj_hpath/lowerlimit -11

      hfactory $scobj_hpath/upperlimit plain mugger float
      hsetprop $scobj_hpath/upperlimit units "T"
      hset     $scobj_hpath/upperlimit 11

      if {[SplitReply [environment_simulation]]=="false"} {
        $sct_controller poll $scobj_hpath/robot_state 1 halt read
      }

      hsetprop $scobj_hpath type part
      foreach snsr {sensor} {
        foreach {rootpath hpath klass priv} "\
          $scobj_hpath $snsr         NXsensor  spy\
          $scobj_hpath $snsr/value   sensor    user\
        " {
          hsetprop $rootpath/$hpath klass $klass
          hsetprop $rootpath/$hpath privilege $priv
          hsetprop $rootpath/$hpath control true
          hsetprop $rootpath/$hpath data true
          hsetprop $rootpath/$hpath nxsave true
        }
        hsetprop $scobj_hpath/$snsr type part
        hsetprop $scobj_hpath/$snsr/value nxalias tc1_${snsr}_value
        hsetprop $scobj_hpath/$snsr/value mutable true
        hsetprop $scobj_hpath/$snsr/value sdsinfo ::nexus::scobj::sdsinfo
      }
      hsetprop $scobj_hpath privilege spy
      ::scobj::hinitprops $tempobj setpoint
      if {[SplitReply [environment_simulation]]=="false"} {
        ansto_makesctdrive ${tempobj}_driveable $scobj_hpath/setpoint $scobj_hpath/sensor/value $sct_controller
      }
    } catch_message ]
    if {$catch_status != 0} {
      return -code error $catch_message
    }
  }
  namespace export mk_sct_robot_pp
}

##
# @brief Create a Pick and Place Robot Controller
#
# @param name, the name of the robot (eg robbie)
# @param IP, the IP address of  the device, this can be a hostname, (eg ca1-kowari)
# @param port, the IP protocol port number of the device
proc add_robot {name IP port } {
  set fd [open "../log/robot_pp.log" a]
  if {[SplitReply [environment_simulation]]=="false"} {
    puts $fd "makesctcontroller sct_robot_pp std ${IP}:$port"
    makesctcontroller sct_robot_pp std ${IP}:$port [binary format cc 0x0D 0x0A] 5.0
  }
  puts $fd "mk_sct_robot_pp sct_robot environment $name"
  mk_sct_robot_pp sct_robot_pp environment $name
  close $fd
}

puts stdout "file evaluation of sct_pickandplace.tcl"
set fd [open "../log/robot_pp.log" w]
puts $fd "file evaluation of sct_pickandplace.tcl"
close $fd

namespace import ::scobj::robot_pp::*

#add_robot robbie 137.157.201.213 502 5
#add_robot robbie localhost 30509
add_robot robbie 137.157.201.26 6000