#!/bin/csh -f
#+
#
# xy x y
#
# moves the telescope x arcsec, y arcsec in detector coordinates.
#
#-
#
# 20051122 - MB: added -noask flag to wfao
# 20060926 - jlyke: added long workaround to FSM faults caused by 
#                   large moves (max following error exceeded)
# 20180527 - jlyke: workaround for TCSU unable to handle 0 0 offsets
# 20210626 - jlyke: Force FSM moves to hope fix the offsetting issues; 
#                   Did not add this part to the breakup of large moves
# 20210901 - jlyke: Force TSS moves similar to FSM, add both to breakup
#                   of large moves, and break up moves into 3 modify's
#

# Boiler plate for "-h" support for command autohelp.

if ("$1" == "-h") then
    help $0
    exit $status
endif

# Boilerplate for syncheck.
# Note that the boiler plate should be transparent for all usages,
# but to make correct use of syncheck you will need to specify the
# correct pattern.

 set noglob
 set CheckStatus = `syncheck -command $0 $* -pattern float float` 
 unset noglob

if ("$CheckStatus" != "OK") then
    help $0
    exit 1
endif

# End of help/syncheck boiler plate.

# set max size of a single move (must be integer)
set threshold = 90
# define zero for comparisons
set format = "%.3f\n"
set z = 0
set zero = `printf "$format" $z`
set negz = `echo -$zero`

if (${#argv} == 2) then
  # check for 0 0 offsets and exit
  set xtotal = `printf "$format" $1`
  set ytotal = `printf "$format" $2`
  
  if ( $xtotal == "$zero" || $xtotal == "$negz" ) then
    if ( $ytotal == "$zero" || $ytotal == "$negz" ) then
      # both offsets are zero...skip
      exit
    endif
  endif

  set dmstat = `show -s ao -terse aodmstat`
  set dtstat = `show -s ao -terse aodtstat`
  set aofmmove = `show -s ao -terse aofmmove`

  if ( $dtstat == "closed" && $aofmmove == "true" ) then
    # moving in steps requires wait4tt or wait4ao to be on
    wait4ao save
    if ( $dmstat == "closed" ) then
      wait4ao on
    else
      wait4tt on
    endif


    #################################################
    ### handle negative numbers
    #################################################
    set xsign = `echo $xtotal | cut -c 1`
    set ysign = `echo $ytotal | cut -c 1`

    if ( $xsign == - ) then
      set xsign = -1.0
      set xtotal = `math -1 x $xtotal`
    else
      set xsign = 1.0
    endif

    if ( $ysign == - ) then
      set ysign = -1.0
      set ytotal = `math -1 x $ytotal`
    else
      set ysign = 1.0
    endif

    #################################################
    ### csh requires integers for comparison
    ### build up offset lists
    #################################################
    set xint = `echo $xtotal | cut -f 1 -d .`
    set yint = `echo $ytotal | cut -f 1 -d .`

    if ( $xint < $threshold ) then
      set xoff = `math $xsign x $xtotal`
    else
      set xoff = ""
      while ( $xint > $threshold ) 
        set xoff = ( $xoff `math $xsign x $threshold` ) 
        set xtotal = `math $xtotal - $threshold`
        set xint = `echo $xtotal | cut -f 1 -d .`
      end
      set xoff = ( $xoff `math $xsign x $xtotal` )
    endif

    if ( $yint < $threshold ) then
      set yoff = `math $ysign x $ytotal`
    else
      set yoff = ""
      while ( $yint > $threshold ) 
        set yoff = ( $yoff `math $ysign x $threshold` ) 
        set ytotal = `math $ytotal - $threshold`
        set yint = `echo $ytotal | cut -f 1 -d .`
      end
      set yoff = ( $yoff `math $ysign x $ytotal` )
    endif

    #################################################
    ### now we need lists of equal length
    #################################################
    while ( ${#xoff} != ${#yoff} ) 
    if ( ${#xoff} > ${#yoff} ) then
      while ( ${#xoff} != ${#yoff} )
        set yoff = ( $yoff 0.0 )
      end
    else
      while ( ${#xoff} != ${#yoff} )
        set xoff = ( $xoff 0.0 )
      end
    endif
    end

    #################################################
    ### now send the moves
    #################################################
    wfao -noask
    while ( ${#xoff} > 0 ) 
      modify -s dcs silent instxoff=$xoff[1] 
      modify -s dcs silent instyoff=$yoff[1] 
      modify -s dcs silent rel2curr=t
      sleep 2
      modify -s ao obfmgoim=1
      modify -s ao obtsgo=1
      wfao
      shift xoff
      shift yoff
    end
    wait4ao restore
    #################################################
    ### else to if ( $dtstat == "closed" ...
    #################################################
  else
    set x = $1
    set y = $2
    wfao -noask
    modify -s dcs silent instxoff=$x 
    modify -s dcs silent instyoff=$y 
    modify -s dcs silent rel2curr=t
    sleep 2
    waitfor -s dcs axestat=tracking
    modify -s ao obfmgoim=1    
    modify -s ao obtsgo=1
  endif
else
  echo "Usage: xy x y"
endif