function [lph] = xyz2lph(xyz,a,f_inv)
%function [lph,VClph] = xyz2lph(xyz,a,f_inv,VCxyz)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function: xyz2lphe.m
% purpose: Convert X, Y, & Z coord's with errors to 
%          lon, lat, & ellipsoidal height with errors
% author: David J. VandenBerg
% status: developed
% version: Matlab 5.2
% last date of development: May '99
% variables:
%   Input:
%     xyz=1x3 column mtx of coordinates
%     a=semi-major axis
%     f_inv=inverse flattening
%     VCxyz=3x3 variance-covariance mtx of coordinates
%   Output:
%     lph=1x3 column mtx of coordinates
%     VClph=3x3 variance-covariance mtx of coordinates
% example usage: [lph,Dlph]=xyz2lphe(mtx,a,f^-1,mtx)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
eSQ=2*1/f_inv-(1/f_inv)^2;                  % first eccentricity
XYdist=sqrt(xyz(:,1).^2+xyz(:,2).^2);           % distance in the XY plane

lph(:,1)=atan2(xyz(:,2),xyz(:,1));                % calc. longitude
j01=find(lph(:,1)<0);
if (isempty(j01)==0)
   lph(j01,1)=2*pi+lph(j01,1);                      % make lon positive
end;

lph(:,2)=[atan(xyz(:,3)./XYdist)+10E-15];        % initial lat. approximation
convergence=1;                              % junk convergence value
while convergence > 10e-12                  % loop & test for convergence
   W=sqrt(1-eSQ*sin(lph(:,2)).^2);             % "W" constant for latitude
   N=a./W;                                   % radius of cuvature in the prime vertical
   size(lph(:,2))     ;% ; by VBN
   size(W)            ;% ; by VBN
   size(N)            ;% ; by VBN
   phi=atan((xyz(:,3)+eSQ.*N.*sin(lph(:,2)))./XYdist);     % Calc. lat
   convergence=max(abs(lph(:,2)-phi));        % calc. convergence
   lph(:,2)=phi;                              % reset lat's
end;

% calc. ellipsoidal height
lph(:,3)=sqrt(xyz(:,1).^2+xyz(:,2).^2+(xyz(:,3)+eSQ.*N.*(sin(lph(:,2)))).^2)-N;

% % check for std dev's
% if nargin==3
%    % no error propagation
% elseif nargin==4
%    % ellipsoid constants
%    W=sqrt(1-eSQ*sin(lph(2))^2);                % "W" constant for latitude
%    M=a*(1-eSQ)/W^3;                            % radius of curvature in the meridian
%    N=a/W;                                      % radius of curvature in the prime vertical
%    
%    % jacobian: xyz WRT lph
%    J=[-(N+lph(3))*sin(lph(1))*cos(lph(2))  -(M+lph(3))*cos(lph(1))*sin(lph(2))  cos(lph(1))*cos(lph(2))
%       (N+lph(3))*cos(lph(1))*cos(lph(2))   -(M+lph(3))*sin(lph(1))*sin(lph(2))  sin(lph(1))*cos(lph(2))
%       0                                    (M+lph(3))*cos(lph(2))               sin(lph(2))];
%    
%    % compile solution mtx
%    J_inv=inv(J);                % inverse of jacobian
%    VClph=J_inv*VCxyz*J_inv';    % solution mtx for loop
% else
%    disp('error: incorrect number of inputs for "xyz2lph"');
% end;
% return;
