clc;

clear all;
close all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Written by: Ryan Crafts
% Date Last Modified: 4/25/2016
% Assignment name: State Vector from Topocentric RI and D
% Description: This file is intended to calculate the -
% state vector from topocentric right
% inclination as well as topocentric declination
% **EMPLOYS ITERATIONS**
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Inputs (user input)

mu = 3.986*10^5;
height = .178; %please make sure this is in KM
latitude = 39.48389; %degrees

t1 = 0*60; %time in seconds at each obs time

t2 = 2*60;
t3 = 4*60;

LSD1 = 170.6250; %all in degrees

LSD2 = 171.1250;
LSD3 = 171.6292;

RA1 = 202.9917; %all in degrees

RA2 = 204.8792;
RA3 = 207.5000;

Dec1 = 11.43111; %all in degrees

Dec2 = 21.68861;
Dec3 = 32.32917;

%% Calculate R1,R2, and R3

Re = 6378; %radius of earth (km)

f = 0.003353; %earth's flattening factor, via book

%R1
m1 = ((Re/sqrt(1-(2*f-f^2)*(sind(latitude))^2))+height)*cosd(latitude);
R1x = m1*cosd(LSD1);
R1y = m1*sind(LSD1);
R1z = (((Re*(1-f)^2)/sqrt(1-(2*f-
f^2)*(sind(latitude))^2))+height)*sind(latitude);

%R2
R2x = m1*cosd(LSD2);
R2y = m1*sind(LSD2);
R2z = (((Re*(1-f)^2)/sqrt(1-(2*f-
f^2)*(sind(latitude))^2))+height)*sind(latitude);
R2 = sqrt(R2x^2+R2y^2+R2z^2);

%R3
R3x = m1*cosd(LSD3);
R3y = m1*sind(LSD3);
R3z = (((Re*(1-f)^2)/sqrt(1-(2*f-
f^2)*(sind(latitude))^2))+height)*sind(latitude);

%% Define Taus

T1 = t1-t2;
T3 = t3-t2;
T = t3-t1;

%% Find all the p-hats

ph1x = cosd(Dec1)*cosd(RA1);
ph1y = cosd(Dec1)*sind(RA1);
ph1z = sind(Dec1);

ph2x = cosd(Dec2)*cosd(RA2);
ph2y = cosd(Dec2)*sind(RA2);
ph2z = sind(Dec2);

ph3x = cosd(Dec3)*cosd(RA3);
ph3y = cosd(Dec3)*sind(RA3);
ph3z = sind(Dec3);

%% Get phat cross products

%1 cross 2
p12cx = ph1y*ph2z-ph1z*ph2y;
p12cy = ph1z*ph2x-ph1x*ph2z;
p12cz = ph1x*ph2y-ph1y*ph2x;

%2 cross 3
p23cx = ph2y*ph3z-ph2z*ph3y;
p23cy = ph2z*ph3x-ph2x*ph3z;
p23cz = ph2x*ph3y-ph2y*ph3x;

%1 cross 3
p13cx = ph1y*ph3z-ph1z*ph3y;
p13cy = ph1z*ph3x-ph1x*ph3z;
p13cz = ph1x*ph3y-ph1y*ph3x;

%% Find all the D's

D0x = p23cx;
D0y = p23cy;
D0z = p23cz;
D0 = D0x*ph1x+D0y*ph1y+D0z*ph1z;

D11x = R1x*p23cx;
D11y = R1y*p23cy;
D11z = R1z*p23cz;
D11 = D11x+D11y+D11z;

D12x = R1x*p13cx;
D12y = R1y*p13cy;
D12z = R1z*p13cz;
D12 = D12x+D12y+D12z;

D13x = R1x*p12cx;
D13y = R1y*p12cy;
D13z = R1z*p12cz;
D13 = D13x+D13y+D13z;

D21x = R2x*p23cx;
D21y = R2y*p23cy;
D21z = R2z*p23cz;
D21 = D21x+D21y+D21z;

