From 78372baae93ed64b7d602753a0de8f9726f886c4 Mon Sep 17 00:00:00 2001
From: Ferdi Franceschini <ffr@ansto.gov.au>
Date: Tue, 30 Sep 2014 15:04:07 +1000
Subject: [PATCH] Fix posname algorithm so that it works if position lists are
 in descending order.

---
 .../config/motors/motor_configuration.tcl     | 82 +++++++++++++------
 1 file changed, 55 insertions(+), 27 deletions(-)

diff --git a/site_ansto/instrument/bilby/config/motors/motor_configuration.tcl b/site_ansto/instrument/bilby/config/motors/motor_configuration.tcl
index 986de170..bc224a55 100644
--- a/site_ansto/instrument/bilby/config/motors/motor_configuration.tcl
+++ b/site_ansto/instrument/bilby/config/motors/motor_configuration.tcl
@@ -57,7 +57,8 @@ foreach pos {27 55 84 114 146 179 213 248 288 333} {
 }
 sapmot positions {*}$poslist
 sapmot position_names D2.5 D5 D7.5 D10 D12.5 D15 D17.5 D20 D30 D40
-sapmot precision 0.001
+sapmot precision 0.002
+sapmot creep_offset 0.1
 # TODO implement target
 ::utility::macro::getset text sample_aperture { {target "NONE"} } {
   return "sample_aperture = [SplitReply [posname sapmot]]"
@@ -93,43 +94,70 @@ proc filter_non_numeric {retval} {
 }
 
 proc posname {pmot} {
-  set pos [ SplitReply [$pmot] ]
-  set tol [ SplitReply [$pmot precision] ]
   set pnames [SplitReply [$pmot position_names]]
+  if {[string len $pnames] == ""} {
+    error "[info level 0]: This motor does not have named positions."
+  }
   set positions [SplitReply [$pmot positions]]
-  set first_pos [lindex $positions 0]
-  set last_pos [lindex $positions end]
+  if {[string len $positions] == ""} {
+    error "[info level 0]: This motor does not have a list of positions."
+  }
 
   if { [llength $pnames] != [llength $positions] } { 
     error "[info level 0]: Name list and position list are not the same size"
-  } else {
-    if { [llength $pnames] < 1} {
-      error "[info level 0]: Motor $pmot does not have a list of positions"
-    }
-  }
-  if { [expr abs($pos - $first_pos)] < $tol } {
-    set named_pos [lindex $pnames 0]
-  } elseif {$pos < $first_pos} {
-    set named_pos "before_[lindex $pnames 0]"
-  }
-  if {[expr abs($pos - $last_pos)] < $tol} {
-    set named_pos [lindex $pnames end]
-  } elseif {$pos > $last_pos} {
-    set named_pos "after_[lindex $pnames end]"
   }
 
-  set next_index 1
-  foreach name [lrange $pnames 0 end-1] posit [lrange $positions 0 end-1] {
+  set pos [ SplitReply [$pmot] ]
+  set tol [ SplitReply [$pmot precision] ]
+  set first_pos [lindex $positions 0]
+  set last_pos [lindex $positions end]
+  if {$last_pos < $first_pos} {
+    set p $last_pos
+    set last_pos $first_pos
+    set first_pos $p
+		set lstart [expr [llength $positions] - 1]
+		set lend 0
+  } else {
+		set lstart 0
+		set lend [expr [llength $positions] - 1]
+	}
+
+  if { [expr abs($pos - $first_pos)] < $tol } {
+    set named_pos [lindex $pnames $lstart]
+    return "$pmot = $named_pos"
+  } elseif {$pos < $first_pos} {
+    set named_pos "before_[lindex $pnames $lstart]"
+    return "$pmot = $named_pos"
+  }
+  if {[expr abs($pos - $last_pos)] < $tol} {
+    set named_pos [lindex $pnames $lend]
+    return "$pmot = $named_pos"
+  } elseif {$pos > $last_pos} {
+    set named_pos "after_[lindex $pnames $lend]"
+    return "$pmot = $named_pos"
+  }
+
+	set prev_posit [lindex $positions 0]
+	set prev_name [lindex $pnames 0]
+	if {[expr abs($pos - $prev_posit)] <= $tol} {
+		return "$pmot = $prev_name"
+	}
+	foreach name [lrange $pnames 1 end] posit [lrange $positions 1 end] {
     if {[expr abs($pos - $posit)] <= $tol} {
       set named_pos $name
-      break
-    } elseif {$pos > [expr {$posit + $tol}] && $pos < [expr {[lindex $positions $next_index] - $tol}]} {
-      set named_pos "between_${name}_[lindex $pnames $next_index]"
-      break
+			return "$pmot = $named_pos"
+		} else {
+			set diff1 [expr $prev_posit - $pos]
+			set diff2 [expr $posit - $pos]
+			if { [expr $diff1 * $diff2] < 0 } {
+				set named_pos "between_${name}_${prev_name}"
+				return "$pmot = $named_pos"
     }
-    incr next_index
   }
-  return "$pmot = $named_pos"
+		set prev_posit $posit
+		set prev_name $name
+	}
+	return $pmot = posname_unknown
 }
 publish posname user