/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2015 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/
/// @todo Documentation Core/Datatypes/Legacy/Matrix/DenseMatrix.cc

#include <sci_defs/lapack_defs.h>
#include <sci_defs/blas_defs.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <cstdio>

#include <iostream>
#include <limits>
#include <sstream>
#include <stdexcept>
#include <string.h>
#include <vector>

#include <Core/Datatypes/ColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>

#include <Core/Exceptions/DimensionMismatch.h>

#include <boost/shared_array.hpp>

#include <Core/Math/MiscMath.h>

namespace SCIRun {

// NOTE: returns 1 if successful, or 0 if unsuccessful (i.e. ignore the solution vector)
int
LinearAlgebra::solve(const DenseMatrix& matrix, ColumnMatrix& sol)
{
  ColumnMatrix b(sol);
  return solve(matrix, b, sol);
}

// NOTE: returns 1 if successful, or 0 if unsuccessful (i.e. ignore the solution vector)
int
LinearAlgebra::solve(const DenseMatrix& matrix, const ColumnMatrix& rhs, ColumnMatrix& lhs)
{
  ASSERT(matrix.nrows() == matrix.ncols());
  ASSERT(rhs.nrows() == matrix.ncols());
  lhs=rhs;

  double **A;
  double **cpy = 0;
  double *lhsp = lhs.get_data_pointer();

  cpy = new double*[matrix.nrows()];

  for (index_type j=0; j < matrix.nrows(); j++)
    cpy[j] = matrix.get_raw_2D_pointer()[j];

  A = cpy;

  // Gauss-Jordan with partial pivoting
  index_type i;
  for (i=0; i < matrix.nrows(); i++)
  {
    double max=Abs(A[i][i]);
    index_type row=i;
    index_type j;
    for (j=i+1; j < matrix.nrows(); j++)
    {
      if(Abs(A[j][i]) > max)
      {
        max=Abs(A[j][i]);
        row=j;
      }
    }
    //  ASSERT(Abs(max) > 1.e-12);
    if (Abs(max) < 1.e-12)
    {
      lhs=rhs;
      delete cpy;
      return 0;
    }
    if(row != i)
    {
      // Switch rows (actually their pointers)
      std::swap(A[i], A[row]);
      std::swap(lhsp[i], lhsp[row]);
    }
    double denom=1./A[i][i];
    double* r1=A[i];
    double s1=lhsp[i];
    for (j=i+1; j<matrix.nrows(); j++)
    {
      double factor=A[j][i]*denom;
      double* r2=A[j];
      for (int k=i; k<matrix.nrows(); k++)
        r2[k]-=factor*r1[k];
      lhsp[j]-=factor*s1;
    }
  }

  // Back-substitution
  for (i=1; i<matrix.nrows(); i++)
  {
    //  cout << "Solve: " << i << " of " << nr << endl;
    //  ASSERT(Abs(A[i][i]) > 1.e-12);
    if (Abs(A[i][i]) < 1.e-12)
    {
      lhs=rhs;
      delete cpy;
      return 0;
    }
    double denom=1./A[i][i];
    double* r1=A[i];
    double s1=lhsp[i];
    for (index_type j=0;j<i;j++)
    {
      double factor=A[j][i]*denom;
      double* r2=A[j];
      for (index_type k=i; k<matrix.nrows(); k++)
        r2[k] -= factor*r1[k];
      lhsp[j] -= factor*s1;
    }
  }

  // Normalize
  for (i=0; i<matrix.nrows(); i++)
  {
    //  cout << "Solve: " << i << " of " << nr << endl;
    //  ASSERT(Abs(A[i][i]) > 1.e-12);
    if (Abs(A[i][i]) < 1.e-12)
    {
      lhs=rhs;
      delete cpy;
      return 0;
    }
    double factor=1./A[i][i];
    for (index_type j=0; j<matrix.nrows(); j++)
      A[i][j] *= factor;
    lhsp[i] *= factor;
  }
  delete cpy;
  return 1;
}

#if defined(HAVE_LAPACK)

void
LinearAlgebra::solve_lapack(const DenseMatrix& matrix, const ColumnMatrix& rhs, ColumnMatrix& lhs)
{
  if (matrix.nrows() != matrix.ncols())
  {
    SCI_THROW(DimensionMismatch(matrix.nrows(), matrix.ncols(), __FILE__, __LINE__));
  }

  if (rhs.nrows() != matrix.ncols())
  {
    SCI_THROW(DimensionMismatch(rhs.nrows(), matrix.ncols(), __FILE__, __LINE__));
  }

  ColumnMatrix rhsCopy(rhs); //need to make a copy because

  try
  {
    lapacksolvelinearsystem(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), rhsCopy.get_data_pointer(), rhs.nrows(), rhs.ncols());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }

  lhs=rhsCopy;

}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, SparseRowMatrixHandle& S, DenseMatrix& VT)
{
  ASSERTEQ(U.ncols(), U.nrows());
  ASSERTEQ(VT.ncols(), VT.nrows());
  ASSERTEQ(U.nrows(), matrix.nrows());
  ASSERTEQ(VT.ncols(), matrix.ncols());

  /*
   * LAPACK function dgesvd argument S is a DOUBLE PRECISION array
   * with dimension (min(M,N)).
   */
  const size_type SIGMA_LEN = std::min(matrix.nrows(), matrix.ncols());
  boost::shared_array<double> sigma(new double[SIGMA_LEN]);

  try
  {
    lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), sigma.get(), U.get_raw_2D_pointer(), VT.get_raw_2D_pointer());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }

  size_type nnz = 0;
  for (size_type i = 0; i < SIGMA_LEN; ++i)
  {
    if ( fabs(sigma[i]) >= std::numeric_limits<double>::epsilon() )
    {
      ++nnz;
    }
  }

  SparseRowMatrix::Data sparseData(matrix.nrows() + 1, nnz);
  const SparseRowMatrix::Rows& rows = sparseData.rows();
  const SparseRowMatrix::Columns& columns = sparseData.columns();
  const SparseRowMatrix::Storage& a = sparseData.data();
  if (!sparseData.allocated())
  {
    std::cerr << "Could not allocate memory for sparse matrix buffers "
    << __FILE__ << ": " << __LINE__ << std::endl;

    return;
  }

  // singular values on diagonal
  index_type count = 0;
  index_type i = 0;
  const index_type NROWS = matrix.nrows();
  for (index_type r = 0; r < NROWS; r++)
  {
    rows[r] = count;
    if ( r < matrix.ncols() && i < SIGMA_LEN && fabs(sigma[i]) >= std::numeric_limits<double>::epsilon() )
    {
      columns[count] = r;
      a[count] = sigma[i];
      ++count;
      ++i;
    }
  }
  rows[matrix.nrows()] = count;

  S = new SparseRowMatrix(matrix.nrows(), matrix.ncols(), sparseData, nnz);
}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, DenseMatrix& S, DenseMatrix& VT)
{
  ASSERTEQ(U.ncols(), U.nrows());
  ASSERTEQ(VT.ncols(), VT.nrows());
  ASSERTEQ(U.nrows(), matrix.nrows());
  ASSERTEQ(VT.ncols(), matrix.ncols());
  ASSERTEQ(S.nrows(), matrix.nrows());
  ASSERTEQ(S.ncols(), matrix.ncols());

  /*
   * LAPACK function dgesvd argument S is a DOUBLE PRECISION array
   * with dimension (min(M,N)).
   */
  const size_type SIGMA_LEN = std::min(matrix.nrows(), matrix.ncols());
  boost::shared_array<double> sigma(new double[SIGMA_LEN]);

  try
  {
    lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), sigma.get(), U.get_raw_2D_pointer(), VT.get_raw_2D_pointer());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }

  // Put singular values on diagonal.
  for (size_type i = 0; i < SIGMA_LEN; ++i)
    S.put(i, i, sigma[i]);
}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, ColumnMatrix& S, DenseMatrix& VT)
{
  // Check whether matrices are square
  if (U.ncols() != U.nrows())
  {
    SCI_THROW(DimensionMismatch(U.ncols(), U.nrows(), __FILE__, __LINE__));
  }

  if (VT.ncols() != VT.nrows())
  {
    SCI_THROW(DimensionMismatch(VT.ncols(), VT.nrows(), __FILE__, __LINE__));
  }

  if (U.nrows())
  {
    if (U.nrows() != matrix.nrows())
    {
      SCI_THROW(DimensionMismatch(U.nrows(), matrix.nrows(), __FILE__, __LINE__));
    }
  }
  if (VT.ncols())
  {
    if (VT.ncols() != matrix.ncols())
    {
      SCI_THROW(DimensionMismatch(VT.ncols(), matrix.ncols(), __FILE__, __LINE__));
    }
  }

  if (matrix.nrows() < matrix.ncols())
  {
    if (S.nrows() != matrix.nrows())
    {
      SCI_THROW(DimensionMismatch(S.nrows(), matrix.nrows(), __FILE__, __LINE__));
    }
  }
  else
  {
    if (S.nrows() != matrix.ncols())
    {
      SCI_THROW(DimensionMismatch(S.nrows(), matrix.ncols(), __FILE__, __LINE__));
    }
  }

  try
  {
    if (U.nrows() == 0 && VT.nrows() == 0)
      lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), S.get_data_pointer(), 0, 0);
    else if (U.nrows() != 0 && VT.nrows() == 0)
      lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), S.get_data_pointer(), U.get_raw_2D_pointer(), 0);
    else if (U.nrows() == 0 && VT.nrows() != 0)
      lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), S.get_data_pointer(), 0, VT.get_raw_2D_pointer());
    else
      lapacksvd(matrix.get_raw_2D_pointer(), matrix.nrows(), matrix.ncols(), S.get_data_pointer(), U.get_raw_2D_pointer(), VT.get_raw_2D_pointer());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }
}

void
LinearAlgebra::eigenvalues(const DenseMatrix& matrix, ColumnMatrix& R, ColumnMatrix& I)
{
  ASSERTEQ(matrix.ncols(), matrix.nrows());
  ASSERTEQ(R.nrows(), I.nrows());
  ASSERTEQ(matrix.ncols(), R.nrows());

  boost::shared_array<double> Er(new double[matrix.nrows()]);
  boost::shared_array<double> Ei(new double[matrix.nrows()]);

  try
  {
    lapackeigen(matrix.get_raw_2D_pointer(), matrix.nrows(), Er.get(), Ei.get());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }

  for (index_type i = 0; i < matrix.nrows(); i++)
  {
    R[i] = Er[i];
    I[i] = Ei[i];
  }
}

void
LinearAlgebra::eigenvectors(const DenseMatrix& matrix, ColumnMatrix& R, ColumnMatrix& I, DenseMatrix& Vecs)
{
  ASSERTEQ(matrix.ncols(), matrix.nrows());
  ASSERTEQ(R.nrows(), I.nrows());
  ASSERTEQ(matrix.ncols(), R.nrows());

  boost::shared_array<double> Er(new double[matrix.nrows()]);
  boost::shared_array<double> Ei(new double[matrix.nrows()]);

  try
  {
    lapackeigen(matrix.get_raw_2D_pointer(), matrix.nrows(), Er.get(), Ei.get(), Vecs.get_raw_2D_pointer());
  }
  catch (const SCIRun::LapackError& exception)
  {
    std::ostringstream oss;
    oss << "Caught LapackError exception: " << exception.message();
    // in the absence of a logging service
    std::cerr << oss.str() << std::endl;
    throw;
  }

  for (index_type i = 0; i<matrix.nrows(); i++)
  {
    R[i] = Er[i];
    I[i] = Ei[i];
  }
}

#else

void
LinearAlgebra::solve_lapack(const DenseMatrix& matrix, const ColumnMatrix& rhs, ColumnMatrix& lhs)
{
  ASSERTFAIL("Build was not configured with LAPACK. LinearAlgebra::solve_lapack is not available.");
}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, SparseRowMatrixHandle& S, DenseMatrix& VT)
{
  ASSERTFAIL("Build was not configured with LAPACK LinearAlgebra::svd is not available.");
}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, ColumnMatrix& S, DenseMatrix& VT)
{
  ASSERTFAIL("Build was not configured with LAPACK LinearAlgebra::svd is not available.");
}

void
LinearAlgebra::svd(const DenseMatrix& matrix, DenseMatrix& U, DenseMatrix& S, DenseMatrix& VT)
{
  ASSERTFAIL("Build was not configured with LAPACK LinearAlgebra::svd is not available.");
}

void
LinearAlgebra::eigenvalues(const DenseMatrix& matrix, ColumnMatrix& R, ColumnMatrix& I)
{
  ASSERTFAIL("Build was not configured with LAPACK LinearAlgebra::svd is not available.");
}

void
LinearAlgebra::eigenvectors(const DenseMatrix& matrix, ColumnMatrix& R, ColumnMatrix& I, DenseMatrix& Vecs)
{
  ASSERTFAIL("Build was not configured with LAPACK LinearAlgebra::svd is not available.");
}