D22x = R2x*p13cx;
D22y = R2y*p13cy;
D22z = R2z*p13cz;
D22 = D22x+D22y+D22z;

D23x = R2x*p12cx;
D23y = R2y*p12cy;
D23z = R2z*p12cz;
D23 = D23x+D23y+D23z;

D31x = R3x*p23cx;
D31y = R3y*p23cy;
D31z = R3z*p23cz;
D31 = D31x+D31y+D31z;

D32x = R3x*p13cx;
D32y = R3y*p13cy;
D32z = R3z*p13cz;
D32 = D32x+D32y+D32z;

D33x = R3x*p12cx;
D33y = R3y*p12cy;
D33z = R3z*p12cz;
D33 = D33x+D33y+D33z;

%% Calculate A, B, and E
A = (1/D0)*(-D12*(T3/T)+D22+D32*(T1/T));
B = (1/(6*D0))*(D12*(T3^2-T^2)*(T3/T)+D32*(T^2-T1^2)*(T1/T));
E = R2x*ph2x+R2y*ph2y+R2z*ph2z;

%% Caculate a, b, and c
a = -(A^2+2*A*E+R2^2);
b = -2*mu*B*(A+E);
c = -mu^2*B^2;

%% Solve the polynomial for r2

p = [1 0 a 0 0 b 0 0 c];
sol2 = roots(p);
r2 = max(sol2);
%% Find the slant ranges (p)

p1 = (1/D0)*((6*(D31*(T1/T3)+D21*(T/T3))*r2^3+mu*D31*(T^2-
T1^2)*(T1/T3))/(6*r2^3+mu*(T^2-T3^2))-D11);
p2 = A + (mu*B)/(r2^3);
p3 = (1/D0)*((6*(D13*(T3/T1)-D23*(T/T1))*r2^3+mu*D13*(T^2-
T3^2)*(T3/T1))/(6*r2^3+mu*(T^2-T1^2))-D33);

%% Find the actual r-vectors

r1x = R1x+p1*ph1x;
r1y = R1y+p1*ph1y;
r1z = R1z+p1*ph1z;
r1 = sqrt(r1x^2+r1y^2+r1z^2);

r2x = R2x+p2*ph2x;
r2y = R2y+p2*ph2y;
r2z = R2z+p2*ph2z;
r2 = sqrt(r2x^2+r2y^2+r2z^2);

r3x = R3x+p3*ph3x;
r3y = R3y+p3*ph3y;
r3z = R3z+p3*ph3z;
r3 = sqrt(r3x^2+r3y^2+r3z^2);

%% Find the lagrange coefficients

f1 = 1-(1/2)*(mu/r2^3)*T1^2;
f3 = 1-(1/2)*(mu/r2^3)*T3^2;

g1 = T1-(1/6)*(mu/r2^3)*T1^3;
g3 = (T3-(1/6)*(mu/r2^3)*T3^3);

%% Find v2
v2x = (1/(f1*g3-f3*g1))*(-f3*r1x+f1*r3x);
v2y = (1/(f1*g3-f3*g1))*(-f3*r1y+f1*r3y);
v2z = (1/(f1*g3-f3*g1))*(-f3*r1z+f1*r3z);
v2 = sqrt(v2x^2+v2y^2+v2z^2);

%% Iterative Chi-finding process

p1n=0;
p2n=0;
p3n=0;
iterations = 0;
while (abs(p1n-p1)>=0.00001 && (abs(p2n-p2)>=0.00001) && (abs(p2n-
p2)>=0.00001))
%% Find the value of the semimajor axis
alpha = 2/r2-v2^2/mu;
semiMajor = 1/alpha;

%% Find the radial velocity at position 2

vr2 = v2x*(r2x/r2)+v2y*(r2y/r2)+v2z*(r2z/r2);

p1 = p1n;
p2 = p2n;
p3 = p3n;
 X1=sqrt(mu)*abs(alpha)*T1;
