# Some useful functions for SICS motor configuration.

# $Revision: 1.4 $
# $Date: 2008-10-27 04:55:48 $
# Author: Douglas Clowes (dcl@ansto.gov.au)
# Last revision by $Author: ffr $

proc relmove {runcmd args} {
  if [ catch {
    if {[llength $args] == 1} {
      set arguments [lindex $args 0]
    } else {
      set arguments $args
    }

    array set motoffset $arguments
    foreach motor [array names motoffset] {
      set currpos [SplitReply [$motor]]
      lappend motdest $motor [expr $currpos + $motoffset($motor)]
    }
    switch $runcmd {
      "relrun" {
        eval "run $motdest"
      }
      "reldrive" {
        eval "drive $motdest"
      }
    }
  } message ] {
    if {$::errorCode=="NONE"} {return }
    return -code error $message
  }
}

proc relrun {args} {
  if [ catch {
    relmove "relrun" $args
  } message ] {
    if {$::errorCode=="NONE"} {return }
    return -code error $message
  }
}
publish relrun user

proc reldrive {args} {
  if [ catch {
    relmove "reldrive" $args
  } message ] {
    if {$::errorCode=="NONE"} {return }
    return -code error $message
  } 
}
publish reldrive user

##
# @brief  A convenience command for fetching motor parameter values
#
# This convenience command is useful for avoiding command 
# substitution problems when defining hdb node read scripts.
proc getmotpar {motor par} {
  return  [SplitReply [$motor $par]]
}

# Functions for the slit motors (echidna, wombat, platypus, ...)
#
# functions used by the generated functions
proc get_gap_width {m1 m2} {
  return [expr [SplitReply [$m1]] - [SplitReply [$m2]]]
}

proc set_gap_width {m1 m2 val} {
  set currentWidth [expr [SplitReply [$m1]] - [SplitReply [$m2]]]
  set diff [expr $val - $currentWidth]
  set newD1R [expr [SplitReply [$m1]] + $diff/2]
  set newD1L [expr [SplitReply [$m2]] - $diff/2]
  return "$m1=$newD1R,$m2=$newD1L"
}

proc get_gap_offset {m1 m2} {
  set S1 [SplitReply [$m1]]
  set S2 [SplitReply [$m2]]
  return [ expr ($S1 + $S2)/2.0 ]
}

proc set_gap_offset {m1 m2 val} {
  set S1 [SplitReply [$m1]]
  set S2 [SplitReply [$m2]]
  set currentoffset [expr ($S1 + $S2)/2.0 ]
  set diff [expr $val - $currentoffset]
  set newD1R [expr $S1 + $diff]
  set newD1L [expr $S2 + $diff]
  return "$m1=$newD1R,$m2=$newD1L"
}

# Generator functions
proc make_gap_motors {vm1 vm1_name vm2 vm2_name m1 m2 aunits agroup} {
  eval "proc get_$vm1 {} { get_gap_width $m1 $m2 }"
  set v {$var}
  eval "proc set_$vm1 {var} { set_gap_width $m1 $m2 $v }"
  MakeConfigurableMotor $vm1
  $vm1 readscript get_$vm1
  $vm1 drivescript set_$vm1
#publish get_$vm1 user
#publish set_$vm1 user

  eval "proc get_$vm2 {} { get_gap_offset $m1 $m2 }"
  set v {$var}
  eval "proc set_$vm2 {var} { set_gap_offset $m1 $m2 $v }"
  MakeConfigurableMotor $vm2
  $vm2 readscript get_$vm2
  $vm2 drivescript set_$vm2
#publish get_$vm2 user
#publish set_$vm2 user

sicslist setatt $vm1 units $aunits
sicslist setatt $vm1 klass slits
sicslist setatt $vm1 long_name $vm1_name
sicslist setatt $vm1 group $agroup
sicslist setatt $vm1 hdbchain $m1,$m2
sicslist setatt $vm2 units $aunits
sicslist setatt $vm2 klass slits
sicslist setatt $vm2 long_name $vm2_name
sicslist setatt $vm2 group $agroup
sicslist setatt $vm2 hdbchain $m1,$m2
}

# Functions for motors with defined positions (quokka, platypus)
#
# functions used by the generated functions
proc set_virtual_1 { phys1 posit } {
  set units [expr [SplitReply [$phys1 posit2unit $posit]]]
  return "$phys1=$units"
}

proc set_virtual_2 { phys1 phys2 posit } {
  set unit1 [expr [SplitReply [$phys1 posit2unit $posit]]]
  set unit2 [expr [SplitReply [$phys2 posit2unit $posit]]]
  return "$phys1=$unit1,$phys2=$unit2"
}

proc get_virtual_1 { phys1 } {
  set p1 [expr [SplitReply [$phys1 posit]]]
  return $p1
}

proc get_virtual_2 { phys1 phys2 } {
  set p1 [expr [SplitReply [$phys1 posit]]]
  set p2 [expr [SplitReply [$phys2 posit]]]
  return [expr ($p1 + $p2) / 2.0]
}
#publish set_virtual_1 user
#publish set_virtual_2 user
#publish get_virtual_1 user
#publish get_virtual_2 user

# Generator functions
proc make_coll_motor_1 { vm1 vm1_name pm1 aunits } {
  eval "proc get_$vm1 {} { get_virtual_1 $pm1 }"
  set v {$var}
  eval "proc set_$vm1 {var} { set_virtual_1 $pm1 $v }"
  MakeConfigurableMotor $vm1
  $vm1 readscript get_$vm1
  $vm1 drivescript set_$vm1
  sicslist setatt $vm1 units $aunits
  sicslist setatt $vm1 klass collimator
  sicslist setatt $vm1 long_name $vm1_name
  sicslist setatt $vm1 hdbchain $pm1
}

proc make_coll_motor_2 { vm1 vm1_name pm1 pm2 aunits } {
  eval "proc get_$vm1 {} { get_virtual_2 $pm1 $pm2 }"
  set v {$var}
  eval "proc set_$vm1 {var} { set_virtual_2 $pm1 $pm2 $v }"
  MakeConfigurableMotor $vm1
  $vm1 readscript get_$vm1
  $vm1 drivescript set_$vm1
  sicslist setatt $vm1 units $aunits
  sicslist setatt $vm1 klass collimator
  sicslist setatt $vm1 long_name $vm1_name
  sicslist setatt $vm1 hdbchain $pm1,$pm2
}