#endif

} // End namespace SCIRun


#include <Rodin/Mesh.h>
#include <Rodin/Solver.h>
#include <Rodin/Variational.h>
#include <RodinExternal/MMG.h>
#include <vector>
#include <typeinfo>

// Dependencies required for eigenvalue computation
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/SparseGenMatProd.h>
#include <Spectra/SymGEigsShiftSolver.h>

using namespace Rodin;
using namespace Rodin::Variational;
using namespace Rodin::External;

Eigen::SparseMatrix<double> mfemToEigenSparse(mfem::SparseMatrix m)
{
  typedef Eigen::Triplet<double> Triplet;

  int nonZero = m.NumNonZeroElems();
  std::vector<Triplet> tripletList;

  // Get the values (i,j, data)
  int* m_I = m.GetI();
  int* m_J = m.GetJ();
  double* m_data = m.GetData();

  for(int r = 0; r < m.NumRows(); ++r){
    for(int j=m_I[r]; j<m_I[r+1]; ++j){
      tripletList.push_back(Triplet(r, m_J[j], m_data[j])); // ???? Inverse row and column since Eigen is CSC
    }
  }
  Eigen::SparseMatrix<double> m_sparse(m.NumRows(), m.NumCols());
  m_sparse.setFromTriplets(tripletList.begin(), tripletList.end()); //SEGFAULT

  return m_sparse;
}

class EigenSolver{

  public:
    // Constructors
    EigenSolver();

    // Destructor
    ~EigenSolver() = default;

    // Accessors
    GridFunction<H1>& getEigenFunction(int index);

    double getEigenValue(int index);
    std::vector<double> getEigenValues();

    // Options
    EigenSolver& setNumEV(unsigned int nev);
    EigenSolver& setShift(double shift);

    // Solves the problem
    void solve(mfem::SparseMatrix A, mfem::SparseMatrix B, FiniteElementSpace<H1>& fes);

  private:
    std::vector<double> m_eigenvalues;
    std::vector<GridFunction<H1>*> m_eigenfunctions;
    unsigned int m_nev;
    double m_shift;
};

int main(int argc, char** argv)
{
  const char* meshFile = "../resources/mfem/levelset-cantilever2d-example.mesh";

  // Define interior and exterior for level set discretization
  int Interior = 1, Exterior = 2;

  // Define boundary attributes
  int Gamma0 = 1, GammaD = 2, GammaN = 3, Gamma = 4;

  // Load mesh
  Mesh Omega;
  Omega.load(meshFile);

  // Solver for hilbertian regularization
  auto solver = Solver::UMFPack();

  // Optimization parameters
  size_t maxIt = 100;
  double eps = 1e-6;
  double hmax = 0.05;
  int k = 1; // The eigenvalue to optimize
  auto alpha = ScalarFunction(4 * hmax * hmax); // Parameter for hilbertian regularization

  std::vector<double> obj;

  /*
  // Scalar field finite element space over the whole domain
  FiniteElementSpace<H1> Vh(Omega);

  // Trim the exterior part of the mesh to solve the elasticity system
  SubMesh trimmed = Omega.trim(Exterior, Gamma);

  // Build a finite element space over the trimmed mesh
  FiniteElementSpace<H1> VhInt(trimmed);

  // Elasticity equation
  TrialFunction uInt(VhInt);
  TestFunction  vInt(VhInt);

  // A
  Problem stiffness(uInt, vInt);
  stiffness = Integral(Grad(uInt), Grad(vInt));
  stiffness.update().assemble();

  // B
  Problem mass(uInt, vInt);
  mass = Integral(uInt, vInt);
  mass.update().assemble();

  auto& m1 = stiffness.getStiffnessMatrix();
  auto& m2 = mass.getStiffnessMatrix();

  // Solve eigenvalue problem
  EigenSolver ES;
  ES.setShift(1.0).setNumEV(k+4).solve(m1, m2, VhInt);

  // Get solution and transfer to original domain
  GridFunction u(Vh);
  ES.getEigenFunction(k).transfer(u);
  double mu = ES.getEigenValue(k);

  // Save solution
  u.save("u.gf");
  Omega.save("Omega.mesh");
  */

  // Optimization loop
  for (size_t i = 0; i < maxIt; i++)
  {
    // Scalar field finite element space over the whole domain
    FiniteElementSpace<H1> Vh(Omega);

    // Trim the exterior part of the mesh to solve the elasticity system
    SubMesh trimmed = Omega.trim(Exterior, Gamma);

    // Build a finite element space over the trimmed mesh
    FiniteElementSpace<H1> VhInt(trimmed);

    // Elasticity equation
    TrialFunction uInt(VhInt);
    TestFunction  vInt(VhInt);

    // A
    Problem stiffness(uInt, vInt);
    stiffness = Integral(Grad(uInt), Grad(vInt));
    stiffness.update().assemble();

    // B
    Problem mass(uInt, vInt);
    mass = Integral(uInt, vInt);
    mass.update().assemble();

    auto& m1 = stiffness.getStiffnessMatrix();
    auto& m2 = mass.getStiffnessMatrix();

    // Solve eigenvalue problem
    EigenSolver ES;
    ES.setShift(1.0).setNumEV(k+4).solve(m1, m2, VhInt);

    // Get solution and transfer to original domain
    GridFunction u(Vh);
    ES.getEigenFunction(k).transfer(u);
    double mu = ES.getEigenValue(k);

    // Compute the boundary shape gradientv
    // Vector field finite element space over the whole domain
    FiniteElementSpace<H1> Uh(Omega, 2);
    auto n = Normal(2);

    // GridFunction dJ(Uh);
    //dJ = Omega.getVolume(Interior)*(Dot(Grad(u), Grad(u)) + mu*Dot(u,u))*n + mu*n; //  /Integral(Dot(u,u)) sur le sous-mesh
    //std::cout << dJ.getFiniteElementSpace().getVectorDimension() << std::endl;

    // Note from Carlos: 26/Avril/2022
    // Salut !
    // 1. I changed some things around, in particular I do not project the
    // expression for dJ on a GridFunction. Instead I wrote the thing directly
    // and hope that Rodin understands what I want hehe
    // 2. I fixed the bug in the transfer function, thank you for testing this
    // and narrowing down the problem! It helps a lot :)
    // 3. I tried to run your code and it seems that the objective goes down
    // but at one point the computation fails. I think due to parasitic
    // components.
    // 4. In the hilbertian procedure I removed the use of DirichletBC and used
    // the usual expression.
    //
    // See you soon!

    // Hilbert extension-regularization procedure
    // This is bullshit
    auto dJ = Dot(Grad(u).traceOf(Interior), Grad(u).traceOf(Interior)) * n;
    TrialFunction g(Uh);
    TestFunction  v(Uh);
    Problem hilbert(g, v);
    hilbert = Integral(alpha * Jacobian(g), Jacobian(v))
            + Integral(g, v)
            + BoundaryIntegral(dJ, v).over(Gamma);
    solver.solve(hilbert);

    // Save data to inspect
    // Omega.save("Omegai.mesh");
    // g.getGridFunction().save("g.gf");

    // Update objective
    obj.push_back(
        mu * Omega.getVolume(Interior));
    std::cout << "[" << i << "] Objective: " << obj.back() << std::endl;

    // Convert data types to mmg types
    auto mmgMesh = Cast(Omega).to<MMG::Mesh2D>();
    auto mmgVel = Cast(g.getGridFunction()).to<MMG::VectorSolution2D>(mmgMesh);

    // Generate signed distance function
    auto mmgLs = MMG::Distancer2D().setInteriorDomain(Interior).distance(mmgMesh);

    // Advect the level set function
    double gInf = std::max(g.getGridFunction().max(), -g.getGridFunction().min());
    double dt = hmax / gInf;
    MMG::Advect2D(mmgLs, mmgVel).step(dt);

    // Recover the implicit domain
    auto mmgImplicit =
      MMG::ImplicitDomainMesher2D().split(Interior, {Interior, Exterior})
                                   .split(Exterior, {Interior, Exterior})
                                   .setRMC(1e-3)
                                   .setHMax(hmax)
                                   .setBoundaryReference(Gamma)
                                   .discretize(mmgLs);

    // Convert back to Rodin data type
    Omega = Cast(mmgImplicit).to<Rodin::Mesh<>>();

    // Save mesh
    Omega.save("Omega.mesh");

    // Test for convergence
    if (obj.size() >= 2 && abs(obj[i] - obj[i - 1]) < eps)
    {
      std::cout << "Convergence!" << std::endl;
      break;
    }

    std::ofstream plt("obj.txt", std::ios::trunc);
    for (size_t i = 0; i < obj.size(); i++)
      plt << i << "," << obj[i] << "\n";
  }

  return 0;
}

// Constructor
EigenSolver::EigenSolver(){
  m_nev = 0;
  m_shift = 0.0;
}

// Accessors
GridFunction<H1>& EigenSolver::getEigenFunction(int index){
  return *m_eigenfunctions[index];
}

double EigenSolver::getEigenValue(int index){
    return m_eigenvalues[index];
}

std::vector<double> EigenSolver::getEigenValues(){
  return m_eigenvalues;
}

// Options
EigenSolver& EigenSolver::setNumEV(unsigned int nev){
  m_nev = nev;
  return *this;
}

EigenSolver& EigenSolver::setShift(double shift){
  m_shift = shift;

  return *this;
}

void EigenSolver::solve(mfem::SparseMatrix A,mfem::SparseMatrix B, FiniteElementSpace<H1>& fes){

  // 1. CA and B to Eigen::SparseMatrix
  //WARNING: MAY BE USELESS SINCE SPECTRA HANDLE OTHER MATRICES TYPES
  //BUT MAY BE FASTER. IN FACT I DON'T HAVE A CLUE.
  Eigen::SparseMatrix<double> A_sparse = mfemToEigenSparse(A);
  Eigen::SparseMatrix<double> B_sparse = mfemToEigenSparse(B);

  // 2. Use Spectra with the shift-inverse method to compute eigenvalue
  // Define the shift-inverse and vector-multiplication operations
  using OpType = Spectra::SymShiftInvert<double, Eigen::Sparse, Eigen::Sparse>;
  using BOpType = Spectra::SparseSymMatProd<double>;
  OpType op(A_sparse, B_sparse);
  BOpType Bop(B_sparse);

  // Construct the generalized eigensolver object
  int ncv = 2*m_nev+1;
  Spectra::SymGEigsShiftSolver<OpType, BOpType, Spectra::GEigsMode::ShiftInvert>
    geigs(op, Bop, m_nev, ncv, m_shift);

  // Initialize and compute
  geigs.init();
  int nconv = geigs.compute(Spectra::SortRule::LargestMagn); // UTILISER LARGESTMAGN ET PAS SMALLESTMAGN CAR ON EST EN SHIFT-INVERSE MODE

  // Retrieve results
  Eigen::VectorXd evalues;
  Eigen::MatrixXd evecs;
  if (geigs.info() == Spectra::CompInfo::Successful)
  {
      evalues = geigs.eigenvalues();
      evecs = geigs.eigenvectors();
  }

  // Store in m_eigenvalues and m_eigenfunctions
  // (and reverse the order to order from lowest to higest)
  int dim = evecs.rows();

  for(int i = 0; i < m_nev; i++){
    m_eigenvalues.push_back(evalues[m_nev-i-1]);

    std::unique_ptr<double[]> data(new double[dim]);
    for(int j=0; j<dim; j++){
      data[j] = evecs.col(m_nev-i-1)[j];
    }
    //std::copy(evecs.col(m_nev-i-1).data(), evecs.col(m_nev-i-1).data()+dim, data.get());

    m_eigenfunctions.push_back(new GridFunction<H1>(fes));
    m_eigenfunctions[i]->setData(std::move(data), dim);
  }
}


#ifndef TVMTL_MANIFOLD_SPD_HPP
#define TVMTL_MANIFOLD_SPD_HPP

#include <cmath>
#include <complex>
#include <iostream>

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/SVD>
#include <unsupported/Eigen/MatrixFunctions>
#include <unsupported/Eigen/KroneckerProduct>

//own includes
#include "enumerators.hpp"
#include "matrix_utils.hpp"

namespace tvmtl {

// Specialization SPD
template <int N>
struct Manifold< SPD, N> {

    public:
	static const MANIFOLD_TYPE MyType;
	static const int manifold_dim ;
	static const int value_dim; // TODO: maybe rename to embedding_dim

	static const bool non_isometric_embedding;

	// Scalar type of manifold
	typedef double scalar_type;
	typedef double dist_type;
	typedef std::complex<double> complex_type;
	typedef std::vector<double> weight_list;

	// Value Typedef
	typedef Eigen::Matrix< scalar_type, N, N>				value_type;
	typedef value_type&							ref_type;
	typedef const value_type&						cref_type;
	typedef std::vector<value_type, Eigen::aligned_allocator<value_type> >	value_list;

