// This file is part of the dune-stuff project:
//   https://github.com/wwu-numerik/dune-stuff/
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_STUFF_LA_SOLVER_EIGEN_HH
#define DUNE_STUFF_LA_SOLVER_EIGEN_HH

#include <string>
#include <vector>
#include <algorithm>
#include <sstream>
#include <cmath>
#include <complex>

#include <dune/stuff/common/disable_warnings.hh>
#if HAVE_EIGEN
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>
//#   if HAVE_UMFPACK
//#     include <Eigen/UmfPackSupport>
//#   endif
//#   include <Eigen/SPQRSupport>
//#   include <Eigen/CholmodSupport>
//#   if HAVE_SUPERLU
//#     include <Eigen/SuperLUSupport>
//#   endif
#endif // HAVE_EIGEN
#include <dune/stuff/common/reenable_warnings.hh>

#include <dune/stuff/common/exceptions.hh>
#include <dune/stuff/common/configuration.hh>
#include <dune/stuff/la/container/eigen.hh>

#include "../solver.hh"

namespace Dune {
namespace Stuff {
namespace LA {

#if HAVE_EIGEN

template <class S, class CommunicatorType>
class Solver<EigenDenseMatrix<S>, CommunicatorType> : protected SolverUtils
{
public:
  typedef EigenDenseMatrix<S> MatrixType;
  typedef typename MatrixType::RealType R;

  Solver(const MatrixType& matrix) : matrix_(matrix) {}

  Solver(const MatrixType& matrix, const CommunicatorType& /*communicator*/) : matrix_(matrix) {}

  static std::vector<std::string> types()
  {
    return {"lu.partialpiv",
            "qr.householder",
            "llt",
            "ldlt",
            "qr.colpivhouseholder",
            "qr.fullpivhouseholder",
            "lu.fullpiv"};
  } // ... types()

  static Common::Configuration options(const std::string type = "")
  {
    const std::string tp = !type.empty() ? type : types()[0];
    SolverUtils::check_given(tp, types());
    Common::Configuration default_options({"type", "post_check_solves_system", "check_for_inf_nan"}, {tp, "1e-5", "1"});
    // * for symmetric matrices
    if (tp == "ldlt" || tp == "llt") {
      default_options.set("pre_check_symmetry", "1e-8");
    }
    return default_options;
  } // ... options(...)

  template <class T1, class T2>
  void apply(const EigenBaseVector<T1, S>& rhs, EigenBaseVector<T2, S>& solution) const
  {
    apply(rhs, solution, types()[0]);
  }

  template <class T1, class T2>
  void apply(const EigenBaseVector<T1, S>& rhs, EigenBaseVector<T2, S>& solution, const std::string& type) const
  {
    apply(rhs, solution, options(type));
  }

  template <class T1, class T2>
  void apply(const EigenBaseVector<T1, S>& rhs, EigenBaseVector<T2, S>& solution,
             const Common::Configuration& opts) const
  {
    if (!opts.has_key("type"))
      DUNE_THROW(Exceptions::configuration_error,
                 "Given options (see below) need to have at least the key 'type' set!\n\n" << opts);
    const auto type = opts.get<std::string>("type");
    SolverUtils::check_given(type, types());
    const Common::Configuration default_opts = options(type);
    // check for inf or nan
    const bool check_for_inf_nan = opts.get("check_for_inf_nan", default_opts.get<bool>("check_for_inf_nan"));
    if (check_for_inf_nan) {
      for (size_t ii = 0; ii < matrix_.rows(); ++ii) {
        for (size_t jj = 0; jj < matrix_.cols(); ++jj) {
          const S& val = matrix_.backend()(ii, jj);
          if (Common::isnan(val) || Common::isinf(val)) {
            std::stringstream msg;
            msg << "Given matrix contains inf or nan and you requested checking (see options below)!\n"
                << "If you want to disable this check, set 'check_for_inf_nan = 0' in the options.\n\n"
                << "Those were the given options:\n\n" << opts;
            if (rhs.size() <= internal::max_size_to_print)
              msg << "\nThis was the given matrix:\n\n" << matrix_ << "\n";
            DUNE_THROW(Exceptions::linear_solver_failed_bc_data_did_not_fulfill_requirements, msg.str());
          }
        }
      }
      for (size_t ii = 0; ii < rhs.size(); ++ii) {
        const S& val = rhs[ii];
        if (Common::isnan(val) || Common::isinf(val)) {
          std::stringstream msg;
          msg << "Given rhs contains inf or nan and you requested checking (see options below)!\n"
              << "If you want to disable this check, set 'check_for_inf_nan = 0' in the options.\n\n"
              << "Those were the given options:\n\n" << opts;
          if (rhs.size() <= internal::max_size_to_print)
            msg << "\nThis was the given right hand side:\n\n" << rhs << "\n";
          DUNE_THROW(Exceptions::linear_solver_failed_bc_data_did_not_fulfill_requirements, msg.str());
        }
      }
    }
    // check for symmetry (if solver needs it)
    if (type == "ldlt" || type == "llt") {
      const R pre_check_symmetry_threshhold = opts.get("pre_check_symmetry", default_opts.get<R>("pre_check_symmetry"));
      if (pre_check_symmetry_threshhold > 0) {
        const MatrixType tmp(matrix_.backend() - matrix_.backend().adjoint());
        // serialize difference to compute L^\infty error (no copy done here)
        const R error = std::max(tmp.backend().cwiseAbs().minCoeff(), tmp.backend().cwiseAbs().maxCoeff());
        if (error > pre_check_symmetry_threshhold) {
          std::stringstream msg;
          msg << "Given matrix is not symmetric and you requested checking (see options below)!\n"
              << "If you want to disable this check, set 'pre_check_symmetry = 0' in the options.\n\n"
              << "  (A - A').sup_norm() = " << error << "\n\n"
              << "Those were the given options:\n\n" << opts;
          if (rhs.size() <= internal::max_size_to_print)
            msg << "\nThis was the given matrix A:\n\n" << matrix_ << "\n";
          DUNE_THROW(Exceptions::linear_solver_failed_bc_data_did_not_fulfill_requirements, msg.str());
        }
      }
    }
    // solve
    if (type == "qr.colpivhouseholder") {
      solution.backend() = matrix_.backend().colPivHouseholderQr().solve(rhs.backend());
    } else if (type == "qr.fullpivhouseholder")
      solution.backend() = matrix_.backend().fullPivHouseholderQr().solve(rhs.backend());
    else if (type == "qr.householder")
      solution.backend() = matrix_.backend().householderQr().solve(rhs.backend());
    else if (type == "lu.fullpiv")
      solution.backend() = matrix_.backend().fullPivLu().solve(rhs.backend());
    else if (type == "llt")
      solution.backend() = matrix_.backend().llt().solve(rhs.backend());
    else if (type == "ldlt")
      solution.backend() = matrix_.backend().ldlt().solve(rhs.backend());
    else if (type == "lu.partialpiv")
      solution.backend() = matrix_.backend().partialPivLu().solve(rhs.backend());
    else
      DUNE_THROW(Exceptions::internal_error,
                 "Given type '" << type << "' is not supported, although it was reported by types()!");
    // check
    if (check_for_inf_nan)
      for (size_t ii = 0; ii < solution.size(); ++ii) {
        const S& val = solution[ii];
        if (Common::isnan(val) || Common::isinf(val)) {
          std::stringstream msg;
          msg << "The computed solution contains inf or nan and you requested checking (see options "
              << "below)!\n"
              << "If you want to disable this check, set 'check_for_inf_nan = 0' in the options.\n\n"
              << "Those were the given options:\n\n" << opts;
          if (rhs.size() <= internal::max_size_to_print)
            msg << "\nThis was the given matrix A:\n\n" << matrix_ << "\nThis was the given right hand side b:\n\n"
                << rhs << "\nThis is the computed solution:\n\n" << solution << "\n";
          DUNE_THROW(Exceptions::linear_solver_failed_bc_data_did_not_fulfill_requirements, msg.str());
        }
      }
    const R post_check_solves_system_threshold =
        opts.get("post_check_solves_system", default_opts.get<R>("post_check_solves_system"));
    if (post_check_solves_system_threshold > 0) {
      auto tmp = rhs.copy();
      tmp.backend() = matrix_.backend() * solution.backend() - rhs.backend();
      const R sup_norm = tmp.sup_norm();
      if (sup_norm > post_check_solves_system_threshold || DSC::isnan(sup_norm) || DSC::isinf(sup_norm)) {
        std::stringstream msg;
        msg << "The computed solution does not solve the system (although the eigen backend reported "
            << "'Success') and you requested checking (see options below)!\n"
            << "If you want to disable this check, set 'post_check_solves_system = 0' in the options."
            << "\n\n"
            << "  (A * x - b).sup_norm() = " << tmp.sup_norm() << "\n\n"
            << "Those were the given options:\n\n" << opts;
        if (rhs.size() <= internal::max_size_to_print)
          msg << "\nThis was the given matrix A:\n\n" << matrix_ << "\nThis was the given right hand side b:\n\n" << rhs
              << "\nThis is the computed solution:\n\n" << solution << "\n";
        DUNE_THROW(Exceptions::linear_solver_failed_bc_the_solution_does_not_solve_the_system, msg.str());
      }
    }
  } // ... apply(...)

private:
  const MatrixType& matrix_;
}; // class Solver

/**
 *  \note lu.sparse will copy the matrix to column major
 *  \note qr.sparse will copy the matrix to column major
 *  \note ldlt.simplicial will copy the matrix to column major
 *  \note llt.simplicial will copy the matrix to column major
 */
template <class S, class CommunicatorType>
class Solver<EigenRowMajorSparseMatrix<S>, CommunicatorType> : protected SolverUtils
{
  typedef ::Eigen::SparseMatrix<S, ::Eigen::ColMajor> ColMajorBackendType;

public:
  typedef EigenRowMajorSparseMatrix<S> MatrixType;
  typedef typename MatrixType::RealType R;

private:
  typedef typename MatrixType::BackendType::Index EIGEN_size_t;

public:
  Solver(const MatrixType& matrix) : matrix_(matrix) {}

