//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SQRT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SQRT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object computes the square root of its parameter.
    For integers it is the truncation of the real square root.

    @par Header <boost/simd/function/sqrt.hpp>

    @par Decorators

    - std_ calls std::sqrt

    - raw_ for floating entries can gain some speed with less accuracy
    on some architectures.

    @see rsqrt, sqr_abs, sqr

    @par Example:

      @snippet sqrt.cpp sqrt

    @par Possible output:

      @snippet sqrt.txt sqrt

  **/
  Value sqrt(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/sqrt.hpp>
#include <boost/simd/function/simd/sqrt.hpp>

#endif


#pragma once
#ifndef CELL_HPP
#define CELL_HPP

// c++ libraries
#include <iosfwd>
// eigen libraries
#include <Eigen/Dense>
// ann - serialization
#include "src/mem/serialize.hpp"

//****************************************************************
//Cell class
//****************************************************************

class Cell{
private:
	//==== members ====
	double vol_;//the volume of the simulation cell
	Eigen::Matrix3d R_;//the lattice vector matrix (lattice vectors are columns of the matrix)
	Eigen::Matrix3d RInv_;//the inverse of the lattice vector matrix
	Eigen::Matrix3d K_;//the repiprocal lattice vector matrix (lattice vectors are columns of the matrix
	Eigen::Matrix3d KInv_;//the inverse of the reciprocal lattice vector matrix
public:
	//==== constructors/destructors ====
	Cell(){defaults();}
	Cell(const Eigen::Matrix3d& R){init(R);}
	~Cell(){}

	//==== operators ====
	friend std::ostream& operator<<(std::ostream& out, const Cell& cell);

	//==== access ====
	double& vol(){return vol_;}
	const double& vol()const{return vol_;}
	const Eigen::Matrix3d& R()const{return R_;}
	const Eigen::Matrix3d& RInv()const{return RInv_;}
	const Eigen::Matrix3d& K()const{return K_;}
	const Eigen::Matrix3d& KInv()const{return KInv_;}

	//==== static functions - vector operations ====
	static Eigen::Vector3d& sum(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& sum, const Eigen::Matrix3d& R, const Eigen::Matrix3d& RInv);
	static Eigen::Vector3d& diff(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& diff, const Eigen::Matrix3d& R, const Eigen::Matrix3d& RInv);
	static double dist(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& tmp, const Eigen::Matrix3d& R, const Eigen::Matrix3d& RInv);
	static double dist(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, const Eigen::Matrix3d& R, const Eigen::Matrix3d& RInv);
	static Eigen::Vector3d& fracToCart(const Eigen::Vector3d& vFrac, Eigen::Vector3d& vCart, const Eigen::Matrix3d& R);
	static Eigen::Vector3d& cartToFrac(const Eigen::Vector3d& vCart, Eigen::Vector3d& vFrac, const Eigen::Matrix3d& RInv);
	static Eigen::Vector3d& returnToCell(const Eigen::Vector3d& v1, Eigen::Vector3d& v2, const Eigen::Matrix3d& R, const Eigen::Matrix3d& RInv);

	//==== vector operations ====
	Eigen::Vector3d& sum(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& sum)const;
	Eigen::Vector3d& diff(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& diff)const;
	double dist(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)const;
	double dist(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& tmp)const;
	double dist2(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, Eigen::Vector3d& tmp)const;
	Eigen::Vector3d& modv(const Eigen::Vector3d& v1, Eigen::Vector3d& v2);

	//==== static functions - modification ====
	static Cell& make_super(const Eigen::Vector3i& s, const Cell& cell1, Cell& cell2);

	//==== member functions ====
	void defaults();
	void clear(){defaults();}
	void init(const Eigen::Matrix3d& R);
};

bool operator==(const Cell& c1, const Cell& c2);
bool operator!=(const Cell& c1, const Cell& c2);

namespace serialize{

	//**********************************************
	// byte measures
	//**********************************************

	template <> int nbytes(const Cell& obj);

	//**********************************************
	// packing
	//**********************************************

	template <> int pack(const Cell& obj, char* arr);

	//**********************************************
	// unpacking
	//**********************************************

	template <> int unpack(Cell& obj, const char* arr);

}

#endif


/* Copyright 2020 Oinam Romesh Meitei

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

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cmath>
#include <vector>
#include "pulsec.h"

/* Piecewise square pulse */

std::complex<double> pcoef(double &t, std::vector< double > &amp,
			   std::vector< double > &tseq,
			   double &freq,
			   double &tfinal){

  double  etmp;
  int i, tlen;
  std::complex <double>  coef;

  std::complex<double> etmp1(0.0, -1.0*freq*t);
  etmp1 = std::exp(etmp1);

  tlen = tseq.size();
  for (i=0;i<tlen;i++){
    if (i==0){
      if (0.0 < t && t <= tseq[i]){
	coef = amp[i] * etmp1;
      }
    }
    else {
      if (tseq[i-1] < t && t <= tseq[i]){
	coef = amp[i] * etmp1;
      }
    }
  }
  if (tseq[tlen-1] < t && t <= tfinal){
    coef = amp[tlen] * etmp1;

  }
  return coef;
}

Eigen::SparseMatrix<std::complex<double> >
getham(double &t, pulsec &pobj,
       std::vector< std::vector< Eigen::SparseMatrix<double,0,ptrdiff_t> > > &hdrive,
       std::vector< std::complex<double> > &dsham, int &dsham_len,
       Eigen::SparseMatrix<std::complex<double> > &matexp_){

  // dsham is different from python version, here it's the diagonal of
  // -1j*hobj.dsham in the python version.

  Eigen::SparseMatrix<std::complex<double> > hamdr;
  std::complex<double> hcoef, hcoefc;

  int i;
  for (i=0;i<pobj.nqubit;i++) {
    hcoef = pcoef( t, pobj.amp[i], pobj.tseq[i], pobj.freq[i],
		   pobj.duration);

    hcoefc = std::conj(hcoef);

    if (i==0){
      hamdr = hcoef * hdrive[i][0];
    } else {
      hamdr += hcoef * hdrive[i][0];
    }

    hamdr += hcoefc * hdrive[i][1];
  }

  for (i=0;i<dsham_len; i++){
    matexp_.coeffRef(i,i) = std::exp(dsham[i] * t);
  }

  Eigen::SparseMatrix<std::complex<double> >
  hamr_ = (matexp_.conjugate().transpose() * hamdr);
  hamr_ = (hamr_ * matexp_).pruned();

  return hamr_;
}


//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2009
//
//============================================================================

#include "StdLinearSolver.hpp"
#include "../IDenseMatrix.hpp"
#include "../ICompressedSparseMatrix.hpp"
#include "NNLS/nnls.h"
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/QR>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>
#include <Eigen/SVD>
#include <type_traits>

namespace Thea {
namespace Algorithms {

namespace StdLinearSolverInternal {

// Implementation of StdLinearSolver functions.
class THEA_DLL_LOCAL StdLinearSolverImpl
{
  public:
    // Constructor.
    StdLinearSolverImpl(StdLinearSolver::Method method_, StdLinearSolver::Constraint constraint_)
    : method(method_), constraint(constraint_), tolerance(-1), max_iters(-1), ndims(0), has_solution(false)
    {}

    // Solve the linear system Ax = b for a dense double-precision matrix A.
    template < typename MatrixT, typename ScalarT,
               typename std::enable_if< std::is_same<typename MatrixT::value_type, ScalarT>::value, int >::type = 0 >
    bool solve(Eigen::MatrixBase<MatrixT> const & a, ScalarT const * b, IOptions const * options = nullptr)
    {
      if (a.rows() < a.cols())
        THEA_WARNING << "StdLinearSolver: Fewer objectives than dimensions -- the solution will not be unique";

      has_solution = false;
      try
      {
        if (a.rows() <= 0 || a.cols() <= 0)
          throw Error("Empty coefficient matrix");

        intx num_objectives = a.rows();
        ndims = a.cols();

        switch (constraint)
        {
          case StdLinearSolver::Constraint::NON_NEGATIVE:
          {
            if (method != StdLinearSolver::Method::DEFAULT && method != StdLinearSolver::Method::NNLS)
              throw Error("Unsupported method for non-negative least squares problems");

            // Values will be overwritten anyway, so make copies
            MatrixX<double, MatrixLayout::COLUMN_MAJOR> nnls_a = a.template cast<double>();  // NNLS needs Fortran COLUMN-MAJOR
            VectorX<double> nnls_b = Eigen::Map< VectorX<ScalarT> const >(b, num_objectives).template cast<double>();

            solution.resize(ndims);

            double         rnorm;
            Array<double>  w((size_t)ndims);
            Array<double>  zz((size_t)num_objectives);
            Array<int>     index((size_t)ndims);
            int            mode;

            int mda = (int)num_objectives, im = (int)num_objectives, in = (int)ndims;

            // NOTE: Assume float64 == double, for passing solution vector to NNLS
            nnls_c(nnls_a.data(), &mda, &im, &in, nnls_b.data(), solution.data(), &rnorm, &w[0], &zz[0], &index[0], &mode);

            if (mode == 1)
              has_solution = true;
            else
            {
              switch (mode)
              {
                // Should never be 2 since we've checked for this above, but recheck all the same
                case 2:  THEA_DEBUG << "StdLinearSolver: NNLS error (bad problem dimensions)"; break;
                case 3:  THEA_DEBUG << "StdLinearSolver: NNLS error (iteration count exceeded)"; break;
                default: THEA_DEBUG << "StdLinearSolver: Unknown NNLS error";
              }
            }

            break;
          }

          case StdLinearSolver::Constraint::UNCONSTRAINED:
          {
            switch (method)
            {
              case StdLinearSolver::Method::HOUSEHOLDER_QR:
              {
                Eigen::HouseholderQR<typename MatrixT::PlainObject> solver(a);
                solution = solver.solve(Eigen::Map< VectorX<ScalarT> const >(b, num_objectives));
                has_solution = true;
                break;
              }

              case StdLinearSolver::Method::DEFAULT:  // slower than plain Householder, but more accurate
              case StdLinearSolver::Method::COL_PIV_HOUSEHOLDER_QR:
              {
                Eigen::ColPivHouseholderQR<typename MatrixT::PlainObject> solver(a);
                if (tolerance >= 0) solver.setThreshold(tolerance);
                solution = solver.solve(Eigen::Map< VectorX<ScalarT> const >(b, num_objectives));
                has_solution = true;
                break;
              }

              case StdLinearSolver::Method::FULL_PIV_HOUSEHOLDER_QR:
              {
                Eigen::FullPivHouseholderQR<typename MatrixT::PlainObject> solver(a);
                if (tolerance >= 0) solver.setThreshold(tolerance);
                solution = solver.solve(Eigen::Map< VectorX<ScalarT> const >(b, num_objectives));
                has_solution = true;
                break;
              }

              case StdLinearSolver::Method::COMPLETE_ORTHOGONAL_DECOMPOSITION:
              {
                Eigen::CompleteOrthogonalDecomposition<typename MatrixT::PlainObject> solver(a);
                if (tolerance >= 0) solver.setThreshold(tolerance);
                solution = solver.solve(Eigen::Map< VectorX<ScalarT> const >(b, num_objectives));
                has_solution = true;
                break;
              }

              case StdLinearSolver::Method::BDCSVD:
              {
                Eigen::BDCSVD<typename MatrixT::PlainObject> solver(a);
                if (tolerance >= 0) solver.setThreshold(tolerance);
                solution = solver.solve(Eigen::Map< VectorX<ScalarT> const >(b, num_objectives));
                has_solution = true;
                break;
              }

              default:
                throw Error("Unsupported method for unconstrained dense least squares problems");
            }

            break;
          }

          default:
            throw Error("Unsupported constraint");
        }
      }
      THEA_STANDARD_CATCH_BLOCKS(return false;, ERROR, "%s",
                                 "StdLinearSolver: Error solving dense linear least-squares system")

      return has_solution;
    }

    // Solve the linear system Ax = b for a sparse ScalarT-precision matrix A.
    template < typename MatrixT, typename ScalarT,
               typename std::enable_if< std::is_same<typename MatrixT::value_type, ScalarT>::value, int >::type = 0 >
    bool solve(Eigen::SparseMatrixBase<MatrixT> const & a, ScalarT const * b, IOptions const * options = nullptr)
    {
      if (a.rows() < a.cols())
        THEA_WARNING << "StdLinearSolver: Fewer objectives than dimensions -- the solution will not be unique";

      has_solution = false;
      try
      {
        if (a.rows() <= 0 || a.cols() <= 0)
          throw Error("Empty coefficient matrix");

        ndims = a.cols();

        switch (constraint)
        {
          case StdLinearSolver::Constraint::UNCONSTRAINED:
          {
            // Return true if a matching solver was found
            if (solveSparseFactorize(a, b)) break;
            if (solveIterative(a, b)) break;

            throw Error("Unsupported method for unconstrained sparse least squares problems");
          }

          default:
            throw Error("Unsupported constraint");
        }
      }
      THEA_STANDARD_CATCH_BLOCKS(return false;, ERROR, "%s",
                                 "StdLinearSolver: Error solving sparse linear least-squares system")

      return has_solution;
    }

  private:
    // Use one of the iterative solvers to solve the dense or sparse problem. Returns true if a suitable solver was found, NOT
    // if the problem was successfully solved.
    template <typename MatrixT, typename ScalarT> bool solveIterative(MatrixT const & a, ScalarT const * b)
    {
      switch (method)
      {
        case StdLinearSolver::Method::CONJUGATE_GRADIENT:
        {
          Eigen::ConjugateGradient<typename MatrixT::PlainObject> solver;
          if (tolerance >= 0) solver.setTolerance((ScalarT)tolerance);
          if (max_iters > 0) solver.setMaxIterations(max_iters);
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        case StdLinearSolver::Method::DEFAULT:
        case StdLinearSolver::Method::LEAST_SQUARES_CONJUGATE_GRADIENT:
        {
          Eigen::LeastSquaresConjugateGradient<typename MatrixT::PlainObject> solver;
          if (tolerance >= 0) solver.setTolerance((ScalarT)tolerance);
          if (max_iters > 0) solver.setMaxIterations(max_iters);
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        case StdLinearSolver::Method::BICGSTAB:
        {
          Eigen::BiCGSTAB<typename MatrixT::PlainObject> solver;
          if (tolerance >= 0) solver.setTolerance((ScalarT)tolerance);
          if (max_iters > 0) solver.setMaxIterations(max_iters);
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        default: return false;
      }

      return true;
    }

    // Use a solver based on sparse factorization, for column-major matrices. Returns true if a suitable solver was found, NOT
    // if the problem was successfully solved.
    template < typename MatrixT, typename ScalarT,
               typename std::enable_if< !(MatrixT::Flags & Eigen::RowMajorBit), int >::type = 0 >
    bool solveSparseFactorize(MatrixT const & a, ScalarT const * b)
    {
      switch (method)
      {
        case StdLinearSolver::Method::SIMPLICIALT_LLT:
        {
          Eigen::SimplicialLLT<typename MatrixT::PlainObject> solver;
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        case StdLinearSolver::Method::SIMPLICIALT_LDLT:
        {
          Eigen::SimplicialLDLT<typename MatrixT::PlainObject> solver;
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        case StdLinearSolver::Method::SPARSE_LU:
        {
          Eigen::SparseLU<typename MatrixT::PlainObject> solver;
          if (tolerance >= 0) solver.setPivotThreshold((ScalarT)tolerance);
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        case StdLinearSolver::Method::DEFAULT:
        case StdLinearSolver::Method::SPARSE_QR:
        {
          Eigen::SparseQR<typename MatrixT::PlainObject, Eigen::AMDOrdering<typename MatrixT::StorageIndex> > solver;
          if (tolerance >= 0) solver.setPivotThreshold((ScalarT)tolerance);
          solver.compute(a);
          if (solver.info() == Eigen::Success)
          {
            solution = solver.solve(Eigen::Map< VectorX<ScalarT> >(const_cast<ScalarT *>(b), a.rows()));
            has_solution = (solver.info() == Eigen::Success);
          }
          break;
        }

        default: return false;
      }

      return true;
    }

    // Use a solver based on sparse factorization, for row-major matrices. Eigen does not provide any such solvers, so this
    // method is empty. Returns false to indicate no solver was found.
    template < typename MatrixT, typename ScalarT,
               typename std::enable_if< (MatrixT::Flags & Eigen::RowMajorBit), int >::type = 0 >
    bool solveSparseFactorize(MatrixT const & a, ScalarT const * b)
    {
      return false;
    }

  public:  // no need for accessor fns for this internal class, and friend declarations are complicated by namespaces
    StdLinearSolver::Method method;          // Solution method.
    StdLinearSolver::Constraint constraint;  // Solution constraint.
    double tolerance;                        // Solution tolerance/threshold.
    intx max_iters;                          // Maximum number of solver iterations, if solver is iterative.
    intx ndims;                              // Solution dimensions.
    bool has_solution;                       // Was a solution computed by the last call to solve()? */
    VectorX<float64> solution;               // The solution vector <b>x</b>.
};

// Shorthand
template <MatrixLayout::Value L, typename StorageIndex> using SM = Eigen::Map< SparseMatrix<double, L, StorageIndex> >;

// Given a storage index type, dispatch a call to StdLinearSolverImpl::solve() with the correct pointer conversions
template <MatrixLayout::Value L, typename ScalarT>
bool
implSolve(StdLinearSolverImpl * impl, int storage_type, intx nr, intx nc, intx nnz, void const * in, void const * out,
          ScalarT const * val, void const * nzc, ScalarT const * b, IOptions const * opt)
{
  void    * in2  = const_cast<void *>(in);
  void    * out2 = const_cast<void *>(out);
  ScalarT * val2 = const_cast<ScalarT *>(val);
  void    * nzc2 = const_cast<void *>(nzc);

  switch (storage_type)
  {
// A casting bug in the current Eigen prevents us using StorageIndex shorter than an int.
// Specifically, OrderingMethods/Amd.h, L104: dense = (std::min)(n-2, dense);
// Here n and dense are of type StorageIndex, but n - 2 is of type int for StorageIndex shorter than an int, so std::min fails
// because the arguments are not of the same type.
//
//     case NumericType::INT8:
//       return impl->solve(SM<L, int8  >(nr, nc, nnz, (int8   *)in2, (int8   *)out2, val2, (int8   *)nzc2), b, opt);
//     case NumericType::INT16:
//       return impl->solve(SM<L, int16 >(nr, nc, nnz, (int16  *)in2, (int16  *)out2, val2, (int16  *)nzc2), b, opt);
    case NumericType::INT32:
      return impl->solve(SM<L, int32 >(nr, nc, nnz, (int32  *)in2, (int32  *)out2, val2, (int32  *)nzc2), b, opt);
    case NumericType::INT64:
      return impl->solve(SM<L, int64 >(nr, nc, nnz, (int64  *)in2, (int64  *)out2, val2, (int64  *)nzc2), b, opt);
    default: THEA_ERROR << "StdLinearSolver: Unsupported index type";
  }

  return false;
}

} // namespace StdLinearSolverInternal

StdLinearSolver::StdLinearSolver(Method method_, Constraint constraint_)
: NamedObject("StdLinearSolver"), impl(new StdLinearSolverInternal::StdLinearSolverImpl(method_, constraint_))
{
}

StdLinearSolver::~StdLinearSolver()
{
  delete impl;
}

StdLinearSolver::Method
StdLinearSolver::getMethod() const
{
  return impl->method;
}

StdLinearSolver::Constraint
StdLinearSolver::getConstraint() const
{
  return impl->constraint;
}

double
StdLinearSolver::getTolerance() const
{
  return impl->tolerance;
}

intx
StdLinearSolver::maxIterations() const
{
  return impl->max_iters;
}

void
StdLinearSolver::setMethod(StdLinearSolver::Method method_)
{
  impl->method = method_;
}

void
StdLinearSolver::setConstraint(StdLinearSolver::Constraint constraint_)
{
  impl->constraint = constraint_;
}

void
StdLinearSolver::setTolerance(double tol)
{
  impl->tolerance = tol;
}

void
StdLinearSolver::setMaxIterations(intx max_iters_)
{
  impl->max_iters = max_iters_;
}

bool
StdLinearSolver::solve(Eigen::Ref< MatrixXd > const & a, float64 const * b, IOptions const * options)
{
  return impl->solve(a, b, options);
}

bool
StdLinearSolver::solve(Eigen::Ref< SparseMatrix<double> > const & a, float64 const * b, IOptions const * options)
{
  return impl->solve(a, b, options);
}

int8
StdLinearSolver::solve(IMatrix<float64> const * a, float64 const * b, IOptions const * options)
{
  alwaysAssertM(a, "StdLinearSolver: Coefficient matrix is null");
  alwaysAssertM(b, "StdLinearSolver: Constant matrix is null");

  if (a->asAddressable() && a->asAddressable()->asDense())
  {
    IDenseMatrix<float64> const & dm = *a->asAddressable()->asDense();
    if (dm.isRowMajor())
    {
      Eigen::Map< MatrixX<float64, MatrixLayout::ROW_MAJOR> const > wrapped(dm.data(), dm.rows(), dm.cols());
      return impl->solve(wrapped, b, options);
    }
    else  // col-major
    {
      Eigen::Map< MatrixX<float64, MatrixLayout::COLUMN_MAJOR> const > wrapped(dm.data(), dm.rows(), dm.cols());
      return impl->solve(wrapped, b, options);
    }
  }
  else if (a->asSparse() && a->asSparse()->asCompressed())
  {
    ICompressedSparseMatrix<float64> const & sm = *a->asSparse()->asCompressed();
    int storage_type = sm.getInnerIndexType();
    if (storage_type != sm.getOuterIndexType() || storage_type != sm.getNonZeroCountType())
    {
      // TODO: Convert to integer arrays of consistent type to work around this problem

      THEA_ERROR << "StdLinearSolver: Different indices have different storage types -- cannot convert to SparseMatrix";
      return false;
    }

    if (sm.isRowMajor())
      return StdLinearSolverInternal::implSolve<MatrixLayout::ROW_MAJOR>(impl, storage_type, sm.rows(), sm.cols(),
                                                                         sm.numStoredElements(), sm.getOuterIndices(),
                                                                         sm.getInnerIndices(), sm.getValues(),
                                                                         sm.getNonZeroCounts(), b, options);
    else
      return StdLinearSolverInternal::implSolve<MatrixLayout::COLUMN_MAJOR>(impl, storage_type, sm.rows(), sm.cols(),
                                                                            sm.numStoredElements(), sm.getOuterIndices(),
                                                                            sm.getInnerIndices(), sm.getValues(),
                                                                            sm.getNonZeroCounts(), b, options);
  }
  else
  {
    THEA_ERROR << "StdLinearSolver: Unsupported matrix type";
    return false;
  }
}

int64
StdLinearSolver::dims() const
{
  return (int64)impl->ndims;
}

int8
StdLinearSolver::hasSolution() const
{
  return impl->has_solution;
}

float64 const *
StdLinearSolver::getSolution() const
{
  return impl->solution.data();
}

int8
StdLinearSolver::getSquaredError(float64 * err) const
{
  return false;
}

} // namespace Algorithms
} // namespace Thea


/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020 Northwestern University
 * 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 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 HOLDER 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.
 *********************************************************************/
/**
 * @file cart.hpp
 * @author Boston Cleek
 * @date 28 Oct 2020
 * @brief Kinematic cart models control wheel velocities or body twist
 */
#ifndef CART_HPP
#define CART_HPP

#include <cmath>
#include <stdexcept>

#include <armadillo>

#include <ergodic_exploration/numerics.hpp>

namespace ergodic_exploration
{
namespace models
{
using arma::mat;
using arma::vec;

/**
 * @brief Kinematic model of 2 wheel differential drive robot
 * @details The state is [x, y, theta] and controls are the velocities of each wheel [u0,
 * u1] corresponding to the left and right wheels
 */
struct Cart
{
  /**
   * @brief Constructor
   * @param wheel_radius - radius of wheel
   * @param wheel_base - distance from point at the center in between both wheels to the
   * center of a wheel
   */
  Cart(double wheel_radius, double wheel_base)
    : wheel_radius(wheel_radius), wheel_base(wheel_base), state_space(3)
  {
  }

  /**
   * @brief Convert wheel velocities to a body frame twist
   * @param u - control [u0, u1]
   * @return twist in body frame Vb = [vx, vy, w]
   */
  vec wheels2Twist(const vec u) const
  {
    const double vx = wheel_radius / 2.0 * (u(0) + u(1));
    const double w = wheel_radius / (2.0 * wheel_base) * (u(1) - u(0));

    return { vx, 0.0, w };
  }

  /**
   * @brief Kinematic model of a 2 wheel differential drive robot
   * @param x - state [x, y, theta]
   * @param u - control [uL, uR]
   * @return [xdot, ydot, thetadot] = f(x,u)
   */
  vec operator()(const vec x, const vec u) const
  {
    vec xdot(3);
    xdot(0) = (u(0) + u(1)) * std::cos(x(2));
    xdot(1) = (u(0) + u(1)) * std::sin(x(2));
    xdot(2) = (u(1) - u(0)) / wheel_base;

    return (wheel_radius / 2.0) * xdot;
  }

  /**
   * @brief Jacobian of the model with respect to the state
   * @param x - state [x, y, theta]
   * @param u - control [uL, uR]
   * @return A = D1(f(x,u)) of shape (3x3)
   */
  mat fdx(const vec x, const vec u) const
  {
    mat A(3, 3, arma::fill::zeros);

    const auto df0dth = -(wheel_radius / 2.0) * (u(0) + u(1)) * std::sin(x(2));
    const auto df1dth = (wheel_radius / 2.0) * (u(0) + u(1)) * std::cos(x(2));

    A(0, 2) = df0dth;
    A(1, 2) = df1dth;

    return A;
  }

  /**
   * @brief Jacobian of the model with respect to the control
   * @param x - state [x, y, theta]
   * @return B = D2(f(x,u)) of shape (3x2)
   */
  mat fdu(const vec x) const
  {
    mat B(3, 2);

    B(0, 0) = std::cos(x(2));
    B(0, 1) = std::cos(x(2));

    B(1, 0) = std::sin(x(2));
    B(1, 1) = std::sin(x(2));

    B(2, 0) = -1.0 / wheel_base;
    B(2, 1) = 1.0 / wheel_base;

    return (wheel_radius / 2.0) * B;
  }

  double wheel_radius;       // radius of wheel
  double wheel_base;         // distance from robot center to wheel center
  unsigned int state_space;  // states space dimension
};

/**
 * @brief Kinematic model of a wheeled differential drive robot
 * @details The state is [x, y, theta] and controls are the linear and
 * angular velocities [vx, vy, w] (body twist)
 */
struct SimpleCart
{
  /** @brief Constructor */
  SimpleCart() : state_space(3)
  {
  }

  /**
   * @brief Kinematic model of a 2 wheel differential drive robot
   * @param x - state [x, y, theta]
   * @param u - body twist control [vx, vy, w]
   * @return xdot = f(x,u)
   */
  vec operator()(const vec x, const vec u) const
  {
    if (!almost_equal(u(1), 0.0))
    {
      throw std::invalid_argument("Invalid twist y-velocity must be 0.");
    }

    return { u(0) * std::cos(x(2)), u(0) * std::sin(x(2)), u(2) };
  }

  /**
   * @brief Jacobian of the model with respect to the state
   * @param x - state [x, y, theta]
   * @param u - body twist control [vx, vy, w]
   * @return A = D1(f(x,u)) of shape (3x3)
   */
  mat fdx(const vec x, const vec u) const
  {
    mat A(3, 3, arma::fill::zeros);
    A(0, 2) = -u(0) * std::sin(x(2));
    A(1, 2) = u(0) * std::cos(x(2));
    return A;
  }

  /**
   * @brief Jacobian of the model with respect to the control
   * @param x - state [x, y, theta]
   * @return B = D2(f(x,u)) of shape (3x3)
   */
  mat fdu(const vec x) const
  {
    mat B(3, 3, arma::fill::zeros);

    B(0, 0) = std::cos(x(2));
    B(1, 0) = std::sin(x(2));
    B(2, 2) = 1.0;

    return B;
  }

  unsigned int state_space;  // states space dimension
};
}  // namespace models
}  // namespace ergodic_exploration
#endif


#include <NTL/ZZ.h>
#include <NTL/BasicThreadPool.h>
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include <NTL/lzz_pXFactoring.h>

#include <cassert>
#include <cstdio>
#include <iostream>
using namespace std;

Ctxt FHE_Add(Ctxt Ea, Ctxt Eb)
{
	Ctxt ctSum = Ea;
	ctSum += Eb;
	return ctSum;
}

Ctxt FHE_Mul(Ctxt Ea, Ctxt Eb)
{
	Ctxt ctMul = Ea;
	ctMul *= Eb;
	return ctMul;
}

Ctxt FHE_Sub(Ctxt Ea, Ctxt Eb, const FHEPubKey& publicKey)
{
	Ctxt minus1(publicKey);
	publicKey.Encrypt(minus1, to_ZZX(-1));
	Ctxt ctSub = Eb;
	ctSub *= minus1;
	ctSub += Ea;
	return ctSub;
}

Ctxt FHE_Div(Ctxt Ea, Ctxt Eb, long p,
			 const FHEPubKey& publicKey, const FHESecKey& secretKey)
{
	int quotient = 0;
	bool flag = true;
	while (flag)
	{
		Ctxt ctSub = FHE_Sub(Ea, Eb, publicKey);
		ZZX ptSub;
		secretKey.Decrypt(ptSub, ctSub);
		long sub;
		conv(sub, ptSub[0]);
		if (sub <= p/2)
		{
			Ea = ctSub;
			quotient ++;
		}
		if (sub >= p/2)
			flag = false;
	}
	Ctxt ctDiv(publicKey);
	publicKey.Encrypt(ctDiv, to_ZZX(quotient));
	return ctDiv;
}

int main()
{
	long m = 0;    // 确定系数
	long p = 1021; // 2^64
	long r = 1;
	long L = 16;
	long c = 3;
	long w = 64;
	long d = 0;
	long k = 128;
	long s = 0;

	m = FindM(k, L, c, p, d, s, 0);

	FHEcontext context(m, p, r);
	buildModChain(context, L, c);

	ZZX G = context.alMod.getFactorsOverZZ()[0];

	FHESecKey secretKey(context);
	const FHEPubKey& publicKey = secretKey;
	secretKey.GenSecKey(w);

	Ctxt Ea(publicKey);
	Ctxt Eb(publicKey);

	Vec<ZZ> h;
	h.SetLength(4);
	h[0]=2;
	h[1]=2;
	h[2]=0;
	h[3]=0;

	publicKey.Encrypt(Ea, to_ZZX(h));
	//publicKey.Encrypt(Eb, to_ZZX(2));

	ZZX ptEa;
	secretKey.Decrypt(ptEa, Ea);
	cout << "ptEa : " << ptEa <<endl;

/*
	ZZX ptSum;
	Ctxt ctSum = FHE_Add(Ea, Eb);
	secretKey.Decrypt(ptSum, ctSum);
	cout << "ptSum : " << ptSum <<endl;

	ZZX ptMul;
	Ctxt ctMul = FHE_Mul(Ea, Eb);
	secretKey.Decrypt(ptMul, ctMul);
	cout << "ptMul : " << ptMul <<endl;

	ZZX ptSub;
	secretKey.Decrypt(ptSub, FHE_Sub(Ea, Eb, publicKey));
	cout << "ptSub : " << ptSub <<endl;

	ZZX ptDiv;
	secretKey.Decrypt(ptDiv, FHE_Div(Ea, Eb, p, publicKey, secretKey));
	cout << "ptDiv : " << ptDiv <<endl;
*/
	return 0;
}


//
// Created by bobin on 17-11-9.
//

#include "PoseEstimate.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>
#include "opencv2/calib3d/calib3d.hpp"

using namespace cv;
using namespace std;

PoseEstimate::PoseEstimate(float _max_trans, float _max_rot) {
    last_r = cv::Mat::zeros(1, 3, CV_64F);
    last_t = cv::Mat::zeros(1, 3, CV_64F);
    max_trans = _max_trans;
    max_rot = _max_rot;
}

float PoseEstimate::pose_distance(const cv::Mat &r, const cv::Mat &t, float &delta_rot, float &delta_trans) {
    delta_trans = cv::norm(t - last_t);
    delta_rot = cv::norm(r - last_r);

}

void PoseEstimate::object_points(int numGridX, int numGridY,
                   float GridWidth, float GridHeight,
                   float GridX, float GridY,
                   int num_ids) {
    int cnt = 0;;
    GridPointXY.resize(num_ids);
    for (int i = 0; i < numGridY; ++i) {
        vector<Point3f> rowGridXY;
        rowGridXY.resize(4);
        for (int j = 0; j < numGridX; ++j) {
            rowGridXY[0] = Point3f(j * GridX, i * GridY, 0);
            rowGridXY[1] = Point3f(j * GridX + GridWidth, i * GridY, 0);
            rowGridXY[2] = Point3f(j * GridX, i * GridY + GridHeight, 0);
            rowGridXY[3] = Point3f(j * GridX + GridWidth, i * GridY + GridHeight, 0);
            GridPointXY[cnt] = rowGridXY;
            cnt++;
            if(cnt >= num_ids)
                break;
        }

    }
}

void PoseEstimate::set_pose(const cv::Mat &r, const cv::Mat &t) {
    unique_lock<mutex> lock(rotMutex);
    last_r = r;
    last_t = t;
}

void PoseEstimate::get_pose(cv::Mat &pose) {
    unique_lock<mutex> lock(rotMutex);
    cv::Mat R, t;
    cv::Rodrigues(last_r, R);
    t = last_t;

    R.copyTo(pose.rowRange(0, 3).colRange(0, 3));
    t.copyTo(pose.rowRange(0, 3).col(3));
}

void PoseEstimate::estimate(vector<Point3f> pts_3d,
                            vector<Point2f> pts_2d,
                            bool check_last
                            ) {
    cv::Mat r, t, R;
    solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法

//    unique_lock<mutex> lock(rotMutex);
    float delta_r, delta_t;
    if (check_last){
        pose_distance(r, t, delta_r, delta_t);
        if ((delta_r > max_rot) || (delta_t > max_trans)) {
            r = last_r;
            t = last_t;
        }
    }
    cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
//    cout << "R=" << endl << r << endl;
//    cout << "t=" << endl << t << endl;
    PoseEstimate::bundleAdjustment(pts_3d, pts_2d, K, R, t);
    cv::Rodrigues(R, r); // r为旋转向量形式，用Rodrigues公式转换为矩阵
    set_pose(r, t);

}

void PoseEstimate::bundleAdjustment(
        const vector<Point3f> points_3d,
        const vector<Point2f> points_2d,
        const Mat &K,
        Mat &R, Mat &t) {
    // 初始化g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3> > Block;  // pose维度为 6, landmark 维度为 3
    Block::LinearSolverType *linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    Block *solver_ptr = new Block(linearSolver);     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

    // vertex
    g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
            R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
            R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2);
    pose->setId(0);
    pose->setEstimate(g2o::SE3Quat(
            R_mat,
            Eigen::Vector3d(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0))
    ));
    optimizer.addVertex(pose);

    int index = 1;
    for (const Point3f p:points_3d)   // landmarks
    {
        g2o::VertexSBAPointXYZ *point = new g2o::VertexSBAPointXYZ();
        point->setId(index++);
        point->setEstimate(Eigen::Vector3d(p.x, p.y, p.z));
        point->setMarginalized(true);
        optimizer.addVertex(point);
    }

    // parameter: camera intrinsics
    g2o::CameraParameters *camera = new g2o::CameraParameters(
            K.at<double>(0, 0), Eigen::Vector2d(K.at<double>(0, 2), K.at<double>(1, 2)), 0
    );
    camera->setId(0);
    optimizer.addParameter(camera);

    // edges
    index = 1;
    for (const Point2f p:points_2d) {
        g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId(index);
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *> ( optimizer.vertex(index)));
        edge->setVertex(1, pose);
        edge->setMeasurement(Eigen::Vector2d(p.x, p.y));
        edge->setParameterId(0, 0);
        edge->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(edge);
        index++;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
//    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
//    cout << "optimization costs time: " << time_used.count() << " seconds." << endl;

//    cout << endl << "after optimization:" << endl;
//    cout << "T=" << endl << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
}


#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <ctime>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <imgproc/derivative_gradient.hpp>
#define DISABLE_DC_ZERO_FIX
#include <imgproc/quadratureG2.hpp>
#include <imgproc/quadratureS.hpp>
#include <imgproc/laplace.hpp>
#include <dlib/optimization.h>

using namespace std;
using namespace lsfm;
using namespace cv;

void showGradient(const std::string &name,const cv::Mat &mag, double mul = 1) {
    double vmin,vmax;
    cv::minMaxIdx(mag,&vmin,&vmax);
    mag -= vmin;
    mag /= vmax - vmin;
    mag *= mul;
    imshow("gradient " + name,mag);
}

double d_lower = 0.1, d_upper = 10, start = 1;
cv::Mat white(18, 18, CV_64F, 1);

template <
    class GRAD,
    class search_strategy_type = dlib::bfgs_search_strategy,
    class stop_strategy_type = dlib::objective_delta_stop_strategy
>
double optimizeGradKernel(GRAD& grad,
    double derivative_prec = 1e-7, search_strategy_type search = dlib::bfgs_search_strategy(),
    stop_strategy_type stop = dlib::objective_delta_stop_strategy(1e-7)) {

    typedef dlib::matrix<double, 0, 1> column_vector;

    auto eval = [&](const column_vector& v) -> double {
        grad.kernelSpacing(v(0));
        return std::abs(cv::sum(grad.kernel())[0]);
    };

    grad.kernelSpacing(start);
    column_vector starting_point(1), lower(1), upper(1);
    starting_point = start;
    lower = d_lower;
    upper = d_upper;
    return dlib::find_min_box_constrained(search, stop,
        eval, dlib::derivative(eval, derivative_prec), starting_point, lower, upper);

}

template <
    class GRAD,
    class search_strategy_type = dlib::bfgs_search_strategy,
    class stop_strategy_type = dlib::objective_delta_stop_strategy
>
double optimizeGradKernel2( GRAD& grad,
    double derivative_prec = 1e-7, search_strategy_type search = dlib::bfgs_search_strategy(),
    stop_strategy_type stop = dlib::objective_delta_stop_strategy(1e-7)) {

    typedef dlib::matrix<double, 0, 1> column_vector;

    auto eval = [&](const column_vector& v) -> double {
        grad.kernelSpacing(v(0));
        grad.process(white);
        cv::Mat mag = grad.even();
        return std::abs(mag.at<double>(0, 0));
    };

    grad.kernelSpacing(start);
    column_vector starting_point(1), lower(1), upper(1);
    starting_point = start;
    lower = d_lower;
    upper = d_upper;
    return dlib::find_min_box_constrained(search, stop,
        eval, dlib::derivative(eval, derivative_prec), starting_point, lower, upper);

}

int main(int argc, char** argv)
{
    const char* filename = argc >= 2 ? argv[1] : "../../images/circle2.png";
    //const char* filename = argc >= 2 ? argv[1] : "../../images/bike.png";
    //const char* filename = argc >= 2 ? argv[1] : "../../images/office1_low.JPG";

    cv::Mat src = cv::imread(filename, IMREAD_GRAYSCALE);
    if (src.empty())
    {
        cout << "Can not open " << filename << endl;
        return -1;
    }

    GaussianBlur(src, src, cv::Size(3, 3),0.6);
    typedef double FT;

    QuadratureG2<uchar, FT> quad3(3, 1.240080);
	QuadratureG2<uchar, FT> quad5(5, 1.008000);
    QuadratureG2<uchar, FT> quad7(7, 0.873226);
    QuadratureG2<uchar, FT> quad9(9, 0.781854);

    quad3.process(src);
    showGradient("Quad3 -", Mat(abs(quad3.even())));

    quad5.process(src);
    showGradient("Quad5 -", Mat(abs(quad5.even())));

    quad7.process(src);
    showGradient("Quad7 -", Mat(abs(quad7.even())));

    quad9.process(src);
    showGradient("Quad9 -", Mat(abs(quad9.even())));

    cv::waitKey();

    QuadratureS<uchar, FT, FT> quadS3(1, 2, 3, 1);
    QuadratureS<uchar, FT, FT> quadS5(1, 2, 5, 1);
    QuadratureS<uchar, FT, FT> quadS7(1, 2, 7, 1);
    QuadratureS<uchar, FT, FT> quadS9(1, 2, 9, 1);

    LoG<uchar, FT> log3(3, 1);
    LoG<uchar, FT> log5(5, 1);
    LoG<uchar, FT> log7(7, 1);
    LoG<uchar, FT> log9(9, 1);
    LoG<uchar, FT> log11(11, 1);
    LoG<uchar, FT> log15(15, 1);
    LoG<uchar, FT> log25(25, 1);
    LoG<uchar, FT> log75(75, 1);
    LoG<uchar, FT> log125(125, 1);

    double e;
    e = optimizeGradKernel(log3);
    std::cout << "LoG3 - error: " << e << ", spacing: " << log3.kernelSpacing() << std::endl;
    //std::cout << log3.kernel() << std::endl;

    e = optimizeGradKernel(log5);
    std::cout << "LoG5 - error: " << e << ", spacing: " << log5.kernelSpacing() << std::endl;
    //std::cout << log5.kernel() << std::endl;

    e = optimizeGradKernel(log7);
    std::cout << "LoG7 - error: " << e << ", spacing: " << log7.kernelSpacing() << std::endl;
    //std::cout << log7.kernel() << std::endl;

    e = optimizeGradKernel(log9);
    std::cout << "LoG9 - error: " << e << ", spacing: " << log9.kernelSpacing() << std::endl;
    //std::cout << log9.kernel() << std::endl;

    e = optimizeGradKernel2(log3);
    std::cout << "LoG3 - error2: " << e << ", spacing: " << log3.kernelSpacing() << std::endl;
    //std::cout << log3.kernel() << std::endl;

    e = optimizeGradKernel2(log5);
    std::cout << "LoG5 - error2: " << e << ", spacing: " << log5.kernelSpacing() << std::endl;
    //std::cout << log5.kernel() << std::endl;

    e = optimizeGradKernel2(log7);
    std::cout << "LoG7 - error2: " << e << ", spacing: " << log7.kernelSpacing() << std::endl;
    //std::cout << log7.kernel() << std::endl;

    e = optimizeGradKernel2(log9);
    std::cout << "LoG9 - error2: " << e << ", spacing: " << log9.kernelSpacing() << std::endl;
    //std::cout << log9.kernel() << std::endl;

    e = optimizeGradKernel2(log11);
    std::cout << "LoG11 - error2: " << e << ", spacing: " << log11.kernelSpacing() << std::endl;
    //std::cout << log11.kernel() << std::endl;

    e = optimizeGradKernel2(log15);
    std::cout << "LoG15 - error2: " << e << ", spacing: " << log15.kernelSpacing() << std::endl;
    //std::cout << log15.kernel() << std::endl;

    e = optimizeGradKernel2(log25);
    std::cout << "LoG25 - error2: " << e << ", spacing: " << log25.kernelSpacing() << std::endl;
    //std::cout << log25.kernel() << std::endl;

    e = optimizeGradKernel2(log75);
    std::cout << "LoG75 - error2: " << e << ", spacing: " << log75.kernelSpacing() << std::endl;
    //std::cout << log75.kernel() << std::endl;

    e = optimizeGradKernel2(log125);
    std::cout << "LoG125 - error2: " << e << ", spacing: " << log125.kernelSpacing() << std::endl;
    //std::cout << log25.kernel() << std::endl;

    /*log3.process(src);
    showGradient("LoG3 -", log3.laplace());

    log5.process(src);
    showGradient("LoG5 -", log5.laplace());

    log7.process(src);
    showGradient("LoG7 -", log7.laplace());

    log9.process(src);
    showGradient("LoG9 -", log9.laplace());

    cv::waitKey();*/

    e = optimizeGradKernel2(quad3);
    std::cout << "Quad3 - error: " << e << ", spacing: " << quad3.kernelSpacing() << std::endl;

    e = optimizeGradKernel2(quad5);
    std::cout << "Quad5 - error: " << e << ", spacing: " << quad5.kernelSpacing() << std::endl;

    e = optimizeGradKernel2(quad7);
    std::cout << "Quad7 - error: " << e << ", spacing: " << quad7.kernelSpacing() << std::endl;

    e = optimizeGradKernel2(quad9);
    std::cout << "Quad9 - error: " << e << ", spacing: " << quad9.kernelSpacing() << std::endl;

    quad3.process(src);
    showGradient("Quad3 -", Mat(abs(quad3.even())));

    quad5.process(src);
    showGradient("Quad5 -", Mat(abs(quad5.even())));

    quad7.process(src);
    showGradient("Quad7 -", Mat(abs(quad7.even())));

    quad9.process(src);
    showGradient("Quad9 -", Mat(abs(quad9.even())));

    cv::waitKey();

    e = optimizeGradKernel(quadS3);
    std::cout << "QuadS3 - error: " << e << ", spacing: " << quadS3.kernelSpacing() << ", scale: " << quadS3.scale() << ", muls: " << quadS3.muls() << std::endl;
    //std::cout << quadS3.kernel() << std::endl;

    e = optimizeGradKernel(quadS5);
    std::cout << "QuadS5 - error: " << e << ", spacing: " << quadS5.kernelSpacing() << ", scale: " << quadS5.scale() << ", muls: " << quadS5.muls() << std::endl;
    //std::cout << quadS5.kernel() << std::endl;

    e = optimizeGradKernel(quadS7);
    std::cout << "QuadS7 - error: " << e << ", spacing: " << quadS7.kernelSpacing() << ", scale: " << quadS7.scale() << ", muls: " << quadS7.muls() << std::endl;
    //std::cout << quadS7.kernel() << std::endl;

    e = optimizeGradKernel(quadS9);
    std::cout << "QuadS9 - error: " << e << ", spacing: " << quadS9.kernelSpacing() << ", scale: " << quadS9.scale() << ", muls: " << quadS9.muls() << std::endl;
    //std::cout << quadS9.kernel() << std::endl;

    e = optimizeGradKernel2(quadS3);
    std::cout << "QuadS3 - error2: " << e << ", spacing: " << quadS3.kernelSpacing() << ", scale: " << quadS3.scale() << ", muls: " << quadS3.muls() << std::endl;
    //std::cout << quadS3.kernel() << std::endl;

    e = optimizeGradKernel2(quadS5);
    std::cout << "QuadS5 - error2: " << e << ", spacing: " << quadS5.kernelSpacing() << ", scale: " << quadS5.scale() << ", muls: " << quadS5.muls() << std::endl;
    //std::cout << quadS5.kernel() << std::endl;

    e = optimizeGradKernel2(quadS7);
    std::cout << "QuadS7 - error2: " << e << ", spacing: " << quadS7.kernelSpacing() << ", scale: " << quadS7.scale() << ", muls: " << quadS7.muls() << std::endl;
    //std::cout << quadS7.kernel() << std::endl;

    e = optimizeGradKernel2(quadS9);
    std::cout << "QuadS9 - error2: " << e << ", spacing: " << quadS9.kernelSpacing() << ", scale: " << quadS9.scale() << ", muls: " << quadS9.muls() << std::endl;
    //std::cout << quadS9.kernel() << std::endl;

    /*quadS3.process(src);
    showGradient("QuadS3 -", quadS3.even());

    quadS5.process(src);
    showGradient("QuadS5 -", quadS5.even());

    quadS7.process(src);
    showGradient("QuadS7 -", quadS7.even());

    quadS9.process(src);
    showGradient("QuadS9 -", quadS9.even());

    cv::waitKey();*/

    /*QuadratureS<uchar, FT, FT> quadS5(1.0, 2.0, 5, 1);
    QuadratureS<uchar, FT, FT> quadS7(1.0, 2.0, 7, 1);
    QuadratureS<uchar, FT, FT> quadS9(1.0, 2.0, 9, 0.1);

    quadS5.process(src);
    quadS7.process(src);
    quadS9.process(src);

    //showGradient("QuadS5 -", quadS5.laplace(),5);
    //showGradient("QuadS7 -", quadS7.laplace(),5);
    showGradient("QuadS9 -", quadS9.laplace(),5);

    //showGradient("QuadS5 m-", quadS5.magnitude());
    //showGradient("QuadS7 m-", quadS7.magnitude());
    showGradient("QuadS9 m-", quadS9.magnitude());

    //showGradient("QuadS5 lm-", quadS5.localMagnitude());
    //showGradient("QuadS7 lm-", quadS7.localMagnitude());
    showGradient("QuadS9 lm-", quadS9.localMagnitude());

    //showGradient("QuadS5 e-", quadS5.even());
    //showGradient("QuadS7 e-", quadS7.even());
    showGradient("QuadS9 e-", quadS9.even());
    cv::waitKey();*/

    return 0;
}


#pragma once
#include "math.h"
#include "math.h"
#include <tuple>
#include "integration.hpp"
//#include "datatypes.hpp"
#include <boost/python.hpp>
#include <boost/math/special_functions.hpp>
#include "gsl/gsl_sf_bessel.h"

namespace gd {
using namespace std;

void py_export_profile();

class Density {
public:
	virtual double densityr(double R) = 0;
};

class Profile : public Density {
public:
	virtual double densityr(double r) = 0;
	virtual double densityR(double R) = 0;
	virtual double I(double r, double I0=1.0) = 0;
	virtual double dphidr(double r) = 0;
	virtual double potentialr(double r) = 0;
	double dphidx2(double x, double y) {
		double r = sqrt(x*x+y*y);
		return dphidr(r) * x / r;
	}
	double dphidy2(double x, double y) {
		double r = sqrt(x*x+y*y);
		return dphidr(r) * y / r;
	}
	std::tuple<double,double> dphidxy(double x, double y) {
		double r = sqrt(x*x+y*y);
		double F = dphidr(r);
		return std::make_tuple(F*x/r, F*y/r);
	};

	double enclosed_mass(double r) {
		auto dmass = [this](double rp){ return this->densityr(rp) * rp*rp * 4 * M_PI; };
		IntegratorGSL<> integratorGSL(dmass); // the integrator
		double integral = integratorGSL.integrate(0,r);
		return integral;
	}
	double total_mass() {
		auto dmass = [this](double rp){ return this->densityr(rp) * rp*rp * 4 * M_PI; };
		IntegratorGSL<> integratorGSL(dmass); // the integrator
		double integral = integratorGSL.integrate_to_inf(0);
		return integral;
	}
};

class ProfileModel {
public:
	virtual double densityr(double r) = 0;
	virtual double densityR(double r) = 0;
	virtual double dphidr(double r) = 0;
	virtual double potentialr(double r) = 0;
	boost::python::tuple get_apo_peri(double E, double L, double rmin, double rcirc, double rmax);
	boost::python::tuple Lmax_and_rcirc_at_E_(double E);
	void Lmax_and_rcirc_at_E(double E, double& Lmax, double& rcirc);
	double Lmax_at_E(double E);
	double rcirc_at_E(double E);
	double rmax_at_E(double E, double rcirc);
};

class ProfileModel1C : public ProfileModel {
public:
	ProfileModel1C(Profile* p) : p(p) {}
	virtual double densityr(double r) { return p->densityr(r); }
	virtual double densityR(double r)  { return p->densityR(r); }
	virtual double dphidr(double r) { return p->dphidr(r); }
	virtual double potentialr(double r) { return p->potentialr(r); }
	Profile* p;
};

class ProfileModel2C : public ProfileModel {
public:
	ProfileModel2C(Profile* p1, Profile* p2) : p1(p1), p2(p2) {}
	virtual double densityr(double r) { return p1->densityr(r) + p2->densityr(r); }
	virtual double densityR(double r)  { return p1->densityR(r); }
	virtual double dphidr(double r) { return p1->dphidr(r) + p2->dphidr(r); }
	virtual double potentialr(double r) { return p1->potentialr(r) + p2->potentialr(r); }
	Profile *p1, *p2;
};

class Plummer : public Profile {
public:
	Plummer(double mass, double scale, double G) : mass(mass), scale(scale), G(G) {
	}
	double densityr(double r) {
		return 3 * mass * scale * scale / (4*M_PI) / pow((r*r + scale*scale), (5./2));
	}
	double densityR(double R) {
		double a = (scale*scale+R*R);
		return mass * scale*scale / (M_PI * a*a);
	}
	double I(double R, double I0=1.0) {
		double a = (scale*scale+R*R);
		return I0 * scale*scale / (M_PI * a*a);
	}
	double dphidr(double r) {
		return G * mass * r / pow((r*r + scale*scale), (3./2));
	}
	double potentialr(double r) {
		return - G * mass / sqrt(r*r + scale*scale);
	}
	/*double_vector dphidr2(double_vector r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		return r * 2;
	}*/
	double mass, scale, G;
};
/*
class ProjectedExponential(Potential):
	def __init__(self, M, scale, G=G):
		self.M = M
		self.scale = scale
		self.rho0 = 1.
		print self.scale
		Mcurrent = self.enclosed_mass(inf)
		self.rho0 = M/Mcurrent
		self._fast = mab.gdfast.ProjectedExponential(M, scale, G)

		#2 \[Pi] Rs (Rs - E^(-(r/Rs)) (r + Rs))

	def densityR(self, r):
		return exp(-r/self.scale) * self.rho0

	def densityr(self, r):
		# kn is the (modified) bessel of second kind (integer order = 0)
		return self.rho0 * scipy.special.kn(0., r) / (self.scale * pi)

	def potentialr(self, r):
		return 0

	def dphidr(self, r):
		return 0
*/
class ProjectedExponential : public Profile {
public:
	ProjectedExponential(double mass, double scale, double G) : mass(mass), scale(scale), G(G) {
		rho0 = 1;
		double current_mass = this->total_mass();
		rho0 *= mass/current_mass;
	}
	double densityr(double r) {
		return rho0 * boost::math::cyl_bessel_k(0, r/scale)  / (scale * M_PI);
	}
	double densityR(double R) {
		return rho0 * exp(-R/scale);
	}
	double I(double R, double I0=1.0) {
		return 0;
	}
	double dphidr(double r) {
		return 0;
	}
	double potentialr(double r) {
		return 0;
	}
	/*double_vector dphidr2(double_vector r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		return r * 2;
	}*/
	double mass, scale, G, rho0;
};

class TestCase : public Profile {
public:
	TestCase(double mass, double scale, double G) : mass(mass), scale(scale), G(G) {
	}
	double densityr(double r) {
		//return 3 * mass * scale * scale / (4*M_PI) / pow((r*r + scale*scale), (5./2));
		return 3 * pow(1+pow(r/20,2), -5./2);
	}
	double densityR(double R) {
		double a = (scale*scale+R*R);
		return mass * scale*scale / (M_PI * a*a);
	}
	double I(double R, double I0=1.0) {
		double a = (scale*scale+R*R);
		return I0 * scale*scale / (M_PI * a*a);
	}
	double dphidr(double r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		return 1/(4*M_PI*2)*(8000*pow(400+r*r, -3./2)*2*r);
	}
	double potentialr(double r) {
		return -1/(4*M_PI)*(8000/sqrt(400+r*r)-178.88);
	}
	/*double_vector dphidr2(double_vector r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		return r * 2;
	}*/
	double mass, scale, G;
};

class Isochrone : public Profile {
public:
	Isochrone(double mass, double scale, double G) : mass(mass), scale(scale), G(G) {
	}
	double densityr(double) {
		return 0;
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double) {
		return 0;
	}
	double potentialr(double) {
		return 0;
	}
	/*double_vector dphidr2(double_vector r) {
		return r * 2;
	}*/
	double mass, scale, G;
};

class LogarithmicProfile : public Profile {
public:
	LogarithmicProfile(double vcirc, double G) : vcirc(vcirc), G(G) {
	}
	double densityr(double) {
		return 0;
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double) {
		return 0;
	}
	double potentialr(double) {
		return 0;
	}
	/*double_vector dphidr2(double_vector r) {
		return r * 2;
	}*/
	double vcirc, G;
};

class NullProfile : public Profile {
public:
	NullProfile() {
	}
	double densityr(double) {
		return 0;
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double) {
		return 0;
	}
	double potentialr(double) {
		return 0;
	}
	/*double_vector dphidr2(double_vector r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		return r*0;
	}*/
};

class Hernquist : public Profile {
public:
	Hernquist(double mass, double scale, double G) : mass(mass), scale(scale), G(G) {
		rho0 = mass / (2*M_PI*scale*scale*scale);
	}
	double potentialr(double r) {
		return - 4 * M_PI * G * rho0 * scale*scale /(2*(1+r/scale));
	}
	double densityr(double r) {
		double m = r/scale;
		double a = (1+m);
		return rho0 / (m * a*a*a);
	}
	double _ddensityR(double r, double R) {
		return 2 * r * densityr(r) / sqrt(r*r-R*R);
	}
	double densityR(double R) {
		/*return -pow(scale,4) * rho0 * (3 * scale * (scale-R)*(scale+R)+sqrt(R*R-scale*scale)*(2*scale*scale+R*R)*acos(scale/R)) / pow(scale*scale-R*R,3);-*/
		//std::tr1::function<double(double)> f(bind(&Hernquist::_ddensityR, this, _1, R));
		auto ddensity = [&R,this](double r){ return 2 * r * this->densityr(r) / sqrt(r*r-R*R); };
		IntegratorGSL<> integratorGSL(ddensity); // the integrator
		double integral = integratorGSL.integrate_to_inf(R);
		return integral;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double r) {
		//return G * mass * r / pow((r*r + scale*scale), (3./2));
		double a = (1+r/scale);
		return 4 * M_PI * G * rho0 * scale / (2*a*a);
	}
	double mass, rho0, scale, G;
};

class NFW : public Profile {
public:
	NFW(double mass200, double rs, double G, double rho_crit) : mass200(mass200), rs(rs), G(G) {
		r200 = pow(mass200/200*3/(4*M_PI)/rho_crit, 1./3);
		c = r200/rs;
		double x = r200 / rs;
		rho0 = mass200 / (4*M_PI*pow(rs,3)*(log(1+x)-x/(1+x)));
	}
	double potentialr(double r) {
		double x = r/rs;
		return - 4 * M_PI * G * rho0 * rs*rs * log(1+x)/x;
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (x * pow(1+x, 2));
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double r) {
		double x = r/rs;
		double f = -4.0*M_PI*G*rho0*pow(rs, 3)/pow(r,2);
		double t1 = log(1.0 + x);
		double t2 = x/(1.0+x);
		//cout << x << ", " << f << ", " << t1 << ", " << t2 << endl;
		return -f*(t1 - t2);
	}
	double mass200, rho0, rs, r200, c, G;
};

class Jaffe : public Profile {
public:
	Jaffe(double rho0, double rs, double G) : rho0(rho0), rs(rs), G(G) {
	}
	double potentialr(double r) {
		return - 4 * M_PI * G * rho0 * rs*rs * log(1+rs/r);
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (pow(x, 2) * pow(1+x, 2));
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double r) {
		/*double x = r/rs;
		double f = -4.0*M_PI*G*rho0*pow(rs, 3)/pow(r,2);
		double t1 = log(1.0 + x);
		double t2 = x/(1.0+x);
		//cout << x << ", " << f << ", " << t1 << ", " << t2 << endl;
		return -f*(t1 - t2);*/
		return - 4 * M_PI * G * rho0 * rs*rs * 1./(1.+rs/r) * -rs/(r*r);
	}
	double rho0, rs, G;
};

class NFWCut : public Density {
public:
	NFWCut(double rho0, double rs, double rte) : rho0(rho0), rs(rs), rte(rte) {
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (x * pow(1+x,2)) / (1+pow(r/rte, 3));
	}
	double rho0, rs, rte;
};

class TwoSlopeDensity : public Density {
public:
	TwoSlopeDensity(double rho0, double alpha, double beta, double rs, double gamma=1.) : rho0(rho0), alpha(alpha), beta(beta), rs(rs), gamma(gamma) {
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (pow(x, -alpha) * pow(1+pow(x, gamma), (-beta+alpha)/gamma));
	}
	double rho0, alpha, beta, rs, gamma;
};

class TwoSlopeDensityCut : public Density {
public:
	TwoSlopeDensityCut(double rho0, double alpha, double beta, double rs, double gamma, double rte) : rho0(rho0), alpha(alpha), beta(beta), rs(rs), gamma(gamma), rte(rte) {
	}
	double densityr(double r) {
		double x = r/rs;
		return rho0 / (pow(x, -alpha) * pow(1+pow(x, gamma), (-beta+alpha)/gamma)) / (1+pow(r/rte, 3));
	}
	double rho0, alpha, beta, rs, gamma, rte;
};

class BrokenPowerLawDensitySoft3 : public Density {
public:
	BrokenPowerLawDensitySoft3(double rho0, double s1, double s2, double s3, double gamma1, double gamma2, double rs1, double rs2) : rho0(rho0), s1(s1), s2(s2), s3(s3), gamma1(gamma1), gamma2(gamma2), rs1(rs1), rs2(rs2) {
	}
	double densityr(double r) {
		double x1 = r/rs1;
		double x2 = r/rs2;
		return rho0 * pow(x1, s1) * pow(1+pow(x1, gamma1), (s2-s1)/gamma1) * pow(1+pow(x2, gamma2), (s3-s2)/gamma2);
	}
	double rho0, s1, s2, s3, gamma1, gamma2, rs1, rs2;
};

class Einasto : public Profile {
public:
	Einasto(double rho_2, double rs_2, double alpha, double G) : rho_2(rho_2), rs_2(rs_2), alpha(alpha), G(G) {
	}
	double potentialr(double) {
		return 0;
	}
	double densityr(double r) {
		double x = r/rs_2;
		return rho_2 * exp((-2/alpha)*(pow(x, alpha)-1));
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double) {
		return 0;
	}
	double rho_2, rs_2, alpha, G;
	/*double M(double r) {
		double x = r/rs_2;
		double a = alpha;
		double Rg;
		gsl_sf_result R1, R2;
		gsl_sf_gamma_inc_P_e(3./a, (2*pow(x,a))/a, &Rg)
		//gsl_sf_gamma_inc_P_e(3./a, (2*pow(x,a))/a, &R1)
		double R = Rg.val * gsl_sf_gamma(3./a);
		return 0;
		//return pow(2, (2.-3./a)) * exp(2./a) * M_PI * pow(pow(1/self.r_2,a)/a, -3./a) * rho_2 * R / a;
	}*/
};

class Burkert : public Profile {
public:
	Burkert(double rho, double rs, double G) : rho(rho), rs(rs), G(G) {
	}
	double potentialr(double) {
		return 0;
	}
	double densityr(double r) {
		double x = r/rs;
		return rho / ((1+x) * (1+x*x));
	}
	double densityR(double) {
		return 0;
	}
	double I(double, double) {
		return 0;
	}
	double dphidr(double) {
		return 0;
	}
	double rho, rs, G;
};

}


#ifndef EXPSUM_HANKEL_MATRIX_VECTOR_PRODUCT_HPP
#define EXPSUM_HANKEL_MATRIX_VECTOR_PRODUCT_HPP

#include <armadillo>

#include "fftw3/shared_plan.hpp"

namespace expsum
{
/*!
 * Fast matrix-vector product for generalized Hankel matrix.
 *
 * A general Hankel matrix is a matrix of special form give as
 *
 * \f[
 *   A = \left[ \begin{array}{cccccc}
 *     h_0 & h_1 & h_2 & h_3 & \cdots & h_{n-1} \\
 *     h_1 & h_2 & h_3 & h_4 & \cdots & h_{n}   \\
 *     h_2 & h_3 & h_4 & h_5 & \cdots & h_{n+1} \\
 *     h_3 & h_4 & h_5 & h_6 & \cdots & h_{n+2} \\
 *     \vdots & \vdots & \vdots &\vdots &\ddots & \vdots \\
 *     h_{m-1} & h_{m} & h_{m+1} & h_{m+2} & \cdots & h_{m+n-1} \\
 *   \end{array} \right],
 * \f]
 *
 * where \f$ m \f$ and \f$ n \f$ are the number of rows and colums of matrix.
 * From the definition above, a general Hankel matrix \f$ A \f$ can fully be
 * determined by a vector \f$ h = [h_0,h_1,...,h_{n+m-1}]^{T}\f$ composed of the
 * elements of first column and last row where \f$ A_{ij} = h_{i+j}.\f$
 *
 * This class compute the matrix-vector product,
 *
 * \f[
 *   \bm{y} = A \bm{x},
 * \f]
 *
 * where \f$ \bm{x} = [x_0,x_1,\dots,x_{n-1}]^{T} \f$ and \f$ \bm{y} =
 * [y_0,y_1,\dots,y_{m-1}]^{T}. \f$ This product can be efficiently computed by
 * the fast Fourier transfor (FFT), as follows.
 *
 * Let define a new vector \f$ \hat{\bm{c}} \f$ of size \f$ n + m - 1 \f$ as
 *
 * \f[
 *   \hat{\bm{c}}=[h_{n-1},\dots,h_{n+m-2},h_{0},\dots,h_{n-2}]^{T},
 * \f]
 *
 * and corresponding circulant matrix
 *
 * \f[
 *   C = \left[ \begin{array}{}
 *         c_0     & c_{n+m-2} & \cdots    & c_{2}  & c_{1} \\
 *         c_1     & c_{0}     & c_{n+m-2} &        & c_{2} \\
 *         \vdots  & c_{1}     & c_{0}     & \ddots & \vdots    \\
 *         c_{n+m-3} &         & \ddots    & \ddots & c_{n+m-2} \\
 *         c_{n+m-2} & c_{n+m-3} & \cdots  & c_{1}  & c_{0}
 *       \end{array} \right].
 * \f]
 *
 * For a given vector \f$ \bm{x} \f$ of length \f$ n, \f$ we define a auxilialy
 * vector of length \f$ n + m - 1 \f$ as
 *
 * \f[
 *   \hat{\bm{x}}=[x_{n-1},x_{n-2},\dots,x_{0},0,\dots,0]^{T}.
 * \f]
 *
 * Then, the result vector \f$ \bm{y} \f$ can be obtained as the first
 * \f$ m \f$-elemets of the vector \f$ \hat{\bm{y}}\equiv C\hat{\bm{x}}. \f$
 * The produt \f$ C\hat{\bm{x}} \f$ can be evaluated as,
 *
 * \f[
 *   \hat{\bm{y}} = \text{IFFT}(\text{FFT}(\hat{\bm{c}}) \odot
 *                              \text{FFT}(\hat{\bm{x}})).
 * \f]
 *
 * Here, \$f \text{FFT}(\bm{v}) \f$ and \$f \text{IFFT}(\bm{v}) \f$ denote
 * one-dimensional FFT and inverse FFT of vector \bm{v}, and \f$ \odot \f$
 * denotes a element-wise multiplication of two vectors.
 *
 * The computational complexity of this algorithm is \f$
 * \mathcal{O}((m+n-1)\log(m+n-1)) \f$, rather than \f$ \mathcal{O}(mn) \f$
 * for ordinary dense matrix-vector operation in BLAS2.
 *
 * For the computational efficiency, the FFT of the vector \f$ \hat{\bm{c}} \f$
 * is pre-computed and stored internally.
 *
 */

template <typename T>
class hankel_gemv
{
public:
    using size_type   = arma::uword;
    using value_type  = T;
    using vector_type = arma::Col<value_type>;

    using real_type           = typename vector_type::pod_type;
    using complex_type        = std::complex<real_type>;
    using complex_vector_type = arma::Col<complex_type>;

private:
    using fft  = fftw3::fft<real_type>;
    using ifft = fftw3::ifft<real_type>;

    typename fft::plan_pointer fft_plan_;
    typename ifft::plan_pointer ifft_plan_;

    size_type nrows_;
    size_type ncols_;

    mutable vector_type work_;
    complex_vector_type caux_;
    mutable complex_vector_type xaux_;

public:
    /// Default constructor
    hankel_gemv() = default;

    /// Create an Hankel matrix operator with memory preallocation.
    hankel_gemv(size_type nrows, size_type ncols, size_type fft_size = 0)
        : fft_plan_(),
          ifft_plan_(),
          nrows_(nrows),
          ncols_(ncols),
          work_(std::max(fft_size, nrows + ncols - 1)),
          caux_(arma::is_complex<value_type>::value ? work_.size()
                                                    : work_.size() / 2 + 1),
          xaux_(caux_.size())
    {
        set_fft_plans();
    }

    /// Copy constructor (default)
    hankel_gemv(const hankel_gemv&) = default;

    /// Move constructor (default)
    hankel_gemv(hankel_gemv&&) = default;

    /// Destructor (default)
    ~hankel_gemv() = default;

    /// Copy assignment operator
    hankel_gemv& operator=(const hankel_gemv&) = default;

    /// Move assignment operator
    hankel_gemv& operator=(hankel_gemv&&) = default;

    /// @return the number of rows of the Hankel matrix
    size_type nrows() const
    {
        return nrows_;
    }
    /// @return the number of columns of the Hankel matrix
    size_type ncols() const
    {
        return ncols_;
    }
    /// @return number of coefficients that defines this Hankel matrix
    size_type size() const
    {
        return nrows() + ncols() - 1;
    }

    /// Reallocate internal memory space
    void resize(size_type nrows, size_type ncols, size_type fft_size = 0)
    {
        fft_size = std::max(fft_size, nrows + ncols - 1);

        nrows_ = nrows;
        ncols_ = ncols;
        work_.set_size(fft_size);
        caux_.set_size(arma::is_complex<value_type>::value
                           ? work_.size()
                           : work_.size() / 2 + 1);
        xaux_.set_size(caux_.size());

        set_fft_plans();
    }
    ///
    /// Set coefficients that defines the Hankel matrix.
    ///
    template <typename T1>
    typename std::enable_if<arma::is_arma_type<T1>::value>::type
    set_coeffs(const T1& coeffs)
    {
        assert(coeffs.is_vec() && coeffs.n_elem == size());
        //
        // Set first column of circulant matrix C. Then compute the discrete
        // Fourier transform this vector and store the result into \c caux.
        //
        const auto nhead    = nrows();
        const auto ntail    = ncols() - 1;
        const auto npadding = work_.size() - nhead - ntail;

        work_.head(nhead) = coeffs.tail(nhead);
        if (npadding > size_type())
        {
            work_.subvec(nhead, nhead + npadding - 1).zeros();
        }
        work_.tail(ntail) = coeffs.head(ntail);

        // caux_ <-- FFT[work_]
        fft::run(fft_plan_, work_.memptr(), caux_.memptr());
        caux_ *= real_type(1) / work_.size();
    }

    ///
    /// Compute `y = A * x + beta * y`
    ///
    template <typename U1, typename U2>
    typename std::enable_if<(arma::is_arma_type<U1>::value &&
                             arma::is_arma_type<U2>::value),
                            void>::type
    apply(const U1& x, value_type beta, U2& y) const
    {
        assert(x.is_vec() && x.n_elem == ncols());
        assert(y.is_vec() && y.n_elem == nrows());
        //
        // Form new vector x' = [x(n-1),x(n-2),...,x(0),0....0] of length
        // n + m - 1, and compute FFT.
        //
        work_.head(ncols()) = arma::flipud(x);
        work_.tail(work_.size() - ncols()).zeros();
        // xaux_ <-- FFT[work_]
        fft::run(fft_plan_, work_.memptr(), xaux_.memptr());
        //
        // y[0:nrows] = IFFT(FFT(c') * FFT(x'))[0:nrows]
        //
        xaux_ %= caux_;
        ifft::run(ifft_plan_, xaux_.memptr(), work_.memptr());
        if (beta == value_type())
        {
            y = work_.head(nrows());
        }
        else
        {
            y = work_.head(nrows()) + beta * y;
        }
    }

    ///
    /// Compute `y = A.t() * x + beta * y`
    ///
    template <typename U1, typename U2>
    typename std::enable_if<(arma::is_arma_type<U1>::value &&
                             arma::is_arma_type<U2>::value),
                            void>::type
    apply_trans(const U1& x, value_type beta, U2& y) const
    {
        assert(x.is_vec() && x.n_rows == nrows());
        assert(y.is_vec() && y.n_rows == ncols());
        //
        // Form new vector x' = [0,0,...,0,x(m-1),x(m-2),...,x(0)] of length
        // n + m - 1, and compute FFT.
        //
        work_.head(work_.size() - nrows()).zeros();
        work_.tail(nrows()) = arma::conj(arma::flipud(x));
        // xaux_ <-- FFT[work_]
        fft::run(fft_plan_, work_.memptr(), xaux_.memptr());
        //
        // y[0:nrows-1] = IFFT(FFT(c') * FFT(x'))[0:nrows-1]
        //
        xaux_ %= caux_;
        ifft::run(ifft_plan_, xaux_.memptr(), work_.memptr());
        if (beta == value_type())
        {
            y = arma::conj(work_.tail(ncols()));
        }
        else
        {
            y = arma::conj(work_.tail(ncols())) + beta * y;
        }
    }

private:
    void set_fft_plans()
    {
        const int n       = static_cast<int>(work_.size());
        const int howmany = 1;
        fft_plan_ = fft::make_plan(n, howmany, work_.memptr(), xaux_.memptr());
        ifft_plan_ =
            ifft::make_plan(n, howmany, xaux_.memptr(), work_.memptr());
    }
};

/*!
 * Create a Hankel matrix in dense form from the sequence of elements.
 *
 * This function creates a \f$ m \times n \f$ Hankel matrix \f$ A \f$ in the
 * dense form from a given vector of matrix element \f$ h =
 * [h_0,h_1,...,h_{n+m-1}]^{T}\f$ such that,
 *
 * \f[
 *   A = \left[ \begin{array}{}
 *     h_0 & h_1 & h_2 & h_3 & \cdots & h_{n-1} \\
 *     h_1 & h_2 & h_3 & h_4 & \cdots & h_{n}   \\
 *     h_2 & h_3 & h_4 & h_5 & \cdots & h_{n+1} \\
 *     h_3 & h_4 & h_5 & h_6 & \cdots & h_{n+2} \\
 *     \vdots & \vdots & \vdots &\vdots &\ddots & \vdots \\
 *     h_{m-1} & h_{m} & h_{m+1} & h_{m+2} & \cdots & h_{m+n-1} \\
 *   \end{array} \right]
 * \f]
 *
 * \param[in] nrows number of rows, \f$ m \f$
 * \param[in] ncols number of columns, \f$ n \f$
 * \param[in] h vector of elments of Hankel matrix with length \c nrows+ncols-1
 * \return \c arma::Mat with same scalar type of input vector type \c T1.
 */
template <typename T1>
typename std::enable_if<arma::is_arma_type<T1>::value,
                        arma::Mat<typename T1::elem_type>>::type
make_dense_hankel(arma::uword nrows, arma::uword ncols, const T1& h)
{
    assert(h.n_elem == nrows + ncols - 1);
    arma::Mat<typename T1::elem_type> A(nrows, ncols);

    for (arma::uword col = 0; col < ncols; ++col)
    {
        for (arma::uword row = 0; row < nrows; ++row)
        {
            A(row, col) = h(row + col);
        }
    }

    return A;
}

namespace detail
{
template <typename T>
inline T abs2(T x)
{
    return x * x;
}

template <typename T>
inline T abs2(std::complex<T> x)
{
    return std::real(x) * std::real(x) + std::imag(x) * std::imag(x);
}
} // namespace: detail

/*!
 * Compute the Frobenius norm of general Hankel matrix.
 *
 * This function computes the Frobenius norm of \c nrows-by-ncols general Hankel
 * matrix defined by the given vector of elements.
 *
 * \param[in] nrows number of rows
 * \param[in] ncols number of columns
 * \param[in] h a vector that determines the Hankel matrix \f$ A \f$.  If <tt>
 * h.size() >= N </tt> with <tt> N = nrows + ncols - 1, </tt> first \c N elemnts
 * are refered as the elements of Hankel matrix. If <tt> h.size() < N, </tt>
 * rest of elements are assumed to be zero.
 */

template <typename T1>
typename T1::pod_type fnorm_hankel(arma::uword nrows, arma::uword ncols,
                                   const T1& h)
{
    arma::uword m, n;
    std::tie(m, n) = std::minmax(nrows, ncols);
    arma::uword l = m + n - 1;

    auto sqsum = typename T1::pod_type();

    if (h.n_elem > m)
    {
        for (arma::uword i = 0; i < m; ++i)
        {
            sqsum += (i + 1) * std::norm(h(i));
        }

        if (h.n_elem > n)
        {
            for (arma::uword i = m; i < n; ++i)
            {
                sqsum += m * std::norm(h(i));
            }

            for (arma::uword i = n; i < std::min(l, h.n_elem); ++i)
            {
                sqsum += (l - i) * std::norm(h(i));
            }
        }
        else
        {
            for (arma::uword i = m; i < h.n_elem; ++i)
            {
                sqsum += m * std::norm(h(i));
            }
        }
    }
    else
    {
        for (arma::uword i = 0; i < h.n_elem; ++i)
        {
            sqsum += (i + 1) * std::norm(h(i));
        }
    }

    return std::sqrt(sqsum);
}
} // namespace: expsum

#endif /* EXPSUM_HANKEL_MATRIX_VECTOR_PRODUCT_HPP */


/**
 * @file locallaplaceqfe.cc
 * @brief NPDE homework ParametricElementMatrices code
 * @author Simon Meierhans
 * @date 27/03/2019
 * @copyright Developed at ETH Zurich
 */

#include "locallaplaceqfe.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Dense>

namespace DebuggingFEM {

Eigen::Matrix<double, 6, 6> LocalLaplaceQFE1::Eval(
    const lf::mesh::Entity &cell) {
  // Query (topological) type of cell/reference element
  const lf::base::RefEl ref_el{cell.RefEl()};
  // Verify that the cell is a triangle
  LF_ASSERT_MSG(ref_el == lf::base::RefEl::kTria(),
                "Implemented for triangles only not for " << ref_el);
  // The final element matrix has size 6x6
  Eigen::Matrix<double, 6, 6> result{};
  // Obtain the vertex coordinates of the triangle
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  LF_ASSERT_MSG(geo_ptr != nullptr, "Invalid geometry!");
  // Matrix storing corner coordinates in its columns
  Eigen::Matrix<double, 2, 3> vertices{geo_ptr->Global(ref_el.NodeCoords())};
  // Comopute element matrix for negative Laplacian and lowest-order Lgrangian
  // finite elements as in Remark 2.4.5.9. in the course notes
  Eigen::Matrix<double, 3, 3> X;  // temporary matrix
  X.block<3, 1>(0, 0) = Eigen::Vector3d::Ones();
  X.block<3, 2>(0, 1) = vertices.transpose();
  const double area = 0.5 * std::abs(X.determinant());
  // Initialize gradients!
  auto grad_bary_coords{X.inverse().block<2, 3>(1, 0)};

  // Returns all gradients of the local shape functions for quadatic Lagrangian
  // finite elements in the columns of a matrix. The gradients are evaluated at
  // a point specified by its reference coordinates.
  auto gradientsLocalShapeFunctions =
      [&grad_bary_coords](
          const Eigen::Vector2d xh) -> Eigen::Matrix<double, 2, 6> {
    Eigen::Matrix<double, 2, 6> gradients;
    // barycentric coordinate functions
    const std::array<double, 3> l{1.0 - xh[0] - xh[1], xh[0], xh[1]};
    gradients.col(0) = grad_bary_coords.col(0) * (4 * l[0] - 1);
    gradients.col(1) = grad_bary_coords.col(1) * (4 * l[1] - 1);
    gradients.col(2) = grad_bary_coords.col(2) * (4 * l[2] - 1);
    gradients.col(3) =
        4 * (grad_bary_coords.col(0) * l[1] + grad_bary_coords.col(1) * l[0]);
    gradients.col(4) =
        4 * (grad_bary_coords.col(1) * l[2] + grad_bary_coords.col(2) * l[1]);
    gradients.col(5) =
        4 * (grad_bary_coords.col(0) * l[2] + grad_bary_coords.col(2) * l[0]);
    return gradients;
  };

  const auto grad_vt_0{gradientsLocalShapeFunctions(Eigen::Vector2d(0, 0))};
  const auto grad_vt_1{gradientsLocalShapeFunctions(Eigen::Vector2d(1, 0))};
  const auto grad_vt_2{gradientsLocalShapeFunctions(Eigen::Vector2d(0, 1))};
  result =
      area / 3.0 *
      (grad_vt_0.transpose() * grad_vt_0 + grad_vt_1.transpose() * grad_vt_1 +
       grad_vt_2.transpose() * grad_vt_2);
  return result;
}

Eigen::Matrix<double, 6, 6> LocalLaplaceQFE2::Eval(
    const lf::mesh::Entity &cell) {
  // Query (topological) type of cell/reference element
  const lf::base::RefEl ref_el{cell.RefEl()};
  // Verify that the cell is a triangle
  LF_ASSERT_MSG(ref_el == lf::base::RefEl::kTria(),
                "Implemented for triangles only not for " << ref_el);
  // The final element matrix has size 6x6
  Eigen::Matrix<double, 6, 6> result{};
  // Obtain the vertex coordinates of the triangle
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  LF_ASSERT_MSG(geo_ptr != nullptr, "Invalid geometry!");
  // Matrix storing corner coordinates in its columns
  Eigen::Matrix<double, 2, 3> vertices{geo_ptr->Global(ref_el.NodeCoords())};
  // Comopute element matrix for negative Laplacian and lowest-order Lgrangian
  // finite elements as in Remark 2.4.5.9. in the course notes
  Eigen::Matrix<double, 3, 3> X;  // temporary matrix
  X.block<3, 1>(0, 0) = Eigen::Vector3d::Ones();
  X.block<3, 2>(0, 1) = vertices.transpose();
  const double area = 0.5 * std::abs(X.determinant());
  auto grad_bary_coords{X.inverse().block<2, 3>(1, 0)};
  auto L{grad_bary_coords.transpose() * grad_bary_coords};

  // See Example 2.7.5.7 in course notes for derivation of the formulas
  result << 3. * L(0, 0), -L(0, 1), -L(0, 2), 4. * L(0, 1), 0, 4. * L(0, 2),
      -L(0, 1), 3. * L(1, 1), -L(1, 2), 4. * L(0, 1), 4. * L(1, 2), 0, -L(0, 2),
      -L(1, 2), 3. * L(2, 2), 0, 4. * L(2, 1), 4. * L(2, 0), 4. * L(0, 1),
      4. * L(0, 1), 0, 8. * (L(0, 0) + L(0, 1) + L(1, 1)), 8 * L(0, 2),
      8 * L(1, 2), 0, 4. * L(1, 2), 4. * L(2, 1), 8. * L(0, 2),
      8. * (L(1, 1) + L(1, 2) + L(2, 2)), 8 * L(0, 1), 4 * L(0, 2), 0,
      4. * L(2, 0), 8. * L(1, 2), 8. * L(0, 1),
      8. * (L(0, 0) + L(0, 2) + L(2, 2));
  result *= (area / 3.);
  return result;
}

// implementation
Eigen::Matrix<double, 6, 6> LocalLaplaceQFE3::Eval(
    const lf::mesh::Entity &cell) {
  // Obtain the element matrix for piecewise linear Lagrangian FEM by using
  // a built-in class of LehrFEM++
  auto linear_lapl_element_matrix = lf::uscalfe::LinearFELaplaceElementMatrix();
  Eigen::Matrix4d L = linear_lapl_element_matrix.Eval(cell);
  // Variable for returning the final 6x6 element matrix
  Eigen::Matrix<double, 6, 6> result{};

  // The element matrix for quadratic finite elements can be constructed from
  // the element matrix for linear FEM, see Example 2.7.5.7. in the lecture
  // notes.
  result << 3. * L(0, 0), -L(0, 1), -L(0, 2), 4. * L(0, 1), 0, 4. * L(0, 2),
      -L(0, 1), 3. * L(1, 1), -L(1, 2), 4. * L(0, 1), 4. * L(1, 2), 0, -L(0, 2),
      -L(1, 2), 3. * L(2, 2), 0, 4. * L(2, 1), 4. * L(2, 0), 4. * L(0, 1),
      4. * L(0, 1), 0, 8. * (L(0, 0) + L(0, 1) + L(1, 1)), 8 * L(0, 2),
      8 * L(1, 2), 0, 4. * L(1, 2), 4. * L(2, 1), 8. * L(0, 2),
      8. * (L(1, 1) + L(1, 2) + L(2, 2)), 8 * L(0, 1), 4 * L(0, 2), 0,
      4. * L(2, 0), 8. * L(1, 2), 8. * L(0, 1),
      8. * (L(0, 0) + L(0, 2) + L(2, 2));
  // A hideous manipulation introducces an error !
  result(3, 3) *= 1.000001;
  return (result / 3.0);
}

}  // namespace DebuggingFEM


// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// 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.

/**
 * \file geometry_tools.cpp
 * \brief Source file for the ASRL vision package
 * \details
 *
 * \author Autonomous Space Robotics Lab (ASRL)
 */
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <opencv2/opencv.hpp>

#include <vtr_logging/logging.hpp>
#include <vtr_vision/geometry/geometry_tools.hpp>

namespace vtr {
namespace vision {

/////////////////////////////////////////////////////////////////////////////////
// @brief Triangulates a point from a rig and keypoints
/////////////////////////////////////////////////////////////////////////////////
Eigen::Vector3d triangulateFromRig(const RigCalibration &rig_calibration,
                                   const std::vector<cv::Point2f> &keypoints,
                                   const FeatureInfos &kp_infos,
                                   double *covariance) {
  return triangulateFromCameras(rig_calibration.intrinsics,
                                rig_calibration.extrinsics, keypoints, kp_infos,
                                covariance);
}

/////////////////////////////////////////////////////////////////////////////////
/// @brief Triangulates a point (linearly) from a set of cameras and keypoints
/////////////////////////////////////////////////////////////////////////////////
Eigen::Vector3d triangulateFromCameras(
    const CameraIntrinsics &intrinsics, const Transforms &extrinsics,
    const std::vector<cv::Point2f> &keypoints,
    const vision::FeatureInfos &kp_infos, double *covariance) {
  // sanity check
  assert(intrinsics.size() == keypoints.size());
  assert(extrinsics.size() == keypoints.size());

  // make up the solution matrix
  Eigen::MatrixXd Z = Eigen::MatrixXd::Zero(keypoints.size() * 2, 4);
  for (unsigned ii = 0; ii < keypoints.size(); ii++) {
    // grab each observation and add it to the matrix to solve
    Eigen::Matrix<double, 3, 4> P =
        intrinsics[ii] * extrinsics[ii].matrix().block(0, 0, 3, 4);
    Z.row(ii * 2) = keypoints[ii].x * P.row(2) - P.row(0);
    Z.row(ii * 2 + 1) = keypoints[ii].y * P.row(2) - P.row(1);
  }

  // solve the linear triangulation problem using SVD
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
      Z, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::MatrixXd V = svd.matrixV();

  // extract the 3D point
  Eigen::Vector3d X = V.col(3).hnormalized();

  // calculate the covariance if required
  if (covariance) {
    // wrap the covariance and zero-initialize
    Eigen::Map<Eigen::Matrix3d> cov_map(covariance);
    cov_map.setZero();

    // loop over the contribution of each measurement
    for (unsigned ii = 0; ii < keypoints.size(); ii++) {
      // Reproject the solved point for linearization
      Eigen::Matrix<double, 3, 4> P =
          intrinsics[ii] * extrinsics[ii].matrix().block(0, 0, 3, 4);
      Eigen::Vector3d xii = P * X.homogeneous();
      const auto &xii2 = xii(2);
      auto xii2_2 = xii2 * xii2;  // helpers

      // homogeneous to cartesian Jacobian for image points
      Eigen::Matrix<double, 2, 3> h2c_jac;
      h2c_jac << 1 / xii2, 0, -xii(0) / xii2_2, 0, 1 / xii2, -xii(1) / xii2_2;

      // full camera projection Jacobian
      auto jac = h2c_jac * P.leftCols<3>();
      // sum the (linearized) precision contributions of each measurement
      cov_map += jac.transpose() * kp_infos[ii].covariance.inverse() * jac;
    }
    // from precision matrix to covariance matrix
    cov_map = cov_map.inverse().eval();
  }

  // return the linearly triangulated 3D point
  return X;
}
#if 0
/////////////////////////////////////////////////////////////////////////////////
/// @brief Estimates a plane from a PCL point cloud
/////////////////////////////////////////////////////////////////////////////////
bool estimatePlane(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                              const double distance_thresh,
                              pcl::ModelCoefficients &coefficients,
                              pcl::PointIndices &inliers) {

  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (distance_thresh);
  seg.setInputCloud (cloud);
  seg.segment (inliers, coefficients);

  return !inliers.indices.empty();
}
#endif
/////////////////////////////////////////////////////////////////////////////////
/// @brief Estimate the distance from a plane
/////////////////////////////////////////////////////////////////////////////////
double estimatePlaneDepth(const Eigen::Vector3d &point,
                          const Eigen::Vector4f &coefficients) {
  // rename for clarity
  const double &a = point(0);
  const double &b = point(1);
  const double &c = point(2);
  const float &pa = coefficients(0);
  const float &pb = coefficients(1);
  const float &pc = coefficients(2);
  const float &pd = coefficients(3);

  // numerator
  double num = std::fabs(a * pa + b * pb + c * pc + pd);

  // denominator
  double den = std::sqrt(pa * pa + pb * pb + pc * pc);

  // result
  double dist = num / den;

  return dist;
}

}  // namespace vision
}  // namespace vtr


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015 - 2016.
// Distributed under the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)
//

// fixed_point_detail_hypergeometric.hpp implements templates
// for computing hypergeometric series used for Taylor-series-like
// expansions. These are used by certain <cmath> functions to
// simplify typing and reduce the complexity of the code.

#ifndef FIXED_POINT_DETAIL_HYPERGEOMETRIC_2015_08_21_HPP_
  #define FIXED_POINT_DETAIL_HYPERGEOMETRIC_2015_08_21_HPP_

  #include <cmath>
  #include <cstdint>
  #include <limits>

  #include <boost/config.hpp>

  namespace boost { namespace fixed_point { namespace detail {

  template<typename NumericType>
  NumericType hypergeometric_0f0(const NumericType& x)
  {
    // Compute the series representation of hypergeometric_0f0.

    // There are no checks on input range or parameter boundaries
    // in this series calculation.

    // As such, this function is designed for small-argument
    // Taylor-series-like expansions only. It is not intended
    // for general purpose calculations of hypergeometric_0f0.

    NumericType term(x);
    NumericType h0f0(1U + term);

    BOOST_CONSTEXPR_OR_CONST std::uint_fast16_t maximum_number_of_iterations = UINT16_C(10000);

    // Perform the series expansion of hypergeometric_0f0(; ; x).
    for(std::uint_fast16_t n = UINT16_C(2); n < maximum_number_of_iterations; ++n)
    {
      term *= x;
      term /= n;

      const bool minimum_number_of_iterations_is_complete = (n > UINT16_C(3));

      using std::fabs;

      if(   minimum_number_of_iterations_is_complete
         && (fabs(term) <= std::numeric_limits<NumericType>::epsilon()))
      {
        break;
      }

      h0f0 += term;
    }

    return h0f0;
  }

  template<typename NumericType>
  NumericType hypergeometric_0f1(const NumericType& b,
                                 const NumericType& x)
  {
    // Compute the series representation of hypergeometric_0f1.

    // There are no checks on input range or parameter boundaries
    // in this series calculation.

    // As such, this function is designed for small-argument
    // Taylor-series-like expansions only. It is not intended
    // for general purpose calculations of hypergeometric_0f1.

    NumericType bp(b);

    NumericType term(x / bp);
    NumericType h0f1(1U + term);

    BOOST_CONSTEXPR_OR_CONST std::uint_fast16_t maximum_number_of_iterations = UINT16_C(10000);

    // Perform the series expansion of hypergeometric_0f1(; b; x).
    for(std::uint_fast16_t n = UINT16_C(2); n < maximum_number_of_iterations; ++n)
    {
      term *= x;
      term /= n;

      ++bp;

      term /= bp;

      const bool minimum_number_of_iterations_is_complete = (n > UINT16_C(3));

      using std::fabs;

      if(   minimum_number_of_iterations_is_complete
         && (fabs(term) <= std::numeric_limits<NumericType>::epsilon()))
      {
        break;
      }

      h0f1 += term;
    }

    return h0f1;
  }

  template<typename NumericType>
  NumericType hypergeometric_2f1(const NumericType& a,
                                 const NumericType& b,
                                 const NumericType& c,
                                 const NumericType& x)
  {
    // Compute the series representation of hypergeometric_2f1 taken from
    // Abramowitz and Stegun 15.1.1.

    // There are no checks on input range or parameter boundaries
    // in this series calculation.

    // As such, this function is designed for small-argument
    // Taylor-series-like expansions only. It is not intended
    // for general purpose calculations of hypergeometric_2f1.

    NumericType ap(a);
    NumericType bp(b);
    NumericType cp(c);

    NumericType term(((ap * bp) / cp) * x);
    NumericType h2f1(1U + term);

    BOOST_CONSTEXPR_OR_CONST std::uint_fast16_t maximum_number_of_iterations = UINT16_C(10000);

    // Perform the series expansion of hypergeometric_2f1(a, b; c; x).
    for(std::uint_fast16_t n = UINT16_C(2); n < maximum_number_of_iterations; ++n)
    {
      term *= x;
      term /= n;

      ++ap;
      term *= ap;

      ++cp;
      term /= cp;

      ++bp;
      term *= bp;

      const bool minimum_number_of_iterations_is_complete = (n > UINT16_C(3));

      using std::fabs;

      if(   minimum_number_of_iterations_is_complete
         && (fabs(term) <= std::numeric_limits<NumericType>::epsilon()))
      {
        break;
      }

      h2f1 += term;
    }

    return h2f1;
  }

  template<typename NumericType>
  NumericType two_to_the_power_of_x(const NumericType& x,
                                    const NumericType& my_ln_two)
  {
    // Compute the series representation of (2^x),
    // which is very closely related to a hypergeometric
    // series.

    // There are no checks on input range or parameter boundaries
    // in this series calculation.

    // As such, this function is designed for small-argument
    // Taylor-series-like expansions only. It is not intended
    // for general purpose calculations of (2^x).

    const NumericType ln_two_times_x(my_ln_two * x);

    NumericType term(ln_two_times_x);
    NumericType sum (1U + term);

    BOOST_CONSTEXPR_OR_CONST std::uint_fast16_t maximum_number_of_iterations = UINT16_C(10000);

    // Perform the series expansion of (2^x).
    for(std::uint_fast16_t n = UINT16_C(2); n < maximum_number_of_iterations; ++n)
    {
      term *= ln_two_times_x;
      term /= n;

      const bool minimum_number_of_iterations_is_complete = (n > UINT16_C(3));

      using std::fabs;

      if(   minimum_number_of_iterations_is_complete
         && (fabs(term) <= std::numeric_limits<NumericType>::epsilon()))
      {
        break;
      }

      sum += term;
    }

    return sum;
  }

  } } } // namespace boost::fixed_point::detail

#endif // FIXED_POINT_DETAIL_HYPERGEOMETRIC_2015_08_21_HPP_