# -*- coding: cp1252 -*-
# Code for Gaussian Method Martin Mason
# Following the technique described in the OD packet by Ran S.
# 7/22/09
from visual import *
from OrbitLIB import *

#################################################################
# Declare variables
# I am to lazy to clean this up, but should give a snapshot.
################################################################

#How many hipsters does it take to flush a toilet?
#A. You can’t touch that toilet, it’s art.


mu = 1.  #In astronomical units
R = [0,0,0] #Earth sun vector
line = [0,0,0]  #Used for reading in data
RA =  [0,0,0]  #yup the RA
DEC = [0,0,0] #yup its the DEC
t = [0,0,0]  #The times of observations in JD.
rho = [0,0,0]  #rho vector (Earth Asteroid)
Tau = [0,0,0]  #Times converted to Gaussian Days. 
r = [0,0,0]  #Sun Asteroid Vector
f = [0,0,0] #for f and g series
g = [0,0,0]
t_corrected= [0,0,0]  #Used for light time correction
TScalProd1 = [0,0,0]  #These are a bunch of scalar triple products used in calculation of rho
TScalProd2 = [0,0,0]
TScalProd3 = [0,0,0]
rho_p = [0,0,0]   #The rho prime based on successive approximations
k = 0.017202099 #RMV

#k = 0.01720209895  #The fundamental constant that governs all goodness. not sqrt (G/M) but close!

######################################################################
# Read in Time of observationsin JD and 3 RA and DEC values for asteroid (Jasonwheeler)
# Have values hard coded, but could read from a file. 
# Format is RA HMS, DEC DMS, JD
# 2nd to last line contains JDephemeris
# Last line contains orbital elements from JPL for comparison
######################################################################

filename = raw_input("Enter file to load EX jasonwheeler.txt(CR for none) ")
if filename <> "":
    line = []
    FILE = open(filename,"r")
    line.append(FILE.readline())
    line.append(FILE.readline())
    line.append(FILE.readline())
    line.append(FILE.readline())
    line.append(FILE.readline())
    JD_Ephem = float(line[3])
    JPL_Elements = str(line[4]).split()
    for i in range(0,6):
        JPL_Elements[i]=float(JPL_Elements[i])
    print "JPL_Elements: ",JPL_Elements
    FILE.close()

RAdec = ConvertHMStoDecDeg(line)

#################################################################
#(topocentric unit vectors)
#################################################################

for i in range(0,3):
    RA[i] = RAdec[i][0]*pi/180.  
    DEC[i] = RAdec[i][1]*pi/180.
    t[i] = RAdec[i][2]
    #Calculate rhos
    rhox = cos(DEC[i])*cos(RA[i])
    rhoy = cos(DEC[i])*sin(RA[i])
    rhoz = sin(DEC[i])
    rho[i] = vector(rhox,rhoy,rhoz)
##    print "RA: ",RA[i]
##    print "DEC: ",DEC[i]
##    print "rho: ",t[i],rho[i].x
    
#######################################################
# Calculate Earth-Sun Vectors (R) Earth Centered for times of observations
# Updated to do automatically.
#########################################################
G = 6.674e-11
sun = CreateSun()
earth = CreateEarth(sun.mass,G)
JD_start = 2455016.5 #July 4 Earth at aphelion
for i in range(0,3):
    JD_stop = t[i]
    if i>0:
        JD_start = t[i-1]
    earth = MoveEarth(earth,sun.mass,sun.pos,JD_start,JD_stop,G)
    R[i] = (sun.pos-earth.pos)/149.6e9 #Convert to AU
    R[i] = rotate(R[i], angle = (23.439281*pi/180),axis=(1,0,0))
    #print "Rs",JD_stop,R[i]



#################################################
# Here are some sets of earth sun vectors that were used for testing
# For asteroids that are Near Earth but not too near, the newtonian approximation
# for the Earth Sun vector is appropriate
# An alternative is to use the Earths orbital elements to generate its Vectors.
# For the small rocks very close to earth, the OD is very senstive to the
# Earth Sun Vector
################################################


##    #Pallas
##R[0] = vector(9.350118648932805E-01,-3.009490457932475E-01,-1.304759343912905E-01)
##R[1] = vector(9.919360806588228E-01,-6.891858264346408E-02,-2.987856846017327E-02)
##R[2] = vector(9.071410046558456E-01, 3.932587816205667E-01, 1.704880435424688E-01)