  Solver(const MatrixType& matrix, const CommunicatorType& /*communicator*/) : matrix_(matrix) {}

  static std::vector<std::string> types()
  {
    return {
        "bicgstab.ilut",
        "lu.sparse",
        "llt.simplicial" // <- does only work with symmetric matrices
        ,
        "ldlt.simplicial" // <- does only work with symmetric matrices
        ,
        "bicgstab.diagonal" // <- slow for complicated matrices
        ,
        "bicgstab.identity" // <- slow for complicated matrices
        ,
        "qr.sparse" // <- produces correct results, but is painfully slow
        ,
        "cg.diagonal.lower" // <- does only work with symmetric matrices, may produce correct results
        ,
        "cg.diagonal.upper" // <- does only work with symmetric matrices, may produce correct results
        ,
        "cg.identity.lower" // <- does only work with symmetric matrices, may produce correct results
        ,
        "cg.identity.upper" // <- does only work with symmetric matrices, may produce correct results
                            //           , "spqr"                  // <- does not compile
                            //           , "llt.cholmodsupernodal" // <- does not compile
                            //#if HAVE_UMFPACK
                            //           , "lu.umfpack"            // <- untested
                            //#endif
                            //#if HAVE_SUPERLU
                            //           , "superlu"               // <- untested
                            //#endif
    };
  } // ... types()

  static Common::Configuration options(const std::string type = "")
  {
    const std::string tp = !type.empty() ? type : types()[0];
    // check
    SolverUtils::check_given(tp, types());
    // default config
    Common::Configuration default_options({"type", "post_check_solves_system", "check_for_inf_nan"}, {tp, "1e-5", "1"});
    Common::Configuration iterative_options({"max_iter", "precision"}, {"10000", "1e-10"});
    iterative_options += default_options;
    // direct solvers
    if (tp == "lu.sparse" || tp == "qr.sparse" || tp == "lu.umfpack" || tp == "spqr" || tp == "llt.cholmodsupernodal"
        || tp == "superlu")
      return default_options;
    // * for symmetric matrices
    if (tp == "ldlt.simplicial" || tp == "llt.simplicial") {
      default_options.set("pre_check_symmetry", "1e-8");
      return default_options;
    }
    // iterative solvers
    if (tp == "bicgstab.ilut") {
      iterative_options.set("preconditioner.fill_factor", "10");
      iterative_options.set("preconditioner.drop_tol", "1e-4");
    } else if (tp.substr(0, 3) == "cg.")
      iterative_options.set("pre_check_symmetry", "1e-8");
    return iterative_options;
  } // ... options(...)

  template <class T1, class T2>
  void apply(const EigenBaseVector<T1, S>& rhs, EigenBaseVector<T2, S>& solution) const
  {
    apply(rhs, solution, types()[0]);
  }

  template <class T1, class T2>
  void apply(const EigenBaseVector<T1, S>& rhs, EigenBaseVector<T2, S>& solution, const std::string& type) const
  {
    apply(rhs, solution, options(type));
  }

