/*
* LEGAL NOTICE
* This computer software was prepared by Battelle Memorial Institute,
* hereinafter the Contractor, under Contract No. DE-AC05-76RL0 1830
* with the Department of Energy (DOE). NEITHER THE GOVERNMENT NOR THE
* CONTRACTOR MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY
* LIABILITY FOR THE USE OF THIS SOFTWARE. This notice including this
* sentence must appear on any copies of this computer software.
*
* EXPORT CONTROL
* User agrees that the Software will not be shipped, transferred or
* exported into any country or used in any manner prohibited by the
* United States Export Administration Act or any other applicable
* export laws, restrictions or regulations (collectively the "Export Laws").
* Export of the Software may require some form of license or other
* authority from the U.S. Government, and failure to obtain such
* export control license may result in criminal liability under
* U.S. laws. In addition, if the Software is identified as export controlled
* items under the Export Laws, User represents and warrants that User
* is not a citizen, or otherwise located within, an embargoed nation
* (including without limitation Iran, Syria, Sudan, Cuba, and North Korea)
*     and that User is not otherwise prohibited
* under the Export Laws from receiving the Software.
*
* Copyright 2011 Battelle Memorial Institute.  All Rights Reserved.
* Distributed as open-source under the terms of the Educational Community
* License version 2.0 (ECL 2.0). http://www.opensource.org/licenses/ecl2.php
*
* For further details, see: http://www.globalchange.umd.edu/models/gcam/
*
*/

/*!
* \file lognrbt.cpp
* \ingroup objects
* \brief LogNRbt (Log Newton-Raphson with backtracking) class source file.
* \author Robert Link
*/

#include "util/base/include/definitions.h"
#include <string>
#include <algorithm>
#include <math.h>
#include <xercesc/dom/DOMNode.hpp>
#include <xercesc/dom/DOMNodeList.hpp>

#include "solution/solvers/include/solver_component.h"
#include "solution/solvers/include/lognrbt.hpp"
#include "solution/util/include/calc_counter.h"
#include "marketplace/include/marketplace.h"
#include "containers/include/world.h"
#include "solution/util/include/solution_info_set.h"
#include "solution/util/include/solution_info.h"
#include "solution/util/include/solver_library.h"
#include "util/base/include/util.h"
#include "util/logger/include/ilogger.h"
#include "util/base/include/xml_helper.h"
#include "solution/util/include/solution_info_filter_factory.h"
#include "solution/util/include/solvable_nr_solution_info_filter.h"

#include "solution/util/include/functor-subs.hpp"
#include "solution/util/include/linesearch.hpp"
#include "solution/util/include/fdjac.hpp"
#include "solution/util/include/edfun.hpp"
#include "solution/util/include/ublas-helpers.hpp"
#include "solution/util/include/jacobian-precondition.hpp"
#include "util/base/include/fltcmp.hpp"

#if USE_LAPACK
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/bindings/lapack/gesvd.hpp>
#include "solution/util/include/svd_invert_solve.hpp"
#else
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/lu.hpp>
#endif

#include "util/base/include/timer.h"

using namespace std;
using namespace xercesc;

std::string LogNRbt::SOLVER_NAME = "log-newton-raphson-backtracking-solver-component";

#if USE_LAPACK
#define UBMATRIX boost::numeric::ublas::matrix<double,boost::numeric::ublas::column_major>
#else
#define UBMATRIX boost::numeric::ublas::matrix<double>
#endif
#define UBVECTOR boost::numeric::ublas::vector<double>

namespace {
  // helper functions for the std::transform algorithm
  double SI2lgprice (const SolutionInfo &si) {return log(si.getPrice());}
  double SI2price (const SolutionInfo &si) {return si.getPrice();}
}

bool LogNRbt::XMLParse( const DOMNode* aNode ) {

    // assume we were passed a valid node.
    assert( aNode );

    // get the children of the node.
    DOMNodeList* nodeList = aNode->getChildNodes();

    // loop through the children
    for ( unsigned int i = 0; i < nodeList->getLength(); ++i ){
        DOMNode* curr = nodeList->item( i );
        string nodeName = XMLHelper<string>::safeTranscode( curr->getNodeName() );

        if( nodeName == "#text" ) {
            continue;
        }
        else if( nodeName == "max-iterations" ) {
            mMaxIter = XMLHelper<unsigned int>::getValue( curr );
        }
        else if( nodeName == "ftol" ) {
            mFTOL = XMLHelper<double>::getValue(curr);
        }
        else if( nodeName == "solution-info-filter" ) {
            mSolutionInfoFilter.reset(
                SolutionInfoFilterFactory::createSolutionInfoFilterFromString( XMLHelper<string>::getValue( curr ) ) );
        }
        else if(nodeName == "linear-price") {
          mLogPricep = false;
        }
        else if(nodeName == "log-price") {
          mLogPricep = true;    // not strictly necessary, as this is the default.
        }
        else if( SolutionInfoFilterFactory::hasSolutionInfoFilter( nodeName ) ) {
            mSolutionInfoFilter.reset( SolutionInfoFilterFactory::createAndParseSolutionInfoFilter( nodeName, curr ) );
        }
        else {
            ILogger& mainLog = ILogger::getLogger( "main_log" );
            mainLog.setLevel( ILogger::WARNING );
            mainLog << "Unrecognized text string: " << nodeName << " found while parsing "
                << getXMLName() << "." << endl;
        }
    }
    return true;
}

/*! \brief Newton-Raphson solver in log-log space with backtracking

 * \details Attempts to solve the selected markets using a
 *          Newton-Raphson solution with backtracking (see Numerical
 *          Recipes sectn. 9.7).  The solution is performed in
 *          log-log space (log(D/S) as a function of log(price)), as
 *          this extends the domain and range to all reals.  Most of
 *          the heavy lifting is done by the function nrsolve(), wile
 *          this function mostly sets up the structures necessary to
 *          call nrsolve.
 * \author Robert Link
 * \param solnset An initial set of SolutionInfo objects representing all markets which can be filtered.
 * \param period Model period.
 * \return A status code to indicate if the algorithm was successful or not.
 */

SolverComponent::ReturnCode LogNRbt::solve( SolutionInfoSet& solnset, int period ) {
    ReturnCode code = SolverComponent::ORIGINAL_STATE;

    // If all markets are solved, then return with success code.
    if( solnset.isAllSolved() ){
        return code = SolverComponent::SUCCESS;
    }

    startMethod();

    // Update the solution vector for the correct markets to solve.
    // Need to update solvable status before starting solution (Ignore return code)
    solnset.updateSolvable( mSolutionInfoFilter.get() );

    ILogger& solverLog = ILogger::getLogger( "solver_log" );
    solverLog.setLevel( ILogger::NOTICE );
    solverLog << "Beginning Newton-Raphson solution for period " << period
              << "Solving " << solnset.getNumSolvable() << "markets.\n";
    ILogger& worstMarketLog = ILogger::getLogger( "worst_market_log" );
    worstMarketLog.setLevel( ILogger::DEBUG );
    ILogger& singleLog = ILogger::getLogger( "single_market_log" );
    singleLog.setLevel( ILogger::DEBUG );

    size_t nsolv = solnset.getNumSolvable();
    if( nsolv == 0 ){
        solverLog << "No markets were assigned to this solver.  Exiting." << endl;
        return SUCCESS;
    }

    solverLog << "Initial market state:\nmkt\tprice\tsupply\tdemand\n";
    std::vector<SolutionInfo> solvables = solnset.getSolvableSet();
    for(size_t i=0; i<solvables.size(); ++i) {
      solverLog << i << "\t" << solvables[i].getPrice()
                << "\t" << solvables[i].getSupply()
                << "\t" << solvables[i].getDemand()
                << "\t\t" << solvables[i].getName() << "\n";
    }

    Timer& solverTimer = TimerRegistry::getInstance().getTimer( TimerRegistry::SOLVER );
    solverTimer.start();

    UBVECTOR x(nsolv), fx(nsolv);
    int neval = 0;

    // set our initial x from the solutionInfoSet
    std::vector<SolutionInfo> smkts(solnset.getSolvableSet());
    if(mLogPricep)
      std::transform(smkts.begin(), smkts.end(), x.begin(), SI2lgprice);
    else
      std::transform(smkts.begin(), smkts.end(), x.begin(), SI2price);

    // This is the closure that will evaluate the ED function
    LogEDFun F(solnset, world, marketplace, period, mLogPricep);

    // scale the initial guess for use in F
    F.scaleInitInputs(x);

    // Call F(x), store the result in fx
    F(x,fx);

    solverLog << "Initial guess:\n" << x << "\nInitial F(x):\n" << fx << "\n";

    // Precondition the x values to avoid singular columns in the Jacobian
    solverLog.setLevel(ILogger::DEBUG);
    UBMATRIX J(F.narg(),F.nrtn());
    fdjac(F, x, fx, J, true);
    int pcfail = jacobian_precondition(x,fx,J,F,&solverLog, mLogPricep);

    if(pcfail) {
      solverLog.setLevel(ILogger::ERROR);
      solverLog << "Unable to find nonsingular initial guess for one or more markets.  nrsolve() will probably fail.\n";
      solverLog.setLevel(ILogger::DEBUG);
    }
    else {
      solverLog << "Revised guess:\n" << x << "\nInitial F(x):\n" << fx << "\n";
    }

    // call the solver
    int nrstatus = nrsolve(F, x, fx, J, neval);

    solverTimer.stop();

    solverLog.setLevel(ILogger::NOTICE);
    solverLog << "Newton-Raphson solver:  neval= " << neval << "\nResult:  ";
    if(nrstatus == 0) {
        solverLog << "NR solution success.\n";
        code = SUCCESS;
    }
    else if(nrstatus == -1) {
        code = FAILURE_ITER_MAX_REACHED;
        solverLog << "NR solution failed: Iteration max reached.\n";
    }
    else if(nrstatus == -3) {
        code = FAILURE_ZERO_GRADIENT;
        solverLog << "NR solution failed:  Encountered zero gradient in F*F.\n";
    }
    else if(nrstatus > 0) {
        code = FAILURE_SINGULAR_MATRIX;
        solverLog << "NR solution failed:  Encountered singular matrix (# singular components = " << nrstatus << ").\n";
    }
    else {
        code = FAILURE_UNKNOWN;
        solverLog << "NR solution failed for unknown reason.\n";
    }
    if(!solnset.isAllSolved()) {
        solverLog << "The following markets were not solved:\n";
        solnset.printUnsolved(solverLog);
    }

    solverLog << endl;
    return code;
}

