# global bank 2 usage
# persistent:
#  0 zero
#  1 id
#  2 lower limit
#  3 upper limit
#  4 precision
#  6 feedback available
#  8 pump number
#  9 hysteresis
# non-persistent:
#  255 init (0/1)

namespace eval trinamic {} {
  variable cmds

  set cmds(motor_stop)     3
  set cmds(move_to_pos)    4
  set cmds(set_axis_par)   5
  set cmds(get_axis_par)   6
  set cmds(save_axis_par)  7
  set cmds(load_axis_par)  8
  set cmds(set_glob_par)   9
  set cmds(get_glob_par)  10
  set cmds(save_glob_par) 11
  set cmds(load_glob_par) 12
  set cmds(set_io)        14
  set cmds(get_io)        15

  # command numbers are translated 1:1
#  foreach key [array names cmds] {
#    set val $cmds($key)
#    set cmds($val) $val
#  }
}

proc stdConfig::trinamic {{usage undefined} {sensirion 0}} {

  # bin <checksum-algorithm> <timeout> <number-of-retries>
  controller bin chksum-crc 1 2
  prop write trinamic::write
  prop read trinamic::read
  prop complete trinamic::complete
  prop update trinamic::update

  variable node
  set node $node/tasks
  set tasknode $node
  # prop start trinamic::startCmd
  prop startcmd "str1 n int 10 / str9"

  pollperiod 5 5

  set f [expr 256 / 1.8]
#  set f 1

  obj trinamic -drive out
  prop status idle
  prop check trinamic::setpos
  prop write trinamic::runmsg
  prop halt trinamic::halt
  prop fact $f
  prop usezero 1
  prop cormode 0
  prop @due 0
  prop runlabel pos
  hsetprop $tasknode objectnode $node

  kids "trinamic stepper motor" {

    node pos rd
    prop adr axis_par 1
    prop fact $f
    prop usezero 1
    prop update trinamic::updatePos

    node encoder rd
    prop adr axis_par 209
    prop fact $f
    prop usezero 1

    node zero wr
    prop fact $f
    prop write trinamic::writezero
    prop read trinamic::readzero
    prop eeprom 1
    prop adr glob_par 0 2
    default -999

    node lowerlimit wr
    prop adr glob_par 2 2
    prop eeprom 1
    prop fact $f

    node upperlimit wr
    prop adr glob_par 3 2
    prop eeprom 1
    prop fact $f

    node disablelimits par 0
    prop enum 1

    node verbose par 0
    prop enum 1

    node target rd
    prop adr axis_par 0
    prop fact $f
    prop usezero 1

    node runstate upd
    prop enum idle,running,finished,error

#    node targetreached rd 5 slow
#    prop adr axis_par 8
#    prop update trinamic::updateTargetReached
#    prop enum idle,running,finished

    node precision wr
    default 0.9
    prop fact $f
    prop eeprom 1
    prop adr glob_par 5 2

    node maxencdif wr
    default 1.8
    prop fact $f
    prop eeprom 1
    prop adr axis_par 212

#    node blindrun wr 0
#    prop eeprom 1
#    prop adr glob_par 8 2
#    prop enum 1

    node id wr 0
    prop eeprom 1
    prop adr glob_par 1 2

    if {$usage eq "hepump"} {
      node pump_number wr 0
      prop eeprom 1
      prop adr glob_par 8 2
    }

    node init wr 0
    prop adr glob_par 255 2

#    node currentspeed rd
#    prop adr axis_par 3
#    prop read trinamic::read_when_running
#    prop update trinamic::updateCurrentSpeed
#    prop running_time 0
#    prop fast 0

    node maxspeed wr
    prop adr axis_par 4
    prop eeprom 1

    node acceleration wr
    prop adr axis_par 5
    prop eeprom 1

    node maxcurrent wr
    prop adr axis_par 6
    prop eeprom 1

    node standbycurrent wr
    prop adr axis_par 7
    prop eeprom 1

    node freewheeling wr
    prop adr axis_par 204
    prop eeprom 1
    prop enum 1

    node output0 wr
    prop adr io 0 2
    prop enum 1

    node output1 wr
    prop adr io 1 2
    prop enum 1

    node input3 rd
    prop adr io 3 0
    prop enum 1
    prop update trinamic::updateInput3

    # node input0 rd
    # prop adr io 0 1
    # prop fact 30
    # prop offset 1.1
    # prop update trinamic::updateInput0
    # prop last 0
    # prop old 0
    # prop perform_calib 0

    # node input0raw upd

    node pullup wr
    prop adr io 0 0

    if {$sensirion} {
      node flow rd
      prop read stdSct::read
      prop update trinamic::updateFlow
      prop readcmd "str1 ? / str18"

      node flowstddev upd

      # ?<nsample>_  nsample=160: <~= 1 sec, nsample=800: <~= 5 sec
      node flowsamples -int out
      prop write stdSct::write
      prop complete trinamic::updatecalib
      prop writecmd "str8 n%6dr / str18"

      node flowoffset wr
      prop check trinamic::checkCalib
      prop read stdSct::read
      prop write stdSct::write
      prop update trinamic::updateCalib
      prop complete trinamic::updateCalib
      prop readcmd "str1 r / str18"
      prop writecmd "str8 t%6.3fr / str18"

      node flowscale out
      prop check trinamic::checkCalib
      prop write stdSct::write
      prop complete trinamic::updateCalib
      prop writecmd "str8 g%6.3fr / str18"

      node flowsave out
      prop enum 1
      prop write trinamic::saveCalib
      prop help "unchecked: current calib is not saved. set checked: save calib"
    }

    # node hysteresis wr
    # prop fact $f
    # prop eeprom 1
    # prop adr glob_par 9 2

    if {$usage eq "hepump"} {
      node nopumpfeedback wr
      prop eeprom 1
      prop adr glob_par 6 2
      prop enum 1
    }
#    node stopstatus rd
#    prop adr axis_par 207
#    prop update trinamic::updateStopstatus

    node eeprom out
    default 1
    prop enum ok,dirty,save,load
    prop write trinamic::eewrite
    prop label "eeprom status"

    node customadr out -text
    default "axis_par 209"
    prop check trinamic::setcustom
    prop write stdSct::complete

    node custompar wr
    prop adr axis_par 209

  }
  variable ctrl
  variable path

  # load
  #  $ctrl queue $path start trinamic::init

  # read zero first (global bank2 parameter 0)
  $ctrl queue $path/zero progress trinamic::readzero
}