  template <class T1, class T2>
  void apply(const EigenBaseVector<T1, S>& rhs, EigenBaseVector<T2, S>& solution,
             const Common::Configuration& opts) const
  {
    if (!opts.has_key("type"))
      DUNE_THROW(Exceptions::configuration_error,
                 "Given options (see below) need to have at least the key 'type' set!\n\n" << opts);
    const auto type = opts.get<std::string>("type");
    SolverUtils::check_given(type, types());
    const Common::Configuration default_opts = options(type);
    // check for inf or nan
    const bool check_for_inf_nan = opts.get("check_for_inf_nan", default_opts.get<bool>("check_for_inf_nan"));
    if (check_for_inf_nan) {
      // iterates over the non-zero entries of matrix_.backend() and checks them
      typedef typename MatrixType::BackendType::InnerIterator InnerIterator;
      for (EIGEN_size_t ii = 0; ii < matrix_.backend().outerSize(); ++ii) {
        for (InnerIterator it(matrix_.backend(), ii); it; ++it) {
          if (DSC::isnan(std::real(it.value())) || DSC::isnan(std::imag(it.value()))
              || DSC::isinf(std::abs(it.value())))
            DUNE_THROW(Exceptions::linear_solver_failed_bc_data_did_not_fulfill_requirements,
                       "Given matrix contains inf or nan and you requested checking (see options below)!\n"
                           << "If you want to disable this check, set 'check_for_inf_nan = 0' in the options.\n\n"
                           << "Those were the given options:\n\n"
                           << opts);
        }
      }
      for (size_t ii = 0; ii < rhs.size(); ++ii) {
        const S& val = rhs[ii];
        if (Common::isnan(val) || Common::isinf(val))
          DUNE_THROW(Exceptions::linear_solver_failed_bc_data_did_not_fulfill_requirements,
                     "Given rhs contains inf or nan and you requested checking (see options below)!\n"
                         << "If you want to disable this check, set 'check_for_inf_nan = 0' in the options.\n\n"
                         << "Those were the given options:\n\n"
                         << opts);
      }
    }
    // check for symmetry (if solver needs it)
    if (type.substr(0, 3) == "cg." || type == "ldlt.simplicial" || type == "llt.simplicial") {
      const R pre_check_symmetry_threshhold = opts.get("pre_check_symmetry", default_opts.get<R>("pre_check_symmetry"));
      if (pre_check_symmetry_threshhold > 0) {
        ColMajorBackendType colmajor_copy(matrix_.backend());
        colmajor_copy -= matrix_.backend().adjoint();
        // iterates over non-zero entries as above
        typedef typename ColMajorBackendType::InnerIterator InnerIterator;
        for (EIGEN_size_t ii = 0; ii < colmajor_copy.outerSize(); ++ii) {
          for (InnerIterator it(colmajor_copy, ii); it; ++it) {
            if (std::max(std::abs(std::real(it.value())), std::abs(std::imag(it.value())))
                > pre_check_symmetry_threshhold)
              DUNE_THROW(Exceptions::linear_solver_failed_bc_data_did_not_fulfill_requirements,
                         "Given matrix is not symmetric/hermitian and you requested checking (see options below)!\n"
                             << "If you want to disable this check, set 'pre_check_symmetry = 0' in the options.\n\n"
                             << "Those were the given options:\n\n"
                             << opts);
          }
        }
      }
    }
    ::Eigen::ComputationInfo info;
    if (type == "cg.diagonal.lower") {
      typedef ::Eigen::ConjugateGradient<typename MatrixType::BackendType,
                                         ::Eigen::Lower,
                                         ::Eigen::DiagonalPreconditioner<S>> SolverType;
      SolverType solver(matrix_.backend());
      solver.setMaxIterations(opts.get("max_iter", default_opts.get<int>("max_iter")));
      solver.setTolerance(opts.get("precision", default_opts.get<R>("precision")));
      solution.backend() = solver.solve(rhs.backend());
      info = solver.info();
    } else if (type == "cg.diagonal.upper") {
      typedef ::Eigen::ConjugateGradient<typename MatrixType::BackendType,
                                         ::Eigen::Upper,
                                         ::Eigen::DiagonalPreconditioner<S>> SolverType;
      SolverType solver(matrix_.backend());
      solver.setMaxIterations(opts.get("max_iter", default_opts.get<int>("max_iter")));
      solver.setTolerance(opts.get("precision", default_opts.get<R>("precision")));
      solution.backend() = solver.solve(rhs.backend());
      info = solver.info();
    } else if (type == "cg.identity.lower") {
      typedef ::Eigen::ConjugateGradient<typename MatrixType::BackendType,
                                         ::Eigen::Lower,
                                         ::Eigen::IdentityPreconditioner> SolverType;
      SolverType solver(matrix_.backend());
      solver.setMaxIterations(opts.get("max_iter", default_opts.get<int>("max_iter")));
      solver.setTolerance(opts.get("precision", default_opts.get<R>("precision")));
      solution.backend() = solver.solve(rhs.backend());
      info = solver.info();
    } else if (type == "cg.identity.upper") {
      typedef ::Eigen::ConjugateGradient<typename MatrixType::BackendType,
                                         ::Eigen::Lower,
                                         ::Eigen::IdentityPreconditioner> SolverType;
      SolverType solver(matrix_.backend());
      solver.setMaxIterations(opts.get("max_iter", default_opts.get<int>("max_iter")));
      solver.setTolerance(opts.get("precision", default_opts.get<R>("precision")));
      solution.backend() = solver.solve(rhs.backend());
      info = solver.info();
    } else if (type == "bicgstab.ilut") {
      typedef ::Eigen::BiCGSTAB<typename MatrixType::BackendType, ::Eigen::IncompleteLUT<S>> SolverType;
      SolverType solver(matrix_.backend());
      solver.setMaxIterations(opts.get("max_iter", default_opts.get<int>("max_iter")));
      solver.setTolerance(opts.get("precision", default_opts.get<R>("precision")));
      solver.preconditioner().setDroptol(
          opts.get("preconditioner.drop_tol", default_opts.get<R>("preconditioner.drop_tol")));
      solver.preconditioner().setFillfactor(
          opts.get("preconditioner.fill_factor", default_opts.get<int>("preconditioner.fill_factor")));
      solution.backend() = solver.solve(rhs.backend());
      info = solver.info();
    } else if (type == "bicgstab.diagonal") {
      typedef ::Eigen::BiCGSTAB<typename MatrixType::BackendType, ::Eigen::DiagonalPreconditioner<S>> SolverType;
      SolverType solver(matrix_.backend());
      solver.setMaxIterations(opts.get("max_iter", default_opts.get<int>("max_iter")));
      solver.setTolerance(opts.get("precision", default_opts.get<R>("precision")));
      solution.backend() = solver.solve(rhs.backend());
      info = solver.info();
    } else if (type == "bicgstab.identity") {
      typedef ::Eigen::BiCGSTAB<typename MatrixType::BackendType, ::Eigen::IdentityPreconditioner> SolverType;
      SolverType solver(matrix_.backend());
      solver.setMaxIterations(opts.get("max_iter", default_opts.get<int>("max_iter")));
      solver.setTolerance(opts.get("precision", default_opts.get<R>("precision")));
      solution.backend() = solver.solve(rhs.backend());
      info = solver.info();
    } else if (type == "lu.sparse") {
      ColMajorBackendType colmajor_copy(matrix_.backend());
      colmajor_copy.makeCompressed();
      typedef ::Eigen::SparseLU<ColMajorBackendType> SolverType;
      SolverType solver;
      solver.analyzePattern(colmajor_copy);
      solver.factorize(colmajor_copy);
      solution.backend() = solver.solve(rhs.backend());
      info = solver.info();
    } else if (type == "qr.sparse") {
      ColMajorBackendType colmajor_copy(matrix_.backend());
      colmajor_copy.makeCompressed();
      typedef ::Eigen::SparseQR<ColMajorBackendType, ::Eigen::COLAMDOrdering<int>> SolverType;
      SolverType solver;
      solver.analyzePattern(colmajor_copy);
      solver.factorize(colmajor_copy);
      solution.backend() = solver.solve(rhs.backend());
      info = solver.info();
    } else if (type == "ldlt.simplicial") {
      ColMajorBackendType colmajor_copy(matrix_.backend());
      colmajor_copy.makeCompressed();
      typedef ::Eigen::SimplicialLDLT<ColMajorBackendType> SolverType;
      SolverType solver;
      solver.analyzePattern(colmajor_copy);
      solver.factorize(colmajor_copy);
      solution.backend() = solver.solve(rhs.backend());
      info = solver.info();
    } else if (type == "llt.simplicial") {
      ColMajorBackendType colmajor_copy(matrix_.backend());
      colmajor_copy.makeCompressed();
      typedef ::Eigen::SimplicialLLT<ColMajorBackendType> SolverType;
      SolverType solver;
      solver.analyzePattern(colmajor_copy);
      solver.factorize(colmajor_copy);
      solution.backend() = solver.solve(rhs.backend());
      info = solver.info();
      //#if HAVE_UMFPACK
      //    } else if (type == "lu.umfpack") {
      //      typedef ::Eigen::UmfPackLU< typename MatrixType::BackendType > SolverType;
      //      SolverType solver;
      //      solver.analyzePattern(matrix_.backend());
      //      solver.factorize(matrix_.backend());
      //      solution.backend() = solver.solve(rhs.backend());
      //      info = solver.info();
      //#endif // HAVE_UMFPACK
      //    } else if (type == "spqr") {
      //      ColMajorBackendType colmajor_copy(matrix_.backend());
      //      colmajor_copy.makeCompressed();
      //      typedef ::Eigen::SPQR< ColMajorBackendType > SolverType;
      //      SolverType solver;
      //      solver.analyzePattern(colmajor_copy);
      //      solver.factorize(colmajor_copy);
      //      solution.backend() = solver.solve(rhs.backend());
      //      if (solver.info() != ::Eigen::Success)
      //        return solver.info();
      //    } else if (type == "cholmodsupernodalllt") {
      //      typedef ::Eigen::CholmodSupernodalLLT< typename MatrixType::BackendType > SolverType;
      //      SolverType solver;
      //      solver.analyzePattern(matrix_.backend());
      //      solver.factorize(matrix_.backend());
      //      solution.backend() = solver.solve(rhs.backend());
      //      if (solver.info() != ::Eigen::Success)
      //        return solver.info();
      //#if HAVE_SUPERLU
      //    } else if (type == "superlu") {
      //      typedef ::Eigen::SuperLU< typename MatrixType::BackendType > SolverType;
      //      SolverType solver;
      //      solver.analyzePattern(matrix_.backend());
      //      solver.factorize(matrix_.backend());
      //      solution.backend() = solver.solve(rhs.backend());
      //      info = solver.info();
      //#endif // HAVE_SUPERLU
    } else
      DUNE_THROW(Exceptions::internal_error,
                 "Given type '" << type << "' is not supported, although it was reported by types()!");
    // handle eigens info
    if (info != ::Eigen::Success) {
      if (info == ::Eigen::NumericalIssue)
        DUNE_THROW(Exceptions::linear_solver_failed_bc_data_did_not_fulfill_requirements,
                   "The eigen backend reported 'NumericalIssue'!\n"
                       << "=> see http://eigen.tuxfamily.org/dox/group__enums.html#ga51bc1ac16f26ebe51eae1abb77bd037b "
                          "for eigens explanation\n"
                       << "Those were the given options:\n\n"
                       << opts);
      else if (info == ::Eigen::NoConvergence)
        DUNE_THROW(Exceptions::linear_solver_failed_bc_it_did_not_converge,
                   "The eigen backend reported 'NoConvergence'!\n"
                       << "=> see http://eigen.tuxfamily.org/dox/group__enums.html#ga51bc1ac16f26ebe51eae1abb77bd037b "
                          "for eigens explanation\n"
                       << "Those were the given options:\n\n"
                       << opts);
      else if (info == ::Eigen::InvalidInput)
        DUNE_THROW(Exceptions::linear_solver_failed_bc_it_was_not_set_up_correctly,
                   "The eigen backend reported 'InvalidInput'!\n"
                       << "=> see http://eigen.tuxfamily.org/dox/group__enums.html#ga51bc1ac16f26ebe51eae1abb77bd037b "
                          "for eigens explanation\n"
                       << "Those were the given options:\n\n"
                       << opts);
      else
        DUNE_THROW(Exceptions::internal_error,
                   "The eigen backend reported an unknown status!\n"
                       << "Please report this to the dune-stuff developers!");
    }
    // check
    if (check_for_inf_nan)
      for (size_t ii = 0; ii < solution.size(); ++ii) {
        const S& val = solution[ii];
        if (Common::isnan(val) || Common::isinf(val))
          DUNE_THROW(Exceptions::linear_solver_failed_bc_data_did_not_fulfill_requirements,
                     "The computed solution contains inf or nan and you requested checking (see options "
                         << "below)!\n"
                         << "If you want to disable this check, set 'check_for_inf_nan = 0' in the options.\n\n"
                         << "Those were the given options:\n\n"
                         << opts);
      }
    const R post_check_solves_system_threshold =
        opts.get("post_check_solves_system", default_opts.get<R>("post_check_solves_system"));
    if (post_check_solves_system_threshold > 0) {
      auto tmp = rhs.copy();
      tmp.backend() = matrix_.backend() * solution.backend() - rhs.backend();
      const R sup_norm = tmp.sup_norm();
      if (sup_norm > post_check_solves_system_threshold || DSC::isnan(sup_norm) || DSC::isinf(sup_norm))
        DUNE_THROW(Exceptions::linear_solver_failed_bc_the_solution_does_not_solve_the_system,
                   "The computed solution does not solve the system (although the eigen backend reported "
                       << "'Success') and you requested checking (see options below)!\n"
                       << "If you want to disable this check, set 'post_check_solves_system = 0' in the options."
                       << "\n\n"
                       << "  (A * x - b).sup_norm() = "
                       << tmp.sup_norm()
                       << "\n\n"
                       << "Those were the given options:\n\n"
                       << opts);
    }
  } // ... apply(...)

private:
  const MatrixType& matrix_;
}; // class Solver

#else // HAVE_EIGEN

template <class S>
class Solver<EigenDenseMatrix<S>>
{
  static_assert(Dune::AlwaysFalse<S>::value, "You are missing Eigen!");
};

template <class S>
class Solver<EigenRowMajorSparseMatrix<S>>
{
  static_assert(Dune::AlwaysFalse<S>::value, "You are missing Eigen!");
};

#endif // HAVE_EIGEN

} // namespace LA
} // namespace Stuff
} // namespace Dune

