%
%   llh = ecef2llh (ecef)
%   
%   ecef2llh: converts ECEF to Lat, Lon, Height in WGS-84
%
%   Input:  ecef [3xN] = x,y,z (user units)
%   Output: llh  [3xN] = lat,long,height location (rad,rad,user units)
%
%   EE6900 - Flight Management Systems
% 
%   Maarten Uijt de Haag
%
%   Based on: OU-ECE-AEC   Oct. 1988  FvG
%
function llh = ecef2llh (ecef)

    A = 6378137.0;
    E = 8.1819190842622e-02;
    ESQ = E*E;

    llh = 0.0*ecef;
    
    X = ecef(1,:);
    Y = ecef(2,:);
    Z = ecef(3,:);

    RSQ = (X.*X) + (Y.*Y);
    H = ESQ .* Z;

    %cnt = 0;
    %done = 0;
    %while cnt < 10 & done == 0,
    for cnt = 1:10,
        %cnt = cnt + 1;
        ZP = Z + H;
        R = sqrt(RSQ + (ZP .* ZP));
        SP = ZP ./ R;
        GSQ = 1.0 - (ESQ .* SP .* SP);
        EN = A ./ sqrt(GSQ);
        P = EN .* ESQ .* SP;
        %if abs(H-P) < 5.0e-04, done = 1; end;
        H = P;
    end;
    P = atan2( ZP, sqrt(RSQ) );
    H = R - EN;
    llh(1,:) = P;
    llh(2,:) = atan2(Y,X);
    llh(3,:) = H;

return