close all;
clear all;
clear;
format long;


error_compare = zeros(9,1);     % pre-allocated for max EMF for s = 4:12
t_combo = zeros(9,12);          % pre-allocated for which t vector +/- combination gave greatest error for s = 4:12
posit_error = zeros(9,1);       % pre-allocated for max position error for s = 4:12 
    
for s = 4:12            %  number of satellites
                                  

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


    phi         =   [2*pi/5; pi/3; pi/7; pi/6; pi/8; pi/2; pi/4; pi/20; 2*pi/7; 7*pi/15; 3*pi/8; 0];
    theta       =   [3*pi/4; pi/4; 5*pi/3; 7*pi/6; pi/2; 0; 2*pi; pi/12; 6*pi/5; 7*pi/4; 4*pi/3; 11*pi/6];
    rho = 26570; 

    for i = 1:s
        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; 
    record = zeros(6000,s);
    for l = 0:6000
        for v = 1:s  
            t(v) = Q(v);                % reset t values at beginning of loop
        end;                                       
        if l >= 1                       % l = 0 leaves t1, t2, t3, t4 alone 
            random = rand(s,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:s
                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:s
                t(u) = t(u) + G(u)*(10^-8);
            end;
        end;
    
        x0 = [12; 10; 6300; 0.002];     %% initial guess for part 4
        tol = .05e-10;                  %% error tolerance
        n = 0;                          %% iteration count
        ae = 10;
        f = zeros(s,1);
        df = zeros(s,4);
    
        while ae>tol                    %% the actual Newton's method
            for z = 1:s
                f(z)    = (x0(1)-A(z))^2 + (x0(2)-B(z))^2 + (x0(3)-C(z))^2 - c^2*(t(z)-x0(4))^2;
                df(z,:) = [2*(x0(1)-A(z)),2*(x0(2)-B(z)),2*(x0(3)-C(z)),2*c^2*(t(z)-x0(4))];
            end;
    
            v = (transpose(df)*df)\(transpose(-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;    
    
    for f = 1:l
        err(f) = norm(runs(:,1)-runs(:,f+1),inf);       % calculate greatest change from the x, y, z positions 
        err_mag(f) = err(f)/(c*(10^-8));                  % of the exact answer to each of the perturbed solutions
    end;
    err_mag = transpose(err_mag);                       % bc column vectors are better
    [Y,I] = max(err_mag);
    t_combo(s-3,1:s) = record(I,:);
    error_compare(s-3) = max(err_mag);
    posit_error(s-3) = max(err);
                                                        % output which change in t vector had greatest error
                                                        % comment this out for
                                                        % the extra function
end;                                                 