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

# TODO Handle sequencing when simultaneously moving multiple axes

AntiCollisionInstall
namespace eval anticollider {
  variable veto_region
}
array unset ::anticollider::veto_region
array set ::anticollider::veto_region ""

# Don't show 'acscript' call on error.  This is done by the anticollider
# module.
proc handle_acscript_exception {status message args} {
  switch $status {
    0 {
      # TCL_OK, This is raised when you just drop out of the
      # bottom of a 'catch' command.
      return -code ok
    }
    1 {
      # TCL_ERROR
      return -code error "$message: $args"
    }
    2 {
      # TCL_RETURN
      return -code return "$message"
    }
    3 {
      # TCL_BREAK
      return -code break
    }
    4 {
      # TCL_CONTINUE
      return -code continue
    }
    default {
    # Propogate user defined return codes with message
      return -code $status "$message"
    }
  }
}
##
# @brief Load an anticollider script
proc ::anticollider::loadscript {args} {
  variable veto_rules

  catch {
    set fh [open $::cfPath(anticollider)/[lindex $args 0] RDONLY ]
    while {[gets $fh line] >= 0} {
      # Skip empty lines and comments
      if [regexp {^\s*$|^ *#} $line] {
        continue
      }
      lappend veto_rules $line
    }
  }
  catch {
    close $fh
  }
}

##
# @brief Compile compile an anticollider declaration into a veto region table
# for the anticollider script.
#
# @param veto_rules, an anticollider declaration as a list of quote enclosed lines.
# @return Generates the ::anticollider::veto_region lookup table.
#
# Example\n
# forbid {160 167} for stth when mtth in {87 88}\n
# forbid { {0 15} {20 25} } for stth when mtth in { {80 90} {139.5 140.5} }\n
# for pcx forbid { {80 130} {-inf 10} }\n
# when mom in {0 45} forbid {{0 15} {345 360}} for pcr\n
# for sphi forbid { {0 5} {10 15} } when schi in { {5 10} {15 20} }\n
# forbid {-inf 5} when mtth in {0 10} for sphi\n
# forbid {0 10} for samx whenall { samrot in {0 5} samy in {0 15} }\n
proc ::anticollider::genveto {veto_rules} {
  variable veto_region
  array unset veto_region
  set lnum 1

  foreach line $veto_rules {
    array unset vp
    array set vp $line
    clientput "::anticollider::veto_rule: $line"

    if [info exists vp(whenall)] {
      foreach {mot in range} $vp(whenall) {
#       if {[llength [join $range]] != 2} {
#         error "ERROR: $range is not a valid range for $mot. Line $lnum of the veto list"
#       }
        lappend condlist $mot $range
      }
      lappend veto_region($vp(for)) [list $vp(forbid) @and $condlist]
      unset condlist
    } elseif [info exists vp(when)] {
      lappend veto_region($vp(for)) [list $vp(forbid) $vp(when) $vp(in)]
    } else {
      lappend veto_region($vp(for)) [list $vp(forbid) @any @all]
    }
    incr lnum
  }
}

##
# @brief Instrument specific configurations should re-implement this if they want
# to override the anticollision detection on some conditions
#
# @param args list of motorname target pairs
# @return "true" (default) enables anticollision detection
# @return "false" disables anticollision detection
proc ::anticollider::enable {args} {
  return "true"
}
##
# @brief Generic anti-collision script for simple collision avoidance.
#
# WARNING: This does not handle sequencing. Only run one motor at a time.
# This script requires that an ::anticollider::veto_region has been generated
# by the ::anticollider::genveto procedure.
#
# The ::anticollider::veto_region is a hash indexed by the names of the motors
# which have been registered with the anticollision module.
proc ::anticollider::veto_region_acscript {args} {
  variable veto_region

  set catch_status [ catch {
    foreach {regmot target} $args {
      if { ! [info exists veto_region($regmot)]  } {
        continue
      }
      foreach row  $veto_region($regmot) {
      if { [lindex $row 1] == "@and"} {
        set forbid [lindex $row 0]
        set veto 1
        foreach {mot range} [lindex $row 2] {
          set pos [SplitReply [$mot]]
          foreach {lower upper} [join $range] {
            if {$pos < $lower || $pos > $upper} {
              set veto 0
              break
            }
          }
        }
        if {!$veto} {
          continue
        } else {
          foreach {min max} $forbid {}
          if {$min <= $target && $target <= $max} {
            foreach {mot range} [lindex $row 2] {
              lappend msg [list $mot in $range]
            }
            error "ERROR:The range ($forbid) is forbidden for $regmot when [join $msg { and }]"
          }
        }
      } else {
        foreach {forbidden_range obstmot obstrange} $row {
          if {$obstmot == "@any"} {
            if {$obstrange == "@all"} {
              foreach {min max} [join $forbidden_range] {
                if {$min <= $target && $target <= $max} {
                  error "ERROR: $regmot target ($target) is in the forbidden region ($forbidden_range)"
                }
              }
            } else {
              error "ERROR: veto table must use @all with @any"
            }
          } else {
            if {$obstrange == "@all"} {
              error "ERROR: veto table must use @any with @all"
            } else {
              foreach {lower upper} [join $obstrange] {
                set pos [SplitReply [$obstmot]]
                if {$lower <= $pos && $pos <= $upper} {
                  foreach {min max} [join $forbidden_range] {
                    if {$min <= $target && $target <= $max} {
                      error "ERROR:The range $min to $max is forbidden for $regmot when $obstmot is in this region ($obstrange)"

                    }
                  }
                }
              }
            }
          }
        }
      }
      }
    }
  } message ]
  handle_exception $catch_status $message
}

##
# @brief Generate anticollider veto_region and register motors with anticollider
proc ::anticollider::init {} {
  variable evp
  variable veto_region

  set catch_status [ catch {
    if { ![info exists ::anticollider::veto_rules] } {
      return
    }
    clientput Load anticollider rules:
    if { [info procs ::anticollider::load_acrules] == "::anticollider::load_acrules" } {
      ::anticollider::load_acrules
    }
    ::anticollider::genveto $::anticollider::veto_rules
    foreach motor [array names veto_region] {
      anticollision register $motor
    }
  } message ]
  handle_exception $catch_status $message
}

lappend ::anticollider::scripts ::anticollider::veto_region_acscript
proc ::anticollider::acscript {args} {
  set catch_status [ catch {
    foreach {regmot target} $args {
      anticollision add 0 $regmot $target
    }
    if {[::anticollider::enable $args] == "true"} {
      foreach script $::anticollider::scripts {
        $script {*}$args
      }
    }
  } message ]
  handle_acscript_exception $catch_status $message
}

publish ::anticollider::acscript user
anticollision script ::anticollider::acscript