int LogNRbt::nrsolve(VecFVec<double,double> &F, UBVECTOR &x, UBVECTOR &fx, UBMATRIX &J,
                     int &neval)
{
#if !USE_LAPACK
  using boost::numeric::ublas::permutation_matrix;
  using boost::numeric::ublas::lu_factorize;
  using boost::numeric::ublas::lu_substitute;
#endif
  using boost::numeric::ublas::axpy_prod;
  using boost::numeric::ublas::inner_prod;
  int nrow = J.size1(), ncol = J.size2();
  // svd decomposition elements (note nrow == ncol)
#if USE_LAPACK
  UBMATRIX Usv(nrow,ncol),VTsv(ncol,ncol);
  UBVECTOR Ssv(ncol);
  int singcount = 0;
  const int scmax = nrow/2;
#else
  permutation_matrix<int> p(F.narg()); // permutation vector for pivoting in L-U decomposition
#endif
  UBMATRIX Jtmp(nrow, ncol);

  ILogger &solverLog = ILogger::getLogger("solver_log");

  // Note that we no longer need to do a jacobian calculation here
  // because we do one in the preconditioner
  F(x,fx);

  solverLog.setLevel(ILogger::DEBUG);

  neval += 1 + x.size();        // initial function evaluation + jacobian calculations

  const double FTINY = mFTOL*mFTOL;
  UBVECTOR dx(F.narg());
  UBVECTOR xnew(F.narg());
  UBVECTOR gx(F.narg());
  assert(F.nrtn() == F.narg());
  assert(x.size() == F.narg());
  assert(fx.size() == F.nrtn());

  // We create a functor that computes f(x) = F(x)*F(x).  It also
  // stores the value of F that it produces as an intermediate.
  FdotF<double,double> fnorm(F);
  double f0 = inner_prod(fx,fx); // already have a value of F on input, so no need to call fnorm yet
  if(f0 < FTINY)
    // Guard against F=0 since it can cause a NaN in our solver.  This
    // is a more stringent test than our regular convergence test
    return 0;

  for(int iter=0; iter<mMaxIter; ++iter) {
    solverLog << "NR iter= " << iter << "\tneval= " << neval << "\n";
    axpy_prod(fx,J,gx);         // compute the gradient of F*F (= fx^T * J == J^T * fx)
    // axpy_prod clears gx on entry, so we don't have to do it.
    // NB: the order of fx and J in that last call is significant!

    // Check for zero gradient.  This indicates a local minimum in f,
    // from which we are unlikely to escape.  We will need to try
    // again with a different initial guess.  (This should be very
    // uncommon)
    if(inner_prod(gx,gx) / (f0+FTINY) < mFTOL) {
      return -3;
    }

    Jtmp = J;                   // save the Jacobian, since gesvd destroys it.

#if USE_LAPACK
    int ierr =
      boost::numeric::bindings::lapack::gesvd('O','A','A', // control parameters
                                              J,           // input matrix
                                              Ssv,Usv,VTsv); // output matrices
    if(ierr != 0) {
      // svd failed.  It's not even clear under what circumstances
      // this can happen
      solverLog.setLevel(ILogger::SEVERE);
      solverLog << "****************SVD failed.  This shouldn't happen.  It can't mean anything good.\n";
      return ierr;
    }

    // At this point, U, S, and VT contain the SVD of the original Jacobian
    solverLog.setLevel(ILogger::DEBUG);
    dx = -1.0*fx;
    int nsing = svdInvertSolve(Usv,Ssv,VTsv,dx, solverLog);

    solverLog.setLevel(ILogger::DEBUG);
    solverLog << "\n****************Iteration " << iter << "\nf0= " << f0
              << "\tnsing= " << nsing
              << "\nx: " << x << "\nF(x): " << fx << "\ndx: " << dx << "\n";

    if(nsing > 0) {
      singcount += nsing;
      if(singcount < scmax) {
        // Try to reset the x value using the preconditioner
        solverLog << "Resetting singular matrix, singcount = " << singcount << "\n";
        J = Jtmp;
        int fail = jacobian_precondition(x, fx, J, F, &solverLog, mLogPricep);
        if(fail)
          return nsing;

        // re-evaluate f0 and gx at the new guess
        double f0 = inner_prod(fx,fx);
        axpy_prod(fx,J,gx);         // compute the gradient of F*F (= fx^T * J == J^T * fx)

        // re-solve for dx using the new Jacobian
        ierr = boost::numeric::bindings::lapack::gesvd('O','A','A', // control parameters
                                                       J,           // input matrix
                                                       Ssv,Usv,VTsv); // output matrices
        if(ierr)
          return nsing;
        dx = -1.0*fx;
        svdInvertSolve(Usv, Ssv, VTsv, dx, solverLog);
      }
      else {
        return nsing;
      }
    }
    else
      singcount = 0;
#else  /* No USE_LAPACK.  Use L-U decomposition to do the solution. */
    int itrial = 0;
    /* If the L-U decomposition fails the first time around, we will
       invoke the jacobian preconditioner and try again.  If it fails
       a second time, we bail out */
    do {
      for(size_t i=0; i<p.size(); ++i) p[i] = i;
      int sing = lu_factorize(J,p);
      if(sing>0) {
        int fail=1;
        if(itrial == 0)
          fail = jacobian_precondition(x, fx, J, F, &solverLog, mLogPricep);

        if(fail) {
          solverLog.setLevel(ILogger::WARNING);
          solverLog << "Singular Jacobian:\n" << Jtmp << "\n";
          return sing;
        }
      }
      else {
        // L-U decomp was successful.  Continue with the next phase of the algorithm.
        break;
      }
    } while(++itrial < 2);

    // J now holds the L-U decomposition of the Jacobian.  Attempt backsubstitution
    dx = -1.0*fx;
    try {
      lu_substitute(J,p,dx);    // solve dx = J^-1 F
    }
    catch (const boost::numeric::ublas::internal_logic &err) {
      // This error seems to be thrown when the Jacobian is
      // ill-conditioned.  We let it go because often the solver will
      // muddle through to a solution.  If not, then it will
      // eventually stop with a genuinely singular matrix.
    }
#endif /* USE_LAPACK */

    // dx now holds the newton step.  Execute the line search along
    // that direction.
    double fnew;
    int lserr = linesearch(fnorm,x,f0,gx,dx, xnew,fnew, neval);

    if(lserr != 0) {
      // line search failed.  This means that the descent direction
      // for F only extends a very short way (roughly TOL * x0).
      // The most likely way for this to happen is if we're close to a
      // discontinuity in (probably several components of) the
      // Jacobian.  Using smoother supply and/or demand functions
      // might help here.

      // It's also possible we failed because we're really close to
      // the solution (sometimes dx gets really tiny near the
      // solution).  Make a relaxed convergence test and return if we
      // have a "close enough" solution.
      double msf = f0/fx.size();
      if(msf < mFTOL)
        // basically, we're letting ourselves converge to the sqrt of
        // our intended tolerance.
        return 0;

      // We're not close enough to the solution, and we don't have a
      // good descent direction.  There are no good options at this point
      // so kick out and hope that the preconditioner can set us straight.
      solverLog << "linesearch failure\n";
        return -4;
    }

    UBVECTOR xstep = xnew-x;    // step in x eventually taken
    solverLog << "################Return from linesearch\nfold= " << f0 << "\tfnew= " << fnew
              << "\n";
    f0 = fnew;
    x  = xnew;
    fnorm.lastF(fx);            // get the last value of big-F
    solverLog << "\nxnew: " << xnew << "\nfxnew: " << fx << "\n";

    // test for convergence
    double maxval = 0.0;
    for(size_t i=0; i<fx.size(); ++i) {
      double val = fabs(fx[i]);
      maxval = val>maxval ? val : maxval;
    }

    solverLog << "Convergence test maxval: " << maxval << "\n";
    if(maxval <= mFTOL) {
      solverLog << "Solution successful.\n";
      return 0;                 // SUCCESS
    }

    fdjac(F,x,fx,J);            // calculate finite difference Jacobian for the next iteration
    neval += x.size();          // N evaluations from calculating the Jacobian
  }

  // if we get here, then we didn't converge in the number of
  // iterations allowed us.  Return an error code
  solverLog << "\n****************Maximum solver iterations exceeded.\nlastx: " << x
            << "\nlastF: " << fx << "\n";
  return -1;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/functional/hash.hpp>
#include <cstddef>
#include <map>
#include <memory>
#include <optional>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>

#include "DataStructures/DataBox/DataBox.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Tags.hpp"
#include "Elliptic/BoundaryConditions/BoundaryCondition.hpp"
#include "Elliptic/BoundaryConditions/BoundaryConditionType.hpp"
#include "Elliptic/DiscontinuousGalerkin/SubdomainOperator/SubdomainOperator.hpp"
#include "Elliptic/Systems/Poisson/BoundaryConditions/Robin.hpp"
#include "Elliptic/Systems/Poisson/FirstOrderSystem.hpp"
#include "Elliptic/Systems/Poisson/Tags.hpp"
#include "NumericalAlgorithms/Convergence/HasConverged.hpp"
#include "NumericalAlgorithms/LinearSolver/ExplicitInverse.hpp"
#include "NumericalAlgorithms/LinearSolver/Gmres.hpp"
#include "NumericalAlgorithms/LinearSolver/LinearSolver.hpp"
#include "Options/Auto.hpp"
#include "Options/Options.hpp"
#include "Parallel/CharmPupable.hpp"
#include "ParallelAlgorithms/LinearSolver/Schwarz/ElementCenteredSubdomainData.hpp"
#include "ParallelAlgorithms/LinearSolver/Schwarz/Tags.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/MakeWithValue.hpp"
#include "Utilities/TMPL.hpp"

/// Linear solvers that approximately invert the
/// `elliptic::dg::subdomain_operator::SubdomainOperator` to make the Schwarz
/// subdomain solver converge faster.
/// \see LinearSolver::Schwarz::Schwarz
namespace elliptic::subdomain_preconditioners {

/// \cond
template <size_t Dim, typename OptionsGroup, typename Solver,
          typename LinearSolverRegistrars>
struct MinusLaplacian;
/// \endcond

namespace Registrars {
template <size_t Dim, typename OptionsGroup,
          typename Solver = LinearSolver::Serial::LinearSolver<tmpl::list<
              ::LinearSolver::Serial::Registrars::Gmres<
                  ::LinearSolver::Schwarz::ElementCenteredSubdomainData<
                      Dim, tmpl::list<Poisson::Tags::Field>>>,
              ::LinearSolver::Serial::Registrars::ExplicitInverse>>>
struct MinusLaplacian {
  template <typename LinearSolverRegistrars>
  using f = subdomain_preconditioners::MinusLaplacian<Dim, OptionsGroup, Solver,
                                                      LinearSolverRegistrars>;
};
}  // namespace Registrars

/*!
 * \brief Approximate the subdomain operator with a flat-space Laplacian
 * for every tensor component separately.
 *
 * This linear solver applies the `Solver` to every tensor component in
 * turn, approximating the subdomain operator with a flat-space Laplacian.
 * This can be a lot cheaper than solving the
 * full subdomain operator and can provide effective preconditioning for an
 * iterative subdomain solver. The approximation is better the closer the
 * original PDEs are to a set of decoupled flat-space Poisson equations.
 *
 * \par Boundary conditions
 * At external boundaries we impose homogeneous Dirichlet or Neumann boundary
 * conditions on the Poisson sub-problems. For tensor components and element
 * faces where the original boundary conditions are of Dirichlet-type we choose
 * homogeneous Dirichlet boundary conditions, and for those where the original
 * boundary conditions are of Neumann-type we choose homogeneous Neumann
 * boundary conditions. This means we may end up with more than one distinct
 * Poisson operator on subdomains with external boundaries, one per unique
 * combination of element face and boundary-condition type among the tensor
 * components.
 *
 * \tparam Dim Spatial dimension
 * \tparam OptionsGroup The options group identifying the
 * `LinearSolver::Schwarz::Schwarz` solver that defines the subdomain geometry.
 * \tparam Solver Any class that provides a `solve` and a `reset` function,
 * but typically a `LinearSolver::Serial::LinearSolver`. The solver will be
 * factory-created from input-file options.
 */
template <size_t Dim, typename OptionsGroup,
          typename Solver = LinearSolver::Serial::LinearSolver<tmpl::list<
              ::LinearSolver::Serial::Registrars::Gmres<
                  ::LinearSolver::Schwarz::ElementCenteredSubdomainData<
                      Dim, tmpl::list<Poisson::Tags::Field>>>,
              ::LinearSolver::Serial::Registrars::ExplicitInverse>>,
          typename LinearSolverRegistrars =
              tmpl::list<Registrars::MinusLaplacian<Dim, OptionsGroup, Solver>>>
class MinusLaplacian
    : public LinearSolver::Serial::LinearSolver<LinearSolverRegistrars> {
 private:
  using Base = LinearSolver::Serial::LinearSolver<LinearSolverRegistrars>;
  using StoredSolverType = tmpl::conditional_t<std::is_abstract_v<Solver>,
                                               std::unique_ptr<Solver>, Solver>;
  // Identifies an external block boundary, for boundary conditions
  using BoundaryId = std::pair<size_t, Direction<Dim>>;

 public:
  static constexpr size_t volume_dim = Dim;
  using options_group = OptionsGroup;
  using poisson_system =
      Poisson::FirstOrderSystem<Dim, Poisson::Geometry::FlatCartesian>;
  using BoundaryConditionsBase =
      typename poisson_system::boundary_conditions_base;
  using SubdomainOperator = elliptic::dg::subdomain_operator::SubdomainOperator<
      poisson_system, OptionsGroup, tmpl::list<>,
      tmpl::list<Poisson::BoundaryConditions::Robin<Dim>>>;
  using SubdomainData = ::LinearSolver::Schwarz::ElementCenteredSubdomainData<
      Dim, tmpl::list<Poisson::Tags::Field>>;
  // Associates Dirichlet or Neumann conditions to every external block
  // boundary. For every configuration of this type we'll need a separate
  // solver, which may cache the Poisson operator matrix that imposes the
  // corresponding boundary conditions. This type is used as key in other maps,
  // so it must be hashable. `std::unordered_map` isn't supported by
  // `boost::hash`, but `std::map` is.
  using BoundaryConditionsSignature =
      std::map<BoundaryId, elliptic::BoundaryConditionType>;
  using solver_type = Solver;

  struct SolverOptionTag {
    static std::string name() { return "Solver"; }
    using type = StoredSolverType;
    static constexpr Options::String help =
        "The linear solver used to invert the Laplace operator. The solver is "
        "shared between tensor components with the same type of boundary "
        "conditions (Dirichlet-type or Neumann-type).";
  };

  struct BoundaryConditions {
    using type = Options::Auto<elliptic::BoundaryConditionType>;
    static constexpr Options::String help =
        "The boundary conditions imposed by the Laplace operator. Specify "
        "'Auto' to choose between homogeneous Dirichlet or Neumann boundary "
        "conditions automatically, based on the configuration of the the full "
        "operator.";
  };

  using options = tmpl::list<SolverOptionTag, BoundaryConditions>;
  static constexpr Options::String help =
      "Approximate the linear operator with a Laplace operator "
      "for every tensor component separately.";

  MinusLaplacian() = default;
  MinusLaplacian(MinusLaplacian&& /*rhs*/) = default;
  MinusLaplacian& operator=(MinusLaplacian&& /*rhs*/) = default;
  ~MinusLaplacian() = default;
  MinusLaplacian(const MinusLaplacian& rhs)
      : Base(rhs),
        solver_(rhs.clone_solver()),
        boundary_condition_type_(rhs.boundary_condition_type_) {}
  MinusLaplacian& operator=(const MinusLaplacian& rhs) {
    Base::operator=(rhs);
    solver_ = rhs.clone_solver();
    boundary_condition_type_ = rhs.boundary_condition_type_;
    return *this;
  }

  /// \cond
  explicit MinusLaplacian(CkMigrateMessage* m) : Base(m) {}
  using PUP::able::register_constructor;
  WRAPPED_PUPable_decl_template(MinusLaplacian);  // NOLINT
  /// \endcond

  MinusLaplacian(
      StoredSolverType solver,
      std::optional<elliptic::BoundaryConditionType> boundary_condition_type)
      : solver_(std::move(solver)),
        boundary_condition_type_(boundary_condition_type) {}

  const Solver& solver() const {
    if constexpr (std::is_abstract_v<Solver>) {
      return *solver_;
    } else {
      return solver_;
    }
  }

  /// The cached solvers for each unique boundary-condition signature. These are
  /// only exposed to allow verifying their consistency.
  const auto& cached_solvers() const { return solvers_; }

  /// Solve the equation \f$Ax=b\f$ by approximating \f$A\f$ with a Laplace
  /// operator for every tensor component in \f$x\f$.
  template <typename LinearOperator, typename VarsType, typename SourceType,
            typename... OperatorArgs>
  Convergence::HasConverged solve(
      gsl::not_null<VarsType*> solution, LinearOperator&& linear_operator,
      const SourceType& source,
      const std::tuple<OperatorArgs...>& operator_args) const;

  void reset() override {
    mutable_solver().reset();
    // These buffers depend on the operator being solved, so they are reset when
    // the operator changes. The other buffers only hold workspace memory so
    // they don't need to be reset.
    bc_signatures_ = std::nullopt;
    solvers_.clear();
    boundary_conditions_.clear();
  }

  // NOLINTNEXTLINE(google-runtime-references)
  void pup(PUP::er& p) override {
    Base::pup(p);
    // Serialize object state
    p | solver_;
    p | boundary_condition_type_;
    // Serialize caches that are possibly expensive to re-create
    p | bc_signatures_;
    p | solvers_;
    // Other properties are memory buffers that don't need to be serialized.
  }

  std::unique_ptr<Base> get_clone() const override {
    return std::make_unique<MinusLaplacian>(*this);
  }

 private:
  // For each tensor component, get the set of boundary conditions that should
  // be imposed by the Laplacian operator on that component, based on the domain
  // configuration. These are cached for successive solves of the same operator.
  // An empty list means the subdomain has no external boundaries.
  template <typename DbTagsList>
  const std::vector<BoundaryConditionsSignature>&
  get_cached_boundary_conditions_signatures(
      const db::DataBox<DbTagsList>& box) const {
    if (bc_signatures_.has_value()) {
      // Use cached value
      return *bc_signatures_;
    }
    bc_signatures_ = std::vector<BoundaryConditionsSignature>{};
    const auto& blocks = db::get<domain::Tags::Domain<Dim>>(box).blocks();
    const auto collect_bc_signatures = [&bc_signatures = *bc_signatures_,
                                        &override_boundary_condition_type =
                                            boundary_condition_type_,
                                        &blocks](
                                           const BoundaryId& boundary_id) {
      if (not bc_signatures.empty() and
          bc_signatures[0].find(boundary_id) != bc_signatures[0].end()) {
        // We have already handled this block boundary in an earlier
        // iteration of the loop. This can happend when the domain is
        // h-refined, or when multiple elements in a subdomain face the
        // block boundary.
        return;
      }
      // Get the type of boundary condition (Dirichlet or Neumann) for
      // each tensor component from the domain configuration
      const auto& [block_id, direction] = boundary_id;
      const auto original_boundary_condition = dynamic_cast<
          const elliptic::BoundaryConditions::BoundaryCondition<Dim>*>(
          blocks.at(block_id)
              .external_boundary_conditions()
              .at(direction)
              .get());
      ASSERT(original_boundary_condition != nullptr,
             "The boundary condition in block "
                 << block_id << ", direction " << direction
                 << " is not of the expected type "
                    "'elliptic::BoundaryConditions::BoundaryCondition<"
                 << Dim << ">'.");
      const std::vector<elliptic::BoundaryConditionType> bc_types =
          original_boundary_condition->boundary_condition_types();
      const size_t num_components = bc_types.size();
      if (bc_signatures.empty()) {
        bc_signatures.resize(num_components);
      } else {
        ASSERT(bc_signatures.size() == num_components,
               "Unexpected number of boundary-condition types. The "
               "boundary condition in block "
                   << block_id << ", direction " << direction << " returned "
                   << num_components << " items, but another returned "
                   << bc_signatures.size()
                   << " (one for each tensor component).");
      }
      for (size_t component = 0; component < num_components; ++component) {
        bc_signatures[component].emplace(
            boundary_id,
            override_boundary_condition_type.value_or(bc_types.at(component)));
      }
    };
    // Collect boundary-condition signatures from all elements in the subdomain
    const auto& element = db::get<domain::Tags::Element<Dim>>(box);
    for (const auto& direction : element.external_boundaries()) {
      collect_bc_signatures({element.id().block_id(), direction});
    }
    const auto& overlap_elements =
        db::get<LinearSolver::Schwarz::Tags::Overlaps<
            domain::Tags::Element<Dim>, Dim, OptionsGroup>>(box);
    for (const auto& [overlap_id, overlap_element] : overlap_elements) {
      (void)overlap_id;
      for (const auto& direction : overlap_element.external_boundaries()) {
        collect_bc_signatures({overlap_element.id().block_id(), direction});
      }
    }
    return *bc_signatures_;
  }

  // For a tensor component with the given boundary-condition configuration, get
  // the solver and the set of boundary conditions that will be passed to the
  // Poisson subdomain operator. The solver is cached for successive solves of
  // the same operator. Caching is very important for performance, since the
  // solver may build up an explicit matrix representation that is expensive to
  // re-create.
  std::pair<const Solver&,
            const std::unordered_map<BoundaryId, const BoundaryConditionsBase&,
                                     boost::hash<BoundaryId>>&>
  get_cached_solver_and_boundary_conditions(
      const std::vector<BoundaryConditionsSignature>& bc_signatures,
      const size_t component) const {
    if (bc_signatures.empty()) {
      // The subdomain has no external boundaries. We use the original solver.
      return {solver(), boundary_conditions_[{}]};
    }
    // Get the cached solver corresponding to this component's
    // boundary-condition configuration
    const auto& bc_signature = bc_signatures.at(component);
    if (solvers_.find(bc_signature) == solvers_.end()) {
      solvers_.emplace(bc_signature, clone_solver());
    }
    const Solver& cached_solver = [this, &bc_signature]() -> const Solver& {
      if constexpr (std::is_abstract_v<Solver>) {
        return *solvers_.at(bc_signature);
      } else {
        return solvers_.at(bc_signature);
      }
    }();
    // Get the cached set of boundary conditions for this component
    if (boundary_conditions_.find(bc_signature) == boundary_conditions_.end()) {
      auto& bc = boundary_conditions_[bc_signature];
      for (const auto& [boundary_id, bc_type] : bc_signature) {
        bc.emplace(boundary_id,
                   bc_type == elliptic::BoundaryConditionType::Dirichlet
                       ? dirichlet_bc
                       : neumann_bc);
      }
    }
    return {cached_solver, boundary_conditions_[bc_signature]};
  }

  Solver& mutable_solver() {
    if constexpr (std::is_abstract_v<Solver>) {
      return *solver_;
    } else {
      return solver_;
    }
  }

  StoredSolverType clone_solver() const {
    if constexpr (std::is_abstract_v<Solver>) {
      return solver_->get_clone();
    } else {
      return solver_;
    }
  }

  // The option-constructed solver for the separate Poisson problems. It is
  // cloned for each unique boundary-condition configuration of the tensor
  // components.
  StoredSolverType solver_{};
  std::optional<elliptic::BoundaryConditionType> boundary_condition_type_;

  // These are caches to keep track of the unique boundary-condition
  // configurations. Each configuration has its own solver, because the solver
  // may cache the operator to speed up repeated solves.
  // - The boundary-condition configuration for each tensor component, or an
  //   empty list if the subdomain has no external boundaries, or `std::nullopt`
  //   to signal a clean cache.
  mutable std::optional<std::vector<BoundaryConditionsSignature>>
      bc_signatures_{};
  // - A clone of the `solver_` for each unique boundary-condition configuration
  mutable std::unordered_map<BoundaryConditionsSignature, StoredSolverType,
                             boost::hash<BoundaryConditionsSignature>>
      solvers_{};
  mutable std::unordered_map<
      BoundaryConditionsSignature,
      std::unordered_map<BoundaryId, const BoundaryConditionsBase&,
                         boost::hash<BoundaryId>>,
      boost::hash<BoundaryConditionsSignature>>
      boundary_conditions_{};

  // These are memory buffers that are kept around for repeated solves. Possible
  // optimization: Free this memory at appropriate times, e.g. when the element
  // has completed a full subdomain solve and goes to the background. In some
  // cases the `subdomain_operator_` is never even used again in subsequent
  // subdomain solves because it is cached as a matrix (see
  // LinearSolver::Serial::ExplicitInverse), so we don't need the memory anymore
  // at all.
  mutable SubdomainOperator subdomain_operator_{};
  mutable SubdomainData source_{};
  mutable SubdomainData initial_guess_in_solution_out_{};

  // These boundary condition instances can be re-used for all tensor components
  // Note that the _linearized_ boundary conditions are applied by the subdomain
  // operator, so the constant (third argument) is irrelevant.
  const Poisson::BoundaryConditions::Robin<Dim> dirichlet_bc{1., 0., 0.};
  const Poisson::BoundaryConditions::Robin<Dim> neumann_bc{0., 1., 0.};
};

namespace detail {
template <size_t Dim, typename LhsTagsList, typename RhsTagsList>
void assign_component(
    const gsl::not_null<::LinearSolver::Schwarz::ElementCenteredSubdomainData<
        Dim, LhsTagsList>*>
        lhs,
    const ::LinearSolver::Schwarz::ElementCenteredSubdomainData<
        Dim, RhsTagsList>& rhs,
    const size_t lhs_component, const size_t rhs_component) {
  // Possible optimization: Once we have non-owning Variables we can use a view
  // into the rhs here instead of copying.
  const size_t num_points_element = rhs.element_data.number_of_grid_points();
  for (size_t i = 0; i < num_points_element; ++i) {
    lhs->element_data.data()[lhs_component * num_points_element + i] =
        rhs.element_data.data()[rhs_component * num_points_element + i];
  }
  for (const auto& [overlap_id, rhs_data] : rhs.overlap_data) {
    const size_t num_points_overlap = rhs_data.number_of_grid_points();
    // The random-access operation is relatively slow because it computes a
    // hash, so it's important for performance to avoid repeating it in every
    // iteration of the loop below.
    auto& lhs_vars = lhs->overlap_data[overlap_id];
    for (size_t i = 0; i < num_points_overlap; ++i) {
      lhs_vars.data()[lhs_component * num_points_overlap + i] =
          rhs_data.data()[rhs_component * num_points_overlap + i];
    }
  }
}
}  // namespace detail

template <size_t Dim, typename OptionsGroup, typename Solver,
          typename LinearSolverRegistrars>
template <typename LinearOperator, typename VarsType, typename SourceType,
          typename... OperatorArgs>
Convergence::HasConverged
MinusLaplacian<Dim, OptionsGroup, Solver, LinearSolverRegistrars>::solve(
    const gsl::not_null<VarsType*> initial_guess_in_solution_out,
    LinearOperator&& /*linear_operator*/, const SourceType& source,
    const std::tuple<OperatorArgs...>& operator_args) const {
  const auto& box = get<0>(operator_args);
  // Solve each component of the source variables in turn, assuming the operator
  // is a Laplacian. For each component we select either homogeneous Dirichlet
  // or Neumann boundary conditions, based on the type of boundary conditions
  // imposed by the full operator.
  static constexpr size_t num_components =
      VarsType::ElementData::number_of_independent_components;
  source_.destructive_resize(source);
  initial_guess_in_solution_out_.destructive_resize(source);
  const std::vector<BoundaryConditionsSignature>& bc_signatures =
      get_cached_boundary_conditions_signatures(box);
  ASSERT(bc_signatures.empty() or bc_signatures.size() == num_components,
         "Expected " << num_components
                     << " boundary-condition signatures (one per tensor "
                        "component), but got "
                     << bc_signatures.size() << ".");
  for (size_t component = 0; component < num_components; ++component) {
    detail::assign_component(make_not_null(&source_), source, 0, component);
    detail::assign_component(make_not_null(&initial_guess_in_solution_out_),
                             *initial_guess_in_solution_out, 0, component);
    const auto& [solver, boundary_conditions] =
        get_cached_solver_and_boundary_conditions(bc_signatures, component);
    solver.solve(make_not_null(&initial_guess_in_solution_out_),
                 subdomain_operator_, source_,
                 std::forward_as_tuple(box, boundary_conditions));
    detail::assign_component(initial_guess_in_solution_out,
                             initial_guess_in_solution_out_, component, 0);
  }
  return {0, 0};
}

/// \cond
template <size_t Dim, typename OptionsGroup, typename Solver,
          typename LinearSolverRegistrars>
// NOLINTNEXTLINE
PUP::able::PUP_ID MinusLaplacian<Dim, OptionsGroup, Solver,
                                 LinearSolverRegistrars>::my_PUP_ID = 0;
/// \endcond

}  // namespace elliptic::subdomain_preconditioners


///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 1998-2011, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
//
// 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 Industrial Light & Magic 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.
//
///////////////////////////////////////////////////////////////////////////

#include "PyIlmBaseConfigInternal.h"

#include "PyImathLine.h"
#include "PyImathDecorators.h"
#include "PyImathExport.h"
#include <Python.h>
#include <boost/python.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/format.hpp>
#include "PyImath.h"
#include "PyImathVec.h"
#include "PyImathMathExc.h"
#include <ImathLineAlgo.h>
#include <ImathMatrix.h>
#include <Iex.h>

namespace PyImath{
using namespace boost::python;
using namespace IMATH_NAMESPACE;

template <class T> struct LineName {static const char *value;};
template <> const char *LineName<float>::value = "Line3f";
template <> const char *LineName<double>::value = "Line3d";

template <class T>
static Line3<T> *
Line3_construct_default()
{
    Vec3<T> point1(T (0), T(0), T(0));
    Vec3<T> point2(T (1), T(0), T(0));

    return new Line3<T>(point1, point2);
}

template <class T>
static Line3<T> *
Line3_tuple_construct(const tuple &t0, const tuple &t1)
{
    Vec3<T> v0, v1;
    if(t0.attr("__len__")() == 3 && t1.attr("__len__")() == 3)
    {
        v0.x = extract<T>(t0[0]);
        v0.y = extract<T>(t0[1]);
        v0.z = extract<T>(t0[2]);

        v1.x = extract<T>(t1[0]);
        v1.y = extract<T>(t1[1]);
        v1.z = extract<T>(t1[2]);

        return new Line3<T>(v0, v1);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "Line3 expects tuple of length 3");
}

template <class T, class S>
static Line3<T> *
Line3_line_construct(const Line3<S> &line)
{
    Line3<T> *l = new Line3<T>;
    l->pos = line.pos;
    l->dir = line.dir;

    return l;
}

template <class T>
static void
set1(Line3<T> &line, const Vec3<T> &p0, const Vec3<T> &p1)
{
    MATH_EXC_ON;
    line.set (p0, p1);
}

template <class T>
static void
setTuple(Line3<T> &line, const tuple &t0, const tuple &t1)
{
    MATH_EXC_ON;
    Vec3<T> v0, v1;
    if(t0.attr("__len__")() == 3 && t1.attr("__len__")() == 3)
    {
        v0.x = extract<T>(t0[0]);
        v0.y = extract<T>(t0[1]);
        v0.z = extract<T>(t0[2]);

        v1.x = extract<T>(t1[0]);
        v1.y = extract<T>(t1[1]);
        v1.z = extract<T>(t1[2]);

        line.set(v0, v1);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "Line3 expects tuple of length 3");
}

template <class T>
static Vec3<T>
pointAt(Line3<T> &line, T t)
{
    MATH_EXC_ON;
    return line.operator()(t);
}

template <class T>
static T
distanceTo1(Line3<T> &line, Vec3<T> &p)
{
    MATH_EXC_ON;
    return line.distanceTo(p);
}

template <class T>
static T
distanceTo2(Line3<T> &line, Line3<T> &other)
{
    MATH_EXC_ON;
    return line.distanceTo(other);
}

template <class T>
static T
distanceToTuple(Line3<T> line, const tuple &t)
{
    Vec3<T> v;
    if(t.attr("__len__")() == 3)
    {
        v.x = extract<T>(t[0]);
        v.y = extract<T>(t[1]);
        v.z = extract<T>(t[2]);

        return line.distanceTo(v);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "Line3 expects tuple of length 3");
}

template <class T>
static Vec3<T>
closestPointTo1(Line3<T> line, const Vec3<T> &p)
{
    MATH_EXC_ON;
    return line.closestPointTo(p);
}

template <class T>
static Vec3<T>
closestPointTo2(Line3<T> line, const Line3<T> &other)
{
    MATH_EXC_ON;
    return line.closestPointTo(other);
}

template <class T>
static Vec3<T>
closestPointToTuple(Line3<T> line, const tuple &t)
{
    MATH_EXC_ON;
    Vec3<T> v;
    if(t.attr("__len__")() == 3)
    {
        v.x = extract<T>(t[0]);
        v.y = extract<T>(t[1]);
        v.z = extract<T>(t[2]);

        return line.closestPointTo(v);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "Line3 expects tuple of length 3");
}

template <class T>
static Vec3<T>
getPosition(Line3<T> &line)
{
    return line.pos;
}

template <class T>
static void
setPosition(Line3<T> &line, const Vec3<T> &pos)
{
    line.pos = pos;
}

template <class T>
static void
setPositionTuple(Line3<T> &line, const tuple &t)
{
    Vec3<T> pos;
    if(t.attr("__len__")() == 3)
    {
        pos.x = extract<T>(t[0]);
        pos.y = extract<T>(t[1]);
        pos.z = extract<T>(t[2]);

        line.pos = pos;
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "Line3 expects tuple of length 3");
}

template <class T>
static Vec3<T>
getDirection(Line3<T> &line)
{
    return line.dir;
}

template <class T>
static void
setDirection(Line3<T> &line, const Vec3<T> &dir)
{
    MATH_EXC_ON;
    line.dir = dir.normalized();
}

template <class T>
static void
setDirectionTuple(Line3<T> &line, const tuple &t)
{
    MATH_EXC_ON;
    Vec3<T> dir;
    if(t.attr("__len__")() == 3)
    {
        dir.x = extract<T>(t[0]);
        dir.y = extract<T>(t[1]);
        dir.z = extract<T>(t[2]);

        line.dir = dir.normalized();
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "Line3 expects tuple of length 3");
}

template <class T>
static void
closestPoints1(Line3<T> &line1, const Line3<T> &line2, Vec3<T> &p0, Vec3<T> &p1)
{
    MATH_EXC_ON;
    IMATH_NAMESPACE::closestPoints(line1, line2, p0, p1);
}

template <class T>
static tuple
closestPoints2(Line3<T> &line1, const Line3<T> &line2)
{
    MATH_EXC_ON;
    Vec3<T> p0, p1;
    IMATH_NAMESPACE::closestPoints(line1, line2, p0, p1);
    tuple p0Tuple = make_tuple(p0.x,p0.y,p0.z);
    tuple p1Tuple = make_tuple(p1.x,p1.y,p1.z);

#if !defined(_MSC_VER) || (_MSC_VER <= 1200)
    tuple t = make_tuple(p0Tuple, p1Tuple);
    return t;
#else
    list v3;
    v3.append(p0Tuple);
    v3.append(p1Tuple);
    return tuple(v3);
#endif
}

template <class T>
static Vec3<T>
closestVertex(Line3<T> &line, const Vec3<T> &v0, const Vec3<T> &v1, const Vec3<T> &v2)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::closestVertex(v0, v1, v2, line);
}

template <class T>
static Vec3<T>
closestVertexTuple(Line3<T> &line, const tuple &t0, const tuple &t1, const tuple &t2)
{
    MATH_EXC_ON;
    if(t0.attr("__len__")() == 3 && t1.attr("__len__")() == 3 && t2.attr("__len__")() == 3)
    {
        Vec3<T> v0, v1, v2;
        v0.x = extract<T>(t0[0]);
        v0.y = extract<T>(t0[1]);
        v0.z = extract<T>(t0[2]);

        v1.x = extract<T>(t1[0]);
        v1.y = extract<T>(t1[1]);
        v1.z = extract<T>(t1[2]);

        v2.x = extract<T>(t2[0]);
        v2.y = extract<T>(t2[1]);
        v2.z = extract<T>(t2[2]);

        return IMATH_NAMESPACE::closestVertex(v0, v1, v2, line);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "Line3 expects tuple of length 3");
}

template <class T>
static bool
intersect1(Line3<T> &line, const Vec3<T> &v0, const Vec3<T> &v1, const Vec3<T> &v2,
          Vec3<T> &pt, Vec3<T> &barycentric, bool &front)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::intersect(line, v0, v1, v2, pt, barycentric, front);
}

