/*
    GNU Octave level-set package.
    Copyright (C) 2014  Daniel Kraft <d@domob.eu>

    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/>.
*/

/* Given a level-set function, initialise the narrow-band distances.  The
   code here does the internal C++ implementation for better performance,
   and is used from the "public" .m file including some more error and
   argument checks as well as test and demo routines.  */

#include "FastMarching.hpp"
#include "Utils.hpp"

#include <octave/oct.h>
#include <octave/lo-ieee.h>

#include <cmath>

using namespace fastMarching;

/* Fraction below which we consider a point exactly on the boundary.  This
   is necessary to prevent underflows from happening for the h's passed
   to the update equation solver.

   Should this be configurable?  But note that this value is just necessary
   to prevent underflows.  The fracTol in the geometry routine, on the other
   hand, influences the resulting geometry and further constructed triangle
   meshes.  (In particular, how distorted triangles may get.)  Thus it makes
   sense to allow configuration there.  */
static const double MIN_FRAC = 1e-16;

/* Octave routine interfacing to .m file code.  */
DEFUN_DLD (__levelset_internal_init_narrowband, args, nargout,
  "  D = internal_init_narrowband (PHI, H)\n\n"
  "Internal routine for ls_init_narrowband.  Given the level-set function\n"
  "PHI on some array with grid-spacing H, calculate the distances from the\n"
  "zero level-set of all grid-points next to it ('narrow band') and return\n"
  "them in D.  All distances are positive, so the sign of PHI is not\n"
  "considered to differentiate between inside and outside.  Entries not\n"
  "in the narrow band will be set to NA.\n")
{
  try
    {
      if (args.length () != 2 || nargout != 1)
        throw std::invalid_argument ("invalid argument counts");

      const NDArray phi = args(0).array_value ();
      const double h = args(1).double_value ();

      const dim_vector size = phi.dims ();
      const dimensionT D = size.length ();

      IndexTuple gridSize(D);
      for (dimensionT i = 0; i < D; ++i)
        gridSize[i] = size(i);

      NDArray dists(size);
      dists.fill (octave_NA);

      /* Now iterate over the grid.  For each cell, check all links to
         neighbours that are still on the grid.  If phi changes sign
         along one of those links, we are in the narrow band.  In this
         case, build up a fast marching update equation from all of those
         links, and solve it to get our distance.  We assume a linear
         model for phi in order to find the "exact" point on the
         boundary along the intersected edge.  */

      Grid grid(gridSize);
      const auto doIt
        = [h, &phi, &dists, &grid] (const IndexTuple& coord)
        {
          const Array<octave_idx_type> idx = getOctaveIdx (coord);
          const double myPhi = phi(idx);

          /* Handle the special case of phi exactly zero here.  */
          if (myPhi == 0.0)
            {
              dists(idx) = 0.0;
              return;
            }

          UpdateEquation eqn;
          bool isNarrowBand = false;

          const auto checkLink
            = [h, myPhi, &eqn, &isNarrowBand, &phi, &dists, &idx]
              (const IndexTuple& neighbour, dimensionT d, indexT)
            {
              const double nPhi = phi(getOctaveIdx (neighbour));

              /* The special case of phi exactly zero at the neighbour
                 is handled as if there were no intersection at all.  */
              if (nPhi == 0.0)
                return;

              if (nPhi * myPhi < 0.0)
                {
                  const double frac = getZeroFraction (myPhi, nPhi);
                  assert (lo_ieee_finite (frac));

                  /* If the neighbour value is infinite leading to a frac
                     of zero, this point is exactly on the boundary.  Thus
                     set distance to zero and we're done.  */
                  if (std::abs (frac) < MIN_FRAC)
                    {
                      isNarrowBand = false;
                      dists(idx) = 0.0;
                      return;
                    }

                  isNarrowBand = true;
                  eqn.add (d, frac * h, 0.0);
                }
            };
          grid.iterateNeighbours (coord, checkLink);

          if (isNarrowBand)
            dists(idx) = eqn.solve (1.0);
        };
      grid.iterate (doIt);

      octave_value_list res;
      res(0) = dists;

      return res;
    }
  catch (const std::exception& exc)
    {
      error (exc.what ());
      return octave_value_list ();
    }
}