	// Tangent space typedefs
	typedef Eigen::Matrix <scalar_type, N * N, N * (N + 1) / 2>   tm_base_type;
	typedef tm_base_type&					    tm_base_ref_type;

	// Derivative Typedefs
	typedef value_type			     deriv1_type;
	typedef deriv1_type&			     deriv1_ref_type;

	typedef Eigen::Matrix<scalar_type, N*N, N*N>				deriv2_type;
	typedef deriv2_type&							deriv2_ref_type;
	typedef	Eigen::Matrix<scalar_type, N * (N + 1) / 2, N * (N + 1) / 2>	restricted_deriv2_type;

	// Manifold distance functions (for IRLS)
	inline static dist_type dist_squared(cref_type x, cref_type y);
	inline static void deriv1x_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);
	inline static void deriv1y_dist_squared(cref_type x, cref_type y, deriv1_ref_type result);

	inline static void deriv2xx_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2xy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);
	inline static void deriv2yy_dist_squared(cref_type x, cref_type y, deriv2_ref_type result);

	// Manifold exponentials und logarithms ( for Proximal point)
	template <typename DerivedX, typename DerivedY>
	inline static void exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result);
	inline static void log(cref_type x, cref_type y, ref_type result);

	inline static void convex_combination(cref_type x, cref_type y, double t, ref_type result);

	// Implementations of the Karcher mean
	// Slow list version
	inline static void karcher_mean(ref_type x, const value_list& v, double tol=1e-10, int maxit=15);
	inline static void weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol=1e-10, int maxit=15);
	// Variadic templated version
	template <typename V, class... Args>
	inline static void karcher_mean(V& x, const Args&... args);
	template <typename V>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y);
	template <typename V, class... Args>
	inline static void variadic_karcher_mean_gradient(V& x, const V& y1, const Args&... args);

	// Basis transformation for restriction to tangent space
	inline static void tangent_plane_base(cref_type x, tm_base_ref_type result);

	// Projection
	inline static void projector(ref_type x);

	// Interpolation pre- and postprocessing
	inline static void interpolation_preprocessing(ref_type x);
	inline static void interpolation_postprocessing(ref_type x);

};

/*-----IMPLEMENTATION SPD----------*/

// Static constants, Outside definition to avoid linker error

template <int N>
const MANIFOLD_TYPE Manifold < SPD, N>::MyType = SPD;

template <int N>
const int Manifold < SPD, N>::manifold_dim = N * (N + 1) / 2;

template <int N>
const int Manifold < SPD, N>::value_dim = N * N;

template <int N>
const bool Manifold < SPD, N>::non_isometric_embedding = true;

// Squared SPD distance function
template <int N>
inline typename Manifold < SPD, N>::dist_type Manifold < SPD, N>::dist_squared( cref_type x, cref_type y ){
    #ifdef TV_SPD_DIST_DEBUG
	std::cout << "\nDist2 function with x=\n" << x << "\nand y=\n" << y << std::endl;
    #endif
// NOTE: If x is not strictly spd, using LDLT completely halts the algorithm
/*    value_type sqrtX = x.sqrt();
    Eigen::LDLT<value_type> ldlt;
    ldlt.compute(sqrtX);

    value_type Z = ldlt.solve(y).transpose();
    return ldlt.solve(Z).transpose().log().squaredNorm();	*/
    value_type invsqrt = x.sqrt().inverse();
    return (invsqrt * y * invsqrt).log().squaredNorm();
}

// Derivative of Squared SPD distance w.r.t. first argument
// TODO: Switch to solve() for N>4?
template <int N>
inline void Manifold < SPD, N>::deriv1x_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    value_type invsqrt = x.sqrt().inverse();
    result = -2.0 * invsqrt * (invsqrt * y * invsqrt).log() * invsqrt;
}
// Derivative of Squared SPD distance w.r.t. second argument
template <int N>
inline void Manifold < SPD, N>::deriv1y_dist_squared( cref_type x, cref_type y, deriv1_ref_type result){
    deriv1x_dist_squared(y, x, result);
}

// Second Derivative of Squared SPD distance w.r.t first argument
template <int N>
inline void Manifold < SPD, N>::deriv2xx_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    value_type T2, T3, T4;
    T2 = x.sqrt().inverse();
    T3 = T2 * y * T2;
    T4 = T3.log();

    deriv2_type dlog, dsqrt;
    KroneckerDLog(T3, dlog);
    KroneckerDSqrt(x, dsqrt);

    deriv2_type T2T4tId, IdT2T4, T2T2, T2yId, IdT2y;
    T2T4tId = Eigen::kroneckerProduct(T2 * T4.transpose(), value_type::Identity());
    IdT2T4 = Eigen::kroneckerProduct(value_type::Identity(), T2 * T4);
    T2T2 = Eigen::kroneckerProduct(T2, T2);
    T2yId = Eigen::kroneckerProduct(T2 * y, value_type::Identity());
    IdT2y = Eigen::kroneckerProduct(value_type::Identity(), T2 * y);

    result =  2 * (T2T4tId + IdT2T4 + T2T2 * dlog * (T2yId + IdT2y) ) * T2T2 * dsqrt;
}
// Second Derivative of Squared SPD distance w.r.t first and second argument
template <int N>
inline void Manifold < SPD, N>::deriv2xy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    value_type isqrtX, T1;
    isqrtX = x.sqrt().eval().inverse();
    T1 = isqrtX * y * isqrtX;

    deriv2_type kp_isqrtX, dlog;
    kp_isqrtX = Eigen::kroneckerProduct(isqrtX, isqrtX);
    KroneckerDLog(T1, dlog);

    result = -2 * kp_isqrtX * dlog * kp_isqrtX;
}
// Second Derivative of Squared SPD distance w.r.t second argument
template <int N>
inline void Manifold < SPD, N>::deriv2yy_dist_squared( cref_type x, cref_type y, deriv2_ref_type result){
    deriv2xx_dist_squared(y, x, result);
}

// Exponential and Logarithm Map
template <int N>
template <typename DerivedX, typename DerivedY>
inline void Manifold <SPD, N>::exp(const Eigen::MatrixBase<DerivedX>& x, const Eigen::MatrixBase<DerivedY>& y, Eigen::MatrixBase<DerivedX>& result){
    #ifdef TV_SPD_EXP_DEBUG
	std::cout << "\nEXP function with x=\n" << x << "\nand y=\n" << y << std::endl;
    #endif
    value_type sqrtX = x.sqrt();
    value_type Z = sqrtX.ldlt().solve(y).transpose();
    result = sqrtX * sqrtX.transpose().ldlt().solve(Z).exp() * sqrtX;
}

template <int N>
inline void Manifold <SPD, N>::log(cref_type x, cref_type y, ref_type result){
    #ifdef TV_SPD_LOG_DEBUG
	std::cout << "\nLOG function with x=\n" << x << "\nand y=\n" << y << std::endl;
    #endif
    value_type sqrtX = x.sqrt();
    value_type Z = sqrtX.ldlt().solve(y).transpose();
    result = sqrtX * sqrtX.transpose().ldlt().solve(Z).log() * sqrtX;
}

// Tangent Plane restriction
template <int N>
inline void Manifold <SPD, N>::tangent_plane_base(cref_type x, tm_base_ref_type result){
    int d = value_type::RowsAtCompileTime;
    int k = 0;

    value_type S, T;
    S = x.sqrt();

    for(int i=0; i<d; i++){
	T.setZero();
	T.col(i) = S.col(i);
	T = T * S;

	result.col(k) = Eigen::Map<Eigen::VectorXd>(T.data(), T.size());
	++k;
    }

    for(int i=0; i<d-1; i++)
	for(int j=i+1; j<d; j++){
	    T.setZero();
	    T.col(i) = S.col(j);
	    T.col(j) = S.col(i);
	    T = T * S;

	    result.col(k) = Eigen::Map<Eigen::VectorXd>(T.data(), T.size());
	    ++k;
	}
}

template <int N>
inline void Manifold <SPD, N>::projector(ref_type x){
    // does not exist since SPD is an open set
    // TODO: Eventually implement projection to semi positive definite matrices
}

// Convex geodesic combinations
template <int N>
inline void Manifold <SPD, N>::convex_combination(cref_type x, cref_type y, double t, ref_type result){
    value_type l;
    log(x, y, l);
    exp(x, l * t, result);
}

