#!/usr/bin/env python3

import time, logging, sys
from astropy import units as u
from astropy.coordinates import Angle
from legacyserver import LegacyServer

#parser = argparse.ArgumentParser()
#parser.add_argument('-fake', '--fake', help="Fake tracking", action="store_true")
#parser.add_argument('-debug', '--debug', help="Enable debug messages", action="store_true")
#parser.add_argument('-stow', '--stow', help="Stow antenna", action="store_true")
#parser.add_argument('-park', '--park', help="Park antenna", action="store_true")
#parser.add_argument('-azel', '--azel', help="Slew telescope to given Az/El", action="store_true")
#parser.add_argument('arg1', metavar='-arg1', nargs='?', default=None, help="First optional argument with leading dash")
#parser.add_argument('arg2', metavar='-arg2', nargs='?', default=None, help="Second optional argument with leading dash")

#parser.add_argument('coords', nargs='*')

#args = parser.parse_args()

debug = False
fake = False
stow =  False
park =  False
azel =  False
J2000 = True


if len(sys.argv)==1: # Need to pass something
    print("""
Usage: mopraGoto.py [-azel] [-stow] [-park] [left right]

Slew Mopra to given coordinate, or Park/Stow antenna. 
Default will interperate left and right as J2000 coordinates.

E.g.:

  mopraGoto.py -stow
  mopraGoto.py --park
  mopraGoto.py 16:00:30 -45:20:10
  mopraGoto.py -azel 70:00:00 30:00:00

""")
    sys.exit(0)

# Need to roll our own command line parsing, to allow negative numbers
def matchArg(a,b):
    if a=='-'+b or a=='--'+b:
        return True
    else:
        return False

args = []
for a in sys.argv[1:]:
    if matchArg(a,'stow'):
        stow = True
        continue
    if matchArg(a,'park'):
        park = True
        continue
    if matchArg(a,'azel'):
        azel = True
        continue
    if matchArg(a,'debug'):
        debug = True
        continue
    if matchArg(a,'fake'):
        fake = True
        continue

    args.append(a)

if debug:
    logging.basicConfig(level=logging.DEBUG)
else:
    logging.basicConfig(level=logging.INFO)

if azel or stow or park:
    J2000 = False

if azel + stow + park >= 2:
    print("Error: Only pass one of -stow, -park and -azel")
    sys.exit(1)

if azel or J2000:
    if len(args) != 2:
        print("Need to pass left and right coordinates")
        sys.exit(1)
        
    if J2000:
        left = Angle(args[0], unit=u.hour)
    else: 
        left = Angle(args[0], unit=u.deg)

    right = Angle(args[1], unit=u.deg)

    print(left, right)
    
server = 'bigrock.atnf.csiro.au'
port = 2334

if debug:
    logging.basicConfig(level=logging.DEBUG)
else:
    logging.basicConfig(level=logging.INFO)

# Connect to antenna
s = LegacyServer((server,port), fake=fake)
s.connect()
s.allocate()
s.enable()
s.drvOn()

if stow:
    s.stow()
    slewState = 'STOWING'
    slewFinished = 'STOWED'
elif park:
    s.park()
    slewState = 'PARKING'
    slewFinished = 'PARKED'
else:
    if J2000:
        s.goto('CLOSEST', 'J2000Mean', str(left.rad), str(right.rad))
    else:
        s.goto('CLOSEST', 'AzElApp', str(left.rad), str(right.rad))
    slewState = 'SLEWING'
    slewFinished = 'TRACKING'

print("Waiting for command to complete")
time.sleep(10) # Give time for commands to get to ACC

# Wait till command completed
update = 5 # Check every 5 seconds        
try:
    while True:
        lastupdate = time.time()
        mon = s.get_mon()

        state = mon.servo_state

        if state == slewFinished:
            print("Done")
            break

        if state != slewState:
            print("Error: Unexpected drive state '{}'. Aborting".format(state))
            sys.exit(1)
           
        time.sleep(update-(time.time()-lastupdate))

except KeyboardInterrupt:
    print("Command aborted via keyboard")

if J2000:
    print("^C script to stop tracking")
    try:
        while True:
            time.sleep(600)
    except KeyboardInterrupt:
        print("Command stopped via keyboard")

s.deallocate()