## 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{X} , @var{Y} , @var{Z} =}  geodetic2ecef (@var{spheroid}, @var{lat} , @var{lon} , @var{alt} , @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
## lat=57.02929569; 
## lon=9.950248114;
## h= 56.95; # meters
## >> [X, Y, Z] = geodetic2ecef ("", lat, lon, h)
## X =     3426949.39675307
## Y =     601195.852419885
## Z =     5327723.99358255
## @end example
## @seealso{ecef2geodetic}
## @end deftypefn

function [X Y Z] = geodetic2ecef ( spheroid , lat , lon , alt , 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 ("geodetic2ecef.m: angleUnit should be one of 'degrees' or 'radians'")
  endif
  
  if (! isnumeric (lat) || ! isreal (lat) || ! isnumeric (lon) || ! isreal (lon) || ! isnumeric (alt) || ! isreal (alt))
    error ("geodetic2ecef.m : numeric input expected");
  elseif (strncmpi (lower (angleUnit), "r", 1) == 1)
    c_p = cos (lat);
    s_p = sin (lat);
    
    c_l = cos (lon);
    s_l = sin (lon);
  else 
    c_p = cosd (lat);
    s_p = sind (lat);
    
    c_l = cosd (lon);
    s_l = sind (lon);
  endif

  
  #Insight From: Algorithms for Global Positioning pg 42
  N = E.SemimajorAxis / sqrt (1 - E.Eccentricity ^ 2 * s_p ^ 2);
  X = (N + alt) * (c_p * c_l) ; 
  Y = (N + alt) * (c_p * s_l) ; 
  Z = (N * (1 - E.Flattening) ^ 2  + alt) * s_p;
  
endfunction

%!test
%!
%! latd = 57.02929569;
%! lond = 9.950248114;
%! h = 56.95; #meters
%! [x, y, z]=geodetic2ecef("wgs84", latd, lond, h);
%! assert( [x, y, z], [3426949.397, 601195.852, 5327723.994] , 10e-3);
%! 
%! lat = deg2rad (57.02929569);
%! lon = deg2rad (9.950248114);
%! [x2, y2, z2]=geodetic2ecef("wgs84", lat, lon, h, "radians");
%! assert( [x2, y2, z2], [3426949.397, 601195.852, 5327723.994], 10e-3);

%!error <angleUnit> geodetic2ecef ("", 45, 45, 50, "km")
%!error <numeric input expected>  geodetic2ecef ("", "A", 45, 50)
%!error <numeric input expected>  geodetic2ecef ("", 45i, 45, 50)
%!error <numeric input expected>  geodetic2ecef ("", 45, "B", 50)
%!error <numeric input expected>  geodetic2ecef ("", 45, 45i, 50)
%!error <numeric input expected>  geodetic2ecef ("", 45, 45, "C")
%!error <numeric input expected>  geodetic2ecef ("", 45, 45, 50i)