// Karcher mean implementations
template <int N>
inline void Manifold<SPD, N>::karcher_mean(ref_type x, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += temp;
	}
	exp(x, 0.5 / v.size() * (L + L.transpose()), temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N>
inline void Manifold<SPD, N>::weighted_karcher_mean(ref_type x, const weight_list& w, const value_list& v, double tol, int maxit){
    value_type L, temp;

    int k = 0;
    double error = 0.0;
    do{
	scalar_type m1 = x.sum();
	L = value_type::Zero();
	for(int i = 0; i < v.size(); ++i){
	    log(x, v[i], temp);
	    L += w[i] * temp;
	}
	exp(x, 0.5 / v.size() * (L + L.transpose()), temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);

}

template <int N>
template <typename V, class... Args>
inline void Manifold<SPD, N>::karcher_mean(V& x, const Args&... args){
    V temp, sum;

    int numArgs = sizeof...(args);
    int k = 0;
    double error = 0.0;
    double tol = 1e-10;
    int maxit = 15;
    do{
	scalar_type m1 = x.sum();
	sum = x;
	variadic_karcher_mean_gradient(sum, args...);
	exp(x, 0.5 / numArgs * (sum + sum.transpose()), temp);
	x = temp;
	error = std::abs(x.sum() - m1);
	++k;
    } while(error > tol && k < maxit);
}

template <int N>
template <typename V>
inline void Manifold<SPD, N>::variadic_karcher_mean_gradient(V& x, const V& y){
    V temp;
    log(x, y, temp);
    x = temp;
}

template <int N>
template <typename V, class... Args>
inline void Manifold<SPD, N>::variadic_karcher_mean_gradient(V& x, const V& y1, const Args& ... args){
    V temp1, temp2;
    temp2 = x;

    log(x, y1, temp1);

    variadic_karcher_mean_gradient(temp2, args...);
    temp1 += temp2;
    x = temp1;
}

template <int N>
inline void Manifold<SPD, N>::interpolation_preprocessing(ref_type x){
    value_type t = x.log();
    x = t;
}

template <int N>
inline void Manifold<SPD, N>::interpolation_postprocessing(ref_type x){
    value_type t = ( 0.5 * (x + x.transpose()) ).exp();
    x = t;
}

} // end namespace tvmtl

#endif


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Eigen>

struct PoseData {
    Eigen::Vector3d p;
    Eigen::Quaterniond q;
    std::string filename;
    int index;
};

inline Eigen::Vector3d logmap(const Eigen::Quaterniond &q) {
    Eigen::AngleAxisd aa(q);
    return aa.angle() * aa.axis();
}

inline std::vector<PoseData> read_7scenes_pose(const std::string &path, int pose_num, const std::string &pose_type) {
    std::vector<PoseData> poses;
    // printf("#idx px py pz qx qy qz qw\n");
    for (int idx = 0; idx < pose_num; idx++) {
        char filename[1024];
        if (pose_type == "gt") {
             sprintf(filename, "%s/frame-%06d.pose.txt", path.c_str(), idx);
        } else if (pose_type == "test") {
            sprintf(filename, "%s/frame-%06d.txt", path.c_str(), idx);
        } else {
            std::runtime_error("Unknown pose_type.");
        }
        std::ifstream in(filename);
        Eigen::Matrix4d e;
        for (int r = 0; r < 4; r++) {
            for (int c = 0; c < 4; c++) {
                in >> e(r, c);
            }
        }
        // origin: Twc
        Eigen::Quaterniond qwc(e.block<3, 3>(0, 0));
        Eigen::Vector3d pwc(e.block<3, 1>(0, 3));
        if (std::abs(qwc.norm() - 1.0) > 1.0e-3) {
           fprintf(stderr, "Warning, rotation matrix may not be valid, qwc.norm() %.4e", qwc.norm());
        }
        if (pose_type == "gt") {
            pwc.x() += 0.0245;
        } else if (pose_type == "test") {
            pwc = -(qwc.conjugate() * pwc);
            qwc = qwc.conjugate();
        } else {
            std::runtime_error("Unknown pose_type.");
        }
        PoseData pose;
        pose.q = qwc;
        pose.p = pwc;
        pose.filename = filename;
        pose.index = idx;
        poses.emplace_back(std::move(pose));
        // printf("%05d %.9e %.9e %.9e %.9e %.9e %.9e %.9e\n", idx, pwc.x(), pwc.y(), pwc.z(), qwc.x(), qwc.y(), qwc.z(), qwc.w());
    }
    return poses;
}

int main(int argc, char **argv) {
    std::string gt_dir = argv[1];
    std::string pred_dir = argv[2];
    const int pose_num = 1000;
    auto gt_poses = read_7scenes_pose(gt_dir, pose_num, "gt");
    auto pred_poses = read_7scenes_pose(pred_dir, pose_num, "test");
    assert(pred_poses.size() == gt_poses.size());
    double APE = 0, ARE = 0, Acount = 0;
    std::vector<double> APEs, AREs;
    for (int i = 0; i < pose_num; ++i) {
        const auto &gt_pose = gt_poses[i];
        const auto &pred_pose = pred_poses[i];
        Eigen::Vector3d p_error = gt_pose.p - pred_pose.p;
        Eigen::Vector3d q_error = logmap(gt_pose.q.conjugate() * pred_pose.q);
        APE += p_error.squaredNorm();
        ARE += q_error.squaredNorm();
        APEs.push_back(p_error.norm() * 1e3);
        AREs.push_back(q_error.norm() * 180 / M_PI);
        Acount++;
    }
    Acount = std::max(Acount, 1.0);
    APE = std::sqrt(APE / Acount) * 1e3;
    ARE = std::sqrt(ARE / Acount) * 180 / M_PI;
    printf("APE RMSE: %.3f [mm]\nARE RMSE %.3f[DEG]\n", APE, ARE);
    std::sort(APEs.begin(), APEs.end());
    std::sort(AREs.begin(), AREs.end());
    printf("APE median: %.3f [mm]\nARE median %.3f[DEG]\n", APEs[APEs.size() / 2], AREs[AREs.size() / 2]);
    return 0;
}


#ifndef __PC_TO_SURFACES_HH__
#define __PC_TO_SURFACES_HH__

#include <vector>
#include <iostream>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>

#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/octree/octree.h>
#include <pcl/octree/octree_search.h>

#include <pcl/visualization/pcl_visualizer.h>

#include <Eigen/Eigenvalues>
#include <Eigen/SVD>

#include <utils.hh>

struct PC2SurfacesParams{
  public:
    PC2SurfacesParams();
    void print();

    int max_inner_iter;
    int max_outer_iter;
    int outlier_id;
    int unclassified_id;
    int object_id;
    double term_perc_crit;
    double normal_dev_thres1; // degrees
    double normal_dev_thres2;
    double contour_fit_thres; // ratio to radius
    double sphere_r; // meters
    double segment_len; // meters
    double normal_search_radius;
    int    num_segments;
    string contour_type;
    double voxel_leaf_size;
    double curvature_thres;
    double var_r;
    double var_azimutal;
    double var_elevation;
    double max_segment_dist;
    double max_segment_rot;
};

class PC2Surfaces{
  private:
    PC2SurfacesParams _params;

    // ------------ BOOKKEEPING --------------------------------------------------- //
    // Segment ID of each point in '_pc_sphere'
    vector<int> _segment_ids;
    vector<int> _valid_segs;
    // Outlier flags for points in '_pc_sphere'
    vector<bool> _outliers;
    // Mapping between segments and their corresponding
    // coordinate triad, origin and contour equations.
    std::map<int, Eigen::Matrix3d> _segment_triad_map;
    std::map<int, Eigen::Vector3d> _segment_origin_map;
    std::map<int, Eigen::VectorXd> _segment_contour_map;

    // ------------ AXIS, CONTOUR & UNCERTAINTY ESTIMATION ------------------------ //
    std::map<int, Eigen::Matrix3d> _segment_Mmatrix;
    // Mapping between segments and eigenpairs of the 'M' matrix as
    // stored in '_segment_Mmatrix'.
    std::map<int, pair<Eigen::Vector3d, Eigen::Matrix3d> > _axis_eigenpairs;
    // Mapping between segments and index of the smallest eigen value
    // of the corresponding eigenpair.
    std::map<int, int> _axis_min_eigval_ind;
    // Mapping between segments and their corresponding axes's
    // uncertainties.
    std::map<int, Eigen::Matrix3d> _axis_uncertainties;
    // Mapping between segments and the intermediate matrix-vector pairs
    // calculated while estimating the contour the corresponding segment.
    // This is a function of the form Ax=b.
    std::map<int, pair<Eigen::MatrixXd, Eigen::VectorXd> > _segment_contour_equ;
    // Mapping between segments and the derivative of the corresponding
    // contour w.r.t. 'x' the solution to Ax=b explained above.
    std::map<int, Eigen::MatrixXd> _segment_dcontour;
    // Mapping between the segments and the corresponding contour
    // uncertainties.
    std::map<int, Eigen::MatrixXd> _contour_uncertainties;

    // ------------ POINTCLOUDS --------------------------------------------------- //
    // The original unfiltered point cloud.
    pcl::PointCloud<pcl::PointXYZ>::Ptr _pc_orig;
    // '_pc_orig' after filtered with voxel grid filter and radius filter.
    pcl::PointCloud<pcl::PointXYZ>::Ptr _pc_sphere;
    pcl::PointCloud<pcl::PointXYZ>::Ptr _pc_outliers;
    pcl::PointCloud<pcl::PointXYZ>::Ptr _pc_objects;
    // Normals of the points in '_pc_sphere'.
    pcl::PointCloud<pcl::Normal>::Ptr _pc_sphere_normals;
    // '_pc_sphere' written in the corresponding segment frame.
    vector<Eigen::Vector3d> _pc_projections;

    // ------------ NORMAL & UNCERTAINTY ESTIMATION ------------------------------- //
    // Some intermediate variables used in estimating surface normals.
    vector<vector<int  > > _nearest_neigh_inds;
    vector<vector<float> > _nearest_neigh_sq_dists;
    // Covariance and centroids for each point in '_pc_sphere'
    vector<Eigen::Matrix3d> _normal_covs;
    vector<Eigen::Vector4d> _normal_centroids;
    // Eigenpairs of each '_normal_covs'
    vector<pair<Eigen::Vector3d, Eigen::Matrix3d> > _normal_eigenpairs;
    // Index of the smallest eigenvalue of the eigenpairs in '_normal_eigenpairs'
    vector<int> _normal_min_eigval_ind;
    // Uncertainties of each normals vector
    vector<Eigen::Matrix3d> _normal_uncertainties;
    // Uncertainties each point in '_pc_sphere' written in the sensor frame.
    vector<Eigen::Matrix3d> _point_uncertainties;

    // PCL's visualization toolbox
    pcl::visualization::PCLVisualizer::Ptr _viewer;

    // ---------------------------------------------------------------------------- //
    // This fucntion estimates surface normals for each point in '_pc_sphere' as well
    // as it estimates each normal's uncertainty.
    int _fit_normals();
    // This function initializes/refines the coordinate frame and the origin of the
    // segment 'seg'. It also generates intermediate variables for later axis
    // uncertainty estimation.
    int _init_triad(int seg);
    // This function iteratively filters for a segment, fits contour and eliminates
    // outliers until convergence.
    int _fit_segment(int seg);
    // This function assigns points satisfying the criterion given in '_params' to 'seg'.
    int _filter_segment(int seg);
    // This function eliminates outliers of segment 'seg' with either using
    // the 'normal' or 'contour' method.
    int _eliminate_outliers(int seg, const std::string &method);
    // This function transforms points of the segment 'seg' from the sensor
    // frame to the corresponding segment frame.
    int _project_pc(int seg);
    // This function fits a contour to the projected points of the segment 'seg'
    // using the method given in '_params'.
    int _fit_contour(int seg);
    // This function estimates axis and contour uncertainties of the segment 'seg'
    int _estimate_uncertainties(int seg);
  public:

    PC2Surfaces();
    PC2Surfaces(const PC2SurfacesParams &params);

    int push_pc(const pcl::PointCloud<pcl::PointXYZ>::Ptr &pc);

    inline int set_params(const PC2SurfacesParams &params){
      _params = params;
      return 0;
    }

    inline int get_params(PC2SurfacesParams &params){
      params = _params;
      return 0;
    }

    inline int get_segment_ids(vector<int> &ids, vector<bool> &outliers){
      ids = _segment_ids;
      outliers = _outliers;
      return _segment_ids.size();
    }

    inline int get_segments(
        map<int, Eigen::Vector3d> &segment_origins,
        map<int, Eigen::Matrix3d> &segment_triads,
        map<int, Eigen::VectorXd> &segment_contours){
      segment_origins.clear();
      segment_triads.clear();
      segment_contours.clear();
      for(int i = 0 ; i < (int)_valid_segs.size() ; i++){
        int seg = _valid_segs[i];
        segment_origins[seg]  = _segment_origin_map[seg];
        segment_triads[seg]   = _segment_triad_map[seg];
        segment_contours[seg] = _segment_contour_map[seg];
      }
      return segment_triads.size();
    }

    inline int get_orig_pc(pcl::PointCloud<pcl::PointXYZ>::Ptr &pc){
      if(_pc_orig){
        pc = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>(*_pc_orig));
        return pc->points.size();
      } else {
        pc = NULL;
        return 0;
      }
    }

    inline int get_pc_sphere(pcl::PointCloud<pcl::PointXYZ>::Ptr &pc){
      if(_pc_sphere){
        pc = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>(*_pc_sphere));
        return pc->points.size();
      } else {
        pc = NULL;
        return 0;
      }
    }

    inline int get_pc_outliers(pcl::PointCloud<pcl::PointXYZ>::Ptr &pc){
      if(_pc_outliers){
        pc = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>(*_pc_outliers));
        return pc->points.size();
      } else {
        pc = NULL;
        return 0;
      }
    }

    inline int get_pc_objects(pcl::PointCloud<pcl::PointXYZ>::Ptr &pc){
      if(_pc_objects){
        pc = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>(*_pc_objects));
        return pc->points.size();
      } else {
        pc = NULL;
        return 0;
      }
    }

    inline int get_normals(pcl::PointCloud<pcl::Normal>::Ptr &normals){
      if(_pc_sphere_normals){
        normals = pcl::PointCloud<pcl::Normal>::Ptr(new pcl::PointCloud<pcl::Normal>(*_pc_sphere_normals));
        return normals->points.size();
      } else {
        normals = NULL;
        return 0;
      }
    }

    inline int get_uncertainties(
        map<int, Eigen::Matrix3d> &axis_uncertainties,
        map<int, Eigen::MatrixXd> &contour_uncertainties){
      axis_uncertainties =  _axis_uncertainties;
      contour_uncertainties =  _contour_uncertainties;
      return axis_uncertainties.size();
    }

    inline int get_point_uncertainties(vector<Eigen::Matrix3d> &covs){
      covs = _point_uncertainties;
      return covs.size();
    }

    inline int get_normal_uncertainties(vector<Eigen::Matrix3d> &covs){
      covs = _normal_uncertainties;
      return covs.size();
    }

    int get_projections(int seg, vector<Eigen::Vector3d> &proj, vector<bool> &outliers);
    int get_segment_pc(int seg, pcl::PointCloud<pcl::PointXYZ> &pc);

    int visualize_fit();
};

#endif


/*************************************************************************
	> File Name: lqr_steer_control.cpp
	> Author: TAI Lei
	> Mail: ltai@ust.hk
	> Created Time: Wed Apr 17 11:48:46 2019
 ************************************************************************/

#include <iostream>
#include <limits>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sys/time.h>
#include <Eigen/Eigen>
#include "cubic_spline.h"
#include "motion_model.h"
#include "cpprobotics_types.h"

#define DT 0.1
#define L 0.5
#define KP 1.0
#define MAX_STEER 45.0/180*M_PI

using namespace cpprobotics;

cv::Point2i cv_offset(
    float x, float y, int image_width=2000, int image_height=2000){
  cv::Point2i output;
  output.x = int(x * 100) + 300;
  output.y = image_height - int(y * 100) - image_height/2;
  return output;
};

Vec_f calc_speed_profile(Vec_f rx, Vec_f ry, Vec_f ryaw, float target_speed){
	Vec_f speed_profile(ryaw.size(), target_speed);

	float direction = 1.0;
	for(unsigned int i=0; i < ryaw.size()-1; i++){
		float dyaw = std::abs(ryaw[i+1] - ryaw[i]);
		float switch_point = (M_PI/4.0< dyaw) && (dyaw<M_PI/2.0);

		if (switch_point) direction = direction * -1;
		if (direction != 1.0) speed_profile[i]= target_speed * -1;
		else speed_profile[i]= target_speed;

		if (switch_point) speed_profile[i] = 0.0;
	}

	speed_profile[speed_profile.size()-1] = 0.0;
	return speed_profile;
};

float calc_nearest_index(State state, Vec_f cx, Vec_f cy, Vec_f cyaw, int &ind){
	float mind = std::numeric_limits<float>::max();
	for(unsigned int i=0; i<cx.size(); i++){
		float idx = cx[i] - state.x;
		float idy = cy[i] - state.y;
		float d_e = idx*idx + idy*idy;

		if (d_e<mind){
			mind = d_e;
			ind = i;
		}
	}
	float dxl = cx[ind] - state.x;
	float dyl = cy[ind] - state.y;
	float angle = YAW_P2P(cyaw[ind] - std::atan2(dyl, dxl));
	if (angle < 0) mind = mind * -1;

	return mind;
};

