# Author: Ferdi Franceschini (ffr@ansto.gov.au)

fileeval $cfPath(anticollider)/anticollider_common.tcl 

namespace eval ::anticollider {
  set guide_posit 1
  set aperture_posit 2
  set apD10_posit 1
  set coltol 0.5
  set aptol 0.1

  for {set n 1} {$n < 8} {incr n} {
    foreach {col ap} "col$n ap$n" {
      set col_limit [expr [SplitReply [$col posit2unit $aperture_posit]] - $coltol]
      set ap_limit [expr [SplitReply [$ap posit2unit $apD10_posit]] - $aptol]
      set acrule [subst {forbid {-inf inf} for $ap when $col in {-inf $col_limit} }]
      lappend ::anticollider::veto_rules $acrule
      set acrule [subst {forbid {-inf inf} for $col when $ap in {$ap_limit inf} }]
      lappend ::anticollider::veto_rules $acrule
      anticollision register $col
      anticollision register $ap
    }
  }

  proc ::anticollider::load_acrules {} {
    variable proximity_rules
    # Proximity rules have the following form,
    #   m1 m2 mdist
    # This means that the position of m2 is always >= m1 and that a drive or
    # run command which tries to drive them closer than mdist will be rejected.
    # TODO Abort driving motors registered with the anticollider if one of them
    #      fails during the drive.
# TESTING WITH SLITS, uncomment this block and the foreach block to test with slits
#   set proximity_rules {
#     ss1l ss1r 10
#   }
  # TODO UNCOMMENT FOLLOWING TO ENABLE ANTICOLLISION FOR DETECTOR CARRIAGES
  # # Detector carriage proximity rules.
  # # det1 = curtain detector
  # # det2 = main detector which is at higher y-pos then det1
  # set proximity_rules {
  #   det1 det2 2000
  # }
  # foreach {m1 m2 minsep} $proximity_rules {
  #   anticollision register $m1
  #   anticollision register $m2
  #   clientput "::anticollider::proximity_rule: $m1 $m2 minsep = $minsep"
  # }
  }

  # @brief Don't allow two axes to get closer than a minimum separation given in the 'proximity_rules' list.
  # Assumes that both objects are on the same open track
  # Both axes are required to move at the same speed with the same accel and decel values.
  #   NOTE: Doesn't check that the speeds and accel set by the Galil match what is requested.
  # If axes are within the minimum distance then you can only drive them
  # apart when the targets are greater than the minimum allowed separation.
  proc proximity_script {args} {
    set catch_status [ catch {
      variable proximity_rules

      if {[info exists proximity_rules] == 0} {
        return
      } elseif {[len $proximity_rules] == 0} {
        return
      }
      foreach {m1 m2 mdist} $proximity_rules {
        set final($m1) [SplitReply [$m1]]
        set final($m2) [SplitReply [$m2]]
      }
      foreach {mot val} $args {
        set final($mot) $val
      }
      foreach {mot val} $args {
        set rulenum 1
        foreach {m1 m2 mdist} $proximity_rules {
          if {$m1 != $mot && $m2 != $mot} {
            continue
          }
          if {$mdist <=0} {
            error "Minimum separation for $m1 and $m2 must be > 0 in proximity_rule $rulenum"
          }
          set speed($m1) [SplitReply [$m1   speed]]
          set accel($m1) [SplitReply [$m1   accel]]
          set decel($m1) [SplitReply [$m1   decel]]
          set pos($m1)   [SplitReply [$m1]]

          set speed($m2) [SplitReply [$m2   speed]]
          set accel($m2) [SplitReply [$m2   accel]]
          set decel($m2) [SplitReply [$m2   decel]]
          set pos($m2)   [SplitReply [$m2]]
          # NOTE Speed and accel tests assume that the Galil actually sets the
          # speeds and accelerations to the same expected steps after taking gear
          # ratios, transmission, (something else?) into account on both axes
          # when we drive the motors.
          if {$speed($m1) != $speed($m2)} {
            error "$m1 and $m2 speeds must be equal to ensure they won't collide"
          }
          if {$accel($m1) != $accel($m2)} {
            error "$m1 and $m2 accelerations must be equal to ensure they won't collide"
          }
          if {$decel($m1) != $decel($m2)} {
            error "$m1 and $m2 decelerations must be equal to ensure they won't collide"
          }
          if {$pos($m2) - $pos($m1) < 0} {
            error "Rule($rulenum): $m1 $m2 $mdist seems to be invalid.  It assumes that $m2 pos is always >= $m1 pos but $m1 is at $pos($m1) and $m2 is at $pos($m2)"
          }
          if {$final($m2) - $final($m1) < $mdist} {
            error "$m1 and $m2 will violate rule:$rulenum, the minimum allowed separation is positive $mdist"
          }
          incr rulenum
        }
      }
    } message ]
    handle_exception $catch_status $message
  }
  lappend ::anticollider::scripts ::anticollider::proximity_script
}

# NOTE: This is called with a list of motorname target pairs
proc ::anticollider::enable {args} {
  if {[SplitReply [::anticollider::protect_detector]] == "false"} {
    return "false"
  } else {
    return "true"
  }
}

::anticollider::loadscript acscript.txt