#----------------------------------------------------
# This is a scriptcontext motor driver for the
# prehistoric Physik Instrumente DC-406, C-804 DC
# motor controller.
#
# copyright: see file COPYRIGHT
#
# Scriptchains:
#        - read       - readreply
#        - write      - writerepy
#        - sendstatus - statusreply - statuspos
#        - speedread  - readreply
#        - writespeed - speedreply
#        - writenull  - speedreply
#
# Mark Koennecke, November 2009, after the
#  C original from 1998
# Made to work, Mark Koennecke, January 2011
#-----------------------------------------------------

namespace eval pimotor {}
#----------------------------------------------------
proc pimotor::read {num} {
  sct send [format "%1.1dTP" $num]
  return readreply
}
#----------------------------------------------------
proc pimotor::readreply {} {
  set result [sct result]
  if {[string first ? $result] >= 0} {
    error $result
  }
  if {[string first ERR $result] >= 0} {
    error $result
  }
  set val [string range $result 3 end]
  sct update $val
  return idle
}
#----------------------------------------------------
proc pimotor::write {num name} {
  set ival [expr int([sct target])]
# After a stop, the motor is switched off. In order to fix this
# we switch the motor on for each drive command
  sct send [format "%1.1dMN,%1.1dMA%10.10d{0}" $num $num $ival]
  hupdate /sics/${name}/status run
  return writereply
}
#----------------------------------------------------
proc pimotor::writereply {name} {
# the DC-406 does not reply on this, so we have for sure a
# timeout here which we ignore. We do nothing else, as we
# need a little wait anyway to get the motor to start
# before starting to check status.
#----------------------------------------------------
  wait 1
  set con [sct controller]
  hset /sics/${name}/status run
  $con queue /sics/${name}/status progress read
  return idle
}
#-----------------------------------------------------
proc pimotor::sendstatus {num} {
  sct send [format "%1.1dTV" $num]
  return statusreply
}
#------------------------------------------------------
proc pimotor::statusreply {num} {
  set result [sct result]
  if {[string first ? $result] >= 0} {
    sct update error
    error $result
  }
  if {[string first ERR $result] >= 0} {
    sct update error
    error $result
  }
  set val [string trimleft [string range $result 3 13] "0-"]
  set val [string trim $val]
  if {[string length $val] > 1} {
      set len [string length $val]
        clientput "Value = $val, length = $len"
	if {abs($val) > 0} {
	    sct update run
	    [sct controller] queue [sct] progress read
            return idle
        }
  }
  pimotor::read $num
  return statuspos
}
#------------------------------------------------------
proc pimotor::statuspos {name} {
  set result [sct result]
  if {[string first ? $result] >= 0} {
    error $result
  }
  if {[string first ERR $result] >= 0} {
    error $result
  }
  set val [string range $result 3 end]
  hupdate /sics/${name}/hardposition $val
  sct update idle
  return idle
}
#-------------------------------------------------------
proc pimotor::readspeed {num} {
  sct send [format "%1.1dTY" $num]
  return readreply
}
#--------------------------------------------------------
proc pimotor::writespeed {num} {
  sct send [format "%1.1dSV%7.7d" $num [sct target]]
  return speedreply
}
#----------------------------------------------------
proc pimotor::speedreply {num} {
  pimotor::readspeed $num
  return readreply
}
#-----------------------------------------------------
proc pimotor::writenull {controller num} {
    $controller send [format "%1.1dDH{0}" $num]
  return Done
}
#------------------------------------------------------
proc pimotor::writeon {controller num} {
    $controller send [format "%1.1dMN{0}" $num]
  return Done
}
#------------------------------------------------------
proc pimotor::halt {controller num} {
    $controller send [format "%1.1dAB{0}" $num]
  return Done
}
#------------------------------------------------------
proc pimotor::makepimotor {name num sct lowlim upperlim} {
    MakeSecMotor $name

    hdel /sics/${name}/hardupperlim
    hdel /sics/${name}/hardlowerlim
    hfactory /sics/${name}/hardupperlim plain internal float
    hfactory /sics/${name}/hardlowerlim plain internal float
    $name hardlowerlim $lowlim
    $name softlowerlim $lowlim
    $name hardupperlim $upperlim
    $name softupperlim $upperlim

    hsetprop /sics/${name}/hardposition read pimotor::read $num
    hsetprop /sics/${name}/hardposition readreply pimotor::readreply
    $sct poll /sics/${name}/hardposition 60

    hsetprop /sics/${name}/hardposition write pimotor::write $num $name
    hsetprop /sics/${name}/hardposition writereply pimotor::writereply $name
    $sct write /sics/${name}/hardposition

    hsetprop /sics/${name}/status read pimotor::sendstatus $num
    hsetprop /sics/${name}/status statusreply pimotor::statusreply $num
    hsetprop /sics/${name}/status statuspos pimotor::statuspos $name
    $sct poll /sics/${name}/status 60

    hfactory /sics/${name}/speed plain user int
    hsetprop /sics/${name}/speed read pimotor::readspeed $num
    hsetprop /sics/${name}/speed readreply pimotor::readreply
    $sct poll  /sics/${name}/speed 120

    hsetprop /sics/${name}/speed write pimotor::writespeed $num
    hsetprop /sics/${name}/speed speedreply pimotor::speedreply $num
    $sct write /sics/${name}/speed

    $name makescriptfunc halt "pimotor::halt $sct $num" user
    $name makescriptfunc on  "pimotor::writeon $sct $num" user
    $name makescriptfunc home "pimotor::writenull $sct $num" user

    hupdate /sics/${name}/status idle
    $sct queue /sics/${name}/hardposition progress read
}