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

%phi = (.001:.001:.004)*pi/2;    % chosen to be within 5% of each other
%theta = (.001:.001:.004)*2*pi; % chosen to be within 5% of each other
phi = [pi/4 (1.01)*pi/4 (.99)*pi/4 (1.02)*pi/4];
theta = [pi/2 (1.01)*pi/2 (.99)*pi/2 (1.02)*pi/2];


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);


%Sett correct positions
correct = zeros(4,4);
for i=1:4
    correct(:,i) = multiNewtonGeneral(A,B,C,t,x,y,z,d);
end

%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 = zeros(4,4);
for i=1:4
    nearMiss(:,i) = multiNewtonGeneral(A,B,C,td,x,y,z,d);
end

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 = zeros(4,4);
for i=1:4
    nearMiss(:,i) = multiNewtonGeneral(A,B,C,td,x,y,z,d);
end

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 = zeros(4,4);
for i=1:4
    nearMiss(:,i) = multiNewtonGeneral(A,B,C,td,x,y,z,d);
end

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 = zeros(4,4);
for i=1:4
    nearMiss(:,i) = multiNewtonGeneral(A,B,C,td,x,y,z,d);
end

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])