proc trinamic::updateFlow {} {
  lassign [sct result] flow stddev
  sct update $flow
  updateval [sct parent]/flowstddev $stddev
  return idle
}

proc trinamic::currentCalib {} {
  return [format {%.4f %.4f} [sctval [sct parent]/flowoffset] [sctval [sct parent]/flowscale]]
}

proc trinamic::updateCalib {} {
  lassign [sct result] offset scale
  updateval [sct parent]/flowoffset $offset
  updateval [sct parent]/flowscale $scale
  set node [sct parent]/flowsave
  if {[silent -1 hgetpropval $node saved] == -1} {
    hsetprop $node saved [currentCalib]
  }
  return idle
}

proc trinamic::checkCalib {} {
  if {[silent "" hgetpropval [sct parent]/flowsave saved] ne [currentCalib]} {
    hupdate [sct parent]/flowsave 0
  } else {
    hupdate [sct parent]/flowsave 1
  }
}

proc trinamic::saveCalib {} {
  if {[sct target]} {
    sct send "str2 sr / str18"
    sct saved [currentCalib]
    sct print "saved calib [sct saved]"
  } else {
    sct print "marked calib as dirty"
  }
  sct update [sct target]
  return stdSct::complete
}

proc trinamic::send {cmd {nr 0} {bank 0} {value 0}} {
  variable cmds
  set cmd $cmds($cmd)
#  sct send "int 1 $cmd $nr $bank int4 $value crc 0 / 2 1 100 $cmd int4 crc"
  sct send "int 1 $cmd $nr $bank int4 $value crc 0 / 2 1 skip1 $cmd int4 crc"
}

