## Copyright (C) 2020
##
## 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  {Functional File} {@var{lat}, @var{lon} =} gcxgc (@var{lat1}, @var{lon1}, @var{az1}, @var{lat2}, @var{lon2}, @var{az2})
## @deftypefnx {Functional File} {@var{lat}, @var{lon} =} gcxgc (@var{lat1}, @var{lon1}, @var{az1}, @var{lat2}, @var{lon2}, @var{az2}, @var{angleUnit})
## Determines the intersection points between two great circles
##
## Input:
## @itemize
## @item
## @var{lat1}, @var{lon1}, @var{az1}: latitude, longitude, and azimuth of
## great circle #1.  These must be scalar values or vectors of equal length.
## @end item
##
## @item
## @var{lat2}, @var{lon2}, @var{az2}: latitude, longitude, and azimuth of
## great circle #2.  These must be scalar values or vectors of equal length.
## @end item
##
## @item
## @var{angleUnit}: string for angular units ('degrees' or 'radians',
## case-insensitive, just the first character will do).  Default is 'degrees'.
## @var{angleUnit} applies to all inputs and outputs.
## @end item
## @end itemize
##
## Output:
## If scalar values have been input, @var{lat} and @var{lon} are both 1x2
## vectors.  If vectors have been input @var{lat} and @var{lon} are Nx2 arrays
## where N is the number of great circle pairs.  The results for multiple
## great circle pairs are concatenated vertically no matter the orientation of
## input vectors.
##
## Example:
## @example
## lat1 = 51.8853;
## lon1 = 0.2545;
## az1  = 108.55;
## lat2 = 49.0034;
## lon2 =  2.5735;
## az2  =  32.44;
## [newlat, newlon] = gcxgc (lat1, lon1, az1, lat2, lon2, az2)
## newlat =
##   50.908  -50.908
## newlon =
##     4.5086  -175.4914
## @end example
## @end deftypefn


function [lat, lon] = gcxgc2 (varargin)

  if (nargin < 6)
    print_usage();
  elseif (nargin == 6)
    angleUnit = "degrees";
  else
    angleUnit = varargin{7};
  endif

  if (! (all (cellfun ("isnumeric", varargin(1:6)) && ...
         all (cellfun ("isreal", varargin(1:6))))))
     error ("gcxgc: numeric values expected for first six inputs");
  endif

  isv = ! cellfun ("isscalar", varargin(1:6));
  if (any (isv))
    ## At least one of the location inputs is a vector. Check sizes
    numval = cellfun ("numel", varargin(isv));
    if (any (diff (numval)))
      error ("gcxgc: all vector inputs must have same lengths");
    endif
    ## Make sure all inputs are column vectors of same length
    for ii=1:6
      if (isv(ii))
        varargin(ii) = {varargin{ii}(:)};
      else
        varargin(ii) = {(repmat (varargin{ii}, numval(1), 1))};
      endif
    endfor
  endif

  if (! ischar (angleUnit))
    error ("gcxgc: character value expected for 'angleUnit'");
  elseif (strncmpi (angleUnit, "degrees", min (length (angleUnit), 7)))
    vect = deg2rad ([varargin{:}]);
  elseif (strncmpi (angleUnit, "radians", min (length (angleUnit), 7)))
    vect = [varargin{:}];
  else
    error ("gcxgc: illegal input for 'angleUnit'");
  endif

  ## Algorithm from http://www.movable-type.co.uk/scripts/latlong.html
  del_lat = abs (vect(:, 1) - vect(:, 4));
  del_lon = abs (vect(:, 2) - vect(:, 5));

  del_12 = 2 * asin (sqrt (sin (del_lat / 2) .^ 2 .+ cos (vect(:, 1)) .* cos (vect(:, 4)) .* sin (del_lon / 2) .^ 2));
  theta_a = acos ((sin (vect(:, 4)) .- sin (vect(:, 1)) .* cos (del_12)) ./ (sin (del_12) .* cos (vect(:, 1))));
  theta_b = acos ((sin (vect(:, 1)) .- sin (vect(:, 4)) .* cos (del_12)) ./ (sin (del_12) .* cos (vect(:, 4))));

  theta_12 = theta_a;
  theta_21 = theta_b;
  isgn = sin (vect(:, 5) .- vect(:, 2)) > 0;
  theta_21(isgn)   = 2 * pi - theta_21(isgn);
  theta_12(! isgn) = 2 * pi - theta_12(! isgn);

  alpha1 = vect(:, 3) .- theta_12;
  alpha2 = theta_21 .- vect(:, 6);

  alpha3 = acos (-cos (alpha1) .* cos(alpha2) + ...
                 sin (alpha1) .* sin (alpha2) .* cos (del_12));

  del_13 = atan2 (sin (del_12) .* sin (alpha1) .* sin (alpha2), ...
                  cos (alpha2) .+ cos (alpha1) .* cos (alpha3));

  lat3 = asin (sin (vect(:, 1)) .* cos (del_13) .+ ...
               cos (vect(:, 1)) .* sin (del_13) .* cos (vect(:, 3)));

  del_lon13 = atan2 (sin (vect(:, 3)) .* sin (del_13) .* cos (vect(:, 1)), ...
                     cos (del_13) .- sin (vect(:, 1)) .* sin (lat3));
  lon3 = vect(:, 2) .+ del_lon13;
  [alat3 alon3] = antipode (lat3, lon3, "r");

  lat = [lat3 alat3];
  lon = [lon3 alon3];
  idx = sin (alpha1) .* sin(alpha2) < 0;
  if (any (idx))
    lon(idx, 1) = 0;    ## Causes ambiguity
    lon(idx, 2) = pi;
  endif

  if (strncmpi (angleUnit, "degrees", length (angleUnit)))
    lat = rad2deg (lat);
    lon = rad2deg (lon);
  endif


endfunction

%!test
%! [lat3, lon3] = gcxgc2 ( 51.8853, 0.2545, 108.55, 49.0034, 2.5735, 32.44);
%! assert(degrees2dms (lat3(1)), [50 54 27], 10-3)
%! assert(degrees2dms (lon3(1)), [04 30 31], 10-3)

%!error <numeric> gcxgc2 ("s", 0, 100, 10, 30, 0)
%!error <numeric> gcxgc2 (3i, 0, 100, 10, 30, 0)
%!error <numeric> gcxgc2 (50, "s", 100, 10, 30, 0)
%!error <numeric> gcxgc2 (50, 2i, 10, 10, 30, 0)
%!error <numeric> gcxgc2 (50, 0, "s", 10, 30, 0)
%!error <numeric> gcxgc2 (50, 0, 100i, 10, 30, 0)
%!error <numeric> gcxgc2 (50, 0, 100, "s", 30, 0)
%!error <numeric> gcxgc2 (50, 0, 100, 10i, 30, 0)
%!error <numeric> gcxgc2 (50, 0, 100, 10, "s", 0)
%!error <numeric> gcxgc2 (50, 0, 100, 10, 30i, 0)
%!error <numeric> gcxgc2 (50, 0, 100, 10, 30, "s")
%!error <numeric> gcxgc2 (50, 0, 100, 10, 30, 2i)
%!error <illegal> gcxgc2 (50, 0, 100, 10, 30, 0, "f")
%!error <illegal> gcxgc2 (50, 0, 100, 10, 30, 0, "degreef")
