#! /bin/tcsh -f
#
#	operate the detector distance using XOS or the direct delta-tau connection
#
#

if(! $?beamline) then
    # default to beamline this computer belongs to
    setenv beamline `beamline.com`
    if($status) then
	echo "ERROR: $beamline"
	exit 9
    endif
endif
#if(! $?beamline) setenv beamline 831
set tempfile = /tmp/distance_temp$$
set pmac = pmac1
set control = bl${beamline}
if ( $beamline !~ 8* ) then
    set pmac = bl${beamline}
    set control = bl${beamline}c
endif

# default to absolute move
unset relative


# interpret command line first
if("$1" != "") set move

set goal = ""
foreach arg ( $* )
    if("$arg" == "by") then
	set relative
	continue
    endif
    if("$arg" == "to") then
	unset relative
	continue
    endif
    if("$arg" == "park") then
	set goal = 600
	continue
    endif
    # interpret numbers as the desired new detector distance (in mm)
    set goal = `echo $arg | awk '$1 ~ /^[0-9-]/ || /^.[0-9]/{print $1+0}' | awk '$1>60 && $1<=2000'`
end


onintr finish

get_position:
# query the motor controller for the detector's current distance in mm
set pos = `echo -n "#1p\r" | sock_exchange.tcl $pmac 14001 1 "\006" |& awk '{gsub("[\006\r]"," ",$0); print}' | awk 'NF==1{print $1/1000+0}'`

# see if deltatau responded amicably
if($#pos == 1) then
    if($?move) goto move
    goto finish
endif
# didn't work, try XOS
set pos = ""



# pretend to be a gui, get motor state, and then disconnect
echo -n "" |\
 xos3_exchange.tcl 1 "set_motor_dependency" 1 >&! ${tempfile}

# retreive detector's absolute distance in mm
set pos = `awk '/^stog_configure_real_motor detector_z/{print $5}' ${tempfile}`
rm -f ${tempfile} >& /dev/null

# see if DCSS responded amicably
if($#pos == 1) then
    # we got a value for detector
    if($?move) goto move
    goto finish
endif
# didn't work, try something else?
set pos = ""


# pretend to be a gui, get motor state, and then disconnect
echo -n "" |\
 xos_exchange.tcl $control 1 "over out" 1 >! ${tempfile}

# retreive detector's absolute distance in mm
set pos = `awk '/^stog_configure_real_motor detector_z/{print $5}' ${tempfile}`
rm -f ${tempfile} >& /dev/null

# see if XOS responded amicably
if($#pos == 1) then
    # we got a value for detector
    if($?move) goto move
    goto finish
endif
# didn't work, try something else?
set pos = ""


# jump ahead if all we need to do is report the distance
if(! $?move) goto finish




move:
if(! $?pos) set pos = ""
if(! $?goal) set goal = ""
if("$goal" == "") goto finish

# compute new desired distance in mm
if($?relative) then
    set goal = `echo "$pos $goal" | awk '{print ($1+$2)}'`
endif

# command the motor to move!
try_again:
echo "$goal" | awk '{printf "I122=20\r#1j=%f\r", 1000*($1-0)}' |\
  sock_exchange.tcl $pmac 14001 2 "\006" >& /dev/null
if($status) goto XOS_move

usleep 100000
set speed = 1
while ("$speed" != "0")
    # now check and see where they are
    set info = `echo -n "#1p\r#1v\r" | sock_exchange.tcl $pmac 14001 2 "\006" | awk '{gsub("[\006\r]"," ",$0); print}'`
    if($#info != 2) then
	echo "problem with network : $info"
	#echo "mcf\n8\n" | sock_exchange.tcl $pmac 9999 > /dev/null
	sleep 2
	continue
    endif
    set reached = `echo "$info" | awk '{printf "%.3f", $1/1000+0}'`
    set speed   = `echo "$info" | awk '{printf "%.0f", $2*4}'`

    # make the damn thing slow down when it gets close
    if ( `echo "$reached $speed" | awk '{print ($1+0<250 && $2<0)}'` ) then
	# re-command the move at reduced speed
	echo "$goal" | awk '{printf "I122=5\r#1j=%f\rI122=20\r", 1000*($1-0)}' |\
  	  sock_exchange.tcl $pmac 14001 2 "\006" >& /dev/null
    endif

    if($?debug) echo "GOTHERE $info $speed" 

    echo "$reached"
    if("$speed" == "0") continue
    usleep 500000
end

# see if we made it
set delta = `echo "$goal $reached" | awk '{printf "%d", 1000*sqrt(($1-$2)^2)}'`
if(($delta > 1)&&($?RETRY)) then
    # finite number of retries 
    if("$RETRY" =~ *[0-9]) set RETRY = `echo $RETRY | awk '{printf "%d", $1-1}'`
    if("$RETRY" == "0") set RETRY = ""
    if("$RETRY" == "") unset RETRY
    set delta = `echo "$goal  $info" | awk '{print $1-$2}'`
    echo "distance is $delta mm off the mark"
    echo "trying again..."
    echo -n "#1j/\r" | sock_exchange.tcl $pmac 14001 1 "\006" 3 > /dev/null
    goto try_again
endif
unset move
goto get_position


XOS_move:
# pretend to be a gui, initiate motor move, and disconnect when it is done
echo "gtos_become_master force\ngtos_start_motor_move detector_z $goal" |\
xos3_exchange.tcl 1 "motor_move_completed detector_z"  30 |&\
awk '/update_motor_position detector_z/{printf "%.1f\n", $3}'
if(! $status) then
    unset move
    goto get_position
endif

# convert goal distance to some other units?
set xosgoal = `echo $goal | awk '{print $1}'`

# pretend to be a gui, initiate motor move, and disconnect when it is done
echo "gtos_become_master force\ngtos_start_motor_move detector_z $xosgoal" |\
xos_exchange.tcl $control 1 "motor_move_completed detector_z"  30 |\
awk '/update_motor_position detector_z/{printf "%.1f\n", $3}'

unset move
goto get_position




finish:
if("$goal" != "") then
    # we were going somewhere, did we get there?
    set toofar = `echo $goal $pos | awk '{print (NF!=2 || sqrt(($1-$2)^2)>0.1)}'`
    if($toofar) set BAD
endif
if("$pos" == "") then
    set pos = "unknown"
else
    set pos = `echo "$pos" | awk '{printf "%.1f mm", $1}'`
endif

echo "detector distance is now $pos"

if($?BAD) exit 9
exit