proc trinamic::response {} {
  if {[llength [sct result]] != 1} {
    error "illegal result on [sct]: [sct result]"
  }
  return [string trim [sct result]]
}

proc trinamic::complete {} {
  response
  return idle
}

proc trinamic::logmsg {text} {
  if {[hval [sct objectPath]/verbose]} {
    clientput $text
  }
}

proc trinamic::halt {} {
  logmsg HALT
  trinamic::endrun
  send motor_stop
  return trinamic::complete
}

proc trinamic::init {} {
  # mark nodes for load
  foreach var [hlist [sct]] {
    if {[silent 1 hgetpropval [sct]/$var eeprom] != 1} {
      hsetprop [sct]/$var eeprom 1
    }
  }
  # load eeprom (with "halt" priority, higher than write)
  [sct controller] queue [sct]/eeprom halt "trinamic::eeprom load"
  hsetprop [sct]/encoder adjust_zero 1
  # set pullup resistors on
  send set_io 0 0 1
  return trinamic::complete
}

proc trinamic::startCmd {} {
  sct try_cnt 3
  send get_glob_par 255 2
  return trinamic::startCmd0
}

proc trinamic::startCmd0 {} {
  if {[response] ne "1" && [response] ne "2"} {
    if {[sct try_cnt] > 0} {
      sct try_cnt [expr [sct try_cnt] - 1]
      send get_glob_par 255 2
      return trinamic::startCmd0
    }
    clientput "[sct] reboot detected"
    send set_glob_par 255 2 1
    return trinamic::startCmd1
  }
  send get_glob_par 1 2
  return complete
}
proc trinamic::startCmd1 {} {
  # clientput "start cmd1 [response]"
  send get_glob_par 1 2
  return trinamic::startUpd
}

proc trinamic::startUpd {} {
  # clientput "start upd [response]"
  [sct controller] queue [sct objectnode] start trinamic::init
  return complete
}

proc trinamic::eewrite {} {
  # mark nodes for load or save
  foreach var [hlist [sct parent]] {
    if {[silent 1 hgetpropval [sct parent]/$var eeprom] == 0} {
      hsetprop [sct parent]/$var eeprom 1
    }
  }
  switch [sct target] {
    2 {
      return "trinamic::eeprom save"
    }
    3 {
      return "trinamic::eeprom load"
    }
  }
  clientlog "nothing to do"
  sct update [hvali [sct]]
  return idle
}

proc trinamic::eeprom {saveorload} {
  foreach var [hlist [sct parent]] {
    if {[silent 0 hgetpropval [sct parent]/$var eeprom]} {
      lassign "[hgetpropval [sct parent]/$var adr] 0 0" typ nr bank
      if {$saveorload eq "load"} {
        set cmd load_$typ
        [sct controller] queue [sct parent]/$var progress read
      } else {
        set cmd save_$typ
      }
#      sct send "int 1 $cmd $nr $bank int4 0 crc / skip8 crc"
      send $cmd $nr $bank
      hsetprop [sct parent]/$var eeprom 0
#      clientlog "$saveorload $var"
      return "trinamic::eeprom $saveorload"
    }
  }
  # set o.k.
  sct update 0
#  clientlog "finished eeprom $saveorload"
  return idle
}

proc trinamic::write {} {
  lassign "[sct adr] 0 0" typ nr bank
  hupdate [sct parent]/eeprom 1
  set val [expr round([sct target] * [silent 1 sct fact])]
#  sct send "int 1 $cmd $nr $bank int4 $val crc / skip8 crc"
  send set_$typ $nr $bank $val
  set old [hvali [sct]]
  if {$old != [sct target]} {
    #clientput "SET [sct] $old -> [sct target]"
  }
  set trigger [silent "" sct trigger_path]
  if {$trigger ne ""} {
    [sct controller] queue $trigger read read
  }
  return read
}

proc trinamic::read {} {
  if {[DoubleTime] < [sct @due]} {
    return idle
  }
  lassign "[sct adr] 0 0" typ nr bank
#  sct send "int 1 $cmd $nr $bank int4 0 crc / skip4 int4 crc"
  send get_$typ $nr $bank
  sct crctry 3
#  clientlog [sct]
  return update
}

