#--------------------------------------------------------------------------
# test and example script for the anticollider 
#
# Mark Koennecke, August 2002
#------------------------------------------------------------------------

proc testlimits tg {
    upvar $tg targets
    if { abs( $targets(om) - $targets(stt)) < 30 } {
	error "!!!!! two theta - omega CRASH!!!!!!!"
    }
    if {$targets(chi) > 190.} {
	error "chi upperlimit crashed"
    }
    if { $targets(om) > -90 && $targets(om) <= -81.5 && $targets(chi) < 152} {
	error "!!!!!! chi - omega CRASH aborted !!!!!"
    }
    if { $targets(om) > -81.5 && $targets(om) <= -55 && $targets(chi) < 137} {
	error "!!!!!! chi - omega CRASH aborted !!!!!"
    }
    if { $targets(om) > -55 && $targets(om) <= -52 && $targets(chi) < 132} {
	error "!!!!!! chi - omega CRASH aborted !!!!!"
    }
    if { $targets(om) > -52 && $targets(om) <= -30 && $targets(chi) < 75} {
	error "!!!!!! chi - omega CRASH aborted !!!!!"
    }
    return
}
#-------------------------------------------------------------------------
proc chiFirst? tg {
    upvar $tg targets
    set om [SplitReply [om]]
    set chi [SplitReply [chi]]
    if {$chi < $targets(chi) } {
	return 1
    } else {
	return 0
    }
}
#---------------------------------------------------------------------------
proc collidertest args {
#----------- read command line targets
    set entries [expr [llength $args] / 2]
    for {set i 0} {$i < $entries} {incr i} {
	set ind [expr $i * 2]
	set targets([lindex $args $ind]) [lindex $args [expr $ind +1]]
    }
#--------- check if all motors are there. If not get targets from
#          current position
    if { [info exists targets(om)] == 0} {
	set targets(om) [SplitReply [om]]
    } 
    if { [info exists targets(stt)] == 0} {
	set targets(stt) [SplitReply [stt]]
    } 
    if { [info exists targets(chi)] == 0} {
	set targets(chi) [SplitReply [chi]]
    } 
    if { [info exists targets(phi)] == 0} {
	set targets(phi) [SplitReply [phi]]
    } 
#---------- proceed to real collision detection hydraulics
#           first: test complex limits 
    set ret [catch {testlimits targets} msg]
    if {$ret != 0} {
	clientput [format "ERROR %s" $msg]
	error $msg
    }
    anticollision add 1 stt $targets(stt)

    if { [chiFirst? targets] == 1} {
	anticollision add 2 chi $targets(chi)
	anticollision add 3 om $targets(om)
    } else {
	anticollision add 2 om $targets(om)
	anticollision add 3 chi $targets(chi)
    }
    anticollision add 3 phi $targets(phi)
}