#! /bin/tcsh -f
#
#	operate the horizontal stage motor 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 pmac = pmac1
if ( $beamline != 831 ) set pmac = bl${beamline}
set control = bl${beamline}c


set tempfile = /tmp/hstage_temp$$

# 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
    # interpret numbers as the desired new stage position (in mm)
    set goal = `echo $arg | awk '$1 ~ /^[0-9-]/ || /^.[0-9]/{print $1+0}' | awk '$1<20 && $1>-10'`
end

onintr report

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

# 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 stage's absolute position in mm
set pos = `awk '/^stog_configure_real_motor horizontal_collimator/{print $5}' ${tempfile}`
rm -f ${tempfile} >& /dev/null

if($#pos == 1) goto got_position

# try LBL/DCS
echo -n "" |\
xos_exchange.tcl $control 1 "over out" 1 >! ${tempfile}

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

# see if XOS responded amicably
if($#pos == 1) goto got_position

# didn't work, try something else?
echo "XOS failed! "
set pos = ""

got_position:
# jump ahead if all we need to do is report the position
if($?move) goto move
goto finish




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

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

# see if "goal" is not really a move
if("$goal" == "zero") then
    # just store the new zero position
    goto finish
endif

# command the motor to move!
try_again:
echo "$goal" | awk '/zero/{exit} {printf "#3j=%f\r", 2000*$1}' |\
  sock_exchange.tcl $pmac 14001 1 "\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 "#3p\r#3v\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/2000}'`
    set speed   = "$info[2]"

    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 "stage is $delta mm off the mark"
    echo "trying again..."
    echo -n "#3j/\r" | sock_exchange.tcl $pmac 14001 1 "\006" 3 > /dev/null
    goto try_again
endif
unset move
goto get_position


XOS_move:
# convert goal position to some other units?
set xosgoal = `echo $goal | awk '{print 1*$1}'`
unset move

# pretend to be a gui, set motor states, and then disconnect
echo "gtos_become_master force\ngtos_start_motor_move horizontal_collimator $xosgoal" |\
xos3_exchange.tcl 1 "stog_motor_move_completed horizontal_collimator" 30 |&\
tee ${tempfile} |\
awk '/update_motor_position horizontal_collimator/{printf "%.3f\n", $3}'
set pos = `awk '/completed horizontal_collimator/{print $3}' ${tempfile} | tail -1`
rm -f ${tempfile}
if ("$pos" != "") goto finish

# try LBL/DCS
set dcsgoal = `echo $goal | awk '{print 1000*$1}'`
if($beamline == 831) set dcsgoal = `echo $goal | awk '{print 10000*$1}'`

echo "gtos_become_master force\ngtos_start_motor_move horizontal_collimator $dcsgoal" |\
xos_exchange.tcl $control 1 "stog_motor_move_completed horizontal_collimator" 30 |\
tee ${tempfile} |\
awk '/update_motor_position horizontal_collimator/{printf "%.3f\n", $3}'
set pos = `tail -1 ${tempfile}`
rm -f ${tempfile}
if("$pos" != "") goto get_position

unset move
goto get_position


report:


finish:
if("$goal" == "zero") then
    change.com horizontal stage zero $pos
endif
if("$pos" == "") then
    set pos = "unknown"
else
    if($?goal) then
	set miss = `echo "$goal $pos" | awk '{miss=sqrt(($1-$2)^2)} miss>0.01{print $1-$1}'`
	if("$miss" != "") set BAD
    endif
    set pos = `echo "$pos" | awk '{printf "%.3f mm", $1}'`
endif

echo "horizontal position is now $pos"


if($?BAD) exit 9
exit