function LLA = xyz2geo( X, Y, Z )

%*********************************************************************
% * Name:    wgs84_xyz2geo(
% *
% * Purpose:     Converts Cartesian (X,Y,Z) Co-ordinates into Geodetic
% *      Latitude, Longitude and Height for the WGS84 datum.
% *
% * The method used here is adopted from Journal Paper "Fast transform from
% * geocentric to geodetic coordinates", published in Journal of Geodesy
% * (1999) Vol 73, pp 603-610, by T. Fukushima.
% *
% * This method has the following advantages:
% * (1) Fastest among all the methods. Eventhough it is iterative, it requres no
% *     trigonomical functions, only addition, multiplication and one devision, 
% *     thus significantly faster in computation.
% * (2) Convergence in the whole global, including polar region and
% *     high altitude.  Very stable and precise.
% * (3) Normally, it only takes 4 ~ 5 iterations.
% *
% * The original implementation of non-iterative method suffers a precision
% * loss in polar regions.  The errors can reach as big as 3Km in height when
% * close to north or south poles.
% ****************************************************************************/

% define some constants for the WGS84 ellipsoid
A_WGS84 = 6378137.0;
B_WGS84 = 6356752.314245179;
E2_WGS84 = 6.69437999013e-3;
SQRT_ONE_MIN_E2 = 9.96647189335258e-1;
c = (A_WGS84 * E2_WGS84);
foure2 = (4.0*E2_WGS84);
e_prime = SQRT_ONE_MIN_E2;
ae_prime = (A_WGS84 * SQRT_ONE_MIN_E2);

t = 0.0;

% Take the sign of Z-componet 
if (Z < 0.0)
   singOfZ = -1.0;
else
   signOfZ = +1.0;
end

% compute the radial distance of the point in the X-Y-plane. 
p = sqrt( X .* X + Y .* Y );
   
% at the poles to within a cm
if( p < 0.01 ) 
  if( Z >= 0.0 ) 
    % at the north pole
    Lat = PI./2.0;
  else
    % at the south pole
    Lat = -PI./2.0;
  end
  Long = 0.0
  Ht = abs(Z) - B_WGS84;
else
  % inverse of p
  p_inv = 1.0./p;
  z_prime = e_prime .* abs(Z);
  z_prime_plus_c  = z_prime + c;
  z_prime_minus_c = z_prime - c;
  u = 2.0 .* z_prime_minus_c;
  v = 2.0 .* z_prime_plus_c;

  % figure out the starting point
  t_m = -z_prime_minus_c .* p_inv;

  if( t_m <= 0.0 )
    t = (p + z_prime_minus_c);
    t = t./(t + z_prime);
  elseif( t_m >= 1.0 )
    t = p./z_prime_plus_c;
  else  
    % Evaluate the function value first 
    t_m3 = t_m .* t_m .* t_m;
    fm = p .* t_m3 .* t_m + u .* t_m3 + v .* t_m - p;

    if( fm >= 0.0 )
      t = p./z_prime_plus_c;
    else  
      t = (p + z_prime_minus_c);
      t = t./(t + z_prime);
    end
  end
    
        
  % We got the starter now.  Go for the iteration for t 
  % Set delta_t to 10e-7 for convergence
  for count=0:10
    count = count + 1;
           
    t_m2 = t .* t;
    pt = p .* t;

    delta_t = (p-((pt+u).*t_m2 + v) .* t)./((4.0.*pt+3.0.*u).*t_m2+v);

    t = t + delta_t;
      
    if (abs(delta_t) <= 0.0000001)
      break;
    end
  end


  % Compute the latitude, longitude and height
  t_m2 = t .* t;
  one_plus_t2  = 1.0 + t_m2;
  one_minus_t2 = 1.0 - t_m2;

  Lat = atan2( one_minus_t2, 2.0 .* e_prime .* t ) .* signOfZ;
    
  % Calculate the Longitude 
  Long = atan2 ( Y, X );

  % Calculate height
  Ht = (2.0.*p.*e_prime.*t+abs(Z).*one_minus_t2-ae_prime.*one_plus_t2)./(sqrt( one_plus_t2 .* one_plus_t2 - foure2 .* t_m2));
end

LLA(:,1) = Lat;
LLA(:,2) = Long;
LLA(:,3) = Ht;