Eigen::Matrix4f solve_DARE(Eigen::Matrix4f A, Eigen::Vector4f B, Eigen::Matrix4f Q, float R){
	Eigen::Matrix4f X = Q;
	int maxiter = 150;
	float eps = 0.01;

	for(int i=0; i<maxiter; i++){
		Eigen::Matrix4f Xn = A.transpose()*X*A-A.transpose()*X*B/(R+B.transpose()*X*B) * B.transpose()*X*A+Q;
		Eigen::Matrix4f error = Xn - X;
		if (error.cwiseAbs().maxCoeff()<eps){
			return Xn;
		}
		X = Xn;
	}

	return X;
};

Eigen::RowVector4f dlqr(Eigen::Matrix4f A, Eigen::Vector4f B, Eigen::Matrix4f Q, float R){
	Eigen::Matrix4f X = solve_DARE(A, B ,Q, R);
	Eigen::RowVector4f K = 1.0/(B.transpose()*X*B + R) * (B.transpose()*X*A);
	return K;
};

float lqr_steering_control(State state, Vec_f cx, Vec_f cy, Vec_f cyaw, Vec_f ck, int& ind, float& pe, float& pth_e){
	float e = calc_nearest_index(state, cx, cy, cyaw, ind);

	float k = ck[ind];
	float th_e = YAW_P2P(state.yaw - cyaw[ind]);

	Eigen::Matrix4f A = Eigen::Matrix4f::Zero();
	A(0, 0) = 1.0;
	A(0 ,1) = DT;
	A(1 ,2) = state.v;
	A(2 ,2) = 1.0;
	A(2 ,3) = DT;

	Eigen::Vector4f B = Eigen::Vector4f::Zero();
	B(3) = state.v/L;

	Eigen::Matrix4f Q = Eigen::Matrix4f::Identity();
	float R = 1;

	// gain of lqr
	Eigen::RowVector4f K = dlqr(A, B, Q, R);

	Eigen::Vector4f x = Eigen::Vector4f::Zero();
	x(0) = e;
	x(1) = (e-pe)/DT;
	x(2) = th_e;
	x(3) = (th_e-pth_e)/DT;

	float ff = std::atan2((L*k), (double)1.0);
	float fb = YAW_P2P((-K * x)(0));
	float delta = ff+fb;

	pe = e;
	pth_e = th_e;
	return delta;
};

void update (State& state, float a, float delta){

    if (delta >= MAX_STEER) delta = MAX_STEER;
    if (delta <= - MAX_STEER) delta = - MAX_STEER;

    state.x = state.x + state.v * std::cos(state.yaw) * DT;
    state.y = state.y + state.v * std::sin(state.yaw) * DT;
    state.yaw = state.yaw + state.v / L * std::tan(delta) * DT;
    state.v = state.v + a * DT;

};

void closed_loop_prediction(Vec_f cx, Vec_f cy, Vec_f cyaw, Vec_f ck, Vec_f speed_profile, Poi_f goal){
	float T = 500.0;
	float goal_dis = 0.5;
	float stop_speed = 0.05;

	State state(-0.0, -0.0, 0.0, 0.0);

	float time_ = 0.0;
	Vec_f x;
	x.push_back(state.x);
	Vec_f y;
	y.push_back(state.y);
	Vec_f yaw;
	yaw.push_back(state.yaw);
	Vec_f v;
	v.push_back(state.v);
	Vec_f t;
	t.push_back(0.0);

	float e = 0;
	float e_th = 0;
	int ind = 0;

  cv::namedWindow("lqr", cv::WINDOW_NORMAL);
  int count = 0;

	cv::Mat bg(2000, 2000, CV_8UC3, cv::Scalar(255, 255, 255));
	for(unsigned int i=1; i<cx.size(); i++){
		cv::line(
			bg,
			cv_offset(cx[i-1], cy[i-1], bg.cols, bg.rows),
			cv_offset(cx[i], cy[i], bg.cols, bg.rows),
			cv::Scalar(0, 0, 0),
			10);
	}

	while (T >= time_){
		float di = lqr_steering_control(state, cx, cy, cyaw, ck, ind, e, e_th);
		float ai = KP * (speed_profile[ind]-state.v);
	  update(state, ai, di);

		if (std::abs(state.v) <= stop_speed) ind += 1;

		float dx = state.x - goal[0];
		float dy = state.y - goal[1];
		if (std::sqrt(dx*dx + dy*dy) <= goal_dis) {
			std::cout<<("Goal")<<std::endl;
			break;
		}

		cv::circle(
			bg,
			cv_offset(state.x, state.y, bg.cols, bg.rows),
			10, cv::Scalar(0, 0, 255), -1);

		//save image in build/bin/pngs
		// struct timeval tp;
		// gettimeofday(&tp, NULL);
		// long int ms = tp.tv_sec * 1000 + tp.tv_usec / 1000;
		// std::string int_count = std::to_string(ms);
		// cv::imwrite("./pngs/"+int_count+".png", bg);
		cv::imshow("lqr", bg);
		cv::waitKey(5);
	}
};

int main(){
	Vec_f wx({0.0, 6.0,  12.5, 10.0, 7.5, 3.0, -1.0});
	Vec_f wy({0.0, -3.0, -5.0,  6.5, 3.0, 5.0, -2.0});

  Spline2D csp_obj(wx, wy);
	Vec_f r_x;
	Vec_f r_y;
	Vec_f ryaw;
	Vec_f rcurvature;
	Vec_f rs;
	for(float i=0; i<csp_obj.s.back(); i+=0.1){
		std::array<float, 2> point_ = csp_obj.calc_postion(i);
		r_x.push_back(point_[0]);
		r_y.push_back(point_[1]);
		ryaw.push_back(csp_obj.calc_yaw(i));
		rcurvature.push_back(csp_obj.calc_curvature(i));
		rs.push_back(i);
	}
	float target_speed = 10.0 / 3.6;
	Vec_f speed_profile = calc_speed_profile(r_x, r_y, ryaw, target_speed);
	closed_loop_prediction(r_x, r_y, ryaw, rcurvature, speed_profile, {{wx.back(), wy.back()}});

}


#define PY_ARRAY_UNIQUE_SYMBOL phist_PyArray_API
#define NO_IMPORT_ARRAY

#include <string>
#include <cmath>

#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include <boost/array.hpp>

#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include "seglib/histogram/histogram.hxx"
#include "seglib/histogram/histogram_python.hxx"

namespace python = boost::python;

namespace histogram {

    vigra::NumpyAnyArray jointColorHistogram(

        vigra::NumpyArray<2,  vigra::TinyVector<float,3>  >    img,
        const vigra::TinyVector<float, 3> &                    min,
        const vigra::TinyVector<float, 3> &                    max,
        const vigra::TinyVector<float, 3> &                    bins,
        const size_t                                           r,
        //output
        vigra::NumpyArray<5, float >                           res = vigra::NumpyArray<5, float >()

    ){

        // allocate output
        typedef typename vigra::NumpyArray<5, float >::difference_type Shape5;
        Shape5 shape(img.shape(0),img.shape(1),int(bins[0]),int(bins[1]),int(bins[2]));
        res.reshapeIfEmpty(shape);
        std::fill(res.begin(),res.end(),0.0);
        //
        Shape5 histCoord;
        const vigra::TinyVector<uint,  3>  ones(1,1,1);
        const vigra::TinyVector<float, 2>  radius(r,r);
        const vigra::TinyVector<float, 2>  radius1(r+1,r+1);
        const vigra::TinyVector<float, 3>  fac = ( (bins-ones) / (max-min) );

        vigra::TinyVector<int,2>  start,end,c;

        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1]){

            for(int d=0;d<2;++d){
                start[d]   = std::max(int(0),            int(histCoord[d]) - int(r));
                end[d]     = std::min(int(img.shape(d)), int(histCoord[d] + (r+1) ));
            }

            for(c[0]=start[0];c[0]<end[0];++c[0])
            for(c[1]=start[1];c[1]<end[1];++c[1]){

                // get the pixel value at c
                vigra::TinyVector<float, 3>  pixelValue = img(c[0],c[1]);
                pixelValue -= min;
                pixelValue *= fac;

                // (the first two coordinates of hist coord are filled)
                histCoord[2]=int(pixelValue[0]);
                histCoord[3]=int(pixelValue[1]);
                histCoord[4]=int(pixelValue[2]);

                PHIST_ASSERT_OP(histCoord[0],<,img.shape(0));
                PHIST_ASSERT_OP(histCoord[1],<,img.shape(1));

                PHIST_ASSERT_OP(histCoord[2],<,bins[0]);
                PHIST_ASSERT_OP(histCoord[3],<,bins[1]);
                PHIST_ASSERT_OP(histCoord[4],<,bins[2]);

                // increment counter
                res(histCoord[0],histCoord[1],histCoord[2],histCoord[3],histCoord[4])+=1.0;
            }
        }

        // normalizes
        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1]){

            float sum=0;

            for(histCoord[2]=0;histCoord[2]<res.shape(2);++histCoord[2])
            for(histCoord[3]=0;histCoord[3]<res.shape(3);++histCoord[3])
            for(histCoord[4]=0;histCoord[4]<res.shape(4);++histCoord[4]){

                sum+=res(histCoord[0],histCoord[1],histCoord[2],histCoord[3],histCoord[4]);
            }
            for(histCoord[2]=0;histCoord[2]<res.shape(2);++histCoord[2])
            for(histCoord[3]=0;histCoord[3]<res.shape(3);++histCoord[3])
            for(histCoord[4]=0;histCoord[4]<res.shape(4);++histCoord[4]){

                res(histCoord[0],histCoord[1],histCoord[2],histCoord[3],histCoord[4])/=sum;
            }
        }

        return res;
    }

    vigra::NumpyAnyArray batchHistogram(
        vigra::NumpyArray<3, vigra::Multiband<float>  >   img,
        vigra::NumpyArray<1, float  >   min,
        vigra::NumpyArray<1, float  >   max,
        const size_t                    bins,
        const size_t                    r,
        //output
        vigra::NumpyArray<4, float >    res = vigra::NumpyArray<4, float >()
    ){
        const size_t nChannels=img.shape(2);
        // allocate output
        typedef typename vigra::NumpyArray<4, float >::difference_type Shape4;
        Shape4 shape(img.shape(0),img.shape(1),nChannels,bins);
        res.reshapeIfEmpty(shape);
        std::fill(res.begin(),res.end(),0.0);

        // coordinate in the res array (pixel wise histogram)
        // (x,y,c,bin)
        Shape4 histCoord;
        const vigra::TinyVector<float, 2>  radius1(r+1,r+1);
        // channel wise factor
        std::vector<float> fac(nChannels);
        for(size_t channel=0;channel<nChannels;++channel){
            fac[channel]= float(bins-1) / (max(channel)-min(channel));
        }

        vigra::TinyVector<int,2>  start,end,c;

        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1]){

            for(int d=0;d<2;++d){
                start[d]   = std::max(int(0),            int(histCoord[d]) - int(r));
                end[d]     = std::min(int(img.shape(d)), int(histCoord[d] + (r+1) ));
            }

            for(c[0]=start[0];c[0]<end[0];++c[0])
            for(c[1]=start[1];c[1]<end[1];++c[1]){

                // iterate over all channels
                for(histCoord[2]=0;histCoord[2]<nChannels;++histCoord[2] ){

                    const float value = img(c[0],c[1],histCoord[2]);

                    histCoord[3] = static_cast<int>((value - min(histCoord[2]) )*fac[histCoord[2]]);

                    /*
                    std::cout<<"\n\n” channel "<<histCoord[2]<<"\n";
                    std::cout<<"value "<< value<<"\n";
                    std::cout<<"mi " << min(histCoord[2])<<"\n";
                    std::cout<<"ma " << max(histCoord[2])<<"\n";
                    std::cout<<"fa " << fac[histCoord[2]]<<"\n";
                    */

                    PHIST_ASSERT_OP(histCoord[3],<,bins);
                    // increment hist
                    res(histCoord[0],histCoord[1],histCoord[2],histCoord[3])+=1.0;
                }
            }
        }

        // normalize

        for(histCoord[0]=0;histCoord[0]<img.shape(0);++histCoord[0])
        for(histCoord[1]=0;histCoord[1]<img.shape(1);++histCoord[1])
        for(histCoord[2]=0;histCoord[2]<img.shape(2);++histCoord[2]){

            float sum=0.0;
            for(histCoord[3]=0;histCoord[3]<bins;++histCoord[3]){
                sum+=res(histCoord[0],histCoord[1],histCoord[2],histCoord[3]);
            }
            for(histCoord[3]=0;histCoord[3]<bins;++histCoord[3]){
                res(histCoord[0],histCoord[1],histCoord[2],histCoord[3])/=sum;
            }
        }
        return res;
    }

    /*

    vigra::NumpyAnyArray labelMaskedHistogram(
        // input data (for example an rgb image with xyc order)
        vigra::NumpyArray<3, vigra::Multiband<float>  >         img,
        // input labeling for which are used as mask images
        vigra::NumpyArray<3, vigra::Multiband<vigra::UInt64> >  labelings,
        vigra::NumpyArray<1, UInt64>                            numberOfLabels,
        vigra::NumpyArray<1, float>                             weights
    ){

        const size_t numberOfInputLabelings = labelings.shape(2);

    }
    */

    void export_histogram(){

        python::def("_jointColorHistogram_",vigra::registerConverters(&jointColorHistogram),
            (
                python::arg("img"),
                python::arg("dmin"),
                python::arg("dmax"),
                python::arg("bins"),
                python::arg("r"),
                python::arg("out")=python::object()
            )
        );

        python::def("_batchHistogram_",vigra::registerConverters(&batchHistogram),
            (
                python::arg("img"),
                python::arg("dmin"),
                python::arg("dmax"),
                python::arg("bins"),
                python::arg("r"),
                python::arg("out")=python::object()
            )
        );

    }

}


