# -*- coding: cp1252 -*-
# Code for Gaussian Method Martin Mason
# 7/22/09
from visual import *
from visual.graph import *
from __future__ import division
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. 
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.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
######################################################################
line[0]="17 06 26.13 -07 20 24.3 2455025.5"
line[1]="17 20 20.02 -11 28 26.5 2455032.5"
line[2]="17 49 47.27 -18 13 04.4 2455044.5"
JD_Ephem = 2455000.5
filename = raw_input("Enter file to load (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 "rho: ",t[i],rho[i]
    
#######################################################
# 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]

#Ssarting Earth Sun vectors for standard sepeartions
##R[0] = vector(-3.579512863984259E-01,8.729123574297091E-01,3.784347228603709E-01)
##R[1] = vector(-4.660163886291803E-01,8.285121124857461E-01,3.591853724931422E-01)
##R[2] = vector(-6.354735797498980E-01,7.260940243383054E-01,3.147809210934917E-01)


#For pallas test data
##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)

###YL29 far data
##R[0]=vector(-2.285686305384900E-01,9.089018427227323E-01,3.940319470653136E-01)
##R[1]=vector(-6.354735797498980E-01,7.260940243383054E-01,3.147809210934917E-01)
##R[2]=vector(-9.061020034232165E-01,4.101711834678812E-01,1.778203343322503E-01)


###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 NMV
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)

#sunrep = sphere(color=color.yellow,radius = 0.1)
for i in range(0,3):
    earthrep=sphere(pos = R[i],color=color.green,radius=0.0002)
    earthreplabel=label(pos = R[i],text = str(i))
    rhovector=arrow(pos=R[i],axis=norm(rho[i])/1000,color=color.yellow,shaftwidth = 0.000001)
scene.center  =earthrep.pos
sadfasf
print "R!: ",R
#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 product",TScalProd1
##print TScalProd2
##print TScalProd3
#One more triple scalar product for denominator:
TScalProdD = abs(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.
# Try an initial f and g series WITHOUT velocity vector.  That didn't help!

u2 = 1 / mag(r[1])**3

#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 
    g[i] = Tau[i] - (1./6.) * u2 * Tau[i]**3 

#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 "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 try equations of Lagrange:

#6 Evaluate A1, A3, B1, B3
B1 = a1/6.*(Tau[1]**2 - Tau[2]**2)
B3 = a3/6.*(Tau[1]**2 - Tau[0]**2)

#7 Evaluate A, B, E and F coefficients A is just rho_p[1] 
A = rho_p[1]
B = -(B1*TScalProd2[0] + B3*TScalProd2[2])/(TScalProdD)
E = -2.*dot(rho[1],R[1])
F = dot(R[1],R[1])

print ""
print "*************************************"
print "A", A
print "B", B
print "E", E
print "F", F
print "*************************************"
##sys.exit()

#8 Evaluate a, b, c coefficients
al = -(A**2 + A*E + F)
bl = -mu*(2*A*B + B*E)
cl = -mu**2 * B**2

##print ""
##print "*************************************"
##print "a", al
##print "b", bl
##print "c", cl
##print "*************************************"
##sys.exit()

#9 Plot and solve the scalar equation of Lagrange for r2
r2mag = solvelagrange(al,bl,cl)
print "r2", r2mag
r[1] = norm(r[1])*r2mag




####################################################
# 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)) > 5.e-3):  #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 - Tau[i]**2/(2.*mag(r[1])**3) + Tau[i]**3 * mag(rdot) / (2 * mag(r[1])**5)
        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)