namespace eval ::scobj::chopper {

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

proc sndMBquery {rdScript cmd nextReq} {
    set chN [sct chopper]
    sct nextSndReq $nextReq
    sct send "$chN:$cmd"
    return $rdScript
}

proc sndMBset {root cmd} {
    set val [sct target]
    #TODO convert val to appropriate units, if [convfact] then  val = val * [sct convfact]
    sct send "$cmd:$val"
    sct target 0
    return RDSETREPLY
}
proc rdStatus {root statnode fields} {
  set currCh [sct chopper]
  set chPath $root/ch$currCh/$statnode
  set oldval [hgetpropval $chPath oldval]
  set val [sct result]
  if {[string match "ASCERR:*" $val]} {
    sct geterror $val
    return idle
  }
  if {$val != $oldval} {
    hsetprop $chPath oldval $val
    binary scan [binary format c $val] B* binNum
    set bitfield [split $binNum ""]
    foreach n $fields b $bitfield {
      hset $chPath/$n $b
    }
  }
  return [sct nextSndReq]
}
proc rdSysStatus {root nodes} {
# Read status byte and set nodes for each bit
  set currCh [sct chopper]
  set chPath $root/ch$currCh/system_status
  set oldval [hgetpropval $chPath oldval]
  set val [sct result]
  if {[string match "ASCERR:*" $val]} {
    sct geterror $val
    return idle
  }
  if {$val != $oldval} {
    hsetprop $chPath oldval $val
    binary scan [binary format c $val] B* binNum
    set bitfield [split $binNum ""]
    binary scan [binary format c $oldval] B* oldbinNum
    set oldbitfield [split $oldbinNum ""]
    foreach n $nodes b $bitfield ob $oldbitfield {
      if {$b != $ob} {
        hset $chPath/$n $b
      }
    }
  }
  return [sct nextSndReq]
}
proc rdIntLkStatus {root nodes} {
# Read status byte and set nodes for each bit
  set currCh [sct chopper]
  set chPath $root/ch$currCh/intlck_status
  set oldval [hgetpropval $chPath oldval]
  set val [sct result]
  if {[string match "ASCERR:*" $val]} {
    sct geterror $val
    return idle
  }
  if {$val != $oldval} {
    hsetprop $chPath oldval $val
    binary scan [binary format c $val] B* binNum
    set bitfield [split $binNum ""]
    binary scan [binary format c $oldval] B* oldbinNum
    set oldbitfield [split $oldbinNum ""]
    foreach n $nodes b $bitfield ob $oldbitfield {
      if {$b != $ob} {
        hset $chPath/$n $b
      }
    }
  }
  return [sct nextSndReq]
}
##
# Reads a list of values in the result and assigns them to the nodes in the args list
proc rdVal {root nodes} {
  set currCh [sct chopper]
  set values [sct result]
  set chPath $root/ch$currCh
  if {[string match "ASCERR:*" $values]} {
    sct geterror $values
    hset $chPath/device_error $values
    return idle
  }
  if {[hval $chPath/device_error] != ""} {
    hset $chPath/device_error ""
  }
  foreach n $nodes v $values {
    set oldval [hval $chPath/$n]
# if {$n == "speed_setpt"} {
#   broadcast rdVal,$n $chPath/$n: oldval=$oldval, v=$v
#   }
    if {$v != $oldval} {
# if {$n == "speed_setpt"} {
#     broadcast rdVal,$n: hset $chPath/$n $v
#   }
      hset $chPath/$n $v
    }
  }
  set nextReq [sct nextSndReq]
  if {$nextReq == "NXTCHOPPER"} {
    if {$currCh <  [sct chMax] } {
      incr currCh
      sct chopper $currCh
    } else {
      sct chopper 1
      return idle
    }
  }
  return $nextReq
}

proc rdSetCmdReply {root chPath} {
  set reply [sct result]
  if {[string match "ASCERR:*" $reply]} {
    sct geterror $reply
    hset $chPath/device_error $reply
    return idle
  }
  if {[hval $chPath/device_error] != ""} {
    hset $chPath/device_error ""
  }
  sct_fermi queue $root progress read
  return idle
}
# Create chopper control
set scobjNS ::scobj::chopper
set sim_mode false
set pollrate 5
MakeSICSObj fermi_chopper SCT_OBJECT user int
sicslist setatt  fermi_chopper klass NXfermi_chopper
set fermiPath /sics/fermi_chopper
::scobj::hinitprops fermi_chopper
  if {$sim_mode == "false"} {
    makesctcontroller sct_fermi tcpmodbus 137.157.202.213:502
  }
  hsetprop $fermiPath chopper 1