#include <boost/random/cauchy_distribution.hpp>


/*
 * Copyright Nick Thompson, 2020
 * Use, modification and distribution are subject to 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)
 */

#include "math_unit_test.hpp"
#include <numeric>
#include <utility>
#include <array>
#include <boost/random/uniform_real.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/interpolators/septic_hermite.hpp>
#include <boost/math/special_functions/next.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::interpolators::septic_hermite;
using boost::math::interpolators::cardinal_septic_hermite;
using boost::math::interpolators::cardinal_septic_hermite_aos;

template<typename Real>
void test_constant()
{

    std::vector<Real> x{0,1,2,3, 9, 22, 81};
    std::vector<Real> y(x.size());
    std::vector<Real> dydx(x.size(), 0);
    std::vector<Real> d2ydx2(x.size(), 0);
    std::vector<Real> d3ydx3(x.size(), 0);
    for (auto & t : y)
    {
        t = 7;
    }

    auto sh = septic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3));

    for (Real t = 0; t <= 81; t += 0.25)
    {
        CHECK_ULP_CLOSE(Real(7), sh(t), 24);
        CHECK_ULP_CLOSE(Real(0), sh.prime(t), 24);
    }

    Real x0 = 0;
    Real dx = 1;
    y.resize(128, 7);
    dydx.resize(128, 0);
    d2ydx2.resize(128, 0);
    d3ydx3.resize(128, 0);
    auto csh = cardinal_septic_hermite(std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3), x0, dx);
    for (Real t = x0; t <= 127; t += 0.25)
    {
        CHECK_ULP_CLOSE(Real(7), csh(t), 24);
        CHECK_ULP_CLOSE(Real(0), csh.prime(t), 24);
        CHECK_ULP_CLOSE(Real(0), csh.double_prime(t), 24);
    }

    std::vector<std::array<Real, 4>> data(128);
    for (size_t i = 0; i < data.size(); ++i)
    {
        data[i][0] = 7;
        data[i][1] = 0;
        data[i][2] = 0;
        data[i][3] = 0;
    }
    auto csh_aos = cardinal_septic_hermite_aos(std::move(data), x0, dx);
    for (Real t = x0; t <= 127; t += 0.25)
    {
        CHECK_ULP_CLOSE(Real(7), csh_aos(t), 24);
        CHECK_ULP_CLOSE(Real(0), csh_aos.prime(t), 24);
        CHECK_ULP_CLOSE(Real(0), csh_aos.double_prime(t), 24);
    }

    // Now check the boundaries:
    auto [tlo, thi] = csh.domain();
    int samples = 5000;
    int i = 0;
    while (i++ < samples)
    {
        CHECK_ULP_CLOSE(Real(7), csh(tlo), 2);
        CHECK_ULP_CLOSE(Real(7), csh(thi), 2);
        CHECK_ULP_CLOSE(Real(7), csh_aos(tlo), 2);
        CHECK_ULP_CLOSE(Real(7), csh_aos(thi), 2);
        CHECK_ULP_CLOSE(Real(0), csh.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(0), csh.prime(thi), 2);
        CHECK_ULP_CLOSE(Real(0), csh_aos.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(0), csh_aos.prime(thi), 2);
        CHECK_ULP_CLOSE(Real(0), csh.double_prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(0), csh.double_prime(thi), 2);
        CHECK_ULP_CLOSE(Real(0), csh_aos.double_prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(0), csh_aos.double_prime(thi), 2);

        tlo = boost::math::nextafter(tlo, std::numeric_limits<Real>::max());
        thi = boost::math::nextafter(thi, std::numeric_limits<Real>::lowest());
    }

}

template<typename Real>
void test_linear()
{
    std::vector<Real> x{0,1,2,3,4,5,6,7,8,9};
    std::vector<Real> y = x;
    std::vector<Real> dydx(x.size(), 1);
    std::vector<Real> d2ydx2(x.size(), 0);
    std::vector<Real> d3ydx3(x.size(), 0);

    auto sh = septic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3));

    for (Real t = 0; t <= 9; t += 0.25)
    {
        CHECK_ULP_CLOSE(Real(t), sh(t), 2);
        CHECK_ULP_CLOSE(Real(1), sh.prime(t), 2);
    }

    boost::random::mt19937 rng;
    boost::random::uniform_real_distribution<Real> dis(0.5,1);
    x.resize(512);
    x[0] = dis(rng);
    Real xmin = x[0];
    for (size_t i = 1; i < x.size(); ++i)
    {
        x[i] = x[i-1] + dis(rng);
    }
    Real xmax = x.back();

    y = x;
    dydx.resize(x.size(), 1);
    d2ydx2.resize(x.size(), 0);
    d3ydx3.resize(x.size(), 0);

    sh = septic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3));

    for (Real t = xmin; t <= xmax; t += 0.125)
    {
        CHECK_ULP_CLOSE(t, sh(t), 25);
        CHECK_ULP_CLOSE(Real(1), sh.prime(t), 850);
    }

    Real x0 = 0;
    Real dx = 1;
    y.resize(10);
    dydx.resize(10, 1);
    d2ydx2.resize(10, 0);
    d3ydx3.resize(10, 0);
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = i;
    }
    auto csh = cardinal_septic_hermite(std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3), x0, dx);
    for (Real t = 0; t <= 9; t += 0.125)
    {
        CHECK_ULP_CLOSE(t, csh(t), 15);
        CHECK_ULP_CLOSE(Real(1), csh.prime(t), 15);
        CHECK_ULP_CLOSE(Real(0), csh.double_prime(t), 15);
    }

    std::vector<std::array<Real, 4>> data(10);
    for (size_t i = 0; i < data.size(); ++i)
    {
        data[i][0] = i;
        data[i][1] = 1;
        data[i][2] = 0;
        data[i][3] = 0;
    }
    auto csh_aos = cardinal_septic_hermite_aos(std::move(data), x0, dx);
    for (Real t = 0; t <= 9; t += 0.125)
    {
        CHECK_ULP_CLOSE(t, csh_aos(t), 15);
        CHECK_ULP_CLOSE(Real(1), csh_aos.prime(t), 15);
        CHECK_ULP_CLOSE(Real(0), csh_aos.double_prime(t), 15);
    }

    // Now check the boundaries:
    auto [tlo, thi] = csh.domain();
    int samples = 5000;
    int i = 0;
    while (i++ < samples)
    {
        CHECK_ULP_CLOSE(Real(tlo), csh(tlo), 2);
        CHECK_ULP_CLOSE(Real(thi), csh(thi), 8);
        CHECK_ULP_CLOSE(Real(tlo), csh_aos(tlo), 2);
        CHECK_ULP_CLOSE(Real(thi), csh_aos(thi), 8);
        CHECK_ULP_CLOSE(Real(1), csh.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(1), csh.prime(thi), 700);
        CHECK_ULP_CLOSE(Real(1), csh_aos.prime(tlo), 2);
        CHECK_ULP_CLOSE(Real(1), csh_aos.prime(thi), 700);
        CHECK_MOLLIFIED_CLOSE(Real(0), csh.double_prime(tlo), std::numeric_limits<Real>::epsilon());
        CHECK_MOLLIFIED_CLOSE(Real(0), csh.double_prime(thi), 1200*std::numeric_limits<Real>::epsilon());
        CHECK_MOLLIFIED_CLOSE(Real(0), csh_aos.double_prime(tlo), std::numeric_limits<Real>::epsilon());
        CHECK_MOLLIFIED_CLOSE(Real(0), csh_aos.double_prime(thi), 1200*std::numeric_limits<Real>::epsilon());

        tlo = boost::math::nextafter(tlo, std::numeric_limits<Real>::max());
        thi = boost::math::nextafter(thi, std::numeric_limits<Real>::lowest());
    }

}

template<typename Real>
void test_quadratic()
{
    std::vector<Real> x{0,1,2,3,4,5,6,7,8,9};
    std::vector<Real> y(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = x[i]*x[i]/2;
    }

    std::vector<Real> dydx(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        dydx[i] = x[i];
    }

    std::vector<Real> d2ydx2(x.size(), 1);
    std::vector<Real> d3ydx3(x.size(), 0);

    auto sh = septic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3));

    for (Real t = 0; t <= 9; t += 0.0078125)
    {
        CHECK_ULP_CLOSE(t*t/2, sh(t), 100);
        CHECK_ULP_CLOSE(t, sh.prime(t), 32);
    }

    boost::random::mt19937 rng;
    boost::random::uniform_real_distribution<Real> dis(0.5,1);
    x.resize(8);
    x[0] = dis(rng);
    Real xmin = x[0];
    for (size_t i = 1; i < x.size(); ++i)
    {
        x[i] = x[i-1] + dis(rng);
    }
    Real xmax = x.back();

    y.resize(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = x[i]*x[i]/2;
    }

    dydx.resize(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        dydx[i] = x[i];
    }

    d2ydx2.resize(x.size(), 1);
    d3ydx3.resize(x.size(), 0);

    sh = septic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3));

    for (Real t = xmin; t <= xmax; t += 0.125)
    {
        CHECK_ULP_CLOSE(t*t/2, sh(t), 50);
        CHECK_ULP_CLOSE(t, sh.prime(t), 300);
    }

    y.resize(10);
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = i*i/Real(2);
    }

    dydx.resize(y.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        dydx[i] = i;
    }

    d2ydx2.resize(y.size(), 1);
    d3ydx3.resize(y.size(), 0);

    Real x0 = 0;
    Real dx = 1;
    auto csh = cardinal_septic_hermite(std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3), x0, dx);
    for (Real t = x0; t <= 9; t += 0.125)
    {
        CHECK_ULP_CLOSE(t*t/2, csh(t), 24);
        CHECK_ULP_CLOSE(t, csh.prime(t), 24);
        CHECK_ULP_CLOSE(Real(1), csh.double_prime(t), 24);
    }

    std::vector<std::array<Real, 4>> data(10);
    for (size_t i = 0; i < data.size(); ++i)
    {
        data[i][0] = i*i/Real(2);
        data[i][1] = i;
        data[i][2] = 1;
        data[i][3] = 0;
    }
    auto csh_aos = cardinal_septic_hermite_aos(std::move(data), x0, dx);
    for (Real t = x0; t <= 9; t += 0.125)
    {
        CHECK_ULP_CLOSE(t*t/2, csh_aos(t), 24);
        CHECK_ULP_CLOSE(t, csh_aos.prime(t), 24);
        CHECK_ULP_CLOSE(Real(1), csh_aos.double_prime(t), 24);
    }
}

template<typename Real>
void test_cubic()
{

    std::vector<Real> x{0,1,2,3,4,5,6,7};
    Real xmax = x.back();
    std::vector<Real> y(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = x[i]*x[i]*x[i];
    }

    std::vector<Real> dydx(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        dydx[i] = 3*x[i]*x[i];
    }

    std::vector<Real> d2ydx2(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        d2ydx2[i] = 6*x[i];
    }
    std::vector<Real> d3ydx3(x.size(), 6);

    auto sh = septic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3));

    for (Real t = 0; t <= xmax; t += 0.0078125)
    {
        CHECK_ULP_CLOSE(t*t*t, sh(t), 151);
        CHECK_ULP_CLOSE(3*t*t, sh.prime(t), 151);
    }

    Real x0 = 0;
    Real dx = 1;
    y.resize(8);
    dydx.resize(8);
    d2ydx2.resize(8);
    d3ydx3.resize(8,6);
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = i*i*i;
        dydx[i] = 3*i*i;
        d2ydx2[i] = 6*i;
    }

    auto csh = cardinal_septic_hermite(std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3), x0, dx);

    for (Real t = 0; t <= xmax; t += 0.0078125)
    {
        CHECK_ULP_CLOSE(t*t*t, csh(t), 151);
        CHECK_ULP_CLOSE(3*t*t, csh.prime(t), 151);
        CHECK_ULP_CLOSE(6*t, csh.double_prime(t), 151);
    }

    std::vector<std::array<Real, 4>> data(8);
    for (size_t i = 0; i < data.size(); ++i) {
        data[i][0] = i*i*i;
        data[i][1] = 3*i*i;
        data[i][2] = 6*i;
        data[i][3] = 6;
    }

    auto csh_aos = cardinal_septic_hermite_aos(std::move(data), x0, dx);

    for (Real t = 0; t <= xmax; t += 0.0078125)
    {
        CHECK_ULP_CLOSE(t*t*t, csh_aos(t), 151);
        CHECK_ULP_CLOSE(3*t*t, csh_aos.prime(t), 151);
        CHECK_ULP_CLOSE(6*t, csh_aos.double_prime(t), 151);
    }
}

