% Reality Check # 4. Problem 1.

% Establishing the Multivariate Newton's Method
function f = multiNewtonGeneral(A,B,C,t,x,y,z,d)

% Current Locaitons of each Satellite.
A1 = A(1);
B1 = B(1);
C1 = C(1);
t1 = t(1);

A2 = A(2);
B2 = B(2);
C2 = C(2);
t2 = t(2);

A3 = A(3);
B3 = B(3);
C3 = C(3);
t3 = t(3);

A4 = A(4);
B4 = B(4);
C4 = C(4);
t4 = t(4);

%'c' = Speed of Light.
c = 299792.458;

% Initial guess
x0 = [x; y; z; d];

% Positions of the 4 satellites
F1 = (x - A1)^2 + (y - B1)^2 + (z - C1)^2 - (c * (t1 - d))^2;
F2 = (x - A2)^2 + (y - B2)^2 + (z - C2)^2 - (c * (t2 - d))^2;
F3 = (x - A3)^2 + (y - B3)^2 + (z - C3)^2 - (c * (t3 - d))^2;
F4 = (x - A4)^2 + (y - B4)^2 + (z - C4)^2 - (c * (t4 - d))^2;

% Partial Derivatives of each satellites

DF1x = 2 * (x - A1);
DF1y = 2 * (y - B1);
DF1z = 2 * (z - C1);
DF1d = 2 * c^2 * (t1 - d);

DF2x = 2 * (x - A2);
DF2y = 2 * (y - B2);
DF2z = 2 * (z - C2);
DF2d = 2 * c^2 * (t2 - d);

DF3x = 2 * (x - A3);
DF3y = 2 * (y - B3);
DF3z = 2 * (z - C3);
DF3d = 2 * c^2 * (t3 - d);

DF4x = 2 * (x - A4);
DF4y = 2 * (y - B4);
DF4z = 2 * (z - C4);
DF4d = 2 * c^2 * (t4 - d);

% DF - The Jacobian Matrix
DF = [DF1x DF1y DF1z DF1d; DF2x DF2y DF2z DF2d; DF3x DF3y DF3z DF3d; DF4x DF4y DF4z DF4d];

% The F matrix contains the four sphere equations
F = [F1; F2; F3; F4];

% One step of iteration.
s =  DF \ -F;
f = x0 + s;

end