#1 hour values for YL29
##R[0] = vector(-.5147058392245055,.8036153388514864,.3483896357588648)
##R[1] = vector(-.5153123219548978,.8032841777856333,.3482460393729922)
##R[2] = vector(-.5159185497736907,.8029526210808421,.3481022715679838)

###Ran magic values for YL29
##R[0] = vector(-4.652692845682E-01,8.288420638589E-01,3.593095009197E-01)
##R[1] = vector(-4.658934173994E-01,8.285524504354E-01,3.591798363927E-01)
##R[2] = vector(-4.665196148979E-01,8.282623817133E-01,3.590502181995E-01)

##### YL29 1 hour seperation 7/23
##R[0] = vector(-5.1591854977E-01,8.0295262108E-01,3.4810227157E-01)
##R[1] = vector(-5.1652452236E-01,8.0262066892E-01,3.4795833243E-01)
##R[2] = vector(-5.1713023939E-01,8.0228832149E-01,3.4781422203E-01)

###YL29 2 hour seperation
##R[0] = vector(-5.153123219548978E-01,8.032841777856333E-01,3.482460393729922E-01)
##R[1] = vector(-5.1652452236E-01,8.0262066892E-01,3.4795833243E-01)
##R[2] = vector(-5.177357005427163E-01,8.019555789626697E-01,3.476699404651580E-01)
##
###YL29 30 min Seperation
##R[0] = vector(-5.162215679905863E-01,8.027866944209462E-01,3.480303234091172E-01)
##R[1] = vector(-5.1652452236E-01,8.0262066892E-01,3.4795833243E-01)
##R[2] = vector(-5.168274128383425E-01,8.024545446005834E-01,3.478862986303722E-01)


#Q: Why are hipsters bad at Karate?  
#a: they can't get past the white belt 
#############################################################
# Transform times of observations to Tau, Tau+ and Tau-
#       (From Rs and rhoss to find scalar equations for the Ranges)
#       (Need to calculate f and g series before can calculate Ranges)
##############################################################

Tau[0] = k*(t[0]-t[1])
Tau[2] = k*(t[2]-t[1])
Tau[1] = Tau[2] - Tau[0]
print "Taus",Tau

#############################################
#Following technique of page 30 in the OD handout
# Define r[1] can be defined as a1r[0] + a3r[2]
# where a1 = Tau[3]/Tau[2] and a3=Tau[1]/Tau[3]
# Define r[2] as (Tau[2]/Tau[1])*r[0]+(Tau[0]/Tau[1])*r[2]
# So then rho[1] = R[1] + r[2]
# Follow equations on bottom of page 30 Note correction to eq for a3!!!!!
#######################################################
a1 = Tau[2]/Tau[1] 
a3 = -Tau[0]/Tau[1]
print "a1 a3: ",a1,a3
# Will need to calculate a bunch of triple scalar products ie dot(cross(R[0],rho[1]),rho[2])
for i in range(0,3):
    TScalProd1[i] = dot(cross(R[i],rho[1]),rho[2])
    TScalProd2[i] = dot(cross(rho[0],R[i]),rho[2])
    TScalProd3[i] = dot(cross(rho[1],R[i]),rho[0])
##print "Tscal product1 ",TScalProd1
##print "Tscal product2 ", TScalProd2
##print "Tscal product3 ", TScalProd3
#One more triple scalar product for denominator:
TScalProdD = dot(cross(rho[0],rho[1]),rho[2])
##print "TScalProdD",TScalProdD
#note if the value a1*TScalProdD is very small, method will fail!
rho_p[0] = (a1*TScalProd1[0]-TScalProd1[1]+a3*TScalProd1[2])/(a1*TScalProdD)
rho_p[1] = (a1*TScalProd2[0]-TScalProd2[1]+a3*TScalProd2[2])/-(TScalProdD)
rho_p[2] = (a1*TScalProd3[0]-TScalProd3[1]+a3*TScalProd3[2])/(a3*TScalProdD)
print "TScalProdD: If Small then crash! ",TScalProdD
for i in range(0,3):
    print "rho prime ", t[i],rho_p[i]
    
