#!/usr/bin/env python3

import argparse, time, sys
from astropy.time import Time
from astropy.time import TimeDelta
from astropy import units as u
from astropy.coordinates import Angle

from legacyserver import LegacyServer

server = 'bigrock.atnf.csiro.au'
port = 2334

class Source:

    def __init__(self, name, RA, Dec):
        self.name = name
        self.RA = Angle(RA, unit=u.hour)
        self.Dec = Angle(Dec, unit=u.deg)

    def __str__(self):
        return("{}: ({},{})".format(self.name, self.RA.to_string(u.hour,precision=2,pad=True,sep=":"),
                                 self.Dec.to_string(u.deg,precision=1,sep=":")))
    def RArad(self):
        return(self.RA.rad)

    def Decrad(self):
        return(self.Dec.rad)

parser = argparse.ArgumentParser()
parser.add_argument('-fake', '--fake', help="Fake tracking", action="store_true")
parser.add_argument('-dur', '--dur', help="Length of scans in minutes", type=float, default=5.0)

args = parser.parse_args()

sources = [Source('Test1', '12:00:00', '-75:00:00'),
           Source('Test2', '14:00:00', '-80:00:00'),
           Source('Test3', '15:00:00', '-77:00:00')]

nsource = len(sources)

if nsource <=0:
    print("No Sources included")
    sys.exit(1)

tm = TimeDelta(args.dur * u.min)
    
s = LegacyServer((server,port), fake=args.fake)
s.connect()
s.allocate()
s.send_cmd('enable')
s.send_cmd('drvOn')

#now = Time.now()
#print("Next scan at {}".format(sched[0][0]))

# Go to first source

s.goto('CLOSEST', 'J2000Mean', str(sources[0].RArad()), str(sources[0].Decrad()))

time.sleep(5) 

mon = s.get_mon()
while mon.servo_state == 'SLEWING':
    print(mon, 'In goto')
    time.sleep(2)
    mon = s.get_mon()

isource = 0

while True:
    finish = Time.now()+tm
    print("Slewing to {}".format(sources[isource]))
    s.goto('CLOSEST', 'J2000Mean', str(sources[isource].RArad()), str(sources[isource].Decrad()))
    
    while Time.now()<finish:
        time.sleep(1)
        
    isource += 1
    if isource>=nsource:
        isource = 0