#! /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-position"
    print >>sys.stderr, "Returns the current (x,y,z(,a)) "
    print >>sys.stderr, "position of the XYZ(A)" 
    print >>sys.stderr, "on the LSST instruments (in 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

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

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

position = xyz.get_position()

xyz.close()

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

print position.get('x'), position.get('y'), position.get('z'), \
    position.get('a', None), 

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