#! /bin/tcsh -f
#
#	operate the detector 2theta angle 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
set tempfile = /tmp/twotheta_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 = 0
	continue
    endif
    # interpret numbers as the desired new detector angle (in deg)
    set goal = `echo $arg | awk '$1 ~ /^[0-9-]/ || /^.[0-9]/{print $1+0}' | awk '$1>-10 && $1<45'`
end


onintr finish

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

# 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 twotheta in degrees
set dist = `awk '/^stog_configure_real_motor detector_z/{print $5}' ${tempfile}`
set pos = `awk '/^stog_configure_real_motor detector_twotheta/{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 twotheta in degrees
set dist = `awk '/^stog_configure_real_motor detector_z/{print $5}' ${tempfile}`
set pos = `awk '/^stog_configure_real_motor detector_2theta/{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 angle
if(! $?move) goto finish




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

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

# sanity check on goal position
set dist = `echo $dist | awk '{printf "%d", $1+0}'`
set igoal = `echo $goal | awk '{printf "%d", $1+0}'`
if ( $igoal > 20 && $dist > 300) then
#   echo "moving to a safe distance first"
#    distance.com 300
endif
if ( $igoal < 20 && $dist < 200) then
#   echo "moving to a safe distance first"
#    distance.com 200
endif

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

sleep 1
set speed = 1
while ("$speed" != "0")
    # now check and see where they are
    set info = `echo -n "#2p\r#2v\r#1p\r#1v\r" | sock_exchange.tcl $pmac 14001 4 "\006" | awk '{gsub("[\006\r]"," ",$0); print}'`
    if($#info != 4) 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/2437.14+0}'`
    set speed   = "$info[2]"
    set dpos    = `echo "$info $dist" | awk '{printf "%.3f", $3/1000+0}'`
    set dspeed  = `echo "$info" | awk '{printf "%d", $4*100}'`

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

    if( $dspeed != 0) then
	# shit! command distance to stay in place!
	echo -n "#1j/\r" | sock_exchange.tcl $pmac 14001 1 "\006" > /dev/null
    endif

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

# see if we made it
set delta = `echo "$goal $reached" | awk '{printf "%d", 2437.14*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 "angle is $delta deg off the mark"
    echo "trying again..."
    echo -n "#1j/#2j/\r" | sock_exchange.tcl $pmac 14001 2 "\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_twotheta $goal" |\
xos3_exchange.tcl 1 "motor_move_completed detector_twotheta"  30 |&\
awk '/update_motor_position detector_twotheta/{printf "%.3f\n", $3}'
if(! $status) then
    unset move
    goto get_position
endif

# convert goal twotheta 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_2theta $xosgoal" |\
xos_exchange.tcl $control 1 "motor_move_completed detector_2theta" |\
awk '/detector_2theta/{printf "%.3f\n", $3}'
if($status) goto finish

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 "%s deg", $1}'`
endif

echo "detector 2theta is now $pos"

if($?BAD) exit 9
exit