# $Revision: 1.9 $
# $Date: 2009-01-23 05:06:03 $
# Author: Ferdi Franceschini (ffr@ansto.gov.au)
# Last revision by: $Author: ffr $

proc sleep {mstime} {
  set x 0
  after $mstime {set x 1}
  vwait x
}

# Requires a configuration array for each axis that you want to simulate.
# eg
#array set B [list AC 25000 TP 7827107 TD 25000 PA 25000 stepsperx 25000 cntsperx 8192 DC 25000 SP 25000]
# The mkSimAxes.tcl script creates files with arrays like the example above.

# Substitutes position values for _TDx and _TPx
# then evaluates the expression.
proc evaluate {args} {
  regsub -all {_T[DP][ABCDEFGH]} $args {[eval [parse &]]} expression;
  regsub -all {\d+(?:\.\d*)?} $expression { double(&) } expression;
  return [uplevel #0 expr $expression];
}

# Evaluates a comma separated list of commands and
# returns a space separated list of values.
proc evalArgList {args} {
    foreach cmd [split $args ,] {lappend values [eval [parse $cmd]]}
    return $values;
}

proc dmset {cmd axis args} {
  set num [scan $args {%d} val];
  if {$num == 1} {
    set val $args;
  } else {
    set val [evaluate $args]
  }
  set inst [string range $cmd 0 1]
  switch $inst {
    "PA" - "PR" {
      uplevel #0 set ${axis}(MVTYPE) $inst
      uplevel #0 set ${axis}($cmd) $val;
    }
    "JG" {
      uplevel #0 set ${axis}(MVTYPE) $inst
      uplevel #0 set ${axis}($cmd) $val;
#     uplevel #0 set ${axis}(SP) $val;
    }
    "SP" - "AC" - "DC" {
      uplevel #0 set ${axis}($cmd) $val;
    }
    default {
      uplevel #0 $cmd $axis $val;
    }
  }
}

proc dmget {cmd axis} {
 uplevel #0 set ${axis}($cmd)
}

proc dmcall {cmd paxis} {
  uplevel #0 eval $cmd $paxis
}

proc DP {axis val} {
    uplevel #0 eval set ${axis}(TD) $val
}
proc TS {axis} {
    uplevel #0 eval set ${axis}(TS)
}
proc ST {_axis} {
  upvar #0 $_axis axis;
  set axis(ST) 1
  while {$axis(BG) == 1} {
    sleep 1000
  }
}

proc SH {args} {}
proc MO {args} {}
proc LV {args} {return "FRED=1\nBARNEY=2"}
proc TI {args} {return 240}
proc XQ {args} {return 1}

proc BG {_axis} {
  upvar #0 $_axis axis;
  set axis(TS) 140; # moving, limit switches open
  set axis(BG) 1; # motor is moving
  set axis(SC) 0; # motor is running
  set timeStep 0.1; # seconds
  set axis(ST) 0
  switch $axis(MVTYPE) {
    "PA" {
      set target $axis(PA);
      set diff [expr $target - $axis(TD)];
      set sign [expr ($axis(PA) - $axis(TD)) < 0 ? -1 : 1];
      set step [expr $sign * $timeStep * $axis(SP) ];
      if {[expr abs($diff) < abs($step)]} {
        set step $diff;
        set timeStep [expr abs($step / $axis(SP))];
      }
      every [expr round($timeStep * 1000)] "nextstep $_axis $step target $_axis $target $step"
    }
    "JG" {
      set step [expr $timeStep * $axis(JG) ];
      every [expr round($timeStep * 1000)] "nextstep $_axis $step jogit"
    }
  }

#  set diff [expr $target - $axis(TD)];
#  set mult [expr $axis(cntsperx).0/$axis(stepsperx)];
#  set axis(TP) [expr round($diff*$mult + $axis(TP))];
#  set axis(TD) $target;
}

# Don't handle _XQ _HX
proc MG {args} {
# Skip formatting
  if {[string index [lindex $args 0] 0] == "F"} {
      set msg [lrange $args 1 end]
    } else {
      set msg $args
    }
# If msg starts with _ then return val for axis
  if {[string index $msg 0] == "_"} {
    return [evalArgList $msg];
  } else {
    return $msg;
  }
}


proc every {ms body} {
  if [eval $body] {
    after $ms [list every $ms $body];
  }
  return;
}

proc nextstep {paxis step args} {
  upvar #0 $paxis axis;
  set finished false
  if {($axis(ATLIM) == "upper") && ($step > 0)} {
    return 0
  } elseif {($axis(ATLIM) == "lower") && ($step < 0)} {
    return 0
  }
  set mult [expr double($axis(cntsperx))/$axis(stepsperx)];
  set axis(TP) [expr int($step * $mult + $axis(TP))];
  set TD_POS [expr int($axis(TD) + $step)];
  set axis(TD) [expr int($TD_POS)];
  set currPos [expr ($axis(TP) - $axis(ABSHOME))/abs(double($axis(cntsperx)))]
  if {$axis(ST) == 1} {
    set axis(TS) 44; # Stopped, limit switches open
    set axis(BG) 0; # motor has stopped 
    set axis(ST) 0; # make sure stop flag is unset
    set axis(SC) 4; # motor stopped by stop command (ST)
    set axis(ATLIM) false
    set finished true
  } elseif {$currPos >= $axis(UPLIM)} {
    set axis(TS) 36; # Stopped on forward limit switch
    set axis(BG) 0; # motor has stopped 
    set axis(ST) 0; # make sure stop flag is unset
    set axis(SC) 2; # motor stopped by limit switch
    set axis(ATLIM) upper
    set finished true
  } elseif {$currPos <= $axis(LOLIM)} {
    set axis(TS) 40; # Stopped on reverse limit switch
    set axis(BG) 0; # motor has stopped 
    set axis(ST) 0; # make sure stop flag is unset
    set axis(SC) 3; # motor stopped by limit switch
    set axis(ATLIM) lower
    set finished true
  } elseif {[eval $args]} {
  # Stop if condition specified in "args" is met
#   set diff [expr $target - $axis(TD)];
#   set axis(TP) [expr int(round($diff*$mult + $axis(TP)))];
#   set axis(TD) [expr int($target)];
    set axis(TS) 44; # Stopped, limit switches open
    set axis(BG) 0; # motor has stopped 
    set axis(ST) 0; # make sure stop flag is unset
    set axis(SC) 1; # motor stopped at commanded position
    set axis(ATLIM) false
    set finished true
  } else {
    set finished false
  }
  if {$finished} {
    set axis(MVTYPE) "unknown"
    return 0
  } else {
    return 1
  }
}

proc target {paxis target step} {
  upvar #0 $paxis axis;
  return [expr abs($target - $axis(TD)) < abs($step)]
}
proc jogit {} {
  return false
}