function [lon,lat,x] = gps()
%  ECE 6390 radiolocation scavenger hunt problem 2
%  By the Gibbs Phenomenon
clear;
format long;
%(Xi - Ux)^2 + (Yi - Uy)^2 + (Zi - Uz)^2 = (Pri-cTau)^2

r = 20200 + 6380;  % radius in kilometers
c = 299860;  % speed of light in km/s

SatPol = [-62.3256 43.2345 0;
        -85.2234 35.5521 -0.001587009;
        -90.9760 29.0510 -0.001373255;
        -95.0234 40.7777 -0.001092284];
    
        
        

for i = 1:4
    Sat(i,1) = r*cos(SatPol(i,2)*2*pi/360)*cos(SatPol(i,1)*2*pi/360);
    Sat(i,2) = r*cos(SatPol(i,2)*2*pi/360)*sin(SatPol(i,1)*2*pi/360);
    Sat(i,3) = r*sin(SatPol(i,2)*2*pi/360);
end

% get atlanta coords
ATLPOL = [-84.4 33.76 6380];
ATL(1) = ATLPOL(3)*cos(ATLPOL(2)*2*pi/360)*cos(ATLPOL(1)*2*pi/360);
ATL(2) = ATLPOL(3)*cos(ATLPOL(2)*2*pi/360)*sin(ATLPOL(1)*2*pi/360);
ATL(3) = ATLPOL(3)*sin(ATLPOL(2)*2*pi/360);

%  Now Matrix Sat contains X, Y, and Z data in its columns, respectively.

%  Now, assume that we're located on the first satellite then use the
%  Jacobian to determine our actual location with respect to it.

x0 = [ATL';0];

eta = zeros(4,1);
H = zeros(4,4);

for j = 1:100

    for(j = 1:4)
        a(j) = (Sat(j,1) - x0(1))^2 + (Sat(j,2) - x0(2))^2 +(Sat(j,3) - x0(3)).^2;
    end    

    for i = 1:4    
        H(i,1) = -(a(i)^(-1/2)/c)*(Sat(i,1) - x0(1));
        H(i,2) = -(a(i)^(-1/2)/c)*(Sat(i,2) - x0(2));
        H(i,3) = -(a(i)^(-1/2)/c)*(Sat(i,3) - x0(3));
        H(i,4) = 1;
    end
    
    for(k = 1:4)
        eta(k) = (1/c)*sqrt((Sat(k,1) - x0(1))^2 + (Sat(k,2) - x0(2))^2 +(Sat(k,3) - x0(3)).^2) + x0(4) - SatPol(k,3);
    end
    
    x = x0 - inv(H)*eta;
    
    x0 = x;
    
end

lon = atan2(x(2),x(1))*360/(2*pi);
lat = atan(x(3)/sqrt(x(1)^2 + x(2)^2))*360/(2*pi);

lat = deg2dms(lat);
lon = deg2dms(lon);