template <class T>
static object
intersect2(Line3<T> &line, const Vec3<T> &v0, const Vec3<T> &v1, const Vec3<T> &v2)
{
    MATH_EXC_ON;
    Vec3<T> pt, bar;
    bool front;

    if(IMATH_NAMESPACE::intersect(line, v0, v1, v2, pt, bar, front))
    {
        return make_tuple (pt, bar, front);
    }
    else
    {
        return object();
    }
}

template <class T>
static tuple
intersectTuple(Line3<T> &line, const tuple &t0, const tuple &t1, const tuple &t2)
{

    if(t0.attr("__len__")() == 3 && t1.attr("__len__")() == 3 && t2.attr("__len__")() == 3)
    {
        Vec3<T> v0, v1, v2, pt, bar;
        bool front;
        v0.x = extract<T>(t0[0]);
        v0.y = extract<T>(t0[1]);
        v0.z = extract<T>(t0[2]);

        v1.x = extract<T>(t1[0]);
        v1.y = extract<T>(t1[1]);
        v1.z = extract<T>(t1[2]);

        v2.x = extract<T>(t2[0]);
        v2.y = extract<T>(t2[1]);
        v2.z = extract<T>(t2[2]);

        if(IMATH_NAMESPACE::intersect(line, v0, v1, v2, pt, bar, front))
        {
            tuple t = make_tuple(pt, bar, front);
            return t;
        }
        else
        {
            tuple t;
            return t;
        }
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "Line3 expects tuple of length 3");
}

