## Copyright (C) 2018 Anonymous
## 
## This program is free software; you can redistribute it and/or modify it under
## the terms of the GNU General Public License as published by the Free Software
## Foundation; either version 3 of the License, or (at your option) any later
## version.
## 
## This program is distributed in the hope that it will be useful, but WITHOUT
## ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
## FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
## details.
## 
## You should have received a copy of the GNU General Public License along with
## this program; if not, see <http://www.gnu.org/licenses/>.

## -*- texinfo -*-
## @deftypefn  {Function File} {@var{lat} , @var{lon} , @var{alt} =}  geodetic2ecef (@var{spheroid}, @var{X}, @var{Y}, @var{Z}, @var{angleUnit}) 
## Converts from geodetic coordinate frame to ecef coordinate frame
## Can take a spheroid 
## using "" is the default wgs84
## The default input is in degrees use optional fifth parameter for radians 
## The output is in meters
## 
## @example
## Aalborg GPS Centre
## X =     3426949.39675307
## Y =     601195.852419885
## Z =     5327723.99358255
## lat=57.02929569; 
## lon=9.950248114;
## h= 56.95; # meters
## >> [lat, lon, alt] = geodetic2ecef ("", X, Y, Z)
## lat = 57.029
## lon = 9.9502
## alt = 56.95
## @end example
## @seealso{geodetic2ecef}
## @end deftypefn

function [lat, lon, alt] = ecef2geodetic (spheroid, X, Y, Z, angleUnit="degrees")

  if (nargin < 4)
    print_usage ();
  endif
  
  if isempty(spheroid)
    E = wgs84Ellipsoid;
  else
    E = referenceEllipsoid (spheroid);
  endif 

  if (! ischar (angleUnit) || ! ismember (lower (angleUnit(1)), {"d", "r"}))
    error ("ecef2geodetic.m: angleUnit should be one of 'degrees' or 'radians'")
  endif
  
  if (! isnumeric (X) || ! isreal (X) || ! isnumeric (Y) || ! isreal (Y) || ! isnumeric (Z) || ! isreal (Z))
    error ("ecef2geodetic.m : numeric input expected");
  endif
  
  #Insight from: http://wiki.gis.com/wiki/index.php/Geodetic_system
  lon = atan2 (Y, X);
  
  ecc = E.Eccentricity;
  e_sq = ecc ^ 2;
  ep_2 = e_sq / (1 - e_sq) ; #This is (e')^2
  r = hypot (X, Y);
  E2 = E.SemimajorAxis ^ 2 - E.SemiminorAxis ^ 2;
  F = 54 * E.SemiminorAxis ^ 2 * Z ^ 2;
  G = r ^ 2 + (1 - e_sq) * Z ^ 2 - e_sq * E2;
  C = (ecc ^ 4 * F * r ^ 2) / (G ^ 3);
  S = (1 + C + sqrt (C ^ 2 + 2 * C)) ^ (1 / 3);
  P = F / (3 * (S + 1 / S + 1) ^ 2 * G ^ 2);
  Q = sqrt (1 + 2 * ecc ^ 4 * P);
  r0 = -(P * e_sq * r) / (1 + Q) + sqrt (.5 * E.SemimajorAxis ^ 2 * (1 + 1 / Q) - (P * (1 - e_sq) * Z ^ 2) / (Q ^ 2 + Q) - .5 * P * r ^ 2);
  U = sqrt ((r - e_sq * r0) ^ 2 + Z ^ 2);
  V = sqrt (( r - e_sq * r0) ^ 2 + (1 - e_sq) * Z ^ 2);
  Frac =  E.SemiminorAxis ^ 2  / (E.SemimajorAxis * V);
  Z0 = Frac * Z;
  alt = U * (1 - Frac);
  lat = atan2 ((Z + ep_2  * Z0) , r) ;
  
  if ( strncmpi (lower (angleUnit), "d", 1) == 1 )
    lon = rad2deg (lon);
    lat = rad2deg (lat);
  endif
  
endfunction

%!test
%!
%! X = 3426949.397;
%! Y = 601195.852;
%! Z = 5327723.994; #meters
%! [latd, lond, h] = ecef2geodetic ('wgs84',X ,Y ,Z);
%! assert( [latd, lond, h] , [57.02929569, 9.950248114, 56.95] , 10e-3);
%!  
%! latr = deg2rad (57.02929569);
%! lonr = deg2rad (9.950248114);
%! [lat, lon, h2] = ecef2geodetic ('wgs84', X, Y, Z, "radians");
%! assert([lat, lon, h2] , [ latr, lonr, 56.95] , 10e-3);

%!error <angleUnit> ecef2geodetic ("", 4500000, 450000, 50000000, "km")
%!error <numeric input expected>  ecef2geodetic ('', "A", 450000, 50000000)
%!error <numeric input expected>  ecef2geodetic ('', 45i, 450000, 50000000)
%!error <numeric input expected>  ecef2geodetic ('', 4500000, "B", 50000000)
%!error <numeric input expected>  ecef2geodetic ('', 4500000, 45i, 50000000)
%!error <numeric input expected>  ecef2geodetic ('', 4500000, 450000, "C")
%!error <numeric input expected>  ecef2geodetic ('', 4500000, 450000, 50i)