  set intlck_fields {test_mode cc_shutdown_req dsp_summ_shtdwn cooling_loss spd_sensor_loss ref_sig_loss over_temp vac_fail overspeed_or_brakefail cc_wd_fail ext_fault ups_fail emerg_stop pos_alarm osc_fail dsp_wd_fail}

set chMax 1
  hsetprop $fermiPath chMax $chMax
for {set chN 1} {$chN <= $chMax} {incr chN} {
set chname ch$chN
  sicslist setatt fermi_chopper klass NXfermi_chopper
  sicslist setatt fermi_chopper long_name fermi_chopper
  hfactory $fermiPath/$chname plain user float
  set chPath $fermiPath/$chname
  hfactory $chPath/device_error plain user text

  foreach field {
  system_status intlck_status       rotation_speed
  phase_veto_count phase_nonveto_count phase_acc
  phase_rep        phase_ok            vetowin100ns
  vetowin50ns      mode                speed_setpt
  prop_gain        int_gain            phase_gain
  ref_delay        ref_period          sync_srce      motdir idle_toggle
  } {
    hfactory $fermiPath/$chname/$field plain user float
  }
  hsetprop $chPath/system_status oldval "UNKNOWN"
  hsetprop $chPath/intlck_status oldval "UNKNOWN"

  foreach field {
    avc_on motdir phase_locked lev_complete alarm run up_to_speed ok
  } {
    hfactory $fermiPath/$chname/system_status/$field plain user int
  }
  foreach field $intlck_fields {
    hfactory $fermiPath/$chname/intlck_status/$field plain user int
  }

 
 hfactory $chPath/control plain user none
 hfactory $chPath/control/device_error plain user text

  foreach {n cmd} [subst -nocommands {
    set_vetowin100  "$chN:16:30:1:U32"
    set_vetowin50   "$chN:16:32:1:U32"
    set_rotspeed    "$chN:16:1000:1:U32"
    set_prop_gain   "$chN:16:1004:1:F32"
    set_int_gain    "$chN:16:1006:1:F32"
    set_phase_gain  "$chN:16:1008:1:F32"
    set_ref_delay   "$chN:16:1010:1:U32"
    set_ref_period  "$chN:16:1012:1:U32"
    set_sync_source "$chN:16:1014:1:U32"
    set_motor_dir   "$chN:16:1016:1:U32"
    start           "$chN:5:1"
    stop            "$chN:5:2"
    idle_toggle     "$chN:5:3"
    reset           "$chN:5:4"
  }] {
    hfactory $chPath/control/$n plain user float
    hsetprop $chPath/control/$n write ${scobjNS}::sndMBset $chPath $cmd
    hsetprop $chPath/control/$n RDSETREPLY ${scobjNS}::rdSetCmdReply $fermiPath $chPath/control
    if {$sim_mode == "false"} {
      sct_fermi write $chPath/control/$n
    }
  }
  }
# Each Req chains to a rdVal proc which chains to the next req
# Add a next state property
  #TODO Read idle_toggle status
  hsetprop $fermiPath nextSndReq "UNKNOWN"
  hsetprop $fermiPath read          ${scobjNS}::sndMBquery "RDSYSSTAT"   "3:10:1:U16"   "INTLKREQ"
  hsetprop $fermiPath "NXTCHOPPER"  ${scobjNS}::sndMBquery "RDSYSSTAT"   "3:10:1:U16"   "INTLKREQ"
  hsetprop $fermiPath "INTLKREQ"    ${scobjNS}::sndMBquery "RDINTLKSTAT" "3:12:1:U16"   "ROTSPDREQ"
  hsetprop $fermiPath "ROTSPDREQ"   ${scobjNS}::sndMBquery "RDROTSPD"    "3:14:1:U16"   "VETOINF"
  hsetprop $fermiPath "VETOINF"     ${scobjNS}::sndMBquery "RDVETOINF"   "3:18:2:U32"   "PHASEINF"
  hsetprop $fermiPath "PHASEINF"    ${scobjNS}::sndMBquery "RDPHASEINF"  "3:24:3:F32"   "VETO"
  hsetprop $fermiPath "VETO"        ${scobjNS}::sndMBquery "RDVETO"      "3:30:3:U32"   "ROTSPSET"
  hsetprop $fermiPath "ROTSPSET"    ${scobjNS}::sndMBquery "RDROTSPSET"  "3:1000:1:U32" "GAINPHASE"
  hsetprop $fermiPath "GAINPHASE"   ${scobjNS}::sndMBquery "RDGAINPHASE" "3:1004:3:F32" "SYNMOTDIR"
  hsetprop $fermiPath "SYNMOTDIR"   ${scobjNS}::sndMBquery "RDSYNMOTDIR" "3:1010:4:U32" "IDLETOGGLE"
  hsetprop $fermiPath "IDLETOGGLE"   ${scobjNS}::sndMBquery "RDIDLE" "1:3:1" "NXTCHOPPER"


  hsetprop $fermiPath "RDSYSSTAT"   ${scobjNS}::rdStatus   $fermiPath system_status {avc_on motdir phase_locked lev_complete alarm run up_to_speed ok}
  hsetprop $fermiPath "RDINTLKSTAT" ${scobjNS}::rdStatus $fermiPath intlck_status $intlck_fields
  hsetprop $fermiPath "RDROTSPD"    ${scobjNS}::rdVal         $fermiPath rotation_speed
  hsetprop $fermiPath "RDVETOINF"   ${scobjNS}::rdVal         $fermiPath {phase_veto_count phase_nonveto_count}
  hsetprop $fermiPath "RDPHASEINF"  ${scobjNS}::rdVal         $fermiPath {phase_acc phase_rep phase_ok}
  hsetprop $fermiPath "RDVETO"      ${scobjNS}::rdVal         $fermiPath {vetowin100ns vetowin50ns mode}
  hsetprop $fermiPath "RDROTSPSET"  ${scobjNS}::rdVal         $fermiPath speed_setpt
  hsetprop $fermiPath "RDGAINPHASE" ${scobjNS}::rdVal         $fermiPath {prop_gain int_gain phase_gain}
  hsetprop $fermiPath "RDSYNMOTDIR" ${scobjNS}::rdVal         $fermiPath {ref_delay ref_period sync_srce motdir}
  hsetprop $fermiPath "RDIDLE"  ${scobjNS}::rdVal         $fermiPath idle_toggle
  

  if {$sim_mode == "false"} {
    sct_fermi poll $fermiPath $pollrate
    sct_fermi queue $fermiPath progress read
  }
}