close all;
clear all;
clear;
format long;

x = 0; y = 0; z = 6370; d = .0001;      % what Newton's should converge to (without perturbation)
c = 299792.458;                         % speed of light

%%%%%%%% data for part 5 %%%%%%%%%

%rho = 26570; phi = [pi/3; pi/3 + pi/80; pi/3 - pi/80; pi/3 + pi/100];    
%theta = [5*pi/6 - pi/22; 5*pi/6 + pi/22; 5*pi/6 + pi/20; 5*pi/6 - pi/21];

%%%%%%%% data for part 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];

%%%%%%%%%%  

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;

Q = t; 

% for l = 0:4                   % run this and comment out other for loop                             
%     t(1) = Q(1);              % to see which t coord gives you greatest
%     t(2) = Q(2);              % error, with others held constant
%     t(3) = Q(3);
%     t(4) = Q(4);
%     if l>= 1
%         t(l) = t(l)+(10^-8);
%     end;

for l = 0:20
    for o = 1:4
        t(o) = Q(o);                % reset t values at beginning of loop
    end;
    if l >= 1                       % l = 0 leaves t1, t2, t3, t4 alone 
        random = rand(4,1)-.5;      % generate a fair balance of random numbers between -1 and 1
        G = sign(random);           % outputs a matrix of 1s and -1s
        for k = 1:4
            if G(k) == 0            % essentially we will give 0 a positive value
                G(k) = G(k) +1;
            end;
        end;
        record(l,:) = G;            % keeps track of which t values were made postive/negative
        for u = 1:4
            t(u) = t(u) + G(u)*(10^-8);   
        end;
    end;
    
    x0 = [0; 100; 6400; 0.0001];   %% initial guess for part 4
    tol = .05e-10;                  %% error tolerance
    n = 0;                         %% iteration count
    ae = 10;
    f = zeros(4,1);
    df = zeros(4,4);
    
    while ae>tol                   %% the actual Newton's method
        for m = 1:4
            f(m)    = (x0(1)-A(m))^2 + (x0(2)-B(m))^2 + (x0(3)-C(m))^2 - c^2*(t(m)-x0(4))^2;
            df(m,:) = [2*(x0(1)-A(m)),2*(x0(2)-B(m)),2*(x0(3)-C(m)),2*c^2*(t(m)-x0(4))];
        end;
    
        v = df\(-f);
        x1 = x0 + v;
        ae = norm(x1 - x0);
        n  = n + 1;
        x0 = x1;
        if n == 10000    %protection against infinite loop
            break
        end;
    runs(:,l+1) = [x1(1); x1(2); x1(3)];    %matrix which stores our Newton's method outputs (position only)
    end;
end;    

n

for i = 1:l
    err(i) = norm(runs(:,1)-runs(:,i+1),inf);       % calculate greatest change from the x, y, z positions 
    err_mag(i) = err(i)/(c*10^-8);                  % of the exact answer to each of the perturbed solutions
end;

err = transpose(err);
max_pos_err = max(err);
max_pos_err
err_mag = transpose(err_mag);                       % bc column vectors are better
[Y,I] = max(err_mag);
disp([Y,I]);                                        % display max error value & index

disp(record(I,:));                                 % output which change in t vector had greatest error
                                                   % comment this out for
                                                   % the extra function
                                                   