##  Copyright (C) 2014  Daniel Kraft <d@domob.eu>
##  GNU Octave level-set package.
##
##  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{d} =} ls_solve_stationary (@var{phi}, @var{f}, @var{h} = 1)
## @deftypefnx {Function File} {@var{d} =} ls_solve_stationary (@var{phi}, @var{f}, @var{h} = 1, @var{nb})
## 
## Solve a generalised Eikonal equation with speeds of arbitrary
## signs.  The equation solved is
## @tex
## \begin{equation*}
##   f \left| \nabla d \right| = 1
## \end{equation*}
## @end tex
## @ifnottex
##
## @example
## f | grad d | = 1
## @end example
##
## @end ifnottex
## with d = 0 on the boundary.  The domain is described by the level-set
## function @var{phi} on a rectangular grid.  @var{f} should contain the values
## of the speed field on the grid points.  @var{h} can be given as the
## grid spacing.  In the second form, where the optional @var{nb} is given,
## it is used to initialise the narrow band with a manual calculation.
## By default, the result of @code{ls_init_narrowband} is used.  Values which
## are not fixed by the narrow band should be set to @code{NA}.
##
## Note that in comparison to @code{fastmarching}, the speed need not be
## positive.  It is the reciprocal of @var{f} in @code{fastmarching}.
##
## This is a preparation step, and afterwards, the evolved geometry according
## to the level-set equation
## @tex
## \begin{equation*}
##   \phi_t + f \left| \nabla \phi \right| = 0
## \end{equation*}
## @end tex
## @ifnottex
##
## @example
## d/dt phi + f | grad phi | = 0
## @end example
##
## @end ifnottex
## can be extracted from @var{d} at arbitrary positive times using
## the supplemental function @code{ls_extract_solution}.
##
## At points where @var{f} is exactly zero, the output will be set
## to @code{NA}.  This case is handled separately in @code{ls_extract_solution}.
## In the narrow band, the returned distances may actually be negative
## even for positive @var{f} and vice-versa.  This helps to avoid
## unnecessary errors introduced into the level-set function due to
## a finite grid-size when the time step is chosen small.
##
## @seealso{ls_extract_solution, ls_signed_distance, ls_nb_from_geom}
## @end deftypefn

function d = ls_solve_stationary (phi, f, h = 1, nb)
  if (nargin () < 2 || nargin () > 4)
    print_usage ();
  endif

  sz = size (phi);
  if (~all (sz == size (f)))
    error ("PHI and F must be of the same size");
  endif

  if (nargin () < 4)
    nb = ls_init_narrowband (phi, h);
  elseif (~all (sz == size (nb)))
    error ("PHI and NB must be of the same size");
  endif

  % Find regions of signs of f.  Below for the main calculations,
  % we will always ignore f = 0 regions and will mostly work with
  % the absolute value of f, fixing the sign of d at the end.
  fp = (f > 0);
  fz = (f == 0);
  fn = (f < 0);
  fabs = abs (f);

  % Find regions of the domain.
  inner = (phi < 0);
  outer = (phi > 0);

  % Initialise narrow-band distances.  Scale them (roughly) according
  % to our f.
  d0 = nb;
  d0(~fz) ./= fabs(~fz);

  % Handle points where the front moves away (depending on the signs of
  % phi and f).  In theory, these distances should be zero according to
  % the definition in the theoretical paper.  Set them to Inf or -Inf
  % in the numerics for points deeply inside, and leave the narrow-band
  % values for points near the boundary.  Since fastmarching does not
  % allow -Inf, choose the signs such that the points will be passed with
  % positive Inf to fastmarching.  The sign will be reverted later
  % on to get the value matching the distance semantics (-Inf inside with
  % front moving away and Inf outside).
  % For phi = 0, nothing special needs to be done as d0 is set to zero there
  % already by internal_init_narrowband.
  d0(isna (d0) & inner & fp) = Inf;
  d0(isna (d0) & outer & fn) = -Inf;
  assert (all (isna (d0(:)) | ~isnan(d0(:))));

  % Solve for both parts.  Invert the negative one's distances.  Also for
  % the "islands" with -Inf there, this is fine, as islands mean that the
  % respective points are never reached by the front, thus never taken
  % out of the domain, and thus D < -t should always be true for them
  % and all arbitrarily large times t.
  dp = solvePart (d0, fp, h, fabs);
  % Be careful with -NA ~= NA!
  invD0 = -d0;
  invD0(isna (d0)) = NA;
  dn = -solvePart (invD0, fn, h, fabs);

  % Build everything together.
  d = NA (size (phi));
  d(fp) = dp(fp);
  d(fn) = dn(fn);
endfunction

%   D = solvePart (D0, REGION, H, FABS)
%
% Utility function that performs one of the two solves required for the
% regions with positive and negative f.  D0 should be the initial distances
% (all positive), REGION a boolean flag of the region we process (f > 0
% or f < 0), H the grid spacing and FABS the absolute value of f.
%
% We solve in the given REGION using fast marching for the distances
% of points that are eventually (but not initially) reached by the front,
% and also handle the case of never-reached "islands".