X3=sqrt(mu)*abs(alpha)*T3;
X10 = 0;
X30 = 0;
iterations = iterations + 1;
while((abs(X1-X10)>0.0001)&&(abs(X3-X30)>0.0001))
X10=X1;
X30=X3;
X1 = X10 - (-sqrt(mu)*T1 +
((r2*vr2)/sqrt(mu))*X10^2*C(alpha*X10^2)+(1-
alpha*r2)*X1^3*S(alpha*X10^2)+r2*X1)/((r2*vr2)/sqrt(mu)*X10*(1-
alpha*X10^2*S(alpha*X10^2))+(1-alpha*r2)*X10^2*C(alpha*X10^2)+r2);
X3 = X30 - (-sqrt(mu)*T3 +
((r2*vr2)/sqrt(mu))*X30^2*C(alpha*X30^2)+(1-
alpha*r2)*X3^3*S(alpha*X30^2)+r2*X3)/((r2*vr2)/sqrt(mu)*X30*(1-
alpha*X30^2*S(alpha*X30^2))+(1-alpha*r2)*X30^2*C(alpha*X30^2)+r2);
end

f1n = (1-(X1^2/r2)*C(alpha*X1^2));
f3n = (1-(X3^2/r2)*C(alpha*X3^2));
g1n = T1-(1/sqrt(mu))*X1^3*S(alpha*X1^2);
g3n = T3-(1/sqrt(mu))*X3^3*S(alpha*X3^2);

f1 = (f1+f1n)/2;
f3 = (f3+f3n)/2;
g1 = (g1+g1n)/2;
g3 = (g3+g3n)/2;

C1 = g3/(f1*g3-f3*g1);
C3 = -g1/(f1*g3-f3*g1);

p1n = (1/D0)*(-D11+(1/C1)*D21-(C3/C1)*D31);
p2n = (1/D0)*(-C1*D12+D22-C3*D32);
p3n = (1/D0)*(-(C1/C3)*D13+(1/C3)*D23-D33);

r1x = R1x+p1n*ph1x;
r1y = R1y+p1n*ph1y;
r1z = R1z+p1n*ph1z;
r1 = sqrt(r1x^2+r1y^2+r1z^2);

r2x = R2x+p2n*ph2x;
r2y = R2y+p2n*ph2y;
r2z = R2z+p2n*ph2z;
r2 = sqrt(r2x^2+r2y^2+r2z^2);

r3x = R3x+p3n*ph3x;
r3y = R3y+p3n*ph3y;
r3z = R3z+p3n*ph3z;
r3 = sqrt(r3x^2+r3y^2+r3z^2);

v2x = (1/(f1*g3-f3*g1))*(-f3*r1x+f1*r3x);
v2y = (1/(f1*g3-f3*g1))*(-f3*r1y+f1*r3y);
v2z = (1/(f1*g3-f3*g1))*(-f3*r1z+f1*r3z);
v2 = sqrt(v2x^2+v2y^2+v2z^2);
end

%% Output