##########################################
#Now calculate r and rdot from these rho values.  This will be the first guess for guassian.
#Now we do a ball park value for r and rdot to feed into our f and g series.  Note these values will
# be way off ,but we will hope that the magnitudes will be in the right ball park!  It might work better
# to just feed in 1 for the magnitude! (Since they are near earth!)
#################################################################
for i in range(0,3):
    r[i] = rho_p[i]*rho[i]-R[i]
print "Bogus r values ",r[1],mag(r[1])
rdot = (r[2]-r[0])/(Tau[2]-Tau[0])
print "rdot",rdot,mag(rdot)
# OK the above values are "Crazy Ridiculous Artificial Pretenders" so we need to do something better.
# Two options here, one is the use the f and g series instead or two use the lagrange method.

####################################################
# I will use the f and g series to recalculate rho and then r.
####################################################

diff = vector (100.,100.,100.)
r0old = [0.,0.,0.]
while (abs(mag(diff)) > 1.e-4):  #Until values converge
    u2 = 1. / mag(r[1])**3
    zeta2 = dot(r[1],rdot)/mag2(r[1])
    Q2 = dot(rdot,rdot)/mag2(r[1]) - u2
#Calculate f and g series based on the details in section 6.4
    for i in range(0,3):
        f[i] = 1. - u2/2.*Tau[i]**2 + u2/2.*zeta2*Tau[i]**3    
        f[i] += (1./24.)*(3.*u2*Q2 - 15.*u2*zeta2**2 + u2**2)*Tau[i]**4 
        g[i] = Tau[i] - (1./6.) * u2 * Tau[i]**3 + u2/4.*zeta2*Tau[i]**4

    #Now use these values for the f and g series to recalculate the rhos
    #Should make a function here and just call it!
    # Follow equations on bottom of page 30 Note correction to eq for a3!!!!
    # Note new defintion for a1 and a3.

    a1 = g[2] / (f[0]*g[2] - f[2]*g[0])
    a3 = -g[0] / (f[0]*g[2] - f[2]*g[0])

##    print "a1 and a3 again ",a1,a3
    #note if the value a1*TScalProdD is very small, method will fail!
    rho_p[0] = (a1*TScalProd1[0]-TScalProd1[1]+a3*TScalProd1[2])/(a1*TScalProdD)
    rho_p[1] = (a1*TScalProd2[0]-TScalProd2[1]+a3*TScalProd2[2])/-(TScalProdD)
    rho_p[2] = (a1*TScalProd3[0]-TScalProd3[1]+a3*TScalProd3[2])/(a3*TScalProdD)

##    print "rho prime ", rho_p[0]
##    print "rho prime ", rho_p[1]
##    print "rho prime ", rho_p[2]

    for i in range(0,3):
        r[i] = rho_p[i]*rho[i]-R[i]
    print "diff ",diff
    diff = r[1] - r0old
    r0old = r[1]
##    print "Successively less bogus r values ",r[1],mag(r[1])
#This naive calculation of rdot doesn't work for beans, see the update below
    rdot = (r[2]-r[0])/(Tau[2]-Tau[0])
##    print "rdot",rdot,mag(rdot)

    d1 = - f[2] / (f[0]*g[2] - f[2]*g[0])
    d3 =  f[0] / (f[0]*g[2] - f[2]*g[0])

    #Calculate rdot via f and g series just like r!  This is a help!
    rdot=(d1*r[0]+d3*r[2])
##    print "rdot",rdot,mag(rdot)


###################################################
#Now do light time correction!  Another big help and the first of the Extra options
####################################################
    
    for i in range(0,3): #correct for light time  173.1446 is speed of light Astro Units. (Thanks wikipedia!)
        t_corrected[i]=(t[i] - rho_p[i] / 173.1446)
    Tau[0] = k*(t_corrected[0]-t_corrected[1])
    Tau[2] = k*(t_corrected[2]-t_corrected[1])
    Tau[1] = Tau[2] - Tau[0]
    
print "rho prime ", rho_p[1]
print "r[1] ",r[1]
print "rdot[1] ",rdot
#Convert r2 and r2dot to ecliptic coordinates

r2ec = rotate(r[1],angle = -(23.439281*pi/180),axis=(1,0,0))
r2dotec = rotate(rdot,angle = -(23.439281*pi/180),axis=(1,0,0))
print "r2ec,r2dotec: ",r2ec,r2dotec
ecct = (dot(r2dotec,r2dotec) - 1/ mag(r2ec))*r2ec - dot(r2ec,r2dotec)* r2dotec

