## Copyright (C) 2020 The Octave Project Developers
## 
## 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  {} {@var{lat}, @var{lon} =} gcxgc (@var{lat1}, @var{lon1}, @var{r1}, @var{lat2}, @var{lon2}, @var{r2})
## @deftypefnx {} {@var{lat}, @var{lon} =} gcxgc (@var{lat1}, @var{lon1}, @var{r1}, @var{lat2}, @var{lon2}, @var{r2}, @var{angleUnit})
## Determines the intersection points between two small circles.
##
## Input:
## @itemize
## @item
## @var{lat1}, @var{lon1}, @var{r1}: latitude, longitude, and range of
## small circle #1.  These must be scalar values or vectors of equal length.
## @end item
##
## @item
## @var{lat2}, @var{lon2}, @var{r2}: latitude, longitude, and range of
## small 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
## @item
## @var{lat} and @var{lon} are both 1x2 vectors
## @end item
##
## Example:
## @example
## lat1 = 37.67;
## lon1 = -90.2;
## rng1 = 1.79;
## lat2 = 36.11;
## lon2 = -90.95;
## rng2 = 2.42;
## [newlat, newlon] = scxsc (lat1, lon1, rng1, lat2, lon2, rng2)
## newlat =
##   36.964   38.260
## newlon =
##  -88.132  -92.343
## @end example
## @seealso{gcxgc, gcxsc, gc2sc}
## @end deftypefn

function [lat, lon] = scxsc (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 ("scxsc: numeric values expected for first six inputs");
  endif
  
    vect = [varargin{1:6}];

  if (! ischar (angleUnit))
    error ("scxsc: character value expected for 'angleUnit'");
  elseif (strncmpi (angleUnit, "degrees", min (length (angleUnit), 7)))
    ## Latitude must be within (-90, 90)
    if (any (abs (vect(:, [1 4])) >= 90))
       error("scxsc: azimuth value(s) out of acceptable range (-90, 90)")
     endif
  vect = deg2rad (vect);
  elseif (strncmpi (angleUnit, "radians", min (length (angleUnit), 7)))
    ## Latitude must be within (-pi/2, pi/2) as azimuth isn't defined there
    if (any (abs (vect(:, [1 4])) >= pi / 2))
       error("scxsc: azimuth value(s) out of acceptable range (-pi/2, pi/2)")
    endif
  else
    error ("scxsc: illegal input for 'angleUnit'");
  endif
  
  c1 = geoc2cart (vect(1), vect(2));
  c2 = geoc2cart (vect(4), vect(5));
  
  ## Algorithm insight comes from
  ## https://gis.stackexchange.com/questions/48937/calculating-intersection-of-two-circles
  
  q = dot (c1, c2);
  q2 = q ^ 2;
  
  if (q2 == 1) 
    error("Octave: coinciding-small-circles\n\
          scxsc: no solution circle centers are either the same or antipodal.\n")
  endif
  n = cross (c1, c2);
  n2 = dot(n, n);
  
  if (n2 == 1) 
    error("Octave: coinciding-small-circles\n\
          scxsc: no solution circle centers are either the same or antipodal.\n")
  endif
  
  r1 = vect(3);
  r2 = vect(6);
  
  a = (cos (r1) - q * cos (r2)) ./ (1 - q2);
  b = (cos (r2) - q * cos (r1)) ./ (1 - q2);
  
  x0 = a * c1 + b * c2;
  x02 = dot(x0, x0);
  if (x02 > 1)
    error("scxsc: circles do not intersect\n")
  endif
  
  t = sqrt ((1 - x02) ./ n2);
  
  p1 = x0 + (t * n);
  p2 = x0 - (t * n);
  
  lat1 = atan2(p1(3), hypot(p1(1),p1(2)));
  lon1 = atan2 (p1(2), p1(1));

  lat2 = atan2 (p2(3), hypot(p2(1),p2(2)));
  lon2 = atan2 (p2(2), p2(1));
  
  lat = [lat1 lat2];
  lon = [lon1 lon2];
  
  if (strncmpi (angleUnit, "degrees", length (angleUnit)))
    lat = rad2deg (lat);
    lon = rad2deg (lon);
  endif

endfunction

function [c] = geoc2cart(lat, lon)
  c(1) = cos (lon) * cos (lat);
  c(2) = sin (lon) * cos (lat);
  c(3) = sin (lat);
endfunction

%!test
%! R1 = rad2deg (107.5 / earthRadius ("NM")); #Convert NM to deg
%! R2 = rad2deg (145 / earthRadius ("NM")); 
%! [lat, lon] = scxsc (37.673442, -90.234036, R1, 36.109997, -90.953669, R2);
%! assert (lat(1), 36.988778646, 1e-6);
%! assert (lon(1), -88.15335362, 1e-6);

%!error <circles> scxsc (45, 90, 1, -30, 60, 1) 
%!error <no solution> scxsc (45, 90, 1, 45, 90, 2)

%!error <numeric> scxsc ("s", 0, 100, 10, 30, 0)
%!error <numeric> scxsc (3i, 0, 100, 10, 30, 0)
%!error <numeric> scxsc (50, "s", 100, 10, 30, 0)
%!error <numeric> scxsc (50, 2i, 10, 10, 30, 0)
%!error <numeric> scxsc (50, 0, "s", 10, 30, 0)
%!error <numeric> scxsc (50, 0, 100i, 10, 30, 0)
%!error <numeric> scxsc (50, 0, 100, "s", 30, 0)
%!error <numeric> scxsc (50, 0, 100, 10i, 30, 0)
%!error <numeric> scxsc (50, 0, 100, 10, "s", 0)
%!error <numeric> scxsc (50, 0, 100, 10, 30i, 0)
%!error <numeric> scxsc (50, 0, 100, 10, 30, "s")
%!error <numeric> scxsc (50, 0, 100, 10, 30, 2i)
%!error <illegal> scxsc (50, 0, 100, 10, 30, 0, "f")
%!error <illegal> scxsc (50, 0, 100, 10, 30, 0, "degreef")
%!error <azimuth value> scxsc (190, 0, 90, -90.000001, 180, 80);
%!error <azimuth value> scxsc (190, 0, -90.001, -90.000001, 180, 80);
%!error <azimuth value> scxsc (pi/1.999, 0, pi/2, pi/2.0001, 2, 2*pi/3, "r");