#endif // DUNE_STUFF_LA_SOLVER_EIGEN_HH


#include "LevelSet.h"

#include <iostream>

#include "tbb/tbb.h"

#include <Eigen/Dense>
#include <Eigen/SVD>

namespace FluidSim2D::SurfaceTrackers
{

void LevelSet::drawGrid(Renderer& renderer, bool doOnlyNarrowBand) const
{
	if (doOnlyNarrowBand)
	{
		forEachVoxelRange(Vec2i(0), size(), [&](const Vec2i& cell)
		{
			if (std::fabs(myPhiGrid(cell)) < myNarrowBand)
				myPhiGrid.drawGridCell(renderer, cell);
		});
	}
	else myPhiGrid.drawGrid(renderer);
}

void LevelSet::drawMeshGrid(Renderer& renderer) const
{
	Transform xform(myPhiGrid.dx(), myPhiGrid.offset() + Vec2f(0.5) * myPhiGrid.dx());
	ScalarGrid<int> tempGrid(xform, myPhiGrid.size());
	tempGrid.drawGrid(renderer);
}

void LevelSet::drawSupersampledValues(Renderer& renderer, float radius, int samples, float sampleSize) const
{
	myPhiGrid.drawSupersampledValues(renderer, radius, samples, sampleSize);
}
void LevelSet::drawNormals(Renderer& renderer, const Vec3f& colour, float length) const
{
	myPhiGrid.drawSampleGradients(renderer, colour, length);
}

void LevelSet::drawSurface(Renderer& renderer, const Vec3f& colour, float lineWidth) const
{
	EdgeMesh surface = buildMSMesh();
	surface.drawMesh(renderer, colour, lineWidth);
}

void LevelSet::drawDCSurface(Renderer& renderer, const Vec3f& colour, float lineWidth) const
{
	EdgeMesh surface = buildDCMesh();
	surface.drawMesh(renderer, colour, lineWidth);
}

// Find the nearest point on the interface starting from the index position.
// If the position falls outside of the narrow band, there isn't a defined gradient
// to use. In this case, the original position will be returned.

Vec2f LevelSet::findSurface(const Vec2f& worldPoint, int iterationLimit) const
{
	assert(iterationLimit >= 0);

	float phi = myPhiGrid.biLerp(worldPoint);

	float epsilon = 1E-2 * dx();
	Vec2f tempPoint = worldPoint;

	int iterationCount = 0;
	if (std::fabs(phi) < myNarrowBand)
	{
		while (std::fabs(phi) > epsilon && iterationCount < iterationLimit)
		{
			tempPoint -= phi * normal(tempPoint);
			phi = myPhiGrid.biCubicInterp(tempPoint);
			++iterationCount;
		}
	}

	return tempPoint;
}

Vec2f LevelSet::findSurfaceIndex(const Vec2f& indexPoint, int iterationLimit) const
{
	Vec2f worldPoint = indexToWorld(indexPoint);
	worldPoint = findSurface(worldPoint, iterationLimit);
	return worldToIndex(worldPoint);
}

void LevelSet::reinit(bool rebuildWithFIM)
{
	UniformGrid<VisitedCellLabels> reinitializedCells(size(), VisitedCellLabels::UNVISITED_CELL);

	// Find the zero crossings, update their distances and flag as source cells
	ScalarGrid<float> tempPhiGrid = myPhiGrid;

	tbb::parallel_for(tbb::blocked_range<int>(0, voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int> &range)
	{
		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = reinitializedCells.unflatten(cellIndex);

			// Check for a zero crossing
			bool isAtZeroCrossing = false;
			for (int axis = 0; axis < 2 && !isAtZeroCrossing; ++axis)
				for (int direction : {0, 1})
				{
					Vec2i adjacentCell = cellToCell(cell, axis, direction);

					if (adjacentCell[axis] < 0 || adjacentCell[axis] >= size()[axis]) continue;

					if ((myPhiGrid(cell) <= 0 && myPhiGrid(adjacentCell) > 0) ||
						(myPhiGrid(cell) > 0 && myPhiGrid(adjacentCell) <= 0))
					{
						isAtZeroCrossing = true;

						Vec2f worldPoint = indexToWorld(Vec2f(cell));
						Vec2f interfacePoint = findSurface(worldPoint, 5);

						float distance = dist(worldPoint, interfacePoint);

						tempPhiGrid(cell) = myPhiGrid(cell) < 0. ? -distance : distance;
						reinitializedCells(cell) = VisitedCellLabels::FINISHED_CELL;

						break;
					}
				}

			// Set unvisited grid cells to background value, using old grid for inside/outside sign
			if (!isAtZeroCrossing)
			{
				assert(reinitializedCells(cell) == VisitedCellLabels::UNVISITED_CELL);
				tempPhiGrid(cell) = myPhiGrid(cell) < 0. ? -myNarrowBand : myNarrowBand;
			}
		}
	});

	//std::swap(myPhiGrid, tempPhiGrid);
	myPhiGrid = tempPhiGrid;

	if (rebuildWithFIM)
		reinitFastIterative(reinitializedCells);
	else
		reinitFastMarching(reinitializedCells);
}