template<typename Real>
void test_quartic()
{

    std::vector<Real> x{0,1,2,3,4,5,6,7,8,9};
    Real xmax = x.back();
    std::vector<Real> y(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = x[i]*x[i]*x[i]*x[i];
    }

    std::vector<Real> dydx(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        dydx[i] = 4*x[i]*x[i]*x[i];
    }

    std::vector<Real> d2ydx2(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        d2ydx2[i] = 12*x[i]*x[i];
    }

    std::vector<Real> d3ydx3(x.size());
    for (size_t i = 0; i < y.size(); ++i)
    {
        d3ydx3[i] = 24*x[i];
    }

    auto sh = septic_hermite(std::move(x), std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3));

    for (Real t = 1; t <= xmax; t += 0.0078125) {
        CHECK_ULP_CLOSE(t*t*t*t, sh(t), 117);
        CHECK_ULP_CLOSE(4*t*t*t, sh.prime(t), 117);
    }

    y.resize(10);
    dydx.resize(10);
    d2ydx2.resize(10);
    d3ydx3.resize(10);
    for (size_t i = 0; i < y.size(); ++i)
    {
        y[i] = i*i*i*i;
        dydx[i] = 4*i*i*i;
        d2ydx2[i] = 12*i*i;
        d3ydx3[i] = 24*i;
    }

    auto csh = cardinal_septic_hermite(std::move(y), std::move(dydx), std::move(d2ydx2), std::move(d3ydx3), Real(0), Real(1));

    for (Real t = 1; t <= xmax; t += 0.0078125)
    {
        CHECK_ULP_CLOSE(t*t*t*t, csh(t), 117);
        CHECK_ULP_CLOSE(4*t*t*t, csh.prime(t), 117);
        CHECK_ULP_CLOSE(12*t*t, csh.double_prime(t), 117);
    }

    std::vector<std::array<Real, 4>> data(10);
    for (size_t i = 0; i < data.size(); ++i)
    {
        data[i][0] = i*i*i*i;
        data[i][1] = 4*i*i*i;
        data[i][2] = 12*i*i;
        data[i][3] = 24*i;
    }

    auto csh_aos = cardinal_septic_hermite_aos(std::move(data), Real(0), Real(1));
    for (Real t = 1; t <= xmax; t += 0.0078125)
    {
        CHECK_ULP_CLOSE(t*t*t*t, csh_aos(t), 117);
        CHECK_ULP_CLOSE(4*t*t*t, csh_aos.prime(t), 117);
        CHECK_ULP_CLOSE(12*t*t, csh_aos.double_prime(t), 117);
    }
}

template<typename Real>
void test_interpolation_condition()
{
    for (size_t n = 4; n < 50; ++n) {
        std::vector<Real> x(n);
        std::vector<Real> y(n);
        std::vector<Real> dydx(n);
        std::vector<Real> d2ydx2(n);
        std::vector<Real> d3ydx3(n);
        boost::random::mt19937 rd;
        boost::random::uniform_real_distribution<Real> dis(0,1);
        Real x0 = dis(rd);
        x[0] = x0;
        y[0] = dis(rd);
        for (size_t i = 1; i < n; ++i) {
            x[i] = x[i-1] + dis(rd);
            y[i] = dis(rd);
            dydx[i] = dis(rd);
            d2ydx2[i] = dis(rd);
            d3ydx3[i] = dis(rd);
        }

        auto x_copy = x;
        auto y_copy = y;
        auto dydx_copy = dydx;
        auto d2ydx2_copy = d2ydx2;
        auto d3ydx3_copy = d3ydx3;
        auto s = septic_hermite(std::move(x_copy), std::move(y_copy), std::move(dydx_copy), std::move(d2ydx2_copy), std::move(d3ydx3_copy));

        for (size_t i = 0; i < x.size(); ++i)
        {
            CHECK_ULP_CLOSE(y[i], s(x[i]), 2);
            CHECK_ULP_CLOSE(dydx[i], s.prime(x[i]), 2);
        }
    }
}

int main()
{
    test_constant<float>();
    test_linear<float>();
    test_quadratic<float>();
    test_cubic<float>();
    test_quartic<float>();
    test_interpolation_condition<float>();

    test_constant<double>();
    test_linear<double>();
    test_quadratic<double>();
    test_cubic<double>();
    test_quartic<double>();
    test_interpolation_condition<double>();

    test_constant<long double>();
    test_linear<long double>();
    test_quadratic<long double>();
    test_cubic<long double>();
    test_quartic<long double>();
    test_interpolation_condition<long double>();

#ifdef BOOST_HAS_FLOAT128
    test_constant<float128>();
    test_linear<float128>();
    test_quadratic<float128>();
    test_cubic<float128>();
    test_quartic<float128>();
    test_interpolation_condition<float128>();
#endif

    return boost::math::test::report_errors();
}


// Copyright (c) 2020-2021 Franz Alt
// This code is licensed under MIT license (see LICENSE.txt for details).

#include <libcvpg/imageproc/algorithms/hog.hpp>

#include <cmath>
#include <cstdint>
#include <cstring>
#include <exception>

#include <boost/asynchronous/algorithm/then.hpp>

namespace {

template<class image_type>
cvpg::histogram<double> calc_hog_cell(std::shared_ptr<image_type> image, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, bool l1_normalilze = true);

template<>
cvpg::histogram<double> calc_hog_cell(std::shared_ptr<cvpg::image_gray_8bit> image, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, bool l1_normalize)
{
    cvpg::histogram<std::size_t> histogram(9);

    const std::uint8_t * raw = image->data(0).get();
    const std::size_t image_width = image->width();

    std::int16_t gx = 0;
    std::int16_t gy = 0;
    double mag = 0.0;
    double angle = 0.0;

    // correct from/to-values to calculate the gradients up to the image borders
    if (from_x > 1)
    {
        --from_x;
    }
    else
    {
        from_x = 1;
    }

    if (to_x < (image->width() - 2))
    {
        ++to_x;
    }
    else
    {
        to_x = image->width() - 2;
    }

    if (from_y > 1)
    {
        --from_y;
    }
    else
    {
        from_y = 1;
    }

    if (to_y < (image->height() - 2))
    {
        ++to_y;
    }
    else
    {
        to_y = image->height() - 2;
    }

    for (std::size_t y = from_y; y <= to_y; ++y)
    {
        for (std::size_t x = from_x; x <= to_x; ++x)
        {
            // calculate the x/y gradients
            gx = static_cast<std::int16_t>(raw[y * image_width + (x + 1)]) - static_cast<std::int16_t>(raw[y * image_width + (x - 1)]);
            gy = static_cast<std::int16_t>(raw[(y + 1) * image_width + x]) - static_cast<std::int16_t>(raw[(y - 1) * image_width + x]);

            // calculate the gradient magnitude
            mag = std::sqrt(static_cast<double>(gx * gx + gy * gy));

            // calculate the absolute gradient angle
            angle = std::fabs((gx == 0) ? 0.0 : atan(static_cast<double>(gy) / static_cast<double>(gx)) * 180.0 / M_PI);

            const std::size_t bin = static_cast<std::size_t>(std::floor(angle / 20.0));

            histogram.at(bin) += static_cast<decltype(histogram)::value_type>(mag);
        }
    }

    cvpg::histogram<double> normalized_histogram(9);

    if (l1_normalize)
    {
        // normalize histogram (L1 norm)
        double sum = 0;

        for (auto const & h : histogram)
        {
            sum += std::fabs(static_cast<double>(h));
        }

        for (std::size_t i = 0; i < histogram.bins(); ++i)
        {
            normalized_histogram.at(i) = static_cast<double>(histogram.at(i)) / sum;
        }
    }
    else
    {
        // normalize histogram (L2 norm)
        double sum = 0;

        for (auto const & h : histogram)
        {
            auto h_ = std::fabs(static_cast<double>(h));
            sum += h_ * h_;
        }

        sum = std::sqrt(sum);

        for (std::size_t i = 0; i < histogram.bins(); ++i)
        {
            normalized_histogram.at(i) = std::min(1.0, static_cast<double>(histogram.at(i)) / sum);
        }
    }

    return normalized_histogram;
}

template<>
cvpg::histogram<double> calc_hog_cell(std::shared_ptr<cvpg::image_rgb_8bit> image, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, bool l1_normalize)
{
    // TODO implement me!

    return cvpg::histogram<double>(9);
}

template<class image_type>
struct hog_col_task : public boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >
{
    hog_col_task(std::shared_ptr<image_type> image, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, std::size_t cell_dimension, std::size_t sequential_cells_per_row)
        : boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >("hog_col")
        , m_image(image)
        , m_from_x(from_x)
        , m_to_x(to_x)
        , m_from_y(from_y)
        , m_to_y(to_y)
        , m_cell_dimension(cell_dimension)
        , m_sequential_cells_per_row(sequential_cells_per_row)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        const std::size_t cells = (m_to_x - m_from_x + 1) / m_cell_dimension;

        if (cells <= m_sequential_cells_per_row)
        {
            std::vector<cvpg::histogram<double> > hogs;
            hogs.reserve(cells);

            for (std::size_t i = 0; i < cells; ++i)
            {
                hogs.push_back(calc_hog_cell(m_image, m_from_x + i * m_cell_dimension, m_from_x + (i + 1) * m_cell_dimension - 1, m_from_y, m_to_y));
            }

            task_res.set_value(std::move(hogs));
        }
        else
        {
            // determine middle of x-range dependet on the cell dimension
            const std::size_t x_half = m_from_x + (cells / 2) * m_cell_dimension;

            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        auto h = std::move(std::get<1>(cont_res).get());

                        std::vector<cvpg::histogram<double> > hogs(std::move(std::get<0>(cont_res).get()));
                        hogs.insert(hogs.end(), h.begin(), h.end());

                        task_res.set_value(std::move(hogs));
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_col_task<image_type>(m_image, m_from_x, x_half - 1, m_from_y, m_to_y, m_cell_dimension, m_sequential_cells_per_row),
                hog_col_task<image_type>(m_image, x_half, m_to_x, m_from_y, m_to_y, m_cell_dimension, m_sequential_cells_per_row)
            );
        }
    }

private:
    std::shared_ptr<image_type> m_image;

    std::size_t m_from_x;
    std::size_t m_to_x;
    std::size_t m_from_y;
    std::size_t m_to_y;

    std::size_t m_cell_dimension;

    std::size_t m_sequential_cells_per_row;
};

template<class image_type>
struct hog_row_task : public boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >
{
    hog_row_task(std::shared_ptr<image_type> image, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y, std::size_t cell_dimension, std::size_t sequential_cells_per_row)
        : boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >("hog_row")
        , m_image(image)
        , m_from_x(from_x)
        , m_to_x(to_x)
        , m_from_y(from_y)
        , m_to_y(to_y)
        , m_cell_dimension(cell_dimension)
        , m_sequential_cells_per_row(sequential_cells_per_row)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        const std::size_t cells = (m_to_y - m_from_y + 1) / m_cell_dimension;

        if (cells <= 1)
        {
            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        task_res.set_value(std::move(std::get<0>(cont_res).get()));
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_col_task<image_type>(m_image, m_from_x, m_to_x, m_from_y, m_to_y, m_cell_dimension, m_sequential_cells_per_row)
            );
        }
        else
        {
            // determine middle of y-range dependet on the cell dimension
            const std::size_t y_half = m_from_y + (cells / 2) * m_cell_dimension;

            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        auto h = std::move(std::get<1>(cont_res).get());

                        std::vector<cvpg::histogram<double> > hogs(std::move(std::get<0>(cont_res).get()));
                        hogs.insert(hogs.end(), h.begin(), h.end());

                        task_res.set_value(std::move(hogs));
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_row_task<image_type>(m_image, m_from_x, m_to_x, m_from_y, y_half - 1, m_cell_dimension, m_sequential_cells_per_row),
                hog_row_task<image_type>(m_image, m_from_x, m_to_x, y_half, m_to_y, m_cell_dimension, m_sequential_cells_per_row)
            );
        }
    }

