%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% doit.m
% This will do lots of stuff.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
format long g;
fprintf(1,'\n\nStarting...\n');

clear;

c = 299792.458;     % Speed of light (in km/sec)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 3 Solve the system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf(1,'\n\nSolve the system using multivariate Newtons method...\n');
fprintf(1,'U[0] = x\n');
fprintf(1,'U[1] = y\n');
fprintf(1,'U[2] = z\n');
fprintf(1,'U[3] = d\n');

U1 = [15600; 7540; 10380; 0.0593200];
U2 = [11760; 2750; 16190; 0.0519200];
U3 = [11610; 14630; 7680; 0.0624200];
U4 = [15170; 610; 13320; 0.0557100];

gps(U1, U2, U3, U4, 10)

fprintf(1,'\n\nTest the conditioning of the GPS problem...\n');
fprintf(1,'R[] = satelite ranges\n');
fprintf(1,'T[] = travel times\n');
%fprintf(1,'E = error maginification factor\n');

x = 0; y = 0; z = 6370; d = 0.0001;
rho = 20200;

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 4 Find the condition number
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf(1,'\n\nFind the condition number...\n');

V = [-1; -1; -1; -1]; size = 4;
condition_number = 0;
for i = 1:(3^size - 1)
    %fprintf(1,'\n\nCheck location and distance using the new satelite values (ans = [0,0,6370,0]...\n');

    U1 = [A(1); B(1); C(1); T(1)];
    U2 = [A(2); B(2); C(2); T(2)];
    U3 = [A(3); B(3); C(3); T(3)];
    U4 = [A(4); B(4); C(4); T(4)];

    answer1 = gps(U1, U2, U3, U4, 10);
    %answer1

    %fprintf(1,'\n\nSove for location and distance using the satelite values (and corrected times)...\n');
    
    U1 = [A(1); B(1); C(1); T(1) + (V(1) * 10^-8)];
    U2 = [A(2); B(2); C(2); T(2) + (V(2) * 10^-8)];
    U3 = [A(3); B(3); C(3); T(3) + (V(3) * 10^-8)];
    U4 = [A(4); B(4); C(4); T(4) + (V(4) * 10^-8)];

    answer2 = gps(U1, U2, U3, U4, 10);
    %answer2
    
    %fprintf(1,'\n\nFind the error magnification factor...\n');
    
    % c * max(10^-8) = 3
    error_magnification = max( [ abs(answer2(1) - answer1(1)), abs(answer2(2) - answer1(2)), abs(answer2(3) - answer1(3)) ] ) / 0.003;

    if error_magnification > condition_number
        condition_number = error_magnification;
    end
    
    V = ternary(V, size);   % Move to the next setup
end

condition_number

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 5 Find the condition number (tightly grouped satelites)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf(1,'\n\nFind the condition number (tightly grouped satelites)...\n');

x = 0; y = 0; z = 6370; d = 0.0001;
rho = 20200;

for i = 1:4
    % Only use 5% check after the first values have been set.
    % After the first number has been set, take that number - 5% and random
    % in a 10% range. Check for out of bounds errors
    if i > 1
        phi(i) = phi(1) - ((pi / 2) * .05) + (rand * ((pi / 2) * .1));
        if phi(i) > (pi / 2)
            phi(i) = (pi / 2);
        end
        if phi(i) < 0
            phi(i) = 0;
        end
        
        theta(i) = theta(1) - ((pi / 2) * .05) + (rand * ((pi / 2) * .1));
        if theta(i) > (pi / 2)
            theta(i) = (pi / 2);
        end
        if theta(i) < 0
            theta(i) = 0;
        end
    else
        phi(i)   = rand * (pi / 2);
        theta(i) = rand * (pi * 2);
    end
    
    A(i) = rho * cos(phi(i)) * cos(theta(i));
    B(i) = rho * cos(phi(i)) * sin(theta(i));
    C(i) = rho * sin(theta(i));
    
    R(i) = sqrt(A(i)^2 + B(i)^2 + (C(i) - 6370)^2);
    T(i) = d + R(i) / c;