void LevelSet::reinitFastIterative(UniformGrid<VisitedCellLabels>& reinitializedCells)
{
	assert(reinitializedCells.size() == size());

	//
	// Before starting the iterations, we want to construct the active list of voxels
	// to reinitialize.
	//

	tbb::enumerable_thread_specific<std::vector<Vec2i>> parallelActiveCellList;

	tbb::parallel_for(tbb::blocked_range<int>(0, voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		std::vector<Vec2i>& localActiveCellList = parallelActiveCellList.local();

		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = reinitializedCells.unflatten(cellIndex);

			if (reinitializedCells(cell) == VisitedCellLabels::FINISHED_CELL)
			{
				// Add neighbours to the list
				for (int axis : {0, 1})
					for (int direction : {0, 1})
					{
						Vec2i adjacentCell = cellToCell(cell, axis, direction);

						if (adjacentCell[axis] < 0 || adjacentCell[axis] >= size()[axis]) continue;

						if (reinitializedCells(adjacentCell) == VisitedCellLabels::UNVISITED_CELL)
						{
							localActiveCellList.push_back(adjacentCell);
							reinitializedCells(adjacentCell) = VisitedCellLabels::VISITED_CELL;
						}
					}
			}
		}
	});

	std::vector<Vec2i> activeCellList;
	mergeLocalThreadVectors(activeCellList, parallelActiveCellList);

	parallelActiveCellList.clear();

	auto vecCompare = [](const Vec2i& a, const Vec2i& b) -> bool
	{
		if (a[0] < b[0]) return true;
		else if (a[0] == b[0] && a[1] < b[1]) return true;
		return false;
	};

	tbb::parallel_sort(activeCellList.begin(), activeCellList.end(), vecCompare);

	float dx = myPhiGrid.dx();

	// Now that the correct distances and signs have been recorded at the interface,
	// it's important to flood fill that the signed distances outwards into the entire grid.
	// We use the Eikonal equation here to build this outward.

	auto solveEikonal = [&](const Vec2i& idx) -> float
	{
		float Ul = (idx[0] == 0) ? std::numeric_limits<float>::max() : myPhiGrid(idx[0] - 1, idx[1]);
		float Ur = (idx[0] == myPhiGrid.size()[0] - 1) ? std::numeric_limits<float>::max() : myPhiGrid(idx[0] + 1, idx[1]);

		float Ub = (idx[1] == 0) ? std::numeric_limits<float>::max() : myPhiGrid(idx[0], idx[1] - 1);
		float Ut = (idx[1] == myPhiGrid.size()[1] - 1) ? std::numeric_limits<float>::max() : myPhiGrid(idx[0], idx[1] + 1);

		float u = std::fabs(myPhiGrid(idx[0], idx[1]));

		int count = 0;

		float a = std::min(std::fabs(Ul), std::fabs(Ur));
		if (u - a <= 0.) a = std::numeric_limits<float>::max();
		else ++count;

		float b = std::min(std::fabs(Ub), std::fabs(Ut));

		if (u - b <= 0.) b = std::numeric_limits<float>::max();
		else ++count;

		if (a > b) std::swap(a, b);

		if (count == 1) u = a + dx;
		else if (count == 2)
		{
			float temp = -sqr(a) - sqr(b) + 2. * a * b + 2. * sqr(dx);
			if (temp < 0.) u = a + dx;
			else u = .5 * (a + b + sqrt(temp));
			assert(std::isfinite(u));
		}
		// There shouldn't be a case where count is 0 but it seems to be happenning..

		return u;

	};

	ScalarGrid<float> tempPhiGrid = myPhiGrid;

	float tolerance = dx * 1E-5;
	bool stillActiveCells = true;

	int activeCellCount = activeCellList.size();

	int iteration = 0;
	int maxIterations = 5 * myNarrowBand / dx;

	while (activeCellCount > 0 && iteration < maxIterations)
	{
		tbb::parallel_for(tbb::blocked_range<int>(0, activeCellCount, tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			auto& localActiveCellList = parallelActiveCellList.local();

			Vec2i oldCell(-1);

			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i newCell = activeCellList[cellIndex];

				if (oldCell == newCell)
					continue;

				oldCell = newCell;

				assert(reinitializedCells(newCell) == VisitedCellLabels::VISITED_CELL);

				float newPhi = solveEikonal(newCell);

				// If we hit the narrow band, we don't need to make any changes
				if (newPhi > myNarrowBand) continue;

				tempPhiGrid(newCell) = myPhiGrid(newCell) < 0 ? -newPhi : newPhi;

				// Check if new phi is converged
				float oldPhi = myPhiGrid(newCell);

				// If the cell is converged, load up the neighbours that aren't currently being VISITED
				if (std::fabs(newPhi - std::fabs(oldPhi)) < tolerance)
				{
					for (int axis : {0, 1})
						for (int direction : {0, 1})
						{
							Vec2i adjacentCell = cellToCell(newCell, axis, direction);

							if (adjacentCell[axis] < 0 || adjacentCell[axis] >= size()[axis]) continue;

							if (reinitializedCells(adjacentCell) == VisitedCellLabels::UNVISITED_CELL)
							{
								float adjacentNewPhi = solveEikonal(adjacentCell);

								if (adjacentNewPhi > myNarrowBand) continue;

								// Check if new phi is less than the current value
								float adjacentOldPhi = std::fabs(myPhiGrid(adjacentCell));

								if ((adjacentNewPhi < adjacentOldPhi) && (std::fabs(adjacentNewPhi - adjacentOldPhi) > tolerance))
								{
									tempPhiGrid(adjacentCell) = myPhiGrid(adjacentCell) < 0 ? -adjacentNewPhi : adjacentNewPhi;

									localActiveCellList.push_back(adjacentCell);
								}
							}

						}
				}
				// If the cell hasn't converged, toss it back into the list
				else
					localActiveCellList.push_back(newCell);
			}
		});

		// Turn off VISITED labels for current list
		tbb::parallel_for(tbb::blocked_range<int>(0, activeCellCount, tbbLightGrainSize), [&](const tbb::blocked_range<int> &range)
		{
			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i cell = activeCellList[cellIndex];
				assert(reinitializedCells(cell) != VisitedCellLabels::FINISHED_CELL);
				reinitializedCells(cell) = VisitedCellLabels::UNVISITED_CELL;
			}
		});

		activeCellList.clear();

		mergeLocalThreadVectors(activeCellList, parallelActiveCellList);

		parallelActiveCellList.clear();

		tbb::parallel_sort(activeCellList.begin(), activeCellList.end(), vecCompare);

		activeCellCount = activeCellList.size();

		// Turn on VISITED labels for new list
		tbb::parallel_for(tbb::blocked_range<int>(0, activeCellCount, tbbLightGrainSize), [&](const tbb::blocked_range<int> &range)
		{
			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i cell = activeCellList[cellIndex];
				assert(reinitializedCells(cell) != VisitedCellLabels::FINISHED_CELL);
				reinitializedCells(cell) = VisitedCellLabels::VISITED_CELL;
			}
		});

		//std::swap(tempPhiGrid, myPhiGrid);
		myPhiGrid = tempPhiGrid;

		++iteration;
	}
}