template <class T>
static Vec3<T>
rotatePoint(Line3<T> &line, const Vec3<T> &p, const T &r)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::rotatePoint(p, line, r);
}

template <class T>
static Vec3<T>
rotatePointTuple(Line3<T> &line, const tuple &t, const T &r)
{
    MATH_EXC_ON;
    if(t.attr("__len__")() == 3)
    {
        Vec3<T> p;
        p.x = extract<T>(t[0]);
        p.y = extract<T>(t[1]);
        p.z = extract<T>(t[2]);

        return IMATH_NAMESPACE::rotatePoint(p, line, r);
    }
    else
        THROW(IEX_NAMESPACE::LogicExc, "Line3 expects tuple of length 3");
}

template <class T>
static std::string Line3_repr(const Line3<T> &v)
{
    Vec3<T> v1 = v.pos;
    Vec3<T> v2 = v.pos + v.dir;

    PyObject *v1Obj = V3<T>::wrap (v1);
    PyObject *v1ReprObj = PyObject_Repr (v1Obj);
#if PY_MAJOR_VERSION > 2
    std::string v1ReprStr = PyUnicode_AsUTF8 (v1ReprObj);
#else
    std::string v1ReprStr = PyString_AsString (v1ReprObj);
#endif
    Py_DECREF (v1ReprObj);
    Py_DECREF (v1Obj);

    PyObject *v2Obj = V3<T>::wrap (v2);
    PyObject *v2ReprObj = PyObject_Repr (v2Obj);
#if PY_MAJOR_VERSION > 2
    std::string v2ReprStr = PyUnicode_AsUTF8 (v2ReprObj);
#else
    std::string v2ReprStr = PyString_AsString (v2ReprObj);
#endif
    Py_DECREF (v2ReprObj);
    Py_DECREF (v2Obj);

    std::stringstream stream;
    stream << LineName<T>::value << "(" << v1ReprStr << ", " << v2ReprStr << ")";
    return stream.str();
}