private:
    std::shared_ptr<image_type> m_image;

    std::size_t m_from_x;
    std::size_t m_to_x;
    std::size_t m_from_y;
    std::size_t m_to_y;

    std::size_t m_cell_dimension;

    std::size_t m_sequential_cells_per_row;
};

template<class image_type>
struct hog_task : public boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >
{
    hog_task(image_type image, std::size_t cell_dimension, std::size_t sequential_cells_per_row = 4)
        : boost::asynchronous::continuation_task<std::vector<cvpg::histogram<double> > >("hog")
        , m_image(std::make_shared<image_type>(std::forward<image_type>(image)))
        , m_cell_dimension(cell_dimension)
        , m_sequential_cells_per_row(sequential_cells_per_row)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        // determine the amount of rows and columns dependent on the desired cell dimension
        const std::size_t cols = m_image->width() / m_cell_dimension;
        const std::size_t rows = m_image->height() / m_cell_dimension;

        boost::asynchronous::create_callback_continuation(
            [task_res = std::move(task_res)](auto cont_res) mutable
            {
                try
                {
                    task_res.set_value(std::move(std::get<0>(cont_res).get()));
                }
                catch (...)
                {
                    task_res.set_exception(std::current_exception());
                }
            },
            hog_row_task<image_type>(m_image, 0, m_image->width() - 1, 0, m_image->height() - 1, m_cell_dimension, m_sequential_cells_per_row)
        );
    }

private:
    std::shared_ptr<image_type> m_image;

    std::size_t m_cell_dimension;

    std::size_t m_sequential_cells_per_row;
};

void paint_hog_cell(std::shared_ptr<cvpg::image_gray_8bit> image, cvpg::histogram<double> const & histogram, std::size_t from_x, std::size_t to_x, std::size_t from_y, std::size_t to_y)
{
    auto raw = image->data(0).get();
    const std::size_t image_width = image->width();

    const std::size_t half_x = from_x + (to_x - from_x) / 2;
    const std::size_t half_y = from_y + (to_y - from_y) / 2;

    auto draw_line_from_center =
        [raw, image_width, h_x = half_x, h_y = half_y](std::int32_t dx, std::int32_t dy, double increment)
        {
            // calculate start point
            const std::int32_t x_s = h_x - abs(dx);
            const std::int32_t y_s = h_y - abs(dy);

            // calculate end point
            const std::int32_t x_e = h_x + abs(dx);
            const std::int32_t y_e = h_y + abs(dy);

            if (abs(dx) > abs(dy))
            {
                // calculate the slope of the line
                const std::int32_t s = dy / dx;

                std::size_t y = y_s;

                for (std::size_t x = x_s; x <= x_e; ++x, y += s)
                {
                    std::uint8_t * r = raw + y * image_width + x;

                    *r = std::min(
                            static_cast<std::uint8_t>(255),
                            static_cast<std::uint8_t>(*r + increment * 255.0)
                         );
                }
            }
            else
            {
                // calculate the slope of the line
                const std::int32_t s = dx / dy;

                std::size_t x = x_s;

                for (std::size_t y = y_s; y <= y_e; ++y, x += s)
                {
                    std::uint8_t * r = raw + y * image_width + x;

                    *r = std::min(
                            static_cast<std::uint8_t>(255),
                            static_cast<std::uint8_t>(*r + increment * 255.0)
                         );
                }
            }
        };

    for (std::size_t i = 0; i < histogram.bins(); ++i)
    {
        auto const & h = histogram.at(i);

        if (h != 0.0)
        {
            if (i == 0)
            {
                // case: angle is at range [0..20) degree
                draw_line_from_center(0, 2, h);
            }
            else if (i == 1)
            {
                // case: angle it at range [20..40) degree
                draw_line_from_center(1, 2, h);
            }
            else if (i == 2)
            {
                // case: angle it at range [40..60) degree
                draw_line_from_center(2, 2, h);
            }
            else if (i == 3)
            {
                // case: angle it at range [60..80) degree
                draw_line_from_center(2, 1, h);
            }
            else if (i == 4)
            {
                // case: angle it at range [80..100) degree
                draw_line_from_center(2, 0, h);
            }
            else if (i == 5)
            {
                // case: angle it at range [100..120) degree
                draw_line_from_center(2, -1, h);
            }
            else if (i == 6)
            {
                // case: angle it at range [120..140) degree
                draw_line_from_center(2, -2, h);
            }
            else if (i == 7)
            {
                // case: angle it at range [140..160) degree
                draw_line_from_center(1, -2, h);
            }
            else if (i == 8)
            {
                // case: angle it at range [160..180) degree
                draw_line_from_center(0, -2, h);
            }
        }
    }
}

struct hog_image_col_task : public boost::asynchronous::continuation_task<void>
{
    hog_image_col_task(std::shared_ptr<cvpg::image_gray_8bit> image, std::shared_ptr<std::vector<cvpg::histogram<double> > > histograms, std::size_t from_cell_col, std::size_t to_cell_col, std::size_t from_cell_row, std::size_t to_cell_row, std::size_t cell_dimension)
        : boost::asynchronous::continuation_task<void>("hog_image_col")
        , m_image(std::move(image))
        , m_histograms(std::move(histograms))
        , m_from_cell_col(from_cell_col)
        , m_to_cell_col(to_cell_col)
        , m_from_cell_row(from_cell_row)
        , m_to_cell_row(to_cell_row)
        , m_cell_dimension(cell_dimension)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        const std::size_t cells = m_to_cell_col - m_from_cell_col + 1;

        if (cells <= 1)
        {
            const std::size_t cells_per_row = m_image->width() / m_cell_dimension;

            // determine histogram for current cell
            cvpg::histogram<double> const & h = m_histograms->at(m_from_cell_row * cells_per_row + m_from_cell_col);

            const std::size_t from_x = m_from_cell_col * m_cell_dimension;
            const std::size_t to_x = from_x + m_cell_dimension - 1;
            const std::size_t from_y = m_from_cell_row * m_cell_dimension;
            const std::size_t to_y = from_y + m_cell_dimension - 1;

            paint_hog_cell(m_image, h, from_x, to_x, from_y, to_y);

            task_res.set_value();
        }
        else
        {
            // determine middle of col-range
            const std::size_t col_half = m_from_cell_col + (cells / 2);

            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        std::get<0>(cont_res).get();
                        std::get<1>(cont_res).get();

                        task_res.set_value();
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_image_col_task(m_image, m_histograms, m_from_cell_col, col_half - 1, m_from_cell_row, m_to_cell_row, m_cell_dimension),
                hog_image_col_task(m_image, m_histograms, col_half, m_to_cell_col, m_from_cell_row, m_to_cell_row, m_cell_dimension)
            );
        }
    }

private:
    std::shared_ptr<cvpg::image_gray_8bit> m_image;

    std::shared_ptr<std::vector<cvpg::histogram<double> > > m_histograms;

    std::size_t m_from_cell_col;
    std::size_t m_to_cell_col;
    std::size_t m_from_cell_row;
    std::size_t m_to_cell_row;

    std::size_t m_cell_dimension;
};

struct hog_image_row_task : public boost::asynchronous::continuation_task<void>
{
    hog_image_row_task(std::shared_ptr<cvpg::image_gray_8bit> image, std::shared_ptr<std::vector<cvpg::histogram<double> > > histograms, std::size_t from_cell_col, std::size_t to_cell_col, std::size_t from_cell_row, std::size_t to_cell_row, std::size_t cell_dimension)
        : boost::asynchronous::continuation_task<void>("hog_image_row")
        , m_image(std::move(image))
        , m_histograms(std::move(histograms))
        , m_from_cell_col(from_cell_col)
        , m_to_cell_col(to_cell_col)
        , m_from_cell_row(from_cell_row)
        , m_to_cell_row(to_cell_row)
        , m_cell_dimension(cell_dimension)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        const std::size_t cells = m_to_cell_row - m_from_cell_row + 1;

        if (cells <= 1)
        {
            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        std::get<0>(cont_res).get();

                        task_res.set_value();
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_image_col_task(m_image, m_histograms, m_from_cell_col, m_to_cell_col, m_from_cell_row, m_to_cell_row, m_cell_dimension)
            );
        }
        else
        {
            // determine middle of row-range
            const std::size_t row_half = m_from_cell_row + (cells / 2);

            boost::asynchronous::create_callback_continuation(
                [task_res = std::move(task_res)](auto cont_res) mutable
                {
                    try
                    {
                        std::get<0>(cont_res).get();
                        std::get<1>(cont_res).get();

                        task_res.set_value();
                    }
                    catch (...)
                    {
                        task_res.set_exception(std::current_exception());
                    }
                },
                hog_image_row_task(m_image, m_histograms, m_from_cell_col, m_to_cell_col, m_from_cell_row, row_half - 1, m_cell_dimension),
                hog_image_row_task(m_image, m_histograms, m_from_cell_col, m_to_cell_col, row_half, m_to_cell_row, m_cell_dimension)
            );
        }
    }

private:
    std::shared_ptr<cvpg::image_gray_8bit> m_image;

    std::shared_ptr<std::vector<cvpg::histogram<double> > > m_histograms;

    std::size_t m_from_cell_col;
    std::size_t m_to_cell_col;
    std::size_t m_from_cell_row;
    std::size_t m_to_cell_row;

    std::size_t m_cell_dimension;
};

struct hog_image_task : public boost::asynchronous::continuation_task<cvpg::image_gray_8bit>
{
    hog_image_task(std::vector<cvpg::histogram<double> > histograms, std::size_t cells_per_row, std::size_t cell_dimension)
        : boost::asynchronous::continuation_task<cvpg::image_gray_8bit>("hog_image")
        , m_histograms(std::make_shared<std::vector<cvpg::histogram<double> > >(std::move(histograms)))
        , m_cells_per_row(cells_per_row)
        , m_cell_dimension(cell_dimension)
    {}

    void operator()()
    {
        auto task_res = this->this_task_result();

        const std::size_t cell_cols = m_cells_per_row;
        const std::size_t cell_rows = m_histograms->size() / cell_cols;

        // create a result image ...
        auto image = std::make_shared<cvpg::image_gray_8bit>(cell_cols * m_cell_dimension, cell_rows * m_cell_dimension);

        // ... and paint it black
        memset(image->data(0).get(), 0, image->width() * image->height());

        boost::asynchronous::create_callback_continuation(
            [task_res = std::move(task_res), image](auto cont_res) mutable
            {
                try
                {
                    std::get<0>(cont_res).get();

                    task_res.set_value(std::move(*image));
                }
                catch (...)
                {
                    task_res.set_exception(std::current_exception());
                }
            },
            hog_image_row_task(image, m_histograms, 0, cell_cols - 1, 0, cell_rows - 1, m_cell_dimension)
        );
    }

private:
    std::shared_ptr<std::vector<cvpg::histogram<double> > > m_histograms;

    std::size_t m_cells_per_row;

    std::size_t m_cell_dimension;
};

}

namespace cvpg::imageproc::algorithms {

boost::asynchronous::detail::callback_continuation<std::vector<histogram<double> > > hog(image_gray_8bit image, std::size_t cell_dimension)
{
    return boost::asynchronous::top_level_callback_continuation<std::vector<histogram<double> > >(
               hog_task(std::move(image), cell_dimension)
           );
}

boost::asynchronous::detail::callback_continuation<std::vector<histogram<double> > > hog(image_rgb_8bit image, std::size_t cell_dimension)
{
    return boost::asynchronous::top_level_callback_continuation<std::vector<histogram<double> > >(
               hog_task(std::move(image), cell_dimension)
           );
}

boost::asynchronous::detail::callback_continuation<image_gray_8bit> hog_image(std::vector<histogram<double> > histograms, std::size_t cells_per_row, std::size_t cell_dimension)
{
    return boost::asynchronous::top_level_callback_continuation<image_gray_8bit>(
               hog_image_task(std::move(histograms), cells_per_row, cell_dimension)
           );
}

boost::asynchronous::detail::callback_continuation<image_gray_8bit> hog_image(image_gray_8bit image)
{
    const std::size_t cell_dimension = 8;

    auto cells_per_row = image.width() / cell_dimension;

    return boost::asynchronous::then(
             hog(std::move(image), cell_dimension),
             [cells_per_row, cell_dimension](auto cont_res)
             {
                 return hog_image(std::move(cont_res.get()), cells_per_row, cell_dimension);
             }
           );
}

boost::asynchronous::detail::callback_continuation<image_gray_8bit> hog_image(image_rgb_8bit image)
{
    const std::size_t cell_dimension = 8;

    auto cells_per_row = image.width() / cell_dimension;

    return boost::asynchronous::then(
             hog(std::move(image), cell_dimension),
             [cells_per_row, cell_dimension](auto cont_res)
             {
                 return hog_image(std::move(cont_res.get()), cells_per_row, cell_dimension);
             }
           );
}

} // namespace cvpg::imageproc::algorithms