void LevelSet::initFromMesh(const EdgeMesh& initialMesh, bool doResizeGrid)
{
	if (doResizeGrid)
	{
		// Determine the bounding box of the mesh to build the underlying grids
		Vec2f minBoundingBox(std::numeric_limits<float>::max());
		Vec2f maxBoundingBox(std::numeric_limits<float>::lowest());

		for (int vertexIndex = 0; vertexIndex < initialMesh.vertexCount(); ++vertexIndex)
			updateMinAndMax(minBoundingBox, maxBoundingBox, initialMesh.vertex(vertexIndex).point());

		// Just for nice whole numbers, let's clamp the bounding box to be an integer
		// offset in index space then bring it back to world space
		float maxNarrowBand = 10.;
		maxNarrowBand = std::min(myNarrowBand / dx(), maxNarrowBand);

		minBoundingBox = dx() * (Vec2f(floor(minBoundingBox / dx())) - 2 * Vec2f(maxNarrowBand));
		maxBoundingBox = dx() * (Vec2f(ceil(maxBoundingBox / dx())) + 2 * Vec2f(maxNarrowBand));

		clear();
		Transform xform(dx(), minBoundingBox);
		// Since we know how big the mesh is, we know how big our grid needs to be (wrt to grid spacing)
		myPhiGrid = ScalarGrid<float>(xform, Vec2i((maxBoundingBox - minBoundingBox) / dx()), myNarrowBand);
	}
	else
		myPhiGrid.resize(size(), myNarrowBand);

	// We want to track which cells in the level set contain valid distance information.
	// The first pass will set cells close to the mesh as FINISHED.
	UniformGrid<VisitedCellLabels> reinitializedCells(size(), VisitedCellLabels::UNVISITED_CELL);
	UniformGrid<int> meshCellParities(size(), 0);

	for (const auto& edge : initialMesh.edges())
	{
		// It's easier to work in our index space and just scale the distance later.
		const Vec2f& startPoint = worldToIndex(initialMesh.vertex(edge.vertex(0)).point());
		const Vec2f& endPoint = worldToIndex(initialMesh.vertex(edge.vertex(1)).point());

		// Record mesh-grid intersections between cell nodes (i.e. on grid edges)
		// Since we only cast rays *left-to-right* for inside/outside checking, we don't
		// need to know if the mesh intersects y-aligned grid edges
		Vec2f vmin, vmax;
		minAndMax(vmin, vmax, startPoint, endPoint);

		Vec2i edgeCeilMin = Vec2i(ceil(vmin));
		Vec2i edgeFloorMin = Vec2i(floor(vmin)) - Vec2i(1);
		Vec2i edgeFloorMax = Vec2i(floor(vmax));

		for (int j = edgeCeilMin[1]; j <= edgeFloorMax[1]; ++j)
			for (int i = edgeFloorMax[0]; i >= edgeFloorMin[0]; --i)
			{
				Vec2f gridNode(i, j);
				IntersectionLabels intersectionResult = exactEdgeIntersect(startPoint, endPoint, gridNode, Axis::XAXIS);

				// TODO: remove once test complete
				if (gridNode[0] < 0 || gridNode[1] < 0 || gridNode[0] >= myPhiGrid.size()[0] ||
					gridNode[1] >= myPhiGrid.size()[1])
				{
					std::cout << "Caught out of bounds. Node: " << gridNode[0] << " " << gridNode[1] << std::endl;
				}

				if (intersectionResult == IntersectionLabels::NO) continue;

				// Increment the parity since the grid_node is
				// "left" of the mesh-edge crossing the grid-edge.
				// This indicates a negative normal in the x-direction
				// and means we're entering into the material.
				int parityChange = -1;
				if (startPoint[1] < endPoint[1])
					parityChange = 1;

				if (intersectionResult == IntersectionLabels::YES)
					meshCellParities(i + 1, j) += parityChange;
				// If the grid node is explicitly on the mesh-edge, set distance to zero
				// since it might not be exactly zero due to floating point error above.
				else
				{
					assert(intersectionResult == IntersectionLabels::ON);
					// Technically speaking, the zero isocountour means we're inside
					// the surface. So we should change the parity at the node that
					// is intersected even though it's zero and the sign is meaningless.
					reinitializedCells(i, j) = VisitedCellLabels::FINISHED_CELL;
					myPhiGrid(i, j) = 0.;
					meshCellParities(i, j) += parityChange;
				}

				break;
			}
	}

	// Now that all the x-axis edge crossings have been found, we can compile the parity changes
	// and label grid nodes that are at the interface
	for (int j = 0; j < size()[1]; ++j)
	{
		int parity = myIsBackgroundNegative ? 1 : 0;

		// We loop x-major because that's how we've set up our edge intersection.
		for (int i = 0; i < size()[0]; ++i) // TODO: double check that I've resized right
		{
			// Update parity before changing sign since the parity values above used the convention
			// of putting the change on the "far" node (i.e. after the mesh-grid intersection).

			Vec2i cell(i, j);
			parity += meshCellParities(cell);
			meshCellParities(cell) = parity;

			// Set inside cells to negative
			if (parity > 0) myPhiGrid(cell) = -std::fabs(myPhiGrid(cell));
		}

		assert(myIsBackgroundNegative ? parity == 1 : parity == 0);
	}

	// With the parity assigned, loop over the grid once more and label nodes that have an implied sign change
	// with neighbouring nodes (this means parity goes from -'ve (and zero) to +'ve or vice versa).
	forEachVoxelRange(Vec2i(1), size() - Vec2i(1), [&](const Vec2i& cell)
	{
		bool isCellInside = meshCellParities(cell) > 0;

		for (int axis : {0, 1})
			for (int direction : {0, 1})
			{
				Vec2i adjacentCell = cellToCell(cell, axis, direction);

				bool isAdjacentCellInside = meshCellParities(adjacentCell) > 0;

				if (isCellInside != isAdjacentCellInside)
					reinitializedCells(cell) = VisitedCellLabels::FINISHED_CELL;
			}
	});

	// Loop over all the edges in the mesh. Level set grid cells labelled as VISITED will be
	// updated with the distance to the surface if it happens to be shorter than the current
	// distance to the surface.
	for (const auto& edge : initialMesh.edges())
	{
		// Using the vertices of the edge, we can update distance values for cells
		// within the bounding box of the mesh. It's easier to work in our index space
		// and just scale the distance later.
		const Vec2f& startPoint = worldToIndex(initialMesh.vertex(edge.vertex(0)).point());
		const Vec2f& endPoint = worldToIndex(initialMesh.vertex(edge.vertex(1)).point());

		// Build bounding box

		Vec2i minBoundingBox = Vec2i(floor(minUnion(startPoint, endPoint))) - Vec2i(2);
		minBoundingBox = maxUnion(minBoundingBox, Vec2i(0));

		Vec2i maxBoundingBox = Vec2i(ceil(maxUnion(startPoint, endPoint))) + Vec2i(2);
		Vec2i top = size() - Vec2i(1);
		maxBoundingBox = minUnion(maxBoundingBox, top);

		// Update distances to the mesh at grid cells within the bounding box
		assert(minBoundingBox[0] >= 0 && minBoundingBox[1] >= 0 && maxBoundingBox[0] < size()[0] && maxBoundingBox[1] < size()[1]);

		forEachVoxelRange(minBoundingBox, maxBoundingBox + Vec2i(1), [&](const Vec2i& cell)
		{
			if (reinitializedCells(cell) != VisitedCellLabels::UNVISITED_CELL)
			{
				Vec2f cellPoint(cell);
				Vec2f vec0 = cellPoint - startPoint;
				Vec2f vec1 = endPoint - startPoint;

				float s = dot(vec0, vec1) / dot(vec1, vec1); // Find projection along edge.
				s = clamp(s, float(0), float(1));

				// Remove on-edge projection to get vector from closest point on edge to cell point.
				float surfaceDistance = mag(vec0 - s * vec1) * dx();

				// Update if the distance to this edge is shorter than previous values.
				if (surfaceDistance < std::fabs(myPhiGrid(cell)))
				{
					// If the parity says the node is inside, set it to be negative
					myPhiGrid(cell) = (meshCellParities(cell) > 0) ? -surfaceDistance : surfaceDistance;
				}
			}
		});
	}

	reinitFastIterative(reinitializedCells);
	//reinitFastMarching(reinitializedCells);
}

void LevelSet::reinitFastMarching(UniformGrid<VisitedCellLabels>& reinitializedCells)
{
	assert(reinitializedCells.size() == size());

	// Now that the correct distances and signs have been recorded at the interface,
	// it's important to flood fill that the signed distances outwards into the entire grid.
	// We use the Eikonal equation here to build this outward
	auto solveEikonal = [&](const Vec2i& cell) -> float
	{
		float max = std::numeric_limits<float>::max();

		float U_bx = (cell[0] > 0) ? std::fabs(myPhiGrid(cell[0] - 1, cell[1])) : max;
		float U_fx = (cell[0] < size()[0] - 1) ? std::fabs(myPhiGrid(cell[0] + 1, cell[1])) : max;

		float U_by = (cell[1] > 0) ? std::fabs(myPhiGrid(cell[0], cell[1] - 1)) : max;
		float U_fy = (cell[1] < size()[1] - 1) ? std::fabs(myPhiGrid(cell[0], cell[1] + 1)) : max;

		float Uh = std::min(U_bx, U_fx);
		float Uv = std::min(U_by, U_fy);
		float U;

		if (std::fabs(Uh - Uv) >= dx())
			U = std::min(Uh, Uv) + dx();
		else
			// Quadratic equation from the Eikonal
			U = (Uh + Uv) / 2. + .5 * std::sqrt(pow(Uh + Uv, 2.) - 2. * (sqr(Uh) + sqr(Uv) - sqr(dx())));

		return U;
	};

	// Load up the BFS queue with the unvisited cells next to the finished ones
	using Node = std::pair<Vec2i, float>;
	auto cmp = [](const Node& a, const Node& b) -> bool { return std::fabs(a.second) > std::fabs(b.second); };
	std::priority_queue<Node, std::vector<Node>, decltype(cmp)> marchingQ(cmp);

	forEachVoxelRange(Vec2i(0), size(), [&](const Vec2i& cell)
	{
		if (reinitializedCells(cell) == VisitedCellLabels::FINISHED_CELL)
		{
			for (int axis : {0, 1})
				for (int direction : {0, 1})
				{
					Vec2i adjacentCell = cellToCell(cell, axis, direction);

					if (adjacentCell[axis] < 0 || adjacentCell[axis] >= size()[axis]) continue;

					if (reinitializedCells(adjacentCell) == VisitedCellLabels::UNVISITED_CELL)
					{
						float dist = solveEikonal(adjacentCell);
						assert(dist >= 0);

						myPhiGrid(adjacentCell) = (myPhiGrid(adjacentCell) < 0.) ? -dist : dist;

						Node node(adjacentCell, dist);

						marchingQ.push(node);
						reinitializedCells(adjacentCell) = VisitedCellLabels::VISITED_CELL;
					}
				}
		}
	});

	while (!marchingQ.empty())
	{
		Node localNode = marchingQ.top();
		Vec2i localCell = localNode.first;
		marchingQ.pop();

		// Since you can't just update parts of the priority queue,
		// it's possible that a cell has been solidified at a smaller distance
		// and an older insert if floating around.
		if (reinitializedCells(localCell) == VisitedCellLabels::FINISHED_CELL)
		{
			// Make sure that the distance assigned to the cell is smaller than
			// what is floating around
			assert(std::fabs(myPhiGrid(localCell)) <= std::fabs(localNode.second));
			continue;
		}
		assert(reinitializedCells(localCell) == VisitedCellLabels::VISITED_CELL);

		if (std::fabs(myPhiGrid(localCell)) < myNarrowBand)
		{
			// Debug check that there is indeed a FINISHED cell next to it
			bool foundFinishedCell = false;

			// Loop over the neighbouring cells and load the unvisited cells
			// and update the visited cells
			for (int axis : {0, 1})
				for (int direction : {0, 1})
				{
					Vec2i adjacentCell = cellToCell(localCell, axis, direction);

					if (adjacentCell[axis] < 0 || adjacentCell[axis] >= reinitializedCells.size()[axis])
						continue;

					if (reinitializedCells(adjacentCell) == VisitedCellLabels::FINISHED_CELL)
						foundFinishedCell = true;
					else // If visited, then we'll update it
					{
						float dist = solveEikonal(adjacentCell);
						assert(dist >= 0);

						if (dist > myNarrowBand) dist = myNarrowBand;

						if (reinitializedCells(adjacentCell) == VisitedCellLabels::VISITED_CELL && dist > std::fabs(myPhiGrid(adjacentCell)))
							continue;

						myPhiGrid(adjacentCell) = myPhiGrid(adjacentCell) < 0 ? -dist : dist;

						Node node(adjacentCell, dist);

						marchingQ.push(node);
						reinitializedCells(adjacentCell) = VisitedCellLabels::VISITED_CELL;
					}
				}

			//Check that a marked cell was indeed visited
			assert(foundFinishedCell);
		}
		// Clamp to narrow band
		else myPhiGrid(localCell) = myPhiGrid(localCell) < 0 ? -myNarrowBand : myNarrowBand;

		// Solidify cell now that we've handled all it's neighbours
		reinitializedCells(localCell) = VisitedCellLabels::FINISHED_CELL;
	}
}

