function v=newtonmgps1(v0,s1,s2,s3,s4)
% Paul McNulty
% Newton method for Project 3 question 2
%
%

tol = 20;        % Tolerance
%x0  = [1.0; 1.0];     % Iteration starting point
%n   = 0;              % Iteration count
ae  = 1;            % Initial dummy setting for absolute error

v = v0;

while ae < tol
    %n = n + 1;
   
    z = Df(v,s1,s2,s3,s4)\((f(v,s1,s2,s3,s4)));
    v = v - z;
    ae = ae+1;
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function fun=f(v2,s1,s2,s3,s4)
    A1=s1(1);
    B1=s1(2);
    C1=s1(3);
    d1=s1(4);
    A2=s2(1);
    B2=s2(2);
    C2=s2(3);
    d2=s2(4);
    A3=s3(1);
    B3=s3(2);
    C3=s3(3);
    d3=s3(4);
    A4=s4(1);
    B4=s4(2);
    C4=s4(3);
    d4=s4(4); 
    x=v2(1);
    y=v2(2);
    z=v2(3);
    t=v2(4);   
    c=299792.458;
    fun=[(x-A1)^2+(y-B1)^2+(z-C1)^2-c^2*(t-d1)^2;(x-A2)^2+(y-B2)^2+(z-C2)^2-c^2*(t-d2)^2;(x-A3)^2+(y-B3)^2+(z-C3)^2-c^2*(t-d3)^2;(x-A4)^2+(y-B4)^2+(z-C4)^2-c^2*(t-d4)^2];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function mat = Df(v1,s1,s2,s3,s4)
A1=s1(1);
    B1=s1(2);
    C1=s1(3);
    d1=s1(4);
    A2=s2(1);
    B2=s2(2);
    C2=s2(3);
    d2=s2(4);
    A3=s3(1);
    B3=s3(2);
    C3=s3(3);
    d3=s3(4);
    A4=s4(1);
    B4=s4(2);
    C4=s4(3);
    d4=s4(4);
c=299792.458;
x=v1(1);
y=v1(2);
z=v1(3);
t=v1(4);

mat=[2*(x-A1) 2*(y-B1) 2*(z-C1) -2*c^2*(t-d1);2*(x-A2) 2*(y-B2) 2*(z-C2) -2*c^2*(t-d2);2*(x-A3) 2*(y-B3) 2*(z-C3) -2*c^2*(t-d3);2*(x-A4) 2*(y-B4) 2*(z-C4) -2*c^2*(t-d4)];

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%