proc trinamic::update {} {
  set v [response]
  set f [silent 1 sct fact]
  set z 0

  sct rawvalue $v
  set vv [expr $v / double($f)]
  if {[silent 0 sct usezero]} {
    set z [silent -999 hval [sct objectPath]/zero]
    if {$z == -999} {
      error "undefined zero"
    }
  }
  set vv [format %.2f [expr $vv + $z]]

  if {[silent 0 sct adjust_zero]} {
    #clientput "encoder $vv raw [expr $vv - $z]"
    set uplim [hval [sct objectPath]/upperlimit]
    set lolim [hval [sct objectPath]/lowerlimit]
    set reserve [expr 360 - ($uplim - $lolim)]
    if {$reserve > 0} {
      set uplim [expr $uplim + $reserve * 0.5]
      set lolim [expr $lolim - $reserve * 0.5]
    }
    #clientput "limits $lolim $uplim reserve $reserve"
    set dz 0
    if {$vv > $uplim && $vv - 360 <= $uplim && $vv - 360 >= $lolim} {
      set dz -360
    } elseif {$vv < $lolim && $vv + 360 >= $lolim && $vv + 360 <= $uplim} {
      set dz 360
    }
    if {$dz != 0} {
      clientput "adjust zero $z to [expr $z + $dz]"
      set z [expr $z + $dz]
      hset [sct objectPath]/zero $z
      set vv [expr $vv + $dz]
    }
    sct adjust_zero 0
  }
#  switch [sct] {
#    /nv/nvmot/pos - /nv/nvmot/target - /nv/nvmot/custompar {
#      clientput "[expr fmod([DoubleTime],60)] [sct] $vv"
#    }
#  }
  sct update $vv
  return idle
}

proc trinamic::writezero {} {
  set val [expr round([sct target] * [silent 1 sct fact])]
  if {$val <= 0} {
    incr val -1
  }
  if {[silent -1 sct eeprom] == 0} {
    sct eeprom 1
    hupdate [sct parent]/eeprom 1
  }
#  sct send "int 1 9 0 2 int4 $val crc / skip8 crc"
  send set_glob_par 0 2 $val
  #clientput "set zero [hvali [sct]] -> [sct target] $val"
  return read
}

proc trinamic::readzero {} {
  if {[DoubleTime] < [sct @due]} {
    return idle
  }
  send get_glob_par 0 2
  return trinamic::updatezero
}

proc trinamic::updatezero {} {
  set v [response]
  if {$v == 0} {
    clientlog "zero undefined"
  }
  if {$v < 0} {
    incr v
  }
  set v [format %.2f [expr $v / double([silent 1 sct fact])]]
#  if {$v != 0} {
#    clientput "zero $v [sct result]"
#  }
  sct update $v
  return idle
}

proc trinamic::updatePos {} {
  set res [trinamic::update]
  updateval [sct parent] [hvali [sct]]
  return $res
}

proc trinamic::setcustom {} {
  hsetprop [sct parent]/custompar adr [sct target]
  sct update [sct target]
  [sct controller] queue [sct parent]/custompar read read
  return idle
}

proc trinamic::setpos {} {
  if {![hvali [sct]/disablelimits]} {
    if {[sct target] > [hval [sct]/upperlimit]} {
      error "ERROR: [sct target] is above upper limit"
    }
    if {[sct target] < [hval [sct]/lowerlimit]} {
      error "ERROR: [sct target] is below lower limit"
    }
  }
  sct goto [sct target]
  set period 0
  if {[sct status] eq "run"} {
    set period 0.1
  }
  if {[silent 0 sct startime] == 0} {
    sct startime [DoubleTime]
  }
#  if {[sct] eq "/nv/nvmot"} {
#    debug nvmot
#  }
  [sct controller] poll [sct] $period progress trinamic::start
}