// Extract a mesh representation of the interface. Useful for rendering but not much
// else since there will be duplicate vertices per grid edge in the current implementation.

EdgeMesh LevelSet::buildMSMesh() const
{
	std::vector<Vec2f> verts;
	std::vector<Vec2i> edges;

	// Run marching squares loop
	forEachVoxelRange(Vec2i(0), size() - Vec2i(1), [&](const Vec2i& cell)
	{
		int mcKey = 0;

		for (int direction = 0; direction < 4; ++direction)
		{
			Vec2i node = cellToNodeCCW(cell, direction);
			if (myPhiGrid(node) <= 0.) mcKey += (1 << direction);
		}

		// Connect edges using the marching squares template
		for (int edgeIndex = 0; edgeIndex < 4 && marchingSquaresTemplate[mcKey][edgeIndex] >= 0; edgeIndex += 2)
		{
			// Find first vertex
			int edge = marchingSquaresTemplate[mcKey][edgeIndex];

			Vec3i faceMap = cellToFaceCCW(cell, edge);

			Vec2i face(faceMap[0], faceMap[1]);
			int axis = faceMap[2];
			Vec2i startNode = faceToNode(face, axis, 0);
			Vec2i endNode = faceToNode(face, axis, 1);

			Vec2f startPoint = interpolateInterface(startNode, endNode);

			// Find second vertex
			edge = marchingSquaresTemplate[mcKey][edgeIndex + 1];
			faceMap = cellToFaceCCW(cell, edge);

			face = Vec2i(faceMap[0], faceMap[1]);
			axis = faceMap[2];

			startNode = faceToNode(face, axis, 0);
			endNode = faceToNode(face, axis, 1);

			Vec2f endPoint = interpolateInterface(startNode, endNode);

			// Store vertices
			Vec2f worldStartPoint = indexToWorld(startPoint);
			Vec2f worldEndPoint = indexToWorld(endPoint);

			verts.push_back(worldStartPoint);
			verts.push_back(worldEndPoint);

			edges.emplace_back(verts.size() - 2, verts.size() - 1);
		}
	});

	return EdgeMesh(edges, verts);
}

// Extract a mesh representation of the interface using dual contouring
EdgeMesh LevelSet::buildDCMesh() const
{
	std::vector<Vec2f> verts;
	std::vector<Vec2i> edges;

	// Create grid to store index to dual contouring point. Note that phi is
	// center sampled so the DC grid must be node sampled and one cell shorter
	// in each dimension
	UniformGrid<int> dcPointIndex(size() - Vec2i(1), -1);

	// Run dual contouring loop
	forEachVoxelRange(Vec2i(0), dcPointIndex.size(), [&](const Vec2i& cell)
	{
		std::vector<Vec2f> qefPoints;
		std::vector<Vec2f> qefNormals;

		for (int axis : {0, 1})
			for (int direction : {0, 1})
			{
				Vec2i face = cellToFace(cell, axis, direction);

				Vec2i backwardNode = faceToNode(face, axis, 0);
				Vec2i forwardNode = faceToNode(face, axis, 1);

				if ((myPhiGrid(backwardNode) <= 0 && myPhiGrid(forwardNode) > 0) ||
					(myPhiGrid(backwardNode) > 0 && myPhiGrid(forwardNode) <= 0))
				{
					// Find interface point
					Vec2f interfacePoint = interpolateInterface(backwardNode, forwardNode);
					qefPoints.push_back(interfacePoint);

					// Find associated surface normal
					Vec2f surfaceNormal = normal(indexToWorld(interfacePoint));
					qefNormals.push_back(surfaceNormal);
				}
			}

		if (qefPoints.size() > 0)
		{
			Eigen::MatrixXd A(qefPoints.size(), 2);
			Eigen::VectorXd b(qefPoints.size());
			Eigen::VectorXd pointCOM = Eigen::VectorXd::Zero(2);

			assert(qefPoints.size() > 1);

			for (int pointIndex = 0; pointIndex < qefPoints.size(); ++pointIndex)
			{
				A(pointIndex, 0) = qefNormals[pointIndex][0];
				A(pointIndex, 1) = qefNormals[pointIndex][1];

				b(pointIndex) = dot(qefNormals[pointIndex], qefPoints[pointIndex]);

				pointCOM[0] += qefPoints[pointIndex][0];
				pointCOM[1] += qefPoints[pointIndex][1];
			}

			pointCOM /= float(qefPoints.size());

			Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
			svd.setThreshold(1E-2);

			Eigen::VectorXd dcPoint = pointCOM + svd.solve(b - A * pointCOM);

			Vec2f vecCOM(pointCOM[0], pointCOM[1]);

			Vec2f boundingBoxMin = floor(vecCOM);
			Vec2f boundingBoxMax = ceil(vecCOM);

			if (dcPoint[0] < boundingBoxMin[0] ||
				dcPoint[1] < boundingBoxMin[1] ||
				dcPoint[0] > boundingBoxMax[0] ||
				dcPoint[1] > boundingBoxMax[1])
				dcPoint = pointCOM;

			verts.push_back(indexToWorld(Vec2f(dcPoint[0], dcPoint[1])));
			dcPointIndex(cell) = verts.size() - 1;
		}
	});

	for (int axis : {0, 1})
	{
		Vec2i start(0); ++start[axis];
		Vec2i end(dcPointIndex.size());

		forEachVoxelRange(start, end, [&](const Vec2i& face)
		{
			Vec2i backwardNode = faceToNode(face, axis, 0);
			Vec2i forwardNode = faceToNode(face, axis, 1);

			if ((myPhiGrid(backwardNode) <= 0 && myPhiGrid(forwardNode) > 0) ||
				(myPhiGrid(backwardNode) > 0 && myPhiGrid(forwardNode) <= 0))
			{
				Vec2i backwardCell = faceToCell(Vec2i(face), axis, 0);
				Vec2i forwardCell = faceToCell(Vec2i(face), axis, 1);

				assert(dcPointIndex(backwardCell) >= 0 && dcPointIndex(forwardCell) >= 0);

				Vec2i edge;
				if (myPhiGrid(backwardNode) <= 0.)
				{
					if (axis == 0)
						edge = Vec2i(dcPointIndex(backwardCell), dcPointIndex(forwardCell));
					else
						edge = Vec2i(dcPointIndex(forwardCell), dcPointIndex(backwardCell));
				}
				else
				{
					if (axis == 0)
						edge = Vec2i(dcPointIndex(forwardCell), dcPointIndex(backwardCell));
					else
						edge = Vec2i(dcPointIndex(backwardCell), dcPointIndex(forwardCell));
				}

				edges.push_back(edge);
			}
		});
	}

	return EdgeMesh(edges, verts);
}

Vec2f LevelSet::interpolateInterface(const Vec2i& startPoint, const Vec2i& endPoint) const
{
	assert((myPhiGrid(startPoint[0], startPoint[1]) <= 0 && myPhiGrid(endPoint[0], endPoint[1]) > 0) ||
			(myPhiGrid(startPoint[0], startPoint[1]) > 0 && myPhiGrid(endPoint[0], endPoint[1]) <= 0));

	//Find weight to zero isosurface
	float s = myPhiGrid(startPoint) / (myPhiGrid(startPoint) - myPhiGrid(endPoint));
	s = clamp(s, float(0), float(1));

	Vec2f dx = Vec2f(endPoint) - Vec2f(startPoint);
	return Vec2f(startPoint) + s * dx;
}

void LevelSet::unionSurface(const LevelSet& unionPhi)
{
	assert(isGridMatched(unionPhi));

	tbb::parallel_for(tbb::blocked_range<int>(0, voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = myPhiGrid.unflatten(cellIndex);
			if (unionPhi(cell) < 2 * unionPhi.dx())
				myPhiGrid(cell) = std::min(myPhiGrid(cell), unionPhi(cell));
		}
	});

	reinitMesh();
}

}


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Ioan Sucan, Jonathan Gammell*/

#include "ompl/util/RandomNumbers.h"
#include "ompl/util/Exception.h"
#include "ompl/util/Console.h"
#include <mutex>
#include <memory>
#include <boost/math/constants/constants.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/random/uniform_on_sphere.hpp>
#include <boost/random/variate_generator.hpp>
#include <Eigen/Core>

