%Set initial conditions
format long g
rho = 26570;
c = 299792.458;

%phi = (0:.25:.75)*pi/2;    % chosen to be 4 equally spaced points
%theta = (0:.25:.75)*2*pi;  % chosen to be 4 equally spaced points
phi = [3*pi/8 pi/4 3*pi/8 pi/4]; %Randomly Selected
theta = [pi/8 pi/2 pi 3*pi/2]; %Randomly Selected


A = rho.*cos(phi).*cos(theta);
B = rho.*cos(phi).*sin(theta);
C = rho.*sin(phi);

x = 0;
y = 0;
z = 6370; 
d = .0001;

R = sqrt((A-x).^2 + (B-y).^2 + (C-z).^2);
t = d + (R/c);


%Set correct positions
correct = multiNewtonGeneral(A,B,C,t,x,y,z,d);

%Setting Delta ts
td = [0 0 0 0];
%    -,+,-+
for i=1:4
    td(i) = t(i);
    td(i) = td(i) + 1e-8*(-1)^i;
end

nearMiss = multiNewtonGeneral(A,B,C,td,x,y,z,d);

FEodds = max(abs(correct - nearMiss))
EMFodds = max(FEodds)/(c*max(abs(td-t)))

%   +,-,+,-
for i=1:4
    td(i) = t(i);
    td(i) = td(i) + 1e-8*(-1)^(i+1);
end

nearMiss = multiNewtonGeneral(A,B,C,td,x,y,z,d);

FEevens = max(abs(correct - nearMiss))
EMFevens = max(FEevens)/(c*max(abs(td-t)))

%    -,-,+,+
for i=1:4
    td(i) = t(i);
end
td(1) = td(1) + 1e-8*(-1);
td(2) = td(2) + 1e-8*(-1);
td(3) = td(3) + 1e-8;
td(4) = td(4) + 1e-8;

nearMiss = multiNewtonGeneral(A,B,C,td,x,y,z,d);

FEft = max(abs(correct - nearMiss))
EMFft = max(FEft)/(c*max(abs(td-t)))

%+,+,-,-
for i=1:4
    td(i) = t(i);
end
td(1) = td(1) + 1e-8;
td(2) = td(2) + 1e-8;
td(3) = td(3) + 1e-8*(-1);
td(4) = td(4) + 1e-8*(-1);

nearMiss = multiNewtonGeneral(A,B,C,td,x,y,z,d);

FElt = max(abs(correct - nearMiss))
EMFlt = max(FElt)/(c*max(abs(td-t)))

maxEMF = max([EMFodds EMFevens EMFft EMFlt])
maxFE = max([FEodds FEevens FEft FElt])