fprintf('Inputs:\n')
fprintf('--------------------------------------------------------------------
------\n')
fprintf('altitude = %f (km)\n',height)
fprintf('latitude = %f (deg)\n',latitude)
fprintf('local sidereal time (observation 1)= %f (deg)\n',LSD1)
fprintf('local sidereal time (observation 2)= %f (deg)\n',LSD2)
fprintf('local sidereal time (observation 3)= %f (deg)\n',LSD3)

fprintf('right ascension (observation 1) = %f (deg)\n',RA1)

fprintf('right ascension (observation 2) = %f (deg)\n',RA2)
fprintf('right ascension (observation 3) = %f (deg)\n',RA3)

fprintf('declination angle (observation 1) = %f (deg)\n',Dec1)

fprintf('declination angle (observation 2) = %f (deg)\n',Dec2)
fprintf('declination angle (observation 3) = %f (deg)\n',Dec3)

fprintf('time of observation 1: %5.5f (s)\n',t1)

fprintf('time of observation 2: %5.5f (s)\n',t2)
fprintf('time of observation 3: %5.5f (s)\n',t3)

fprintf('\nIntermediate Calculations\n')
fprintf('--------------------------------------------------------------------
------\n')
fprintf('R1 vector = (%5.5f)i + (%5.5f)j + (%5.5f)k (km)\n',R1x,R1y,R1z)
fprintf('R2 vector = (%5.5f)i + (%5.5f)j + (%5.5f)k (km)\n',R2x,R2y,R2z)
fprintf('R3 vector = (%5.5f)i + (%5.5f)j + (%5.5f)k (km)\n\n',R3x,R3y,R3z)

fprintf('Tau = %5.5f (s)\n',T)

fprintf('Tau1 = %5.5f (s)\n',T1)
fprintf('Tau3 = %5.5f (s)\n\n',T3)

fprintf('p1 unit vector = (%5.5f)i + (%5.5f)j + (%5.5f)k\n',ph1x,ph1y,ph1z)

fprintf('p2 unit vector = (%5.5f)i + (%5.5f)j + (%5.5f)k\n',ph2x,ph2y,ph2z)
fprintf('p3 unit vector = (%5.5f)i + (%5.5f)j + (%5.5f)k\n\n',ph3x,ph3y,ph3z)

fprintf('phat1 X phat2 = (%5.5f)i + (%5.5f)j + (%5.5f)k

\n',p12cx,p12cy,p12cz)
fprintf('phat2 X phat3 = (%5.5f)i + (%5.5f)j + (%5.5f)k
\n',p23cx,p23cy,p23cz)
fprintf('phat1 X phat3 = (%5.5f)i + (%5.5f)j + (%5.5f)k
\n\n',p13cx,p13cy,p13cz)

fprintf('D0 = %5.5f\n',D0)
fprintf('D11 = %5.5f\n',D11)
fprintf('D12 = %5.5f\n',D12)
fprintf('D13 = %5.5f\n',D13)
fprintf('D21 = %5.5f\n',D21)
fprintf('D22 = %5.5f\n',D22)
fprintf('D23 = %5.5f\n',D23)
fprintf('D31 = %5.5f\n',D31)
fprintf('D32 = %5.5f\n',D32)
fprintf('D33 = %5.5f\n\n',D33)

fprintf('A = %5.5f\n',A)
fprintf('B = %5.5f\n',B)
fprintf('E = %5.5f\n',E)
fprintf('a = %5.5f\n',a)
fprintf('b = %5.5f\n',b)
fprintf('c = %5.5f\n\n',c)

fprintf('r2 = %5.5f (km)\n\n',r2)

fprintf('p1 = %5.5f (km)\n',p1)
fprintf('p2 = %5.5f (km)\n',p2)
fprintf('p3 = %5.5f (km)\n\n',p3)

fprintf('r1 vector = (%5.5f)i + (%5.5f)j + (%5.5f)k (km)\n',r1x,r1y,r1z)

fprintf('r1 = %5.5f (km)\n',r1)
fprintf('r2 vector = (%5.5f)i + (%5.5f)j + (%5.5f)k (km)\n',r2x,r2y,r2z)
fprintf('r2 = %5.5f (km)\n',r2)
fprintf('r3 vector = (%5.5f)i + (%5.5f)j + (%5.5f)k (km)\n',r3x,r3y,r3z)
fprintf('r3 = %5.5f (km)\n',r3)

fprintf('\nf1 = %5.5f\n',f1)
fprintf('f3 = %5.5f\n',f3)
fprintf('g1 = %5.5f\n',g1)
fprintf('g3 = %5.5f\n',g3)

fprintf('\nFinal State Vector 2\n')

fprintf('--------------------------------------------------------------------
------\n')
fprintf('r2 = (%5.5f)I + (%5.5f)J + (%5.5f)K (km)\n',r2x,r2y,r2z)
fprintf('r2 = %5.5f (km)\n',r2)
fprintf('v2 = (%5.5f)I + (%5.5f)J + (%5.5f)K (km/s)\n',v2x,v2y,v2z)
fprintf('v2 = %5.5f (km/s)\n',v2)