#!/usr/bin/python2.5

import matplotlib.pylab as mplot
import os
import ephem
import math
from datetime import *
from array import *

# Main routine if we are called as a script
if __name__ == "__main__":


  #DEFAULT VALUES FOR KEYWORDS
  DateTime        = '2012/01/01 20:20:00'

  #Format the time appropriately
  Year  = int(DateTime[0:4])
  Month = int(DateTime[5:7])
  Day   = int(DateTime[8:10])
  Hours = int(DateTime[11:13])
  Mins  = int(DateTime[14:16])
  Secs  = int(DateTime[17:19])
  DateTime = datetime(Year, Month, Day, Hours, Mins, Secs)

  #Convert this into a PyEphem Date (# days since noon on 1899 December 31) 
  EpDate_0 = ephem.date(DateTime)
  EpDate   = EpDate_0
  EpDate   = EpDate #+ ephem.second*0.01*97000

  #Form the satellite object
  tle1 = 'CXBN'
  tle2 = '1 99999U          11158.66666667  .00001767  00000-0  83150-4 0 00005'
  tle3 = '2 99999 060.0472 001.9175 0286684 357.7762 001.3936 14.89594857000013'
  CXBN = ephem.readtle(tle1,tle2,tle3)

  #Set the time increment for the propogation
  dt       = 0.01                  #Time difference in seconds
  t_final  = 1*6000.                #Final time in seconds
  NumTimes = t_final/dt
  Times    = range(int(NumTimes))

  #Set the initial time to t=0
  Time=0

  #Get the initial RA and DEC
  CXBN.compute(EpDate)
  RA_rad       = float(CXBN.ra)
  RA_rad_prev  = float(CXBN.ra)
  DEC_rad_prev = float(CXBN.dec)
  DEC_rad      = float(CXBN.dec)

  #Set up arrays
  RA_arr  = array('f')
  DEC_arr = array('f')
  Vel_arr = array('f')

  #Now convert to ECI
  Radius  = float(CXBN.elevation+6738100.)
  x_eci_prev = Radius*math.cos(DEC_rad)*math.cos(RA_rad)
  y_eci_prev = Radius*math.cos(DEC_rad)*math.sin(RA_rad)
  z_eci_prev = Radius*math.sin(DEC_rad)

  #############################################################################################################
  #PROPAGATE THE SATELLITE IN ITS ORBIT
  #############################################################################################################
  for i in Times:  

    #Inrement the time 
    EpDate = EpDate + ephem.second*dt
    Time  += dt

    #Recompute the solution with the SGP4
    CXBN.compute(EpDate)

    #First get the RA and DEC from satellite object
    RA_rad  = float(CXBN.ra)
    DEC_rad = float(CXBN.dec)
    Radius  = float(CXBN.elevation+6738100.)

    #Now convert to ECI
    x_eci = Radius*math.cos(DEC_rad)*math.cos(RA_rad)
    y_eci = Radius*math.cos(DEC_rad)*math.sin(RA_rad)
    z_eci = Radius*math.sin(DEC_rad)

    #Get the velocity vector
    v_eci = math.sqrt((x_eci-x_eci_prev)**2+(y_eci-y_eci_prev)**2+(z_eci-z_eci_prev)**2)

    #Append the differences and velocity
    Vel_arr.append(v_eci)
    RA_arr.append(RA_rad-RA_rad_prev)
    DEC_arr.append(DEC_rad-DEC_rad_prev)

    #Set the ra to the previous ra and dec to the previous dec
    RA_rad_prev  = RA_rad
    DEC_rad_prev = DEC_rad
    x_eci_prev   = x_eci
    y_eci_prev   = y_eci
    z_eci_prev   = z_eci 
    
#Plot the RA and DEC differences to show the discontinuities
mplot.figure(0)
mplot.clf()
mplot.plot(RA_arr)
mplot.xlabel('Time Step')
mplot.ylabel('Difference in RA over Time Step')
mplot.savefig('./DiscontinuityInRA_wPyEphem.png')

mplot.figure(1)
mplot.clf()
mplot.plot(DEC_arr)
mplot.xlabel('Time Step')
mplot.ylabel('Difference in DEC over Time Step')
mplot.savefig('./DiscontinuityInDEC_wPyEphem.png')

mplot.figure(2)
mplot.clf()
mplot.plot(Vel_arr)
mplot.xlabel('Time Step')
mplot.ylabel('Velocity')

