% Step 5

clc
five_percent_theta=(2*pi)*.05;
%rand(1)*five_percent_theta*pi/3
%rand(1)*five_percent_theta*pi/6
p=26570;
c=299792.458;
A1=p*cos((rand(1)*five_percent_theta)+pi/4)*cos((rand(1)*five_percent_theta)+pi/3);
A2=p*cos((rand(1)*five_percent_theta)+pi/4)*cos((rand(1)*five_percent_theta)+pi/3);
A3=p*cos((rand(1)*five_percent_theta)+pi/4)*cos((rand(1)*five_percent_theta)+pi/3);
A4=p*cos((rand(1)*five_percent_theta)+pi/4)*cos((rand(1)*five_percent_theta)+pi/3);
B1=p*cos((rand(1)*five_percent_theta)+pi/4)*sin((rand(1)*five_percent_theta)+pi/3);
B2=p*cos((rand(1)*five_percent_theta)+pi/4)*sin((rand(1)*five_percent_theta)+pi/3);
B3=p*cos((rand(1)*five_percent_theta)+pi/4)*sin((rand(1)*five_percent_theta)+pi/3);
B4=p*cos((rand(1)*five_percent_theta)+pi/4)*sin((rand(1)*five_percent_theta)+pi/3);
C1=p*sin((rand(1)*five_percent_theta)+pi/4);
C2=p*sin((rand(1)*five_percent_theta)+pi/4);
C3=p*sin((rand(1)*five_percent_theta)+pi/4);
C4=p*sin((rand(1)*five_percent_theta)+pi/4);
plot3(A1,B1,C1,'bo')
hold on
plot3(A2,B2,C2,'go')
plot3(A3,B3,C3,'ko')
plot3(A4,B4,C4,'ro')
plot3(0,0,0,'ro','linewidth',15)
maxA = max([A1,A2,A3,A4]);
maxB = max([B1,B2,B3,B4]);
maxC = max([C1,C2,C3,C4]);
axis([-26570 26570 -26570 26570 -26570 26570 ])
R1=sqrt((A1^2)+(B1^2)+((C1-6370)^2));
R2=sqrt((A2^2)+(B2^2)+((C2-6370)^2));
R3=sqrt((A3^2)+(B3^2)+((C3-6370)^2));
R4=sqrt((A4^2)+(B4^2)+((C4-6370)^2));
x0=[0;0;6370;.0001];
x=x0(1);
y=x0(2);
z=x0(3);
d=x0(4);
T1=d+(R1/c);
T2=d+(R2/c);
T3=d+(R3/c);
T4=d+(R4/c);

%% Vary t_i by 10^-8
plus_minus = [-1,1];
t1_old = T1;
t2_old = T2;
t3_old = T3;
t4_old = T4;
%index = plus_minus(randi(2));
del_t1 = plus_minus(randi(2))*1e-8;
del_t2 = plus_minus(randi(2))*1e-8;
del_t3 = plus_minus(randi(2))*1e-8;
del_t4 = plus_minus(randi(2))*1e-8;
T1 = T1 + del_t1;
T2 = T2 + del_t2;
T3 = T3 + del_t3;
T4 = T4 + del_t4;
% T1 = T1 + 1e-8;
% T2 = T2 - 1e-8;
% T3 = T3 + 1e-8;
% T4 = T4 - 1e-8;
t = [T1;T2;T3;T4];
%% Run Gauss Newton Method
%clc
x0=[0;10;6370;.0001];
x=x0(1);
y=x0(2);
z=x0(3);
d=x0(4);
for i=1:50
    Df=[(2*x-2*A1) (2*y-2*B1) (2*z-2*C1) (-2*d*(c^2)+2*T1*(c^2));...
        (2*x-2*A2) (2*y-2*B2) (2*z-2*C2) (-2*d*(c^2)+2*T2*(c^2));...
        (2*x-2*A3) (2*y-2*B3) (2*z-2*C3) (-2*d*(c^2)+2*T3*(c^2));...
        (2*x-2*A4) (2*y-2*B4) (2*z-2*C4) (-2*d*(c^2)+2*T4*(c^2))];   
    f=[((x-A1)^2)+((y-B1)^2)+((z-C1)^2)-((c^2)*((T1-d)^2));...
       ((x-A2)^2)+((y-B2)^2)+((z-C2)^2)-((c^2)*((T2-d)^2));...
       ((x-A3)^2)+((y-B3)^2)+((z-C3)^2)-((c^2)*((T3-d)^2));...
       ((x-A4)^2)+((y-B4)^2)+((z-C4)^2)-((c^2)*((T4-d)^2))];
    g=-Df\f;
    x0=[x;y;z;d];
    x1=x0+g;
    x=x1(1);
    y=x1(2);
    z=x1(3);
    d=x1(4);
    index = i;
end
x
y
z
d
change_in_pos = x^2+y^2+(6370-z)^2
plot3(x,y,z,'g.','linewidth',4)
delta_x = x;
delta_y = y;
delta_z = abs(6370-z);
delta = [delta_x; delta_y; delta_z];
normx = norm(delta,inf)
%plot3(0,0,0,'ro--','linewidth',450)
%%
%delta_d = abs(-.0000426-d)
delta_t = [del_t1;del_t2;del_t3;del_t4];
emf = norm(delta,inf)/(c*norm(delta_t,inf))