function d = solvePart (d0, region, h, fabs)
  d = d0;
  d(~region) = Inf;
  fval = NA (size (fabs));
  fval(region) = h ./ fabs(region);
  d = fastmarching (d, fval);

  % Set distances from which the front moved away from Inf to -Inf.
  d(d == Inf & region) = -Inf;

  % If a region is not reached by the fast marching (i. e., NA), this means
  % that it is separated from the boundary by a sign change of f.  This also
  % means that it will actually never be reached, and thus we can set its
  % distance to Inf.
  d(isna (d)) = Inf;

  assert (all (~isnan (d(:))));
endfunction

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Tests.

% Test for error conditions.
%!error <Invalid call to>
%!  ls_solve_stationary (1)
%!error <Invalid call to>
%!  ls_solve_stationary (1, 2, 3, 4, 5)
%!error <PHI and F must be of the same size>
%!  ls_solve_stationary ([1, 2], [3; 4]);
%!error <PHI and NB must be of the same size>
%!  ls_solve_stationary ([1, 2], [1, 2], :, [3; 4]);

% Check 0D case.
%!test
%!  assert (ls_solve_stationary ([], []), []);

% Check invariants expected for the output in terms
% of finite/infinite values and signs.
%!function checkOutput (phi, f, d)
%!  assert (all (isna (d(f == 0))));
%!  where = (f ~= 0);
%!  assert (all (isfinite (d(where)) | isinf (d(where))));
%!  assert (sign (d(where)), sign (phi(where)));
%!endfunction

% For the test phi's, just check that solving works
% without any errors.
%!test
%!  phis = ls_get_tests ();
%!  for i = 1 : length (phis)
%!    f = ones (size (phis{i}));
%!    for j = -1 : 1
%!      phi = ls_normalise (phis{i});
%!      d = ls_solve_stationary (phi, j * f);
%!      checkOutput (phi, j * f, d);
%!    endfor
%!  endfor

% Test for island handling and the general output conditions
% (that phi = 0 leads to D = NA and otherwise we only ever get
% some ordinary values with signs depending on the sign of f
% or infinities with appropriate signs).
%
% The constructed example contains in each half-plane (left / right of the
% y-axis) an initial circle as domain, and a "ring" around the domain boundary
% where f is positive or negative.  This ensures we have a sign change of f
% both in the interior and exterior of the initial domain.  Depending on the
% sign of f (and thus the half-plane), one of them will be an "island".
%
%!test
%!  n = 101;
%!  x = linspace (-10, 10, n);
%!  h = x(2) - x(1);
%!  [XX, YY] = meshgrid (x, x);
%!
%!  r1 = (XX - 5).^2 + YY.^2;
%!  r2 = (XX + 5).^2 + YY.^2;
%!
%!  phi = ls_union (r1 - 2^2, r2 - 2^2);
%!  f1 = max (r1 - 3^2, 1^2 - r1);
%!  f2 = max (r2 - 3^2, 1^2 - r2);
%!  f = min (f1, f2) .* sign (XX);
%!  assert (~all (f(:) ~= 0));
%!
%!  d = ls_solve_stationary (phi, f, h);
%!
%!  checkOutput (phi, f, d);
%!  where = (f ~= 0);
%!  assert (any (isinf (d(where))));
%!
%!  %{
%!  contour (phi, [0, 0]);
%!  colorbar ();
%!  figure ();
%!  imagesc (f);
%!  colorbar ();
%!  figure ();
%!  imagesc (d);
%!  colorbar ();
%!  %}

% Test manual NB initialisation.
%!test
%!  phi = [3, 1, -1, 1, 3];
%!  f = ones (size (phi));
%!  dists1 = ls_solve_stationary (phi, f);
%!  dists2 = ls_solve_stationary (phi, f, :, [NA, NA, -0.1, NA, NA]);
%!
%!  assert (dists1, [1.5, 0.5, -0.5, 0.5, 1.5], sqrt (eps));
%!  assert (dists2, [1.9, 0.9, -0.1, 0.9, 1.9]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Demos.

% Visualise refraction and diffraction of a wave front.  If we only
% use positive speeds, then the different times can be plotted nicely
% just with 'contour'.
%
%!demo
%!  n = 100;
%!  x = linspace (-10, 10, n);
%!  h = x(2) - x(1);
%!  [XX, YY] = meshgrid (x, x);
%!
%!  phi0 = YY - XX + 15;
%!  f = sign (XX) + 2;
%!
%!  d = ls_solve_stationary (phi0, f, h);
%!  figure ();
%!  contour (XX, YY, d);
%!  line ([0, 0], [-10, 10]);
%!  title ("Refraction");
%
%!demo
%!  n = 101;
%!  x = linspace (-10, 10, n);
%!  h = x(2) - x(1);
%!  [XX, YY] = meshgrid (x, x);
%!
%!  phi0 = XX + 9;
%!  f = ones (size (phi0));
%!  f(XX == 0 & abs (YY) > 2) = 0;
%!
%!  d = ls_solve_stationary (phi0, f, h);
%!  figure ();
%!  contour (XX, YY, d);
%!  line ([0, 0], [-10, -2]);
%!  line ([0, 0], [2, 10]);
%!  title ("Diffraction");

% XXX: Can we use this for ray-tracing as a nice example???
