#! /usr/bin/env python

import sys, os, os.path
import getopt
import time

# import lsst.instruments.pollux as ltp
import lsst.instruments.pollux.xyz as ltpxyz

# --------- Parse command line arguments ------------------

def usage():
    print >>sys.stderr, "usage: xyz-move"
    print >>sys.stderr, "         a=position | da=offset"
    print >>sys.stderr, "Move axis 'a' (X,Y,Z) to position 'position'" 
    print >>sys.stderr, "on the LSST test bench (unit is mm)."

try:
    opts, args = \
        getopt.getopt(sys.argv[1:], "hv", \
                          ["help", "verbose"])
except getopt.GetoptError:
    # print help information and exit
    usage()
    sys.exit(1)
        
# print opts
# print args

verbose = False

for option, arg in opts:
    if option in ("-h", "--help"):
        help()
        sys.exit(0)
    if option in ("-v", "--verbose"):
        verbose = True

if len(args) < 1:
    usage()
    sys.exit(1)

# ----- Analyse each provided argument --------------------

commands = []

for arg in args:
    if '=' not in arg:
        print >>sys.stderr, "error: invalid argument [%s]" % arg
        usage()
        sys.exit(2)
    eqparts = arg.split('=')
    if len(eqparts) != 2:
        print >>sys.stderr, "error: invalid argument format [%s]" % arg
        usage()
        sys.exit(3)
    
    axis_spec = eqparts[0].lower()
    if len(axis_spec) < 1:
        print >>sys.stderr, "error: invalid argument format (axis) [%s]" % arg
        usage()
        sys.exit(4)
    relative = False
    if axis_spec[0] == 'd': # relative
        relative = True
        if len(axis_spec) != 2:
            print >>sys.stderr, \
                "error: invalid argument format (axis) [%s]" % arg
            usage()
            sys.exit(5)
    axis = axis_spec[-1].lower()
    if axis not in ['x', 'y', 'z']:
        print >>sys.stderr, \
            "error: invalid argument format (unknown axis) [%s]" % arg
        usage()
        sys.exit(6)
        
    value = eqparts[1]
    if len(value) < 1:
        print >>sys.stderr, "error: invalid argument format (value) [%s]" % arg
        usage()
        sys.exit(4)

    value = float(value)
        
    commands.append(dict({ 'axis': axis,
                           'relative': relative,
                           'value': value }))

if verbose:
    print >>sys.stderr, commands

# ----- Init testbench motors, send request, then close ---

xyz = ltpxyz.XYZ(debug = verbose)
xyz.open()
xyz.setup()

for command in commands:
    arguments = { 'check': True, 'wait': True }
    if command['relative']:
        arguments['d%s' % command['axis']] = command['value']
    else:
        arguments['%s'  % command['axis']] = command['value']
    if verbose:
        print >>sys.stderr, arguments

    xyz.move(**arguments)

xyz.close()

# ---------------------------------------------------------