template <class T>
static bool
equal(const Line3<T> &l1, const Line3<T> &l2)
{
    if(l1.pos == l2.pos && l1.dir == l2.dir)
        return true;
    else
        return false;
}

template <class T>
static bool
notequal(const Line3<T> &l1, const Line3<T> &l2)
{
    if(l1.pos != l2.pos || l1.dir != l2.dir)
        return true;
    else
        return false;
}

template <class T>
class_<Line3<T> >
register_Line()
{
    const char *name = LineName<T>::value;

    class_<Line3<T> > line_class(name);
    line_class
        .def("__init__", make_constructor(Line3_construct_default<T>), "initialize point to (0,0,0) and direction to (1,0,0)")
        .def("__init__", make_constructor(Line3_tuple_construct<T>))
        .def("__init__", make_constructor(Line3_line_construct<T,float>))
        .def("__init__", make_constructor(Line3_line_construct<T,double>))
        .def(init<const Vec3<float> &, const Vec3<float> &>("Line3(point1, point2) construction"))
        .def(init<const Vec3<double> &, const Vec3<double> &>("Line3(point1, point2) construction"))
        .def(self * Matrix44<T>())
        .def("__eq__", &equal<T>)
        .def("__ne__", &notequal<T>)

        .def_readwrite("pos", &Line3<T>::pos)
        .def_readwrite("dir", &Line3<T>::dir)

        .def("pos", &getPosition<T>,
        "l.pos() -- returns the start point of line l")

        .def("dir", &getDirection<T>,
        "l.dir() -- returns the direction of line l\n")

        .def("setPos", &setPosition<T>,
        "l.setPos(p) -- sets the start point of line l to p")
        .def("setPos", &setPositionTuple<T>)

        .def("setDir", &setDirection<T>,
        "l.setDir(d) -- sets the direction of line l\n"
		"to d.normalized().\n")
        .def("setDir", &setDirectionTuple<T>)

        .def("set", &set1<T>,
        "l.set(p1, p2) -- sets the start point\n"
		"and direction of line l by calling\n"
        "   l.setPos (p1)\n"
        "   l.setDir (p2 - p1)\n")

        .def("set", &setTuple<T>)

        .def("pointAt", &pointAt<T>,
        "l.pointAt(t) -- returns l.pos() + t * l.dir()")

        .def("distanceTo", &distanceTo1<T>,
        "l.distanceTo(p) -- returns the distance from\n"
		"   line l to point p\n")

        .def("distanceTo", &distanceTo2<T>,
        "l1.distanceTo(l2) -- returns the distance from\n"
        "   line l1 to line l2\n")

        .def("distanceTo", &distanceToTuple<T>)

        .def("closestPointTo", &closestPointTo1<T>,
        "l.closestPointTo(p) -- returns the point on\n"
		"   line l that is closest to point p\n"
        "\n")

        .def("closestPointTo", &closestPointToTuple<T>)
        .def("closestPointTo", &closestPointTo2<T>,
        "l1.closestPointTo(l2) -- returns the point on\n"
		"   line l1 that is closest to line l2\n")

        .def("closestPoints", &closestPoints1<T>,
        "l1.closestPoints(l2,p0,p1)")

        .def("closestPoints", &closestPoints2<T>,
        "l1.closestPoints(l2) -- returns a tuple with\n"
		"two points:\n"
        "   (l1.closestPoint(l2), l2.closestPoint(l1)\n")

        .def("closestTriangleVertex", &closestVertex<T>,
        "l.closestTriangleVertex(v0, v1, v2) -- returns\n"
		"a copy of v0, v1, or v2, depending on which is\n"
        "closest to line l.\n")

        .def("closestTriangleVertex", &closestVertexTuple<T>)
        .def("intersectWithTriangle", &intersect2<T>)
        .def("intersectWithTriangle", &intersect1<T>,
            "l.intersectWithTriangle(v0, v1, v2) -- computes the\n"
			"intersection of line l and triangle (v0, v1, v2).\n"
			"\n"
			"If the line and the triangle do not intersect,\n"
			"None is returned.\n"
			""
			"If the line and the triangle intersect, a tuple\n"
			"(p, b, f) is returned:\n"
			"\n"
			"   p  intersection point in 3D space\n"
			"\n"
			"   b  intersection point in barycentric coordinates\n"
			"\n"
			"   f  1 if the line hits the triangle from the\n"
			"      front (((v2-v1) % (v1-v2)) ^ l.dir() < 0),\n"
			"      0 if the line hits the trianble from the\n"
			"      back\n"
			"\n")
        .def("intersectWithTriangle", &intersectTuple<T>)

        .def("rotatePoint", &rotatePoint<T>,
            "l.rotatePoint(p,r) -- rotates point p around\n"
			"line by angle r (in radians), and returns the\n"
			"result (p is not modified)\n")

        .def("rotatePoint", &rotatePointTuple<T>)
        .def("__repr__",&Line3_repr<T>)
        ;

    decoratecopy(line_class);

    return line_class;
}