/// @cond IGNORE
namespace
{
    /// We use a different random number generator for the seeds of the
    /// other random generators. The root seed is from the number of
    /// nano-seconds in the current time, or given by the user.
    class RNGSeedGenerator
    {
    public:
        RNGSeedGenerator()
          : firstSeed_(std::chrono::duration_cast<std::chrono::microseconds>(
                           std::chrono::system_clock::now() - std::chrono::system_clock::time_point::min())
                           .count())
          , sGen_(firstSeed_)
          , sDist_(1, 1000000000)
        {
        }

        std::uint_fast32_t firstSeed()
        {
            std::lock_guard<std::mutex> slock(rngMutex_);
            return firstSeed_;
        }

        void setSeed(std::uint_fast32_t seed)
        {
            std::lock_guard<std::mutex> slock(rngMutex_);
            if (seed > 0)
            {
                if (someSeedsGenerated_)
                {
                    OMPL_ERROR("Random number generation already started. Changing seed now will not lead to "
                               "deterministic sampling.");
                }
                else
                {
                    // In this case, since no seeds have been generated yet, so we remember this seed as the first one.
                    firstSeed_ = seed;
                }
            }
            else
            {
                if (someSeedsGenerated_)
                {
                    OMPL_WARN("Random generator seed cannot be 0. Ignoring seed.");
                    return;
                }
                OMPL_WARN("Random generator seed cannot be 0. Using 1 instead.");
                seed = 1;
            }
            sGen_.seed(seed);
        }

        std::uint_fast32_t nextSeed()
        {
            std::lock_guard<std::mutex> slock(rngMutex_);
            someSeedsGenerated_ = true;
            return sDist_(sGen_);
        }

    private:
        bool someSeedsGenerated_{false};
        std::uint_fast32_t firstSeed_;
        std::mutex rngMutex_;
        std::ranlux24_base sGen_;
        std::uniform_int_distribution<> sDist_;
    };

    std::once_flag g_once;
    boost::scoped_ptr<RNGSeedGenerator> g_RNGSeedGenerator;

    void initRNGSeedGenerator()
    {
        g_RNGSeedGenerator.reset(new RNGSeedGenerator());
    }

    RNGSeedGenerator &getRNGSeedGenerator()
    {
        std::call_once(g_once, &initRNGSeedGenerator);
        return *g_RNGSeedGenerator;
    }
}  // namespace
/// @endcond

/// @cond IGNORE
class ompl::RNG::SphericalData
{
public:
    /** \brief The container type for the variate generators. */
    using container_type_t = std::vector<double>;

    /** \brief The uniform_on_sphere distribution type. */
    using spherical_dist_t = boost::uniform_on_sphere<double, container_type_t>;

    /** \brief The resulting variate generator type. */
    using variate_generator_t = boost::variate_generator<std::mt19937 *, spherical_dist_t>;

    /** \brief Constructor */
    SphericalData(std::mt19937 *generatorPtr) : generatorPtr_(generatorPtr){};

    /** \brief The generator for a specified dimension. Will create if not existent */
    container_type_t generate(unsigned int dim)
    {
        // Assure that the dimension is in the range of the vector.
        growVector(dim);

        // Assure that the dimension is allocated:
        allocateDimension(dim);

        // Return the generator
        return (*dimVector_.at(dim).second)();
    };

    /** \brief Iterate over all the dimensions and reset the generators that exist. */
    void reset()
    {
        // Iterate over each dimension
        for (auto &i : dimVector_)
            // Check if the variate_generator is allocated
            if (bool(i.first))
                // It is, reset THE DATA (not the pointer)
                i.first->reset();
        // No else, this is an uninitialized dimension.
    };

private:
    /** \brief The pair of distribution and variate generator. */
    using dist_gen_pair_t = std::pair<std::shared_ptr<spherical_dist_t>, std::shared_ptr<variate_generator_t>>;

    /** \brief A vector distribution and variate generators (as pointers) indexed on dimension. */
    std::vector<dist_gen_pair_t> dimVector_;

    /** \brief A pointer to the generator owned by the outer class. Needed for creating new variate_generators */
    std::mt19937 *generatorPtr_;

    /** \brief Grow the vector until it contains an (empty) entry for the specified dimension. */
    void growVector(unsigned int dim)
    {
        // Iterate until the index associated with this dimension is in the vector
        while (dim >= dimVector_.size())
            // Create a pair of empty pointers:
            dimVector_.emplace_back();
    };

    /** \brief Assure that a distribution/generator is allocated for the specified index. */
    void allocateDimension(unsigned int dim)
    {
        // Only do this if unallocated, so check that:
        if (dimVector_.at(dim).first == nullptr)
        {
            // It is not allocated, so....
            // First construct the distribution
            dimVector_.at(dim).first = std::make_shared<spherical_dist_t>(dim);
            // Then the variate generator
            dimVector_.at(dim).second = std::make_shared<variate_generator_t>(generatorPtr_, *dimVector_.at(dim).first);
        }
        // No else, the pointer is already allocated.
    };
};
/// @endcond

std::uint_fast32_t ompl::RNG::getSeed()
{
    return getRNGSeedGenerator().firstSeed();
}

void ompl::RNG::setSeed(std::uint_fast32_t seed)
{
    getRNGSeedGenerator().setSeed(seed);
}

ompl::RNG::RNG()
  : localSeed_(getRNGSeedGenerator().nextSeed())
  , generator_(localSeed_)
  , sphericalDataPtr_(std::make_shared<SphericalData>(&generator_))
{
}

ompl::RNG::RNG(std::uint_fast32_t localSeed)
  : localSeed_(localSeed), generator_(localSeed_), sphericalDataPtr_(std::make_shared<SphericalData>(&generator_))
{
}

void ompl::RNG::setLocalSeed(std::uint_fast32_t localSeed)
{
    // Store the seed
    localSeed_ = localSeed;

    // Change the generator's seed
    generator_.seed(localSeed_);

    // Reset the distributions used by the variate generators, as they can cache values
    uniDist_.reset();
    normalDist_.reset();
    sphericalDataPtr_->reset();
}

double ompl::RNG::halfNormalReal(double r_min, double r_max, double focus)
{
    assert(r_min <= r_max);

    const double mean = r_max - r_min;
    double v = gaussian(mean, mean / focus);

    if (v > mean)
        v = 2.0 * mean - v;
    double r = v >= 0.0 ? v + r_min : r_min;
    return r > r_max ? r_max : r;
}

int ompl::RNG::halfNormalInt(int r_min, int r_max, double focus)
{
    auto r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus));
    return (r > r_max) ? r_max : r;
}

// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III,
//       pg. 124-132
void ompl::RNG::quaternion(double value[4])
{
    double x0 = uniDist_(generator_);
    double r1 = sqrt(1.0 - x0), r2 = sqrt(x0);
    double t1 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_),
           t2 = 2.0 * boost::math::constants::pi<double>() * uniDist_(generator_);
    double c1 = cos(t1), s1 = sin(t1);
    double c2 = cos(t2), s2 = sin(t2);
    value[0] = s1 * r1;
    value[1] = c1 * r1;
    value[2] = s2 * r2;
    value[3] = c2 * r2;
}

// From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004
void ompl::RNG::eulerRPY(double value[3])
{
    value[0] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
    value[1] = acos(1.0 - 2.0 * uniDist_(generator_)) - boost::math::constants::pi<double>() / 2.0;
    value[2] = boost::math::constants::pi<double>() * (-2.0 * uniDist_(generator_) + 1.0);
}

void ompl::RNG::uniformNormalVector(std::vector<double> &v)
{
    // Generate a random value, the variate_generator is returning a shallow_array_adaptor, which will modify the value
    // array:
    v = sphericalDataPtr_->generate(v.size());
}

// See: http://math.stackexchange.com/a/87238
void ompl::RNG::uniformInBall(double r, std::vector<double> &v)
{
    // Draw a random point on the unit sphere
    uniformNormalVector(v);

    // Draw a random radius scale
    double radiusScale = r * std::pow(uniformReal(0.0, 1.0), 1.0 / static_cast<double>(v.size()));

    // Scale the point on the unit sphere
    std::transform(v.begin(), v.end(), v.begin(), [radiusScale](double x) { return radiusScale * x; });
}

void ompl::RNG::uniformProlateHyperspheroidSurface(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr,
                                                   double value[])
{
    // Variables
    // The spherical point as a std::vector
    std::vector<double> sphere(phsPtr->getDimension());

    // Get a random point on the sphere
    uniformNormalVector(sphere);

    // Transform to the PHS
    phsPtr->transform(&sphere[0], value);
}

void ompl::RNG::uniformProlateHyperspheroid(const std::shared_ptr<const ProlateHyperspheroid> &phsPtr, double value[])
{
    // Variables
    // The spherical point as a std::vector
    std::vector<double> sphere(phsPtr->getDimension());

    // Get a random point in the sphere
    uniformInBall(1.0, sphere);

    // Transform to the PHS
    phsPtr->transform(&sphere[0], value);
}