end
phi
theta
R
T

V = [-1; -1; -1; -1]; size = 4;
condition_number = 0;
for i = 1:(3^size - 1)
    %fprintf(1,'\n\nCheck location and distance using the new satelite values (ans = [0,0,6370,0]...\n');

    U1 = [A(1); B(1); C(1); T(1)];
    U2 = [A(2); B(2); C(2); T(2)];
    U3 = [A(3); B(3); C(3); T(3)];
    U4 = [A(4); B(4); C(4); T(4)];

    answer1 = gps(U1, U2, U3, U4, 10);
    %answer1

    %fprintf(1,'\n\nSove for location and distance using the satelite values (and corrected times)...\n');
    
    U1 = [A(1); B(1); C(1); T(1) + (V(1) * 10^-8)];
    U2 = [A(2); B(2); C(2); T(2) + (V(2) * 10^-8)];
    U3 = [A(3); B(3); C(3); T(3) + (V(3) * 10^-8)];
    U4 = [A(4); B(4); C(4); T(4) + (V(4) * 10^-8)];

    answer2 = gps(U1, U2, U3, U4, 10);
    %answer2
    
    %fprintf(1,'\n\nFind the error magnification factor...\n');
    
    % c * max(10^-8) = 3
    error_magnification = max( [ abs(answer2(1) - answer1(1)), abs(answer2(2) - answer1(2)), abs(answer2(3) - answer1(3)) ] ) / 0.003;

    if error_magnification > condition_number
        condition_number = error_magnification;
    end
    
    V = ternary(V, size);   % Move to the next setup
end

condition_number

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 6 Add more satelites
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf(1,'\n\nAdd more satelites...\n');

x = 0; y = 0; z = 6370; d = 0.0001;
rho = 20200;

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

V = [-1; -1; -1; -1; -1; -1; -1; -1]; size = 8;
condition_number = 0;
for i = 1:(3^size - 1)
    %fprintf(1,'\n\nCheck location and distance using the new satelite values (ans = [0,0,6370,0]...\n');

    U1 = [A(1); B(1); C(1); T(1)];
    U2 = [A(2); B(2); C(2); T(2)];
    U3 = [A(3); B(3); C(3); T(3)];
    U4 = [A(4); B(4); C(4); T(4)];
    U5 = [A(5); B(5); C(5); T(5)];
    U6 = [A(6); B(6); C(6); T(6)];
    U7 = [A(7); B(7); C(7); T(7)];
    U8 = [A(8); B(8); C(8); T(8)];

    answer1 = gps_8(U1, U2, U3, U4, U5, U6, U7, U8, 10);
    %answer1

    %fprintf(1,'\n\nSove for location and distance using the satelite values (and corrected times)...\n');
    
    U1 = [A(1); B(1); C(1); T(1) + (V(1) * 10^-8)];
    U2 = [A(2); B(2); C(2); T(2) + (V(2) * 10^-8)];
    U3 = [A(3); B(3); C(3); T(3) + (V(3) * 10^-8)];
    U4 = [A(4); B(4); C(4); T(4) + (V(4) * 10^-8)];
    U5 = [A(5); B(5); C(5); T(5) + (V(5) * 10^-8)];
    U6 = [A(6); B(6); C(6); T(6) + (V(6) * 10^-8)];
    U7 = [A(7); B(7); C(7); T(7) + (V(7) * 10^-8)];
    U8 = [A(8); B(8); C(8); T(8) + (V(8) * 10^-8)];

    answer2 = gps_8(U1, U2, U3, U4, U5, U6, U7, U8, 10);
    %answer2
    
    %fprintf(1,'\n\nFind the error magnification factor...\n');
    
    % c * max(10^-8) = 3
    error_magnification = max( [ abs(answer2(1) - answer1(1)), abs(answer2(2) - answer1(2)), abs(answer2(3) - answer1(3)) ] ) / 0.003;

    if error_magnification > condition_number
        condition_number = error_magnification;
    end
    
    V = ternary(V, size);   % Move to the next setup
end

condition_number