#Quick check before going wild!
print "Is the eccentricity even close?", mag(ecct)

###################################################
# Now Calculate the orbital Elements:
###################################################

#Q: How many hipsters does it take to screw in a light bulb?
#A: Dude, the light bulb was cooler before it changed.


##print ""
##print "************************************"
##print "r2ec", r2ec
##print "r2dotec", r2dotec
##print "************************************"
##sys.exit()

#Compute h
h = cross(r2ec,r2dotec)
##print ""
##print "************************************"
##print "h", h
##print "************************************"
##sys.exit()

#Get a
a = 1./(2./mag(r2ec) - mag2(r2dotec)/mu)
##print ""
##print "************************************"
##print "a", a
##print "************************************"
##sys.exit()

#Get e, the eccentricity  (As per handout!)
ecc = sqrt(1. - mag2(h)/(mu*a))
ecct = (dot(r2dotec,r2dotec) - 1/ mag(r2ec))*r2ec - dot(r2ec,r2dotec)* r2dotec
##print ""
##print "************************************"
##print "e", ecc
##print "************************************"
##sys.exit()

# Get i
cosi = h.z/mag(h)
i = acos(cosi)
ideg = i * 180./pi

##print ""
##print "************************************"
##print "i", ideg
##print "************************************"
##sys.exit()

# Get omegacap
sinomega = h.x/(mag(h)*sin(i))
cosomega = -h.y/(mag(h)*sin(i))
omegacap = findquadrant(cosomega,sinomega)
##print ""
##print "************************************"
##print "cosomegacap, sinomegacap", cosomega, sinomega
##print "omegacap", omegacap5
##print "************************************"
##sys.exit()

# omega

sinu = r2ec.z/(mag(r2ec)*sin(i))
cosu = (r2ec.x*cosomega + r2ec.y*sinomega)/mag(r2ec)
u = findquadrant(cosu,sinu)
print ""
print "******************************************"
print "cosu, sinu", cosu, sinu
print "u", u
print "******************************************"
#sys.exit()

# Get nu
cosnu = 1./ecc * (a*(1.-ecc**2)/mag(r2ec) - 1.)
sinnu = a*(1.-ecc**2)/ecc * dot(r2dotec,r2ec)/(mag(h)*mag(r2ec))
nu = findquadrant(cosnu,sinnu)
##print ""
##print "******************************************"
##print "cosnu, sinnu", cosnu, sinnu
##print "nu", nu
##print "******************************************"
##sys.exit()

# Get omega
omega = u - nu
#Make sure omega is expressed as positive angle.
if (omega<0):
    omega = omega + 360.

# Get E
##print "ecc", ecc
##print "mag(r2ec)", mag(r2ec)
cose = 1./ecc * (1. - mag(r2ec)/a)
#print "cose", cose
eccanom = acos(cose)
if (nu>180.):
    eccanom = 2*pi - eccanom
#print "E", eccanom*180./pi
#m0 is mean anomaly at JD.  This should be converted into published but let the students do it!
m0 = eccanom - ecc*sin(eccanom)
m0 = m0*180./pi

print ""
print "ORBITAL ELEMENTS**Calculated***JPL****** % Diff"
print "a ", a , JPL_Elements[0], (a-JPL_Elements[0])/((a+JPL_Elements[0])/2)*100
print "e ", ecc, JPL_Elements[1], (ecc-JPL_Elements[1])/((ecc+JPL_Elements[1])/2)*100
print "i ", ideg, JPL_Elements[2], (ideg-JPL_Elements[2])/((ideg+JPL_Elements[2])/2)*100
print "omegacap ", omegacap, JPL_Elements[3],(omegacap-JPL_Elements[3])/((omegacap+JPL_Elements[3])/2)*100
print "omega ", omega, JPL_Elements[4],(omega-JPL_Elements[4])/((omega+JPL_Elements[4])/2)*100
print "M ", m0, JPL_Elements[5], (m0-JPL_Elements[5])/((m0+JPL_Elements[5])/2)*100
print "***************************"


GenerateOrbit(ecc,a,i,omegacap,omega,m0,JD_start,JD_Ephem,earth,sun)