function [ y ] = GPSCalc( x, terr, varargin)
%UNTITLED2 Summary of this function goes here
%   Detailed explanation goes here

numArg = size(varargin,2);
if numArg > 1
    disp('This function can only handle 3 inputs, x, terror, and optional a, b, c, and t matrices');
elseif numArg == 1
    abct = varargin{1};
    a = abct(1,:)';
    b = abct(2,:)';
    c = abct(3,:)';
    t = abct(4,:)';
else
    a = [15600;11760;11610;15170];
    b = [7540;2750;14630;610];
    c = [10380;16190;7680;13320];
    t = [.05932;.05192;.06242;.05571];
end

t = t + terr*1e-8;

light = 299792.458;


for i = 1:20
f(1,1) = (x(1) - a(1))^2 + (x(2) - b(1))^2 + (x(3) - c(1))^2 - (light*(t(1) - x(4)))^2;
f(2,1) = (x(1) - a(2))^2 + (x(2) - b(2))^2 + (x(3) - c(2))^2 - (light*(t(2) - x(4)))^2;
f(3,1) = (x(1) - a(3))^2 + (x(2) - b(3))^2 + (x(3) - c(3))^2 - (light*(t(3) - x(4)))^2;
f(4,1) = (x(1) - a(4))^2 + (x(2) - b(4))^2 + (x(3) - c(4))^2 - (light*(t(4) - x(4)))^2;

dF = [2*(x(1)-a(1)) 2*(x(2)-b(1)) 2*(x(3)-c(1)) 2*light^2*(t(1)-x(4));... 
      2*(x(1)-a(2)) 2*(x(2)-b(2)) 2*(x(3)-c(2)) 2*light^2*(t(2)-x(4));...
      2*(x(1)-a(3)) 2*(x(2)-b(3)) 2*(x(3)-c(3)) 2*light^2*(t(3)-x(4));...
      2*(x(1)-a(4)) 2*(x(2)-b(4)) 2*(x(3)-c(4)) 2*light^2*(t(4)-x(4))];
  
y = x - dF\f;

x = y;
end  

end