template PYIMATH_EXPORT class_<Line3<float> > register_Line<float>();
template PYIMATH_EXPORT class_<Line3<double> > register_Line<double>();

} // namespace PyImath


// Copyright 2019 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

// This file contains modified code from the following open source projects
// published under the licenses listed below:
//
// Software License Agreement (BSD License)
//
//  Point Cloud Library (PCL) - www.pointclouds.org
//  Copyright (c) 2010-2011, Willow Garage, Inc.
//  Copyright (c) 2012-, Open Perception, 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 copyright holder(s) 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.

#ifndef NDT__NDT_OPTIMIZATION_PROBLEM_HPP_
#define NDT__NDT_OPTIMIZATION_PROBLEM_HPP_

#include <ndt/ndt_map.hpp>
#include <ndt/ndt_scan.hpp>
#include <ndt/ndt_config.hpp>
#include <optimization/optimization_problem.hpp>
#include <optimization/utils.hpp>
#include <ndt/utils.hpp>
#include <experimental/optional>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <tuple>
#include "common/types.hpp"

using autoware::common::types::bool8_t;
using autoware::common::types::float64_t;

namespace autoware
{
namespace localization
{
namespace ndt
{

template<typename ScalarT>
bool8_t is_valid_probability(ScalarT p)
{
  bool8_t ret = true;
  if (std::isnan(p) || p > ScalarT{1.0} || p < ScalarT{0.0}) {
    ret = false;
  }
  return ret;
}

/// P2D ndt objective. This class implements the P2D ndt score function, its analytical
/// jacobian and hessian values.
/// \tparam MapT Type of map to be used.
template<typename MapT>
class P2DNDTObjective : public common::optimization::CachedExpression<P2DNDTObjective<MapT>,
    EigenPose<Real>, 1U, 6U, common::optimization::EigenComparator>
{
public:
  // getting aliases from the base class.
  using ExpressionT = common::optimization::CachedExpression<P2DNDTObjective<MapT>,
      EigenPose<Real>, 1U, 6U, common::optimization::EigenComparator>;
  using DomainValue = typename ExpressionT::DomainValue;
  using Value = typename ExpressionT::Value;
  using Jacobian = typename ExpressionT::Jacobian;
  using Hessian = typename ExpressionT::Hessian;
  using Map = MapT;
  using Scan = P2DNDTScan;
  using Point = typename Map::Point;
  using Comparator = common::optimization::EigenComparator;
  using ComputeMode = common::optimization::ComputeMode;
  using PointGrad = Eigen::Matrix<float64_t, 3, 6>;
  using PointHessian = Eigen::Matrix<float64_t, 18, 6>;

  /// Constructor.
  ///
  /// It should be noted here that ndt optimization problem does not take ownership of neither the
  /// scan nor the map but uses the references. Hence an optimization problem must not outlive the
  /// scan or the map.
  ///
  /// @param      scan    Scan to align with the map.
  /// @param      map     NDT map to be aligned.
  /// @param      config  Optimization config for this objective.
  ///
  P2DNDTObjective(
    const P2DNDTScan & scan, const Map & map, const P2DNDTOptimizationConfig config)
  : m_scan_ref(scan), m_map_ref(map)
  {
    init(config.outlier_ratio());
  }

  void evaluate_(const DomainValue & x, const ComputeMode & mode)
  {
    // Convert pose vector to transform matrix for easy point transformation
    Eigen::Transform<float64_t, 3, Eigen::Affine, Eigen::ColMajor> transform;
    transform.setIdentity();
    transform_adapters::pose_to_transform(x, transform);

    Value score{0.0};

    Jacobian jacobian;
    std::experimental::optional<GradientAngleParameters> grad_params;

    Hessian hessian;
    std::experimental::optional<HessianAngleParameters> hessian_params;

    {
      // Angle parameters to be used by all elements (eq. 6.12) [Magnusson 2009]
      const AngleParameters angle_params{x};
      // Only construct jacobian/hessian variables if they are needed.
      if (mode.jacobian() || mode.hessian()) {
        jacobian.setZero();
        grad_params.emplace(angle_params);
      }
      if (mode.hessian()) {
        hessian.setZero();
        hessian_params.emplace(angle_params);
      }
    }

    for (const auto & pt : m_scan_ref) {
      PointGrad point_gradient;
      PointHessian point_hessian;

      if (mode.jacobian() || mode.hessian()) {
        point_gradient.setZero();
        point_gradient.block<3, 3>(0, 0).setIdentity();
        compute_point_gradients(grad_params.value(), pt, point_gradient);

        if (mode.hessian()) {
          point_hessian.setZero();
          compute_point_hessians(hessian_params.value(), pt, point_hessian);
        }
      }

      const Point pt_trans = transform * pt;
      const auto & cells = m_map_ref.cell(pt_trans);

      for (const auto & cell : cells) {
        const Point pt_trans_norm = pt_trans - cell.centroid();
        // Cell iteration used for compatibility with maps with multi-cell lookup
        if (cell.usable()) {
          const auto & inv_cov = cell.inverse_covariance();
          // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
          Real e_x_cov_x = std::exp(
            -m_gauss_d2 * pt_trans_norm.dot(
              inv_cov * pt_trans_norm) / 2.0);

          if (mode.score()) {
            score += -m_gauss_d1 * e_x_cov_x;
          }

          if (mode.jacobian() || mode.hessian()) {
            const auto d2_e_x_cov_x = m_gauss_d2 * e_x_cov_x;

            // Error checking for invalid values.
            // TODO(yunus.caliskan): Can be removed after covariance is checked
            //  for definiteness #216
            if (!is_valid_probability(d2_e_x_cov_x)) {
              continue;
            }

            // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
            const auto d1_d2_e_x_cov_x = m_gauss_d1 * d2_e_x_cov_x;

            for (auto i = 0U; i < jacobian.rows(); ++i) {
              const Point cov_dxd_pi = inv_cov * point_gradient.col(i);
              if (mode.jacobian()) {
                jacobian(i) += pt_trans_norm.dot(cov_dxd_pi) * d1_d2_e_x_cov_x;
              }
              if (mode.hessian()) {
                for (auto j = 0U; j < hessian.cols(); ++j) {
                  hessian(i, j) += d1_d2_e_x_cov_x * (-m_gauss_d2 * pt_trans_norm.dot(cov_dxd_pi) *
                    pt_trans_norm.dot(inv_cov * point_gradient.col(j)) +
                    pt_trans_norm.dot(inv_cov * point_hessian.block<3, 1>(3 * i, j)) +
                    point_gradient.col(j).dot(cov_dxd_pi));
                }
              }
            }
          }
        }
      }
    }
    if (mode.score()) {
      this->set_score(score);
    }
    if (mode.jacobian()) {
      this->set_jacobian(jacobian);
    }
    if (mode.hessian()) {
      this->set_hessian(hessian);
    }
  }

private:
  /// Struct encapculating the intermediate parameters used in (eq. 6.17) [Magnusson 2009]
  struct AngleParameters
  {
public:
    static constexpr auto approx_thresh{10e-5};
    AngleParameters() = delete;
    explicit AngleParameters(const DomainValue & pose)
    {
      if (std::fabs(pose(3)) < approx_thresh) {
        cx = 1.0;
        sx = 0.0;
      } else {
        cx = std::cos(pose(3));
        sx = std::sin(pose(3));
      }

      if (std::fabs(pose(4)) < approx_thresh) {
        cy = 1.0;
        sy = 0.0;
      } else {
        cy = std::cos(pose(4));
        sy = std::sin(pose(4));
      }

      if (std::fabs(pose(5)) < approx_thresh) {
        cz = 1.0;
        sz = 0.0;
      } else {
        cz = std::cos(pose(5));
        sz = std::sin(pose(5));
      }
    }

    Real cx{0.0};
    Real cy{0.0};
    Real cz{0.0};
    Real sx{1.0};
    Real sy{1.0};
    Real sz{1.0};
  };

  /// Struct encapculating the intermediate parameters used in (eq. 6.19) [Magnusson 2009]
  struct GradientAngleParameters
  {
public:
    GradientAngleParameters() = delete;
    explicit GradientAngleParameters(const AngleParameters & params)
    {
      j_ang_a(0) = -params.sx * params.sz + params.cx * params.sy * params.cz;
      j_ang_a(1) = -params.sx * params.cz - params.cx * params.sy * params.sz;
      j_ang_a(2) = -params.cx * params.cy;

      j_ang_b(0) = params.cx * params.sz + params.sx * params.sy * params.cz;
      j_ang_b(1) = params.cx * params.cz - params.sx * params.sy * params.sz;
      j_ang_b(2) = -params.sx * params.cy;

      j_ang_c(0) = -params.sy * params.cz;
      j_ang_c(1) = params.sy * params.sz;
      j_ang_c(2) = params.cy;

      j_ang_d(0) = params.sx * params.cy * params.cz;
      j_ang_d(1) = -params.sx * params.cy * params.sz;
      j_ang_d(2) = params.sx * params.sy;

      j_ang_e(0) = -params.cx * params.cy * params.cz;
      j_ang_e(1) = params.cx * params.cy * params.sz;
      j_ang_e(2) = -params.cx * params.sy;

      j_ang_f(0) = -params.cy * params.sz;
      j_ang_f(1) = -params.cy * params.cz;
      j_ang_f(2) = 0.0;

      j_ang_g(0) = params.cx * params.cz - params.sx * params.sy * params.sz;
      j_ang_g(1) = -params.cx * params.sz - params.sx * params.sy * params.cz;
      j_ang_g(2) = 0.0;

      j_ang_h(0) = params.sx * params.cz + params.cx * params.sy * params.sz;
      j_ang_h(1) = params.cx * params.sy * params.cz - params.sx * params.sz;
      j_ang_h(2) = 0.0;
    }

    Point j_ang_a, j_ang_b, j_ang_c, j_ang_d, j_ang_e, j_ang_f, j_ang_g, j_ang_h;
  };

  /// Struct encapculating the intermediate parameters used in (eq. 6.21) [Magnusson 2009]
  struct HessianAngleParameters
  {
public:
    HessianAngleParameters() = delete;
    explicit HessianAngleParameters(const AngleParameters & params)
    {
      h_ang_a2(0) = -params.cx * params.sz - params.sx * params.sy * params.cz;
      h_ang_a2(1) = -params.cx * params.cz + params.sx * params.sy * params.sz;
      h_ang_a2(2) = params.sx * params.cy;

      h_ang_a3(0) = -params.sx * params.sz + params.cx * params.sy * params.cz;
      h_ang_a3(1) = -params.cx * params.sy * params.sz - params.sx * params.cz;
      h_ang_a3(2) = -params.cx * params.cy;

      h_ang_b2(0) = params.cx * params.cy * params.cz;
      h_ang_b2(1) = -params.cx * params.cy * params.sz;
      h_ang_b2(2) = params.cx * params.sy;

      h_ang_b3(0) = params.sx * params.cy * params.cz;
      h_ang_b3(1) = -params.sx * params.cy * params.sz;
      h_ang_b3(2) = params.sx * params.sy;

      h_ang_c2(0) = -params.sx * params.cz - params.cx * params.sy * params.sz;
      h_ang_c2(1) = params.sx * params.sz - params.cx * params.sy * params.cz;
      h_ang_c2(2) = 0.0;

      h_ang_c3(0) = params.cx * params.cz - params.sx * params.sy * params.sz;
      h_ang_c3(1) = -params.sx * params.sy * params.cz - params.cx * params.sz;
      h_ang_c3(2) = 0.0;

      h_ang_d1(0) = -params.cy * params.cz;
      h_ang_d1(1) = params.cy * params.sz;
      h_ang_d1(2) = params.sy;

      h_ang_d2(0) = -params.sx * params.sy * params.cz;
      h_ang_d2(1) = params.sx * params.sy * params.sz;
      h_ang_d2(2) = params.sx * params.cy;

      h_ang_d3(0) = params.cx * params.sy * params.cz;
      h_ang_d3(1) = -params.cx * params.sy * params.sz;
      h_ang_d3(2) = -params.cx * params.cy;

      h_ang_e1(0) = params.sy * params.sz;
      h_ang_e1(1) = params.sy * params.cz;
      h_ang_e1(2) = 0.0;

      h_ang_e2(0) = -params.sx * params.cy * params.sz;
      h_ang_e2(1) = -params.sx * params.cy * params.cz;
      h_ang_e2(2) = 0.0;

      h_ang_e3(0) = params.cx * params.cy * params.sz;
      h_ang_e3(1) = params.cx * params.cy * params.cz;
      h_ang_e3(2) = 0.0;

      h_ang_f1(0) = -params.cy * params.cz;
      h_ang_f1(1) = params.cy * params.sz;
      h_ang_f1(2) = 0.0;

      h_ang_f2(0) = -params.cx * params.sz - params.sx * params.sy * params.cz;
      h_ang_f2(1) = -params.cx * params.cz + params.sx * params.sy * params.sz;
      h_ang_f2(2) = 0.0;

      h_ang_f3(0) = -params.sx * params.sz + params.cx * params.sy * params.cz;
      h_ang_f3(1) = -params.cx * params.sy * params.sz - params.sx * params.cz;
      h_ang_f3(2) = 0.0;
    }

    Point h_ang_a2, h_ang_a3,
      h_ang_b2, h_ang_b3,
      h_ang_c2, h_ang_c3,
      h_ang_d1, h_ang_d2, h_ang_d3,
      h_ang_e1, h_ang_e2, h_ang_e3,
      h_ang_f1, h_ang_f2, h_ang_f3;
  };

  void compute_point_gradients(
    const GradientAngleParameters & params,
    const Point & x,
    PointGrad & point_gradient)
  {
    point_gradient(1, 3) = x.dot(params.j_ang_a);
    point_gradient(2, 3) = x.dot(params.j_ang_b);
    point_gradient(0, 4) = x.dot(params.j_ang_c);
    point_gradient(1, 4) = x.dot(params.j_ang_d);
    point_gradient(2, 4) = x.dot(params.j_ang_e);
    point_gradient(0, 5) = x.dot(params.j_ang_f);
    point_gradient(1, 5) = x.dot(params.j_ang_g);
    point_gradient(2, 5) = x.dot(params.j_ang_h);
  }

  void compute_point_hessians(
    const HessianAngleParameters & params,
    const Point & x,
    PointHessian & point_hessian)
  {
    const Point a{0.0, x.dot(params.h_ang_a2), x.dot(params.h_ang_a3)};
    const Point b{0.0, x.dot(params.h_ang_b2), x.dot(params.h_ang_b3)};
    const Point c{0.0, x.dot(params.h_ang_c2), x.dot(params.h_ang_c3)};
    const Point d{x.dot(params.h_ang_d1), x.dot(params.h_ang_d2),
      x.dot(params.h_ang_d3)};
    const Point e{x.dot(params.h_ang_e1), x.dot(params.h_ang_e2),
      x.dot(params.h_ang_e3)};
    const Point f{x.dot(params.h_ang_f1), x.dot(params.h_ang_f2),
      x.dot(params.h_ang_f3)};

    point_hessian.block<3, 1>(9, 3) = a;
    point_hessian.block<3, 1>(12, 3) = b;
    point_hessian.block<3, 1>(15, 3) = c;
    point_hessian.block<3, 1>(9, 4) = b;
    point_hessian.block<3, 1>(12, 4) = d;
    point_hessian.block<3, 1>(15, 4) = e;
    point_hessian.block<3, 1>(9, 5) = c;
    point_hessian.block<3, 1>(12, 5) = e;
    point_hessian.block<3, 1>(15, 5) = f;
  }

  /// Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009]
  /// \param outlier_ratio Outlier ratio to be used in the gaussian distribution variation
  /// used in (eq. 6.7) [Magnusson 2009]
  void init(Real outlier_ratio)
  {
    if (!is_valid_probability(outlier_ratio)) {
      throw std::domain_error("Outlier ratio must be between 0 and 1");
    }
    const auto c_size = m_map_ref.cell_size();
    // The gaussian fitting parameters below are taken from the PCL implementation.
    // 10.0 seems to be a magic number. For details on the gaussian
    // approximation of the mixture probability in see [Biber et al, 2004] and [Magnusson 2009].
    const auto gauss_c1 = 10.0 * (1.0 - outlier_ratio);
    const auto gauss_c2 = outlier_ratio / static_cast<Real>(c_size.x * c_size.y * c_size.z);
    const auto gauss_d3 = -std::log(gauss_c2);
    m_gauss_d1 = -std::log(gauss_c1 + gauss_c2) - gauss_d3;
    m_gauss_d2 = -2 *
      std::log((-std::log(gauss_c1 * std::exp(-0.5) + gauss_c2) - gauss_d3) / m_gauss_d1);
  }

  // references as class members to be initialized at constructor.
  const Scan & m_scan_ref;
  const Map & m_map_ref;
  // States:
  Real m_gauss_d1{0.0};
  Real m_gauss_d2{0.0};
};

template<typename MapT>
using P2DNDTOptimizationProblem =
  common::optimization::UnconstrainedOptimizationProblem<P2DNDTObjective<MapT>, EigenPose<Real>,
    6U>;
}  // namespace ndt
}  // namespace localization
}  // namespace autoware

#endif  // NDT__NDT_OPTIMIZATION_PROBLEM_HPP_