## Copyright (C) 2014-2020 Michael Hirsch, Ph.D.
## Copyright (C) 2013-2020 Felipe Geremia Nievinski
## Copyright (C) 2019-2020 Philip Nienhuis
##
## Redistribution and use in source and binary forms, with or without 
## modification, are permitted provided that the following conditions are met:
## 1. Redistributions of source code must retain the above copyright notice, 
##    this list of conditions and the following disclaimer.
## 2. Redistributions in binary form must reproduce the above copyright notice, 
##    this list of conditions and the following disclaimer in the documentation 
##    and/or other materials provided with the distribution.
## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
## AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
## THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
## ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
## LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
## CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
## SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
## OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
## WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
## (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
## SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

## -*- texinfo -*-
## @deftypefn {Function File} {@var{u}, @var{v}, @var{w} =} enu2uvw (@var{east}, @var{north}, @var{up}, @var{lat0}, @var{lon0})
## @deftypefnx {Function File} {@var{u}, @var{v}, @var{w} =} enu2uvw (@var{east}, @var{north}, @var{up}, @var{lat0}, @var{lon0}, @var{angleUnit})
## Convert ENU coordinates to UVW coordinates.
##
## Inputs:
##
## @var{east}, @var{north}, @var{up}: East, North, Up: coordinates of point(s)
## (meters)
##
## @var{lat0}, @var{lon0}: geodetic coordinates of observer/reference point
## (degrees)
##
## @var{angleUnit}: string for angular units ('degrees' or 'radians',
## case-insensitive, first character will suffice).  Default = 'degrees'.
##
## Outputs:
## 
## @var{u}, @var{v}, @var{w}:  coordinates of point(s) (meters).
##
## @end deftypefn

## Function adapted by anonymous contributor, see:
## https://savannah.gnu.org/patch/index.php?8377

function [u, v, w] = enu2uvw (east, n, up, lat0, lon0, angleUnit = "degrees")

  if (nargin < 5)
    print_usage ();
  endif  

  if (! isnumeric (east) || ! isreal (east) || ...
      ! isnumeric (n)    || ! isreal (n) || ...
      ! isnumeric (up)   || ! isreal (up) || ...
      ! isnumeric (lat0) || ! isreal (lat0) || ...
      ! isnumeric (lon0) || ! isreal (lon0))
    error ("enu2uvw.m : numeric values expected for first 5 arguments");
  endif
  
  if (! ischar (angleUnit))
    error ("enu2uvw.m: character value expected for 'angleUnit'");
  elseif (strncmpi (angleUnit, "degrees", length (angleUnit)))
    lat0 = deg2rad (lat0);
    lon0 = deg2rad (lon0);
  elseif (! strncmpi (angleUnit, "radians", length (angleUnit)))
    error ("enu2uvw.m: illegal input for 'angleUnit'");
  endif    
  
  t = cos (lat0) * up - sin (lat0) * n;
  w = sin (lat0) * up + cos (lat0) * n;

  u = cos (lon0) * t - sin (lon0) * east;
  v = sin (lon0) * t + cos (lon0) * east;
  
endfunction

%!test
%! [u, v, w] = enu2uvw (186.277521, 286.84222, 939.69262, 42, -82, "d"); 
%! assert ([u, v, w], [254.940936348589, -475.5397947444, 841.942404132992], 10e-6)

%!test
%! [u, v, w] = enu2uvw (186.277521, 286.84222, 939.69262, ... 
%!                      0.733038285837618, -1.43116998663535, "r"); 
%! assert ([u, v, w], [254.940936348589, -475.5397947444, 841.942404132992], 10e-6)

%!error <numeric> enu2uvw("s", 25, 1e3, 0, 0)
%!error <numeric> enu2uvw(3i, 25, 1e3, 0, 0)
%!error <numeric> enu2uvw(33, "s", 1e3, 0, 0)
%!error <numeric> enu2uvw(33, 3i, 1e3, 0, 0)
%!error <numeric> enu2uvw(33, 25, "s", 0, 0)
%!error <numeric> enu2uvw(33, 25, 3i, 0, 0)
%!error <numeric> enu2uvw(33, 25, 1e3, "s", 0)
%!error <numeric> enu2uvw(33, 25, 1e3, 3i, 0)
%!error <numeric> enu2uvw(33, 25, 1e3, 0, "s")
%!error <numeric> enu2uvw(33, 25, 1e3, 0, 3i)
%!error <illegal> enu2uvw (33, 70, 1e3, 0, 0, "f");
%!error <illegal> enu2uvw (33, 70, 1e3, 0, 0, "degreef");