proc trinamic::start {} {
  set now [DoubleTime]
  if {[sct status] eq "run"} {
    if {$now < [sct startime] + 60} {
      return idle
    }
    clientlog "[sct] timeout when waiting for idle"
  }
  sct startime 0
  sct adjustcnt 0
  [sct controller] poll [sct] 0.1 progress trinamic::adjustPosNew
  sct afteradjust trinamic::start1
  # set idle mode
  hupdate [sct]/runstate 0
  sct status run
  return unpoll
}

proc trinamic::calcdelay {} {
  set s [hgetpropval [sct]/maxspeed actvalue]
  set a [hgetpropval [sct]/acceleration actvalue]
  set p [hgetpropval [sct]/pos rawvalue]
  set t [sct moveto]
  # calculate time: pulse_div 3 assumed, ramp_div 7 assumed"
  set delay [expr abs($t - $p) / 30.5 / $s + $s / $a / 15.25 + 0.5]
  return $delay
}

proc trinamic::adjustPosNew {} {
  set adjustcnt [sct adjustcnt]
  sct adjustcnt [expr $adjustcnt+1]
  if {$adjustcnt % 10 == 0} {
    # read encoder and pos before start
    [sct controller] queue [sct]/pos progress trinamic::read
    [sct controller] queue [sct]/encoder progress trinamic::read
    return idle
  }
  if {$adjustcnt % 10 == 1} {
    set enc [hgetpropval [sct]/encoder rawvalue]
    set pos [hgetpropval [sct]/pos rawvalue]
    if {abs($enc - $pos) < 256} {
      [sct controller] queue [sct] progress [sct afteradjust]
      return unpoll
    }
    send set_axis_par 1 0 $enc
    logmsg "[sct]: correct pos [hvali [sct]/pos] to enc [hvali [sct]/encoder]"
    return "trinamic::adjustPosNew1 $enc"
  }
  return idle
}

proc trinamic::adjustPosNew1 {target} {
  send set_axis_par 0 0 $target
  return "trinamic::complete"
}

proc trinamic::adjustPos {next} {
  set e [hval [sct]/encoder]
  set p [hval [sct]/pos]
  set dif [format %.2f [expr $e - $p]]
  set prec [hvali [sct]/precision]
  if {abs($dif) <= $prec && abs($dif) <= [hvali [sct]/maxencdif] * 0.9} {
    return $next
  }
  temp_value maxcurrent 0
  temp_value maxspeed 2047
  temp_value acceleration 2047
  temp_value maxencdif 0
  sct moveto [hgetpropval [sct]/encoder rawvalue]
#  if {abs($dif) > [hvali [sct]/maxencdif]} {
#    clientput "[sct] correct motor position by $dif"
#  }
  logmsg "[sct] correct motor position by $dif"
  sct delay [trinamic::calcdelay]
  sct afterrun $next
  return trinamic::setrunpars
}

proc trinamic::setrunpars {} {
  foreach var [hlist [sct]] {
    set val [silent none hgetpropval [sct]/$var actvalue]
    if {$val ne "none"} {
      set f [silent 1 hgetpropval [sct]/$var fact]
      set val [expr round($val * $f)]
      lassign "[hgetpropval [sct]/$var adr] 0 0" typ nr bank
      send set_$typ $nr $bank $val
      hsetprop [sct]/$var actvalue none
      return trinamic::setrunpars
    }
  }
  sct @due [expr [DoubleTime] + [sct delay]]
  send move_to_pos 0 0 [sct moveto]
  [sct controller] poll [sct] 0.1 progress "trinamic::wait"
  return trinamic::complete
}

proc trinamic::wait {} {
  set now [DoubleTime]
  if {$now < [sct @due]} {
    return idle
  }
  [sct controller] queue [sct]/pos progress trinamic::read
  [sct controller] queue [sct]/encoder progress trinamic::read
  sct finitime $now
  [sct controller] queue [sct] progress trinamic::fini
  return unpoll
}

proc trinamic::fini {} {
  # get target reached
  send get_axis_par 8
  return trinamic::targetReached
}

