import numpy as np
import matplotlib.pyplot as plt

#Time
T=[2.45452831948422E+006,2.45452837470377E+006,2.45452842543483E+006]

#Delta T
Delta10=T[1]-T[0]
Delta21=T[2]-T[1]

#Coordinates of the asteroid in degrees
RA=[84.94302291666666,84.9599270833333,84.97283833333]
DEC=[28.46809722222,28.4690725,28.4690725]

#Cartesian equatorial coordinates of the sun
X=[0.94542896,0.94573013,0.946006096]
Y=[-0.27358003,-0.272745277,-0.271978009]
Z=[-0.11860989,-0.118247971,-0.11791531]

#Constants
k=0.01720209895


#Vector definition
R=[]
LA=[]
MU=[]
NU=[]
i=0
while i<3 :
    R.append(np.sqrt(X[i]**2+Y[i]**2+Z[i]**2)) #Vecteur Terre-Soleil
    LA.append(np.cos((DEC[i]))*np.cos(RA[i]))
    MU.append(np.cos((DEC[i]))*np.sin(RA[i]))
    NU.append(np.sin(DEC[i]))
    i=i+1


N0=[LA[0],MU[0],NU[0]]
N1=[LA[1],MU[1],NU[1]]
N2=[LA[2],MU[2],NU[2]]

#Q calculation
Qnum=Delta21*(LA[0]*(MU[1]*Z[1]-NU[1]*Y[1])+MU[0]*(NU[1]*X[1]-LA[1]*Z[1])+NU[0]*(LA[1]*Y[1]-MU[1]*X[1]))
Qdenum=Delta10*(LA[1]*(MU[2]*Z[1]-NU[2]*Y[1])+MU[1]*(NU[2]*X[1]-LA[2]*Z[1])+NU[1]*(LA[2]*Y[1]-MU[2]*X[1]))
Q=(Qnum/Qdenum)
print('Q =',Q)

#Newton Method

DeltaC=[]
ValD1=[]
L=(LA[0]-Q*LA[2])**2+(MU[0]-Q*MU[2])**2+(NU[0]-Q*NU[2])**2
M=2*((X[2]-X[0])*(LA[0]-Q*LA[2])+(Y[2]-Y[0])*(MU[0]-Q*MU[2])+(Z[2]-Z[0])*(NU[0]-Q*NU[2]))
N=(X[2]-X[0])**2+(Y[2]-Y[0])**2+(Z[2]-Z[0])**2

i=0
imax=50000
D1=0
eps=1e-6
while i<imax:
    #r1 & r2 calculation
    r1=np.sqrt(D1*D1-2*D1*(LA[0]*X[0]+MU[0]*Y[0]+NU[0]*Z[0])+R[0]*R[0])
    r3=np.sqrt(Q*Q*D1*D1-2*Q*D1*(LA[2]*X[2]+MU[2]*Y[2]+NU[2]*Z[2])+R[2]*R[2])
    #PHi calculation
    PHInum=2*k*(T[2]-T[0])
    PHIdenum=(r1+r3)**(3/2)
    PHI=PHInum/PHIdenum
    #First value of c
    c1=np.sqrt(L*D1*D1+M*D1+N)
    #Second value of c
    c2=(r1+r3)*PHI*(1+(1/24)*PHI*PHI+(5/384)*PHI*PHI*PHI*PHI+(59/9216)*PHI**6)
    DeltaC.append(c1-c2)
    ValD1.append(D1)
    D1=D1+0.000001
    i=i+1
    if abs((c1-c2))<= eps :
        print('r1 =',r1);
        print('r2 =',r3);
        print('D1 =',D1);
        print('D3 =',Q*D1)
        print('c2-c1 =',(c2-c1))
        D3=Q*D1
        break

#plt.plot(ValD1,DeltaC)
#plt.show()

#sun-asteroid vector
vr1=[LA[0]*D1-X[0],MU[0]*D1-Y[0],NU[0]*D1-Z[0]];
vr3=[LA[2]*D3-X[2],MU[2]*D3-Y[2],NU[2]*D3-Z[2]];
eps=(23.43929111111111); #Degres
vr1bis=[vr1[0],vr1[1]*np.cos(eps)+vr1[2]*np.sin(eps),-vr1[1]*np.sin(eps)+vr1[2]*np.cos(eps)];
vr3bis=[vr3[0],vr3[1]*np.cos(eps)+vr3[2]*np.sin(eps),-vr3[1]*np.sin(eps)+vr3[2]*np.cos(eps)];

vC=[vr1bis[1]*vr3bis[2]-vr1bis[2]*vr3bis[1],vr1bis[2]*vr3bis[0]-vr3bis[2]*vr1bis[0],vr1bis[0]*vr3bis[1]-vr3bis[0]*vr1bis[1]];
C=np.sqrt(vC[0]**2+vC[1]**2+vC[2]**2);

#Inclinaison
I=np.arccos(vC[2]/C);

#Longitude of the ascending node
ohm=np.arctan(-vC[0]/vC[1]);

#distance to perihelion calculation
qnum=r1*r3*np.sin((NU[2]-NU[0])/2)**2
qdenum=r1+r3-2*np.sqrt(r1*r3)*np.cos((NU[2]-NU[0])/2);
q=qnum/qdenum;

#perihelion To calculation
s=np.tan(NU[0]/2);
To=T[0]-(np.sqrt(2)*q**(3/2)/k)*(s**3/3+s);
print('Inclinaison',I,'°')
print('Longitude du neoud ascendant',ohm,'°')
print('Distance au périhélie',q,'UA')
print('Passage au périhélie',To)