/*
    GNU Octave level-set package.
    Copyright (C) 2013-2016  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/>.
*/

/* Interfacing the C++ fast marching code to Octave.  This is not the actual
   Octave interface, but the routine performing the work.  The real Octave
   function is written as .m file and performs some more preparation work
   on the arguments before calling in here.  */

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

#include <octave/oct.h>

#include <cassert>

using namespace fastMarching;

/* Octave routine interfacing to fast marching code.  */
DEFUN_DLD (__levelset_internal_fastmarching, args, nargout,
  "[U, G, REACHED] = internal_fastmarching (DOMAIN, U0, G0, ALIVE, F)\n\n"
  "Internal routine for fastmarching.  DOMAIN, U0, ALIVE and F must all be\n"
  "arrays of the same dimension and size, specifying the grid.  The solution\n"
  "is returned in the array U, giving the final distance at each grid point.\n"
  "REACHED is boolean array storing for each point whether it is actually\n"
  "reached by the method in finite distance from some point that is\n"
  "initially alive.\n\n"
  "DOMAIN and ALIVE are boolean arrays defining whether some point belongs\n"
  "to the domain (holes in the domain are allowed and treated as absolutely\n"
  "unreachable and untraversable regions), and whether the point belongs to\n"
  "the initially alive set.  For those points, U0 is assumed to contain\n"
  "the initialisation distance.\n")
{
  try
    {
      if (args.length () != 5 || nargout != 3)
        throw std::invalid_argument ("invalid argument counts");

      const boolNDArray domain = args(0).bool_array_value ();
      const NDArray u0 = args(1).array_value ();
      const NDArray g0 = args(2).array_value ();
      const boolNDArray alive = args(3).bool_array_value ();
      const NDArray f = args(4).array_value ();

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

      if (size != u0.dims () || size != g0.dims ()
          || size != alive.dims () || size != f.dims ())
        throw std::invalid_argument ("argument dimensions mismatch");

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

      Grid grid(gridSize);

      const auto fillIn
        = [D, &domain, &u0, &g0, &alive, &f, &grid] (const IndexTuple& c)
        {
          const Array<octave_idx_type> idx = getOctaveIdx (c);
          assert (c.size () == D
                  && static_cast<dimensionT> (idx.numel ()) == D);

          if (domain(idx))
            {
              Entry* e;
              if (alive(idx))
                e = Entry::newAlive (c, u0(idx), g0(idx));
              else
                e = Entry::newFarAway (c, f(idx));

              grid.setInitial (e);
            }
        };
      grid.iterate (fillIn);

      grid.perform ();

      NDArray u(size);
      NDArray g(size);
      boolNDArray reached(size);
      const auto getOut
        = [D, &grid, &u, &g, &reached] (const IndexTuple& c)
        {
          const Array<octave_idx_type> idx = getOctaveIdx (c);
          assert (c.size () == D
                  && static_cast<dimensionT> (idx.numel ()) == D);

          const Grid& constGrid(grid);
          const Entry* e = constGrid.get (c);

          if (e)
            {
              const realT dist = e->getDistance ();
              reached(idx) = (dist != LARGE_DIST);
              u(idx) = dist;
              g(idx) = e->getFunction ();
            }
          else
            reached(idx) = false;
        };
      grid.iterate (getOut);

      octave_value_list res;
      res(0) = u;
      res(1) = g;
      res(2) = reached;

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