%% Step 2 - Code and Process
% Code developed by Donnelly Phillips
% Inspired by Timothy Sauer

%% Part 1 - Setup

% the speed of light
c = 299792.458;

% coordinates of satalites in the form (Ai,Bi,Ci)
A1 = 15600;
B1 = 7540;
C1 = 20140;

A2 = 18760;
B2 = 2750;
C2 = 18610;

A3 = 17610;
B3 = 14630;
C3 = 13480;

A4 = 19170;
B4 = 610;
C4 = 18390;

% specific times recorded by satalites
t1 = .07074;
t2 = .07220;
t3 = .07690;
t4 = .07242;


%% Part 2 - Vector Equation

% The following vectors stem from the following vector equation:
% ux*x + uy*y + uz*z + ud*d + w = 0
% This equation is produced by subtracting the second, third, and fourth
% equation from the first equation, then setting it equal to 0.

ux = -2*[A1 - A2;A1 - A3;A1 - A4];
uy = -2*[B1 - B2;B1 - B3;B1 - B4];
uz = -2*[C1 - C2;C1 - C3;C1 - C4];
ud = 2*(c^2)*[t1 - t2;t1 - t3;t1 - t4];
w = [A1^2 - A2^2 + B1^2 - B2^2 + C1^2 - C2^2 - (c^2)*(t1^2) + (c^2)*(t2^2); 
    A1^2 - A3^2 + B1^2 - B3^2 + C1^2 - C3^2 - (c^2)*(t1^2) + (c^2)*(t3^2); 
    A1^2 - A4^2 + B1^2 - B4^2 + C1^2 - C4^2 - (c^2)*(t1^2) + (c^2)*(t4^2)];

%% Part 3 - Determinant Equation

% Finding the next equations involves manipulation of these equations:

% det[uy | uz | ux + uy + uz + ud + w] = 0
% det[ux | uz | ux + uy + uz + ud + w] = 0
% det[ux | uy | ux + uy + uz + ud + w] = 0

% These equations stem from the fact that any determinant of a matrix with 
% a column of zeros will be zero.
% Then, by using more properties of determinants, these equations allow for
% one to solve for x, y, and z in terms of d.

% The values presented under the comments are coefficients for the
% following equations:

% x = i*d + j
% y = k*d + l
% z = m*d + n


i = -1*det([uy uz ud])/det([uy uz ux]);
j = -1*det([uy uz w])/det([uy uz ux]);
k = -1*det([ux uz ud])/det([ux uz uy]);
l = -1*det([ux uz w])/det([ux uz uy]);
m = -1*det([ux uy ud])/det([ux uy uz]);
n = -1*det([ux uy w])/det([ux uy uz]);


%% Part 3 - Quadratic Formula

% With x, y, and z solved for in terms of d, it can then be used to find d.
% This is done by plugging in these expressions into the first equation,
% will produces a quadratic equation entirely in terms of d.

% (i*d + j - A1)^2 + (k*d + l - B1)^2 + (m*d + n - C1)^2 = c^2*(t1 - d)^2

% This equation yields this quadratic equation:

% a*d^2 + b*d + cc = 0

% With a, b, and cc defined below. Those who know no pain can verify this.
% Beyond this, it is simply a matter of using the quadratic formula, which
% produces two solutions for d. However, only one of these solutions will
% be on the surface of the Earth.


a = i^2 + k^2 + m^2 - (c^2);
b = 2*i*j - 2*A1*i + 2*k*l - 2*B1*k + 2*m*n - 2*C1*m + 2*(c^2)*t1;
cc = j^2 - 2*A1*j + A1^2 + l^2 - 2*B1*l + B1^2 + n^2 - 2*C1*n + C1^2 - (c^2)*(t1^2);


d1 = (-b + sqrt(b^2 - 4*a*cc))/(2*a);
d2 = (-b - sqrt(b^2 - 4*a*cc))/(2*a);

%% Part 4 - Getting x, y, and z.

% To actually find the coordinate, one only needs to plug in both d's into
% x, y, and z from the previous equations from part 2. This will produce
% two coordinates, where only one of which is on the surface of the Earth.

% This can be tested by running the following lines of code:

% sqrt(x1^2 + y1^2 + z1^2)
% sqrt(x2^2 + y2^2 + z2^2)

x1 = i*d1 + j;
y1 = k*d1 + l;
z1 = m*d1 + n;

x2 = i*d2 + j;
y2 = k*d2 + l;
z2 = m*d2 + n;