proc trinamic::targetReached {} {
  set r [response]
#clientput "targetreached [expr fmod([DoubleTime],60)] $r"
  if {$r == 1} {
    updateval [sct]/runstate 2 ;# finished
    # get stop status (for clearing only)
    send get_axis_par 207
    return [sct afterrun]
  }
  if {$r != 0} {
    error "[sct] illegal value for target reached: $r"
  }
  # get stop status
  send get_axis_par 207
  return trinamic::stopStatus
}

proc trinamic::stopStatus {} {
  set r [response]
  if {$r > 0 && $r <= 3} {
    logmsg "stopped with error $r"
    updateval [sct]/runstate 3
    return [sct afterrun]
  }
  if {$r == 0} {
    if {[DoubleTime] < [sct finitime] + 2} {
      return trinamic::fini
    }
    updateval [sct]/runstate 3
    clientlog "[sct] run timeout [sct delay] [expr [DoubleTime] - [sct @due]]"
    return [sct afterrun]
  }
  error "[sct] illegal stopstatus: $r"
}

proc trinamic::runmsg {} {
#  sct print "run [sct objectName] to [sct target]"
  return idle
}

proc trinamic::start1 {} {
  normal_value maxcurrent
  normal_value maxspeed
  normal_value acceleration
  normal_value maxencdif
  sct enc0 [hvali [sct]/encoder]
  set val [sct goto]
  set f [silent 1 sct fact]
  set val [expr round(($val - [hval [sct objectPath]/zero]) * $f)]
  sct moveto $val
  sct delay [trinamic::calcdelay]
#  sct afterrun "trinamic::adjustPos trinamic::endrun"
  sct afterrun "trinamic::endrun"
  return trinamic::setrunpars
}

proc trinamic::temp_value {var badval} {
  set val [sctval [sct]/$var]
  if {$val != $badval} {
    hsetprop [sct]/$var restorevalue $val
  }
  hsetprop [sct]/$var actvalue $badval
}

proc trinamic::normal_value {var} {
  set val [sctval [sct]/$var]
  hsetprop [sct]/$var actvalue [silent $val hgetpropval [sct]/$var restorevalue]
}

proc trinamic::endrun {} {
  if {[sct] eq "/hepump" && [result _hepump debug] >= 0} {
    debug off
  }
  set enc [hvali [sct]/encoder]
  set dif [expr abs($enc - [sct goto])]
  set prec [hvali [sct]/precision]
  if {[hvali [sct]/runstate] == 3 && $dif > $prec
      && $dif <= abs([sct enc0] - [sct goto]) - $prec} {
    logmsg "[sct] try again enc=$enc goto=[sct goto]"
    return trinamic::start1
  }
  sct status idle
  foreach var [hlist [sct]] {
    set val [silent none hgetpropval [sct]/$var restorevalue]
    if {$val ne "none"} {
      hset [sct]/$var $val
      hdelprop [sct]/$var restorevalue
    }
  }
  # make sure input3 (endswitch) is read immediately
  send get_io 3 0
  return "trinamic::updateInput3 ismainpath"
}

proc trinamic::updateInput3 {{ismainpath 0}} {
  if {$ismainpath eq "ismainpath"} {
    set path [sct]
  } else {
    set path [sct parent]
  }
  if {[silent 0 hvali $path/nopumpfeedback] == 1} {
    # assume that pump is on when output0 is 1
    updateval $path/input3 [expr ![hval $path/output0]]
  } else {
    updateval $path/input3 [response]
  }
  return idle
}

proc trinamic::updateInput0 {} {
  # input0: 12 bit (0...4095) for 0-10 V
  # make an average over 0.5 sec (about 5 readings)
  set v [response]
  sct sum [expr [silent 0 sct sum] + $v]
  sct cnt [expr [silent 0 sct cnt] + 1]
  set now [DoubleTime]
  if {[sct cnt] < 5 && $now < [sct last] + 1} {
    return idle
  }
  set v [expr double([sct sum]) / [sct cnt]]
  set volt [expr $v / 409.6]
  set scaled [expr ($volt - [sct offset]) * [sct fact]]
  hupdate [sct parent]/input0raw $volt
  sct update $scaled
  sct cnt 0
  sct sum 0
  sct last $now
  return idle
}