close all;
clear all;
clear;
%format long;

%x0 = [0; 0; 6370; 0];           %% initial guess for part 1
x0 = [10; 0; 6480; 0.0001];           %% initial guess for part 4
tol = .05e-8;                  %% error tolerance
n = 0;                          %% iteration count
ae = 10;
c = 299792.458;
f = zeros(4,1);
df = zeros(4,4);

rho = 26570; phi = [2*pi/5; pi/3; pi/7; pi/6]; theta = [3*pi/4; pi/4; 5*pi/3; 7*pi/6];
x = 0; y = 0; z = 6370; d = .0001;
c = 299792.458;

A = zeros(4,1);
B = zeros(4,1);
C = zeros(4,1);
R = zeros(4,1);
t = zeros(4,1);

for i = 1:4
    A(i) = rho*cos(phi(i))*cos(theta(i));
    B(i) = rho*cos(phi(i))*sin(theta(i));
    C(i) = rho*sin(phi(i));
    R(i) = sqrt(A(i)^2+B(i)^2+(C(i)-6370)^2);
    t(i) = d + (R(i)/c);
end;

A1 = A(1); B1 = B(1);  C1 = C(1);  t1 = t(1);  %%coordinates of first satellite
A2 = A(2); B2 = B(2);  C2 = C(2);  t2 = t(2);  %%coordinates of first satellite
A3 = A(3); B3 = B(3);  C3 = C(3);  t3 = t(3);  %%coordinates of first satellite
A4 = A(4); B4 = B(4);  C4 = C(4);  t4 = t(4);  %%coordinates of first satellite

%%%%%%%%%%%%%%%% USE THIS FOR PART 4 %%%%%%%%%%%%%%%%%%%

% A1 = -5805.8; B1 = 5805.8;  C1 = 25270;   t1 = 0.0688;  %%coordinates of first satellite
% A2 = 9393.9;  B2 = 9393.9;  C2 = 23010;   t2 = 0.0711;  %%coordinates of second satellite
% A3 = 11969;   B3 = -20732;  C3 = 11528;   t3 = 0.0818;  %%coordinates of third satellite
% A4 = -19928;  B4 = -11505;  C4 = 13285;   t4 = 0.0802;  %%coordinates of fourth satellite



%%%%%%%%%%%%%%%%%% USE THIS FOR PART 1 %%%%%%%%%%%%%%%
   
% A1 = 15600; B1 = 7540;  C1 = 20140;  t1 = 0.07074;  %%coordinates of first satellite
% A2 = 18760; B2 = 2750;  C2 = 18610;  t2 = 0.07220;  %%coordinates of second satellite
% A3 = 17610; B3 = 14630; C3 = 13480;  t3 = 0.07690;  %%coordinates of third satellite
% A4 = 19170; B4 = 610;   C4 = 18390;  t4 = 0.07242;  %%coordinates of fourth satellite

while ae>tol
    f(1) = (x0(1)-A1)^2 + (x0(2)-B1)^2 + (x0(3)-C1)^2 - c^2*(t1-x0(4))^2;
    f(2) = (x0(1)-A2)^2 + (x0(2)-B2)^2 + (x0(3)-C2)^2 - c^2*(t2-x0(4))^2;
    f(3) = (x0(1)-A3)^2 + (x0(2)-B3)^2 + (x0(3)-C3)^2 - c^2*(t3-x0(4))^2;
    f(4) = (x0(1)-A4)^2 + (x0(2)-B4)^2 + (x0(3)-C4)^2 - c^2*(t4-x0(4))^2;
    df(1,:) = [2*(x0(1)-A1),2*(x0(2)-B1),2*(x0(3)-C1),2*c^2*(t1-x0(4))];
    df(2,:) = [2*(x0(1)-A2),2*(x0(2)-B2),2*(x0(3)-C2),2*c^2*(t2-x0(4))];
    df(3,:) = [2*(x0(1)-A3),2*(x0(2)-B3),2*(x0(3)-C3),2*c^2*(t3-x0(4))];
    df(4,:) = [2*(x0(1)-A4),2*(x0(2)-B4),2*(x0(3)-C4),2*c^2*(t4-x0(4))];
    
    %v = transpose(f\df);
    v = df\-f;
    x1 = x0 + v;
    ae = norm(x1 - x0);
    n  = n + 1;
    x0 = x1;
    if n == 10000
        break
    end;
    
end;
    
disp(x1)
n