#include <iostream>
#include <stdio.h>

#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <ceres/ceres.h>
#include <sophus/sim3.hpp>

#include "Sim3Optimizer.h"

class PoseGraphError : public ceres::SizedCostFunction<7, 7, 7> {
public:
  PoseGraphError(const Sophus::Sim3d &Sji,
                 const Eigen::Matrix<double, 7, 7> &information)
      : Sji_(Sji) {
    Eigen::LLT<Eigen::Matrix<double, 7, 7>> llt(information);
    sqrt_information_ = llt.matrixL();
  }

  // Si means world frame in i frame, S_iw
  virtual bool Evaluate(double const *const *parameters_ptr,
                        double *residuals_ptr, double **jacobians_ptr) const {
    Eigen::Map<const Eigen::Matrix<double, 7, 1>> lie_j(*parameters_ptr);
    Eigen::Map<const Eigen::Matrix<double, 7, 1>> lie_i(*(parameters_ptr + 1));

    Sophus::Sim3d Si = Sophus::Sim3d::exp(lie_i);
    Sophus::Sim3d Sj = Sophus::Sim3d::exp(lie_j);
    Sophus::Sim3d error = Sji_ * Si * Sj.inverse();
    Eigen::Map<Eigen::Matrix<double, 7, 1>> residuals(residuals_ptr);
    residuals = error.log();

    if (jacobians_ptr) {
      Eigen::Matrix<double, 7, 7> Jacobian_i;
      Eigen::Matrix<double, 7, 7> Jacobian_j;
      Eigen::Matrix<double, 7, 7> Jr = Eigen::Matrix<double, 7, 7>::Zero();

      Jr.block<3, 3>(0, 0) = Sophus::RxSO3d::hat(residuals.tail(4));
      Jr.block<3, 3>(0, 3) = Sophus::SO3d::hat(residuals.head(3));
      Jr.block<3, 1>(0, 6) = -residuals.head(3);
      Jr.block<3, 3>(3, 3) = Sophus::SO3d::hat(residuals.block<3, 1>(3, 0));
      Eigen::Matrix<double, 7, 7> I = Eigen::Matrix<double, 7, 7>::Identity();
      Jr = sqrt_information_ * (I + 0.5 * Jr + 1.0 / 12. * (Jr * Jr));

      Jacobian_i = Jr * Sj.Adj();
      Jacobian_j = -Jacobian_i;
      int k = 0;
      for (int i = 0; i < 7; i++) {
        for (int j = 0; j < 7; ++j) {
          if (jacobians_ptr[0])
            jacobians_ptr[0][k] = Jacobian_j(i, j);
          if (jacobians_ptr[1])
            jacobians_ptr[1][k] = Jacobian_i(i, j);
          k++;
        }
      }
    }
    residuals = sqrt_information_ * residuals;
    return true;
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  static ceres::CostFunction *
  Create(const Sophus::Sim3d &Sji,
         const Eigen::Matrix<double, 7, 7> &sqrt_information) {
    return new PoseGraphError(Sji, sqrt_information);
  }

private:
  const Sophus::Sim3d Sji_;
  Eigen::Matrix<double, 7, 7> sqrt_information_;
};

Sim3Optimizer::Sim3Optimizer() {}

class CERES_EXPORT Sim3Parameterization : public ceres::LocalParameterization {
public:
  virtual ~Sim3Parameterization() {}
  virtual bool Plus(const double *x, const double *delta,
                    double *x_plus_delta) const;
  virtual bool ComputeJacobian(const double *x, double *jacobian) const;
  virtual int GlobalSize() const { return 7; }
  virtual int LocalSize() const { return 7; }
};

bool Sim3Parameterization::Plus(const double *x, const double *delta,
                                double *x_plus_delta) const {
  Eigen::Map<const Eigen::Matrix<double, 7, 1>> lie(x);
  Eigen::Map<const Eigen::Matrix<double, 7, 1>> delta_lie(delta);
  Sophus::Sim3d T = Sophus::Sim3d::exp(lie);
  Sophus::Sim3d delta_T = Sophus::Sim3d::exp(delta_lie);
  Eigen::Map<Eigen::Matrix<double, 7, 1>> x_plus_delta_lie(x_plus_delta);
  x_plus_delta_lie = (T * delta_T).log();
  return true;
}

bool Sim3Parameterization::ComputeJacobian(const double *x,
                                           double *jacobian) const {
  ceres::MatrixRef(jacobian, 7, 7) = ceres::Matrix::Identity(7, 7);
  return true;
}

bool Sim3Optimizer::optimize(int iter) {
  if (vertexes.empty() == true || edges.empty() == true)
    return false;

  ceres::Problem problem;
  for (size_t m = 0; m < edges.size(); ++m) {
    ceres::CostFunction *cost_function =
        PoseGraphError::Create(edges[m].pose, edges[m].information);
    problem.AddResidualBlock(cost_function, nullptr,
                             vertexes[edges[m].j].data(),
                             vertexes[edges[m].i].data());
  }

  for (auto &it : vertexes) {
    problem.SetParameterization(it.second.data(), new Sim3Parameterization());
  }

  problem.SetParameterBlockConstant(vertexes[70].data());

  ceres::Solver::Options options;
  options.max_num_iterations = iter;
  options.minimizer_progress_to_stdout = true;
  options.function_tolerance = 1e-16;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.FullReport() << "\n";

  return true;
}

void Sim3Optimizer::ErrorAndJacobianCalculation(
    const Sophus::Sim3d& Sji, const Eigen::Matrix<double, 7, 7>& information,
    const Eigen::Matrix<double, 7, 1>& lie_i,
    const Eigen::Matrix<double, 7, 1>& lie_j,
    Eigen::Matrix<double, 7, 1>& residuals,
    Eigen::Matrix<double, 7, 7>& Jacobian_i,
    Eigen::Matrix<double, 7, 7>& Jacobian_j) {
  const Sophus::Sim3d _Sji = Sji;
  Sophus::Sim3d Si = Sophus::Sim3d::exp(lie_i);
  Sophus::Sim3d Sj = Sophus::Sim3d::exp(lie_j);
  Sophus::Sim3d Serror = _Sji * (Si * Sj.inverse());

  Eigen::Matrix<double, 7, 1> error = Serror.log();
  Eigen::Matrix<double, 7, 7> Jr = Eigen::Matrix<double, 7, 7>::Zero();

  Jr.block<3, 3>(0, 0) = Sophus::RxSO3d::hat(error.tail(4));
  Jr.block<3, 3>(0, 3) = Sophus::SO3d::hat(error.head(3));
  Jr.block<3, 1>(0, 6) = -error.head(3);
  Jr.block<3, 3>(3, 3) = Sophus::SO3d::hat(error.block<3, 1>(3, 0));
  Eigen::Matrix<double, 7, 7> I = Eigen::Matrix<double, 7, 7>::Identity();
  Jr = information * (I + 0.5 * Jr + 1.0 / 12. * (Jr * Jr));

  Jacobian_i = Jr * Sj.Adj();
  Jacobian_j = -Jacobian_i;
  residuals = information * error;
}

double Sim3Optimizer::IterateOnce(
    Eigen::Matrix<double, Eigen::Dynamic, 1>& delta_sim) {
  int n_error = (int)edges.size();
  int n_vertex = (int)vertexes.size();
  delta_sim.resize(n_vertex * 7, 1);

  Eigen::MatrixXd Jacobian(n_error * 7, n_vertex * 7);
  Eigen::MatrixXd error(n_error * 7, 1);

  for (size_t m = 0; m < edges.size(); m++) {
    Eigen::Matrix<double, 7, 7> Jacobian_i, Jacobian_j;
    Eigen::Matrix<double, 7, 1> residuals;

    int i = vertexes_remapped[edges[m].i];
    int j = vertexes_remapped[edges[m].j];

    ErrorAndJacobianCalculation(edges[m].pose, edges[m].information,
                                vertexes[edges[m].i], vertexes[edges[m].j],
                                residuals, Jacobian_i, Jacobian_j);
    Jacobian.block<7, 7>(m * 7, i * 7) = Jacobian_i;
    Jacobian.block<7, 7>(m * 7, j * 7) = Jacobian_j;
    error.block<7, 1>(m * 7, 0) = residuals;
  }

  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A;
  Eigen::Matrix<double, Eigen::Dynamic, 1> b;
  A.resize(n_vertex * 7, n_vertex * 7);
  b.resize(n_vertex * 7, 1);

  A = Jacobian.transpose() * Jacobian;
  b = -Jacobian.transpose() * error;

  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> ldlt;//(A);
  ldlt.compute(A.sparseView());
  delta_sim = ldlt.solve(b);
  return error.norm();
}

/**
 *        S0
 *        S1
 *        ..
 *  X = [ Si ]
 *        ..
 *        Sn
 *              ..., d(error0) / d(Si), ..., d(error0)/d(Sj), ...
 *              ..., d(error1) / d(Si), ..., d(error1)/d(Sj), ...
 *                                      ...
 *  Jacobian = [..., d(errorm) / d(Si), ..., d(errorm)/d(Sj), ... ]
 *                                      ...
 *              ..., d(errorn) / d(Si), ..., d(errorn)/d(Sj), ...
 *
 */
bool Sim3Optimizer::LocalBAOptimize(int iter) {
  int index = 0;
  double last_error = -1.0;
  for (auto it = vertexes.begin(); it != vertexes.end(); it++) {
    vertexes_remapped[it->first] = index;
    inversed_vertexes_remapped[index] = it->first;
    index++;
  }

  Eigen::Matrix<double, Eigen::Dynamic, 1> delta_sim;
  for (int n = 0; n < iter; n++) {
    double error = IterateOnce(delta_sim);
    if (error > 0 && error < 1e-6) {
      LOG(INFO) << "Iteration times: " << n << " error: " << error;
      break;
    }
    if (std::abs(error - last_error) < 1e-6) {
      LOG(INFO) << "Iteration times: " << n << " error: " << error;
      break;
    }
    last_error = error;
    for (size_t i = 0; i < vertexes.size(); i++) {
      Eigen::Matrix<double, 7, 1> lie_i =
          vertexes[inversed_vertexes_remapped[i]];
      Eigen::Matrix<double, 7, 1> delta_i = delta_sim.block<7, 1>(i * 7, 0);
      Sophus::Sim3d updated =
          Sophus::Sim3d::exp(lie_i) * Sophus::Sim3d::exp(delta_i);
      vertexes[inversed_vertexes_remapped[i]] = updated.log();
    }
  }
  return true;
}


#include <NTL/ZZ_pXFactoring.h>
#include <NTL/ZZ_pEXFactoring.h>

NTL_CLIENT

int main()
{
   ZZ_p::init(to_ZZ(17));

   ZZ_pX P;
   BuildIrred(P, 10);

   ZZ_pE::init(P);

   ZZ_pEX f, g, h;

   random(f, 20);
   SetCoeff(f, 20);

   random(h, 20);

   g = MinPolyMod(h, f);

   if (deg(g) < 0) Error("bad ZZ_pEXTest (1)");
   if (CompMod(g, h, f) != 0)
      Error("bad ZZ_pEXTest (2)");

   vec_pair_ZZ_pEX_long v;

   long i;
   for (i = 0; i < 5; i++) {
      long n = RandomBnd(20)+1;
      cerr << n << " ";

      random(f, n);
      SetCoeff(f, n);

      v = CanZass(f);

      g = mul(v);
      if (f != g) cerr << "oops1\n";

      long i;
      for (i = 0; i < v.length(); i++)
         if (!DetIrredTest(v[i].a))
            Error("bad ZZ_pEXTest (3)");

   }

   cerr << "\n";

   cerr << "ZZ_pEXTest OK\n";
}


#include "../Boost_Float.hxx"

#include <El.hpp>

#include <boost/math/constants/constants.hpp>

#include <vector>

// Rescaled Laguerre
std::vector<Boost_Float> sample_points(const size_t &num_points)
{
  std::vector<Boost_Float> result;
  result.reserve(num_points);

  Boost_Float rho_crossing(3 - 2 * sqrt(Boost_Float(2))),
    constant(-boost::math::constants::pi_sqr<Boost_Float>()
                    / (64 * num_points * log(rho_crossing)));

  for(size_t k = 0; k < num_points; ++k)
    {
      result.push_back((-1 + 4 * k) * (-1 + 4 * k) * constant);
    }
  return result;
}


/**
 * @file shapeFlow.cpp
 * @brief ShapeFlow plugin for Maya
 * @section LICENSE The MIT License
 * @section  requirements:  Eigen library, Maya
 * @version 0.10
 * @date  01/Nov/2013
 * @author Shizuo KAJI
 */

#pragma comment(linker, "/export:initializePlugin /export:uninitializePlugin")

#include "StdAfx.h"

#include <maya/MFnPlugin.h>

#include <Eigen/Dense>
#include <numeric>

using namespace Eigen;
using namespace std;

class ShapeFlow : public MPxDeformerNode{
public:
    ShapeFlow()  {};
    virtual MStatus deform( MDataBlock& data, MItGeometry& itGeo, const MMatrix &localToWorldMatrix, unsigned int mIndex );
    static void*   creator();
    static MStatus initialize();
	static MTypeId id;
    static MString nodeName;
    static MObject aActive;
	static MObject aStartShape;
	static MObject aSlider;
	static MObject aDeltaTime;   // time step interval
	static MObject aShapeMatchingWeight;       // weight of "normal" shape matching
    int num; // number of points in the end shape
    MatrixXd current;  // current shape
private:
    Matrix3d rotationPart(const Matrix3d m);
};

MTypeId ShapeFlow::id( 0x00000010 );
MString ShapeFlow::nodeName( "shapeFlow" );
MObject ShapeFlow::aStartShape;
MObject ShapeFlow::aActive;
MObject ShapeFlow::aSlider;
MObject ShapeFlow::aDeltaTime;
MObject ShapeFlow::aShapeMatchingWeight;

void* ShapeFlow::creator() { return new ShapeFlow; }

// main
MStatus ShapeFlow::deform( MDataBlock& data, MItGeometry& itGeo, const MMatrix &localToWorldMatrix, unsigned int mIndex ) {
    MStatus status;
//    MThreadUtils::syncNumOpenMPThreads();    // for OpenMP

    // read end shape
    MObject oStartShape = data.inputValue( aStartShape ).asMesh();
    if ( oStartShape.isNull() )    {
        return MS::kSuccess;
    }
    MFnMesh fnEndShape( oStartShape, &status );
    CHECK_MSTATUS_AND_RETURN_IT( status );
    MPointArray endPoints, pts;
    fnEndShape.getPoints( endPoints );
    itGeo.allPositions(pts);
	MDataHandle hDeltaTime = data.inputValue( aDeltaTime );
    bool active = data.inputValue( aActive ).asBool();
	float delta = hDeltaTime.asFloat();
	float sm_weight = data.inputValue( aShapeMatchingWeight ).asFloat();
    int num = pts.length();
    // dummy attribute to force deform to be called
    MDataHandle hSlider = data.inputValue(aSlider);
    // if first time, load target shape
    if ( !active ){
        current = MatrixXd(3,num);
        for (int i = 0; i < num; i++) {
            current(0,i) = pts[i].x;
            current(1,i) = pts[i].y;
            current(2,i) = pts[i].z;
        }
        return MS::kSuccess;
    }
    // number of current and end points must be equal
    if (endPoints.length() != num) {
        return MS::kSuccess;
    }
    // load end shape
    MatrixXd end(3,num);
    for (int i = 0; i < num; i++) {
        end(0,i) = endPoints[i].x;
        end(1,i) = endPoints[i].y;
        end(2,i) = endPoints[i].z;
    }
    // compute next step
	MatrixXd Diff(3, num), Grad(3, num);
    Vector3d current_center = current.rowwise().mean();
    Vector3d end_center = end.rowwise().mean();

    // prepare moment matrix
    current.colwise() -= current_center;
    end.colwise() -= end_center;

	Matrix3d A, B, AB, ABB;
	A = current * end.transpose();
	B = (end * end.transpose()).inverse();
    AB = A * B;    // moment matrix ( minimizer of |AB P - Q|
    ABB = AB * B;
    Diff = rotationPart(AB) * end - current;

    // compute gradient
	Grad = end.norm() * current.norm() * ABB * (A.transpose() * ABB - Matrix3d::Identity()) * end;
    // update current position
    current += sm_weight * delta * Diff - delta * Grad;
    current.colwise() += current_center;
    /** FOR DEBUG: compute the energy
    * Matrix3d C =  B * A.transpose() * A * B;
    * float   energy = (C * C).trace() - 2 * C.trace() + 3;
    */
    // update points
    for (int i = 0; i < num; i++) {
        pts[i].x = current(0,i);
        pts[i].y = current(1,i);
        pts[i].z = current(2,i);
    }
    itGeo.setAllPositions(pts);

	return MS::kSuccess;
}

// Polar decomposition
Matrix3d ShapeFlow::rotationPart(const Matrix3d m){
    Matrix3d A= m*m.transpose();
	SelfAdjointEigenSolver<Matrix3d> eigensolver;
	eigensolver.computeDirect(A);
    Vector3d s = eigensolver.eigenvalues();
    Matrix3d U = Matrix3d(eigensolver.eigenvectors());
    s << sqrtf(s[0]), sqrtf(s[1]), sqrtf(s[2]);
    DiagonalMatrix<double,3> D(1.0f/s[0], 1.0f/s[1], 1.0f/s[2]);
    return m * U*D*U.transpose();
}

// setup attributes
MStatus ShapeFlow::initialize() {
    MFnTypedAttribute tAttr;
	MFnNumericAttribute nAttr;

	aStartShape = tAttr.create( "startShape", "ss", MFnData::kMesh );
    addAttribute( aStartShape );
    attributeAffects( aStartShape, outputGeom );
	aSlider = nAttr.create( "slider", "slider", MFnNumericData::kFloat, 0.0 );
    addAttribute( aSlider );
    attributeAffects( aSlider, outputGeom );
	aActive = nAttr.create( "active", "active", MFnNumericData::kBoolean, 0 );
    addAttribute( aActive );
    attributeAffects( aActive, outputGeom );
	aDeltaTime = nAttr.create( "delta", "delta", MFnNumericData::kFloat, 0.01 );
    addAttribute( aDeltaTime );
	aShapeMatchingWeight = nAttr.create( "shapeMatching", "smw", MFnNumericData::kFloat, 5.0 );
    addAttribute( aShapeMatchingWeight );

	return MS::kSuccess;
}

// (un)init plugin
MStatus initializePlugin( MObject obj ) {
    MStatus status;
    MFnPlugin plugin( obj, "CREST", "0.1", "Any");
    status = plugin.registerNode( ShapeFlow::nodeName, ShapeFlow::id, ShapeFlow::creator, ShapeFlow::initialize, MPxNode::kDeformerNode );
    CHECK_MSTATUS_AND_RETURN_IT( status );
    return status;
}
MStatus uninitializePlugin( MObject obj ) {
    MStatus   status;
    MFnPlugin plugin( obj );
    status = plugin.deregisterNode( ShapeFlow::id );
    CHECK_MSTATUS_AND_RETURN_IT( status );
    return status;
}


#include <cstdlib>
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/eigen/matrix.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<double> vector;
    typedef ublas::matrix<double, ublas::column_major> matrix;
    typedef vector::size_type size_type;
    rand_normal<double>::reset();
    size_type n=8;
    matrix A(n, n), A_u(n, n), A_l(n, n);
    for (size_type j=0; j<n; ++j) {
      A(j, j)=rand_normal<double>::get();
      A_u(j, j)=A(j, j);
      A_l(j, j)=A(j, j);
      for (size_type i=0; i<j; ++i) {
	A(i, j)=rand_normal<double>::get();
	A(j, i)=A(i, j);
	A_u(i, j)=A(i, j);
	A_u(j, i)=0;
	A_l(i, j)=0;
	A_l(j, i)=A(j, i);
      }
    }
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<double>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<double>::get();
    double alpha(rand_normal<double>::get());
    double beta(rand_normal<double>::get());
    vector y1(alpha*ublas::prod(A, x)+beta*y);
    vector y2(y);
    blas::symv(alpha, blas::lower(A_l), x, beta, y2);
    vector y3(y);
    blas::symv(alpha, blas::upper(A_u), x, beta, y3);
    vector y4(y);
    blas::hemv(alpha, blas::lower(A_l), x, beta, y4);
    vector y5(y);
    blas::hemv(alpha, blas::upper(A_u), x, beta, y5);
    std::cout << "testing boost::ublas containers\n"
    	      << "using ublas            : " << print_vec(y1) << '\n'
    	      << "using blas symv (lower): " << print_vec(y2) << '\n'
    	      << "using blas symv (upper): " << print_vec(y3) << '\n'
    	      << "using blas hemv (lower): " << print_vec(y4) << '\n'
    	      << "using blas hemv (upper): " << print_vec(y5) << '\n'
    	      << '\n';
  }
  {
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<double>::reset();
    size_type n=8;
    matrix A(n, n), A_u(n, n), A_l(n, n);
    for (size_type j=0; j<n; ++j) {
      A(j, j)=rand_normal<double>::get();
       A_u(j, j)=A(j, j);
      A_l(j, j)=A(j, j);
      for (size_type i=0; i<j; ++i) {
    	A(i, j)=rand_normal<double>::get();
    	A(j, i)=A(i, j);
    	A_u(i, j)=A(i, j);
    	A_u(j, i)=0;
    	A_l(i, j)=0;
    	A_l(j, i)=A(j, i);
      }
    }
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<double>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<double>::get();
    double alpha(rand_normal<double>::get());
    double beta(rand_normal<double>::get());
    vector y1(alpha*A*x+beta*y);
    vector y2(y);
    blas::symv(alpha, blas::lower(A_l), x, beta, y2);
    vector y3(y);
    blas::symv(alpha, blas::upper(A_u), x, beta, y3);
    vector y4(y);
    blas::hemv(alpha, blas::lower(A_l), x, beta, y4);
    vector y5(y);
    blas::hemv(alpha, blas::upper(A_u), x, beta, y5);
    std::cout << "testing Eigen containers\n"
    	      << "using ublas            : " << print_vec(y1) << '\n'
    	      << "using blas symv (lower): " << print_vec(y2) << '\n'
    	      << "using blas symv (upper): " << print_vec(y3) << '\n'
    	      << "using blas hemv (lower): " << print_vec(y4) << '\n'
    	      << "using blas hemv (upper): " << print_vec(y5) << '\n'
    	      << '\n';
  }
  return EXIT_SUCCESS;
}


#ifndef TESTS_HPP_
#define TESTS_HPP_

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/algebra/array_algebra.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <algorithm>
#include <random>
#include <utility>
#include <vector>

#include "se3.hpp"

/**
 * Return a constant vector of size N
 *
 * f: R -> R^N
 */
template<std::size_t _N>
struct Constant
{
  static constexpr char name[] = "Constant";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = 1;

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    return Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1>::Ones(x.size());
  }
};

/**
 * Apply a series of coefficient-wise operations and sum the result
 *
 * f: R^N -> R
 */
template<std::size_t _N>
struct ManyToOne
{
  static constexpr char name[] = "ManyToOne";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = N;

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, 1, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    return Eigen::Matrix<typename Derived::Scalar, 1, 1>(
      (x + x.cwiseInverse()).array().sin().matrix().sum()
    );
  }
};

/**
 * Compute series x_{y+2} = sin(cos(x_y))
 *
 * f: R -> R^N
 */
template<std::size_t _N>
struct OneToMany
{
  static constexpr char name[] = "OneToMany";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = 1;

  template<typename Derived>
  Eigen::Matrix<
    typename Derived::Scalar,
    Derived::RowsAtCompileTime == -1 ? -1 : static_cast<int>(N),
    1
  >
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    using std::sin, std::cos;

    using Scalar = typename Derived::Scalar;
    static constexpr int ValuesAtCompileTime =
      Derived::RowsAtCompileTime == -1 ? -1 : static_cast<int>(N);
    Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ret(N);
    ret(0) = x(0);
    for (std::size_t i = 0; i < N - 1; ++i) {
      if (i % 2 == 0) {
        ret(i + 1) = sin(ret(i));
      } else {
        ret(i + 1) = cos(ret(i));
      }
    }
    return ret;
  }
};

/**
 * Integrate an N-order integrator for 100 steps using a RK4 scheme
 *
 * f: R^N -> R^N
 */
template<std::size_t _N>
struct ODE
{
  static constexpr char name[] = "ODE";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = N;

  ODE()
  {
    // matrix with ones on super-diagonal
    A_.setZero();
    A_.template block(0, 1, N - 1, N - 1).diagonal().setOnes();
  }

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    using scalar_t = typename Derived::Scalar;
    using state_t = Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1>;

    auto x0 = x.eval();
    const auto Ac = A_.template cast<scalar_t>().eval();

    boost::numeric::odeint::integrate_n_steps(
      boost::numeric::odeint::runge_kutta4<
        state_t, scalar_t, state_t, scalar_t, boost::numeric::odeint::vector_space_algebra
      >{},
      [&Ac](const state_t & x, state_t & dxdt, const scalar_t) {
        dxdt = Ac * x;
      },
      x0, scalar_t{0.}, scalar_t{0.01}, 100
    );

    return x0;
  }

private:
  Eigen::Matrix<double, N, N> A_;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

/**
 * Three-layer fully connected neural network with one channel and tanh activations
 *
 * Weights are statically allocated
 *
 * f: R^N -> R
 */
template<std::size_t _N>
struct NeuralNet
{
  static constexpr char name[] = "NeuralNet";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = N;

  NeuralNet()
  {
    std::minstd_rand gen(101);  // fixed seed
    std::normal_distribution<double> dis(0, 1);
    auto gen_fcn = [&]() {return dis(gen);};

    W1 = Eigen::Matrix<double, n1, n0>::NullaryExpr(gen_fcn);
    W2 = Eigen::Matrix<double, n2, n1>::NullaryExpr(gen_fcn);
    W3 = Eigen::Matrix<double, n3, n2>::NullaryExpr(gen_fcn);
  }

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, 1, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    const auto z1 = (W1.template cast<typename Derived::Scalar>() * x.normalized()).eval();
    const auto a1 = z1.array().tanh().matrix().eval();

    const auto z2 = (W2.template cast<typename Derived::Scalar>() * a1).eval();
    const auto a2 = z2.array().tanh().matrix().eval();

    const auto z3 = (W3.template cast<typename Derived::Scalar>() * a2).eval();
    const auto a3 = z3.array().tanh().matrix().eval();

    return Eigen::Matrix<typename Derived::Scalar, 1, 1>(
      (a3 - Eigen::Matrix<typename Derived::Scalar, n3, 1>::Ones()).squaredNorm()
    );
  }

private:
  static constexpr std::size_t n0 = N;
  static constexpr std::size_t n1 = std::max<int>(1, n0 / 2);
  static constexpr std::size_t n2 = std::max<int>(1, n1 / 2);
  static constexpr std::size_t n3 = std::max<int>(1, n2 / 2);

  Eigen::Matrix<double, n1, n0> W1;
  Eigen::Matrix<double, n2, n1> W2;
  Eigen::Matrix<double, n3, n2> W3;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

/**
 * Camera reprojection error for N points
 *
 * f: R^6 -> R
 *
 * f(x) = \sum_i ( x_C_i - proj(CM * (P_CW * exp(x)) * x_W_i) ) .^ 2
 *
 * where - x_C_i the i:th 2d pixel point
 *       - x_W_i the i:th 3d world point
 *       - P_CW a nominal camera pose
 *       - x is a tangent space element defining an incremental pose
 */
template<std::size_t _N>
struct ReprojectionError
{
  static constexpr char name[] = "Reprojection";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = 6;

  ReprojectionError()
  {
    // nominal pose
    P_CW_nom = SE3<double>{
      Eigen::Quaterniond::Identity(),
      Eigen::Vector3d{0.1, -0.3, 0.2}
    };

    // camera matrix
    CM.setZero();
    CM(0, 0) = 700;  // fx
    CM(1, 1) = 690;  // fy
    CM(0, 2) = 320;  // cx
    CM(1, 2) = 240;  // cy
    CM(2, 2) = 1;

    // generate random data
    std::minstd_rand gen(101);  // fixed seed
    std::normal_distribution<double> dis(0, 1);
    auto gen_fcn = [&]() {return dis(gen);};

    for (std::size_t i = 0; i != N; ++i) {
      pts_world[i] = Eigen::Vector3d{0, 0, 3} + Eigen::Vector3d::NullaryExpr(gen_fcn);
      Eigen::Vector3d proj = CM * (P_CW_nom * pts_world[i]);
      pts_image[i] = proj.template head<2>() / proj(2);
    }
  }

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, 1, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    using Scalar = typename Derived::Scalar;
    using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
    using Quat = Eigen::Quaternion<Scalar>;

    SE3<Scalar> P_CW = SE3<Scalar>::exp(Scalar{0.01} * x) * P_CW_nom.template cast<Scalar>();
    auto CMc = CM.template cast<Scalar>().eval();

    // Transform world points to camera frame, re-project, square
    Eigen::Matrix<Scalar, 1, 1> ret(0);
    for (std::size_t i = 0; i != N; ++i) {
      Vec3 proj = CMc * (P_CW * pts_world[i].template cast<Scalar>().eval());
      ret(0) +=
        (proj.template head<2>() / proj(2) - pts_image[i].template cast<Scalar>()).squaredNorm();
    }
    return ret;
  }

private:
  Eigen::Matrix<double, 3, 3> CM;            // camera matrix
  SE3<double> P_CW_nom{};                    // nominal camera pose
  std::array<Eigen::Vector3d, N> pts_world;  // points in world frame
  std::array<Eigen::Vector2d, N> pts_image;  // points in image plane

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};

/**
 * Differentiate the end effector position in an N link robotic arm
 *
 * f: R^6 -> R^6
 */
template<std::size_t _N>
struct Manipulator
{
  static constexpr char name[] = "Manipulator";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = 6;

  Manipulator()
  {
    // generate random link positions
    std::minstd_rand gen(101);  // fixed seed
    std::uniform_real_distribution<double> dis(-1, 1);
    auto gen_fcn = [&]() {return dis(gen);};

    for (std::size_t i = 0; i != N; ++i) {
      link_pose[i] = SE3<double>{
        Eigen::AngleAxis(M_PI_2 * dis(gen), Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxis(M_PI_2 * dis(gen), Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxis(M_PI_2 * dis(gen), Eigen::Vector3d::UnitZ()),
        Eigen::Vector3d{2 * dis(gen), 2 * dis(gen), 2 * dis(gen)}
      };
    }
  }

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, 3, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    using Scalar = typename Derived::Scalar;
    SE3<Scalar> P = SE3<Scalar>::exp(x);

    for (std::size_t i = 0; i != N; ++i) {
      P *= link_pose[i].template cast<Scalar>();
    }

    return P * Eigen::Matrix<Scalar, 3, 1>::UnitX().eval();
  }

private:
  std::array<SE3<double>, N> link_pose;
};

/**
 * Integrate a system on SE(3) for N steps using the RK4 scheme
 *
 * f: R^6 -> R^6
 */
template<std::size_t _N>
struct SE3ODE
{
  static constexpr char name[] = "SE3ODE";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = 6;

  SE3ODE()
  {
    velocity << 0.1, -0.2, 0.3, 0.1, -0.2, 0.3;
    Pfinal = SE3<double>{} * SE3<double>::exp(static_cast<double>(N) * 0.01 * velocity);
  }

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, 6, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    using scalar_t = typename Derived::Scalar;
    using state_t = SE3<scalar_t>;
    using deriv_t = typename state_t::Tangent;

    const auto vel_c = velocity.template cast<scalar_t>().eval();

    // set initial pose
    SE3<scalar_t> P = SE3<scalar_t>::exp(x);

    boost::numeric::odeint::integrate_n_steps(
      boost::numeric::odeint::runge_kutta4<state_t, scalar_t, deriv_t, scalar_t,
      boost::numeric::odeint::vector_space_algebra, lie_operations>{},
      [&vel_c](const state_t & X, deriv_t & dXdt, const scalar_t) {
        dXdt = vel_c;
      },
      P, scalar_t{0.}, scalar_t{0.01}, N
    );

    const SE3<scalar_t> Pfinalinv = Pfinal.inv().template cast<scalar_t>();
    return (Pfinalinv * P).log();
  }

private:
  Eigen::Matrix<double, 6, 1> velocity;
  SE3<double> Pfinal{};

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

#endif  // TESTS_HPP_


#pragma once

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <cudd/cplusplus/cuddObj.hh>
#include <vector>

#include "number_representation.hpp"

namespace abo::error_metrics {

/**
 * @brief Computes bounds on the average relative difference between f and f_hat
 * It is defined as the average of |f(x) - f_hat(x)| / max(1, |f(x)|) for all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * This function returns a range in which the actual average relative error is guaranteed to lie
 * The computed bounds are always within a factor of 2, meaning that the maximum error
 * returned by this function is at most twice the minimum error
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return {min, max}, the lower and upper bound on the average case relative error
 */
std::pair<boost::multiprecision::cpp_dec_float_100,
            boost::multiprecision::cpp_dec_float_100>
    acre_bounds(const Cudd& mgr, const std::vector<BDD>& f,
                  const std::vector<BDD>& f_hat,
                  const abo::util::NumberRepresentation num_rep
                        = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the average relative difference between f and f_hat
 * It is defined as the average of |f(x) - f_hat(x)| / max(1, |f(x)|) for all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with ADDs and might be quite slow
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return the average relative difference of the inputs
 */
boost::multiprecision::cpp_dec_float_100
    acre_add(const Cudd& mgr, const std::vector<BDD>& f,
           const std::vector<BDD>& f_hat,
           const abo::util::NumberRepresentation num_rep
                = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the average relative difference between f and f_hat
 * It is defined as the average of |f(x) - f_hat(x)| / max(1, |f(x)|) for all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with BDDs using a symbolic division and might be quite slow
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_extra_bits The number of additional fixed precision bits to use during the division
 * As the result of each division is not an integer, the result is described as a fixed point number
 * with exactly num_extra_bits bits with a lower significance than one. Roughly correlates the the
 * precision of the result
 * @param num_rep The number representation for f and f_hat
 * @return the average relative difference of the inputs
 */
boost::multiprecision::cpp_dec_float_100 acre_symbolic_division(
    const Cudd& mgr, const std::vector<BDD>& f, const std::vector<BDD>& f_hat,
    unsigned int num_extra_bits = 16,
    const abo::util::NumberRepresentation num_rep
        = abo::util::NumberRepresentation::BaseTwo);
} // namespace abo::error_metrics


#include <catch2/catch.hpp>

#include <Eigen/Dense>

#include <cannon/physics/euler_integrator.hpp>
#include <cannon/log/registry.hpp>

using namespace Eigen;

using namespace cannon::physics;
using namespace cannon::log;

void linear_system(const VectorXd& x, VectorXd& dxdt, double /*t*/) {
  static MatrixXd A(2, 2);
  A << -1.0, 0.0,
       0.0, -1.0;

  dxdt = A * x;
}

TEST_CASE("EulerIntegrator", "[physics]") {
  auto e = make_euler_integrator(linear_system, 2, 0.01);
  VectorXd s(2);
  s << 1.0, 0.0;
  e.set_state(s);

  for (int i = 0; i < 1000; i++) {
    e.step();
    log_info("State is now", e.get_state(), "at time", e.get_time());
  }

  REQUIRE(e.get_state().norm() < 0.001);
}


/**
* @file distance.cpp
* @author Shivam Akhauri (Driver),Toyas Dhake (Navigator)
* @date 19 October 2019
* @copyright 2019 Toyas Dhake, Shivam Akhauri
* @brief This is a class for depth perception module based on image from single
* camera.
*/

#include <dlib/opencv.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_io.h>
#include <iostream>
#include <distance.hpp>
#include <face.hpp>
#include <opencv2/highgui/highgui.hpp>

/**
* @brief Function to calculate focal length of camera from a
* reference iamge.
* @return Focal length of camera calculated
*/
double CalculateDistance::calculateFocalLength() {
    // constructor call for the dlib face detector
    dlib::frontal_face_detector detector = dlib::get_frontal_face_detector();
    dlib::matrix<dlib::bgr_pixel>  referenceImg;
    // load the refernce image
    dlib::load_image(referenceImg, "../app/reference.jpg");
    // Detect faces
    std::vector<dlib::rectangle> faces = detector(referenceImg);
    // find the width of the detected face in pixels
    double refWidth = 0;
    for (auto&& face : faces) {
        refWidth = face.r - face.l;
    }
    // calculate the focal length from the refernce frame
    double focalLength = (refWidth * knownDistance) / knownWidth;
    return focalLength;
}

/**
* @brief Constructor to set the width of human face and find focal length from reference image.
* @params _width Width of head
* @params _focalLength Focal length of camera
*/
CalculateDistance::CalculateDistance() {
    focalLength = calculateFocalLength();
}

/**
* @brief This is the function which computes the distance of each human from the camera
* @params image Image captured by camera, dlib face detector object
* @return vector of Distances for each face in the frame
*/
std::vector<Face> CalculateDistance::getDistance(cv::Mat image,
                                    dlib::frontal_face_detector detector) {
    std::vector<Face> facesWithDistance;
    dlib::cv_image<dlib::bgr_pixel> cimg(image);
    std::vector<dlib::rectangle> faces = detector(cimg);
    // for each detected face calculate the distance
    for (auto&& face : faces) {
        // calculate width for each face
        double width = face.r - face.l;
        // calculate the distance for each face
        realTimeDistance = calDist(width, focalLength);
        Face faceWithDistance(face.l, face.t, face.r, face.b, realTimeDistance);
        // append the distance of each face in a vecor
        facesWithDistance.emplace_back(faceWithDistance);
    }
    return facesWithDistance;
}

/**
* @brief This function contains the formula for distance calculation
* @params width of the detected face, focalLength of the camera
* @return double value of the calculated distance
*/
double CalculateDistance::calDist(double width, double focalLength) {
    // formula for distance calculation
    double distInches = (focalLength*knownWidth) / width;
    // convert inches measurement to meters
    double distMetres = distInches * 0.0254;
    return distMetres;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// With contributions from Cornelius Steinhardt

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/operation/secular.hpp>
#include <boost/numeric/mtl/operation/sort.hpp>

using namespace std;

int main(int , char**)
{
    using namespace mtl;
    typedef dense_vector<double>  Vector;
    Vector                    lambda(2, 0.0), z(2), d(2);
    z[0]=1; z[1]=1;
    d[0]=-5; d[1]=-1;

    //lambda= mtl::secular_f<Vector>(lambda, z, d, 5.0).f(3.0);

    mtl::vec::secular_f<Vector>   ss(z, d, 5.0);
    std::cout<<"lambda  =" << ss.f(3.0) <<"\n";
    std::cout<<"lambda  =" << ss.f(0.0) <<"\n";
    std::cout<<"lambda  =" << ss.f(-3.0) <<"\n";
    std::cout<<"lambda  =" << ss.f(13.0) <<"\n";
    std::cout<<"lambda  =" << ss.f(113.0) <<"\n";

    std::cout<<"lambda  =" << ss.grad_f(13.0) <<"\n";
    std::cout<<"lambda  =" << ss.grad_f(113.0) <<"\n";
    std::cout<<"roots  =" << secular(z, d, 5.0) <<"\n";
    //std::cout<<"lambda  =" << lambda <<"\n";

    Vector x(5, 0.0);
    for(int i = 0; i < 5; i++)
	x[i]=5-i;
    x[1]=1;
    std::cout<< "\n x=" << x << "\n";
    sort(x);
    std::cout<< "x=" << x << "\n";
    MTL_THROW_IF(x[0] != 1.0, mtl::runtime_error("Error in sorting."));

    return 0;
}


#include <boost/test/unit_test.hpp>

#include "../math/math.h"

USING_NAMESPACE(sway)

BOOST_AUTO_TEST_SUITE(TVector4TestSuite)

/*!
 * Убеждаемся, что конструктор по умолчанию приводит все компоненты к нулю.
 */
BOOST_AUTO_TEST_CASE(TVector4TestCase_DefaultConstructor) {
	const math::TVector4<s32> vec4;

	BOOST_CHECK_EQUAL(vec4.getX(), 0);
	BOOST_CHECK_EQUAL(vec4.getY(), 0);
	BOOST_CHECK_EQUAL(vec4.getZ(), 0);
	BOOST_CHECK_EQUAL(vec4.getW(), 0);
}

/*!
 * Убеждаемся, что конструктор устанавливает все значения компонентов в те,
 * которые были заданы.
 */
BOOST_AUTO_TEST_CASE(TVector4TestCase_ComponentConstructor) {
	const s32 x = 1, y = 2, z = 3, w = 4;
	const math::TVector4<s32> vec4(x, y, z, w);

	BOOST_CHECK_EQUAL(vec4.getX(), x);
	BOOST_CHECK_EQUAL(vec4.getY(), y);
	BOOST_CHECK_EQUAL(vec4.getZ(), z);
	BOOST_CHECK_EQUAL(vec4.getW(), w);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <cstdlib>
#include <iostream>
#include <sstream>
#include <cassert>
#include <cmath>
#include <limits>
#include <type_traits>
#include <utility>
#include <vector>
#include <unordered_map>
#include <string_view>
#include <atomic>

#include <fmt/core.h>
#include <fmt/ostream.h>

#include <boost/functional/hash.hpp>
//#include <boost/align/aligned_allocator.hpp>

#ifdef __GNUC__
#define unlikely(x) __builtin_expect(!!(x), 0)
#define likely(x) __builtin_expect(!!(x), 1)
#else
#define unlikely(x) x
#define likely(x) x
#endif

namespace util
{

template<class T>
inline T Sqr(const T &x)
{
  return x*x;
}

template<class T>
inline T Cubed(const T &x)
{
  return x*x*x;
}

template<class T>
inline T Heaviside(const T &x)
{
  return x>T{} ? T{1} : T{0};
}

inline double Rcp(double x)
{
  return 1./x;
}

inline float Rcp(float x)
{
  return 1.f/x;
}

inline constexpr double Pow(double x, std::uint32_t e)
{
  // This is the binary exponentiation algorithm
  // https://de.wikipedia.org/wiki/Bin%C3%A4re_Exponentiation
  std::uint32_t bit = 1<< (sizeof(e)*8-1);
  double ret = 1.;
  while (bit)
  {
    ret = ret*ret;
    ret = (e&bit) ? ret*x : ret;
    bit >>= 1;
  }
  return ret;
}

template<class T>
inline constexpr T Modulus(T a, T m, std::enable_if_t<std::is_integral_v<T> && std::is_signed_v<T>>* = nullptr)
{
  //  Example for operator%: -5 % 3 = -2
  //  Should be one though. Can simply add m if result is negative.
  const T tmp = a % m;
  return tmp < 0 ? tmp + m : tmp;
}

template<class T>
inline constexpr T Modulus(T a, T m, std::enable_if_t<std::is_integral_v<T> && std::is_unsigned_v<T>>* = nullptr)
{
  return a % m;
}

template<class T>
inline constexpr T Modulus(T a, T m, std::enable_if_t<std::is_floating_point_v<T>>* = nullptr)
{
  //  Example for operator%: -5 % 3 = -2
  //  Should be one though. Can simply add m if result is negative.
  const T tmp = std::fmod(a, m);
  return tmp < 0 ? tmp + m : tmp;
}

// t = 0: Returns a
// t = 1: Returns b
// otherwise linear inter/extra-polation
template<class T, class U>
inline T Lerp(const T &a, const T &b, U t)
{
  return (std::remove_reference_t<U>(1) - t) * a + t * b;
}

// Note: Will happily take the signbit from zero. So the result for 0 is basically random.
template<class T, typename std::enable_if_t<std::is_floating_point<T>{}, int> = 0>
inline T Sign(const T &x)
{
  return std::copysign(T(1.), x);
}

// Also from PBRT. Used to compute error bounds for floating point arithmetic. See pg. 216.
template<class T>
inline constexpr T Gamma(int n) {
    constexpr T eps_half = std::numeric_limits<T>::epsilon();
    return (n * eps_half) / (1 - n * eps_half);
}

template<class T>
inline bool Quadratic(T a, T b, T c, T &t0, T &t1)
{
  //from PBRT pg. 1080
  const T d = b*b - T(4)*a*c;
  if (d < T(0))
    return false;
  const T sd = std::sqrt(d);
  const T q = b<0 ? -b+sd : -b-sd;
  t0 = q/T(2)/a;
  t1 = T(2)*c/q;
  if (t0 > t1)
    std::swap(t0 ,t1);
  return true;
}

namespace quadratic_internal
{

template<class T>
inline T Errorformula1(T A, T B, T C, T D, T sD, T eA, T eB, T eC)
{
  constexpr T eps = std::numeric_limits<T>::epsilon();
  const T xi = B<T(0) ? T(1) : T(-1);
  const T G = B-xi*sD;
  const T Ainv = T(1)/A;
  const T sDinv = T(1)/sD;
  const T E1 = std::abs(G*Ainv) + T(3)/T(4)*std::abs(sD*Ainv) + std::abs(C*sDinv) + std::abs(B*B*Ainv*sDinv)/T(4);
  const T E2 = eA*std::abs((C*xi*Ainv*sDinv - G*Ainv*Ainv/T(2))) + eB/T(2)*std::abs(Ainv*(B*xi*sDinv-T(1))) + eC*std::abs(sDinv);
  return eps*E1 + E2;
}

template<class T>
inline T Errorformula2(T A, T B, T C, T D, T sD, T eA, T eB, T eC)
{
  constexpr T eps = std::numeric_limits<T>::epsilon();
  const T xi = B<T(0) ? T(1) : T(-1);
  const T G = B - xi*sD;
  const T sDinv = T(1)/sD;
  const T GGsDinv = T(1)/(G*G*sD);
  const T E1 = std::abs(GGsDinv)*(T(4)*std::abs(C*G*sD) + T(3)*std::abs(C*D) + T(4)*std::abs(A*C*C) + std::abs(B*B*C));
  const T E2 = std::abs(GGsDinv)*(eA*T(4)*std::abs(C*C)+ T(2)*eB*std::abs(C*(B*xi - sD)) + eC*std::abs((T(4)*A*C*xi - T(2)*G*sD)));
  return eps*E1 + E2;
}

};

template<class T>
inline bool Quadratic(T a, T b, T c, T ea, T eb, T ec, T &t0, T &t1, T &err0, T &err1)
{
  using namespace quadratic_internal;
  //from PBRT pg. 1080
  const T d = b*b - T(4)*a*c;
  if (d < T(0))
    return false;
  const T sd = std::sqrt(d);
  err0 = Errorformula1(a, b, c, d, sd, ea, eb, ec);
  err1 = Errorformula2(a, b, c, d, sd, ea, eb, ec);
  const T q = b<0 ? -b+sd : -b-sd;
  t0 = q/T(2)/a;
  t1 = T(2)*c/q;
  if (t0 > t1)
  {
    std::swap(t0 ,t1);
    std::swap(err0, err1);
  }
  return true;
}

inline bool startswith(const std::string &a, const std::string &b)
{
  if (a.size() < b.size())
    return false;
  return a.substr(0, b.size()) == b;
}

inline bool endswith(const std::string &a, const std::string &b)
{
  if (a.size() < b.size())
    return false;
  return a.substr(a.size()-b.size(), b.size()) == b;
}

//  There is no hash support for pairs in the STL.
template<class A, class B>
struct pair_hash
{
  std::size_t operator()(const std::pair<A,B> &v) const
  {
    std::size_t seed = boost::hash_value(v.first);
    boost::hash_combine(seed, boost::hash_value(v.second));
    return seed;
  }
};

// Copy & Paste Fu! https://stackoverflow.com/questions/27140778/range-based-for-with-pairiterator-iterator
template <typename I>
struct iter_pair : std::pair<I, I>
{
    using std::pair<I, I>::pair;

    I begin() { return this->first; }
    I end() { return this->second; }
};

#if 1
template<class T, size_t alignment>
class AlignedAllocator
{
  public:
    using value_type = T;
    using propagate_on_container_move_assignment = std::true_type;
    using is_always_equal = std::true_type;

    static constexpr size_t ComputeTrueAlignment()
    {
      // By default we may have some random align a. But if type specify
      // alignas(b) with b>a, I'd get crashes without this little correction.
      if constexpr (std::is_void_v<T>)
        return alignment;
      else
        return std::max(alignment, alignof(T));
    }

    static constexpr size_t true_alignment = ComputeTrueAlignment();
    // Check requirements for posix_memalign.
    static_assert(true_alignment % sizeof(void*) == 0);

    template<class U>
    struct rebind {
      typedef AlignedAllocator<U, alignment> other;
    };

    constexpr AlignedAllocator() noexcept = default;

    template <class U>
    constexpr AlignedAllocator(const AlignedAllocator<U, alignment>&) noexcept
    {
    }

    [[nodiscard]] T* allocate(std::size_t n)
    {
      #ifdef _MSC_VER
            return (T*)_aligned_malloc(n*sizeof(T), true_alignment);
      #else
            void* result = nullptr;
            posix_memalign(&result, true_alignment, n*sizeof(T)); // returns 0 on success. Not using it obviously.
            return (T*)result;
      #endif
    }

    void deallocate(T* p, std::size_t n)
    {
      #ifdef _MSC_VER
        _aligned_free(p);
      #else
        free(p);
      #endif
    }
};

template <class T, class U, size_t a>
bool operator==(const AlignedAllocator<T,a>&, const AlignedAllocator<U,a>&) noexcept
{
  return true;
}

template <class T, class U, size_t a>
bool operator!=(const AlignedAllocator<T, a>&, const AlignedAllocator<U, a>&) noexcept
{
  return false;
}
#else
template<class T, size_t a>
using AlignedAllocator = boost::alignment::aligned_allocator<T,a>;
#endif

// std::vector with 16 byte aligment as required by Eigen's fixed size types.
// This class also comes with range checking in debug mode.
template<class T, class Alloc = AlignedAllocator<T,16>>
class ToyVector : public std::vector<T, Alloc>
{
  using B = std::vector<T, Alloc>;
public:
  using B::B;

  inline typename B::const_reference operator[](typename B::size_type i) const
  {
    assert(i >= 0 && i<B::size());
    return B::operator[](i);
  }

  inline typename B::reference operator[](typename B::size_type i)
  {
    assert(i >= 0 && i<B::size());
    return B::operator[](i);
  }
};

template<class K, class T, class F, class Hash, class Pred>
inline T GetOrInsertFromFactory(std::unordered_map<K, T, Hash, Pred> &m, const K &k, F factory)
{
  auto it = m.find(k);
  if (it == m.end())
  {
    auto& t = m[k] = factory();
    return t;
  }
  else
    return it->second;
}

template<class T>
inline T ASSERT_NOT_NULL(T x, typename std::enable_if<std::is_pointer<T>::value>::type* = 0)
{
  assert(x != nullptr);
  return x;
}

// Adapted from http://the-witness.net/news/2012/11/scopeexit-in-c11/
template <typename F>
struct ScopeExit {
    ScopeExit(F f) : f(f) {}
    ~ScopeExit() { f(); }
    F f;
};

template <typename F>
ScopeExit<F> MakeScopeExit(F f) {
    return ScopeExit<F>(f);
};

#define SCOPE_EXIT(code) \
    auto scope_exit_ ## __LINE__ = util::MakeScopeExit([=](){ code })

inline int RowMajorOffset(int x, int y, int size_x, int size_y)
{
  return x + y*size_x;
}

inline std::pair<int, int> RowMajorPixel(int offset, int size_x, int size_y)
{
  int y = offset / size_x;
  int x = offset - y*size_x;
  return std::make_pair(x,y);
}

template<class T>
struct enable_if_has_size_member
{
    using type = decltype(std::declval<T&>().size());
};

template<class Container>
inline int isize(const Container &c, typename enable_if_has_size_member<Container>::type = 0)
{
    return static_cast<int>(c.size());
}

template<class Container>
inline long lsize(const Container &c, typename enable_if_has_size_member<Container>::type = 0)
{
  return static_cast<long>(c.size());
}

template<class T>
struct enable_if_has_insert_begin_and_end_members
{
  using type = decltype(std::declval<T&>().insert(
    std::declval<T&>().begin(),
    std::declval<T&>().end(),
    std::declval<T&>().begin() // here this makes actually sense.
  ));
};

template<class Container, typename = typename enable_if_has_insert_begin_and_end_members<Container>::type>
inline void Append(Container &a, const Container &b)
{
  a.insert(a.end(), b.begin(), b.end());
}

template<class T, class Alloc>
inline void PushBackToEnsureSize(std::vector<T, Alloc> &v, std::size_t required_size, const T &filler)
{
  assert(required_size >= v.size());
  std::fill_n(std::back_inserter(v), required_size - v.size(), filler);
}

template<class T>
struct has_begin_end
{
  using type1 = decltype(std::declval<T&>().begin());
  using type2 = decltype(std::declval<T&>().end());
  static constexpr bool value = true;
};

template<class Container, class Func, typename = std::enable_if_t<has_begin_end<Container>::value>>
inline auto TransformVector(Container &a, Func &&f)
{
  using TIn = typename Container::reference;
  using TOut = std::invoke_result_t<Func, TIn>;
  using OutContainer = ToyVector<TOut>;
  OutContainer result;
  std::transform(a.begin(), a.end(), std::back_inserter(result), f);
  return result;
}

// Adapted from https://stackoverflow.com/questions/41660062/how-to-construct-an-stdarray-with-index-sequence
// Use variadic templates and a pack of integers to build the output array without
// invoking default c'tors.
namespace detail {
  template<typename T, typename U, typename F, std::size_t... Is>
  constexpr auto transform_array(F& f, const U* input, std::index_sequence<Is...>)
     -> std::array<T, sizeof...(Is)>
  {
    return {{f(input[std::integral_constant<std::size_t, Is>{}])...}};
  }
}

template<class Func, class T, std::size_t n>
inline constexpr auto TransformArray(std::array<T,n> &a, Func &&f)
{
  using TOut = std::invoke_result_t<Func, T>;
  return detail::transform_array<TOut>(f, a.data(), std::make_index_sequence<n>{});
}

// From https://stackoverflow.com/questions/41660062/how-to-construct-an-stdarray-with-index-sequence
// Use variadic templates and a pack of integers to build the output array without
// invoking default c'tors.
namespace detail {
  template<typename T, typename F, std::size_t... Is>
  constexpr auto generate_array(F& f, std::index_sequence<Is...>)
   -> std::array<T, sizeof...(Is)> {
    return {{f(std::integral_constant<std::size_t, Is>{})...}};
  }
}

template<std::size_t N, typename F>
inline constexpr auto GenerateArray(F &&f) {
  using TOut = std::invoke_result_t<F, std::size_t>;
  return detail::generate_array<TOut>(f, std::make_index_sequence<N>{});
}

/*
 * Perform an atomic addition to the float via spin-locking
 * on compare_exchange_weak. Memory ordering is release on write
 * consume on read
 *
 * from https://www.reddit.com/r/cpp/comments/338pcj/atomic_addition_of_floats_using_compare_exchange/
 */
inline float AtomicAdd(std::atomic<float> &f, float d) {
  float old = f.load(std::memory_order_consume);
  float desired = old + d;  while (!f.compare_exchange_weak(old, desired,
    std::memory_order_release, std::memory_order_consume))
  {
    desired = old + d;
  }
  return desired;
}

// From https://arne-mertz.de/2018/05/overload-build-a-variant-visitor-on-the-fly/
template <class ...Fs>
struct Overload : Fs... {
  template <class ...Ts>
  Overload(Ts&& ...ts) : Fs{std::forward<Ts>(ts)}...
  {}

  using Fs::operator()...;
};

template <class ...Ts>
Overload(Ts&&...) -> Overload<std::remove_reference_t<Ts>...>;

// TODO: check if we have an iterator in It
template<class It, class Trafo>
inline std::string Join(const std::string &sep, It begin, It end, Trafo trafo)
{
  if (begin == end)
    return {};
  std::ostringstream os;
  It prev = begin;
  ++begin;
  while (begin != end)
  {
    os << trafo(*prev) << sep;
    prev = begin;
    ++begin;
  }
  os << trafo(*prev);
  return os.str();
}

} // namespace util

using util::Overload;
using util::Sqr;
using util::Cubed;
using util::Heaviside;
using util::Rcp;
using util::Lerp;
using util::Sign;
using util::ToyVector;
using util::isize;
using util::lsize;
using util::AlignedAllocator;
using util::ASSERT_NOT_NULL;


/**
 * Copyright 2020, Massachusetts Institute of Technology,
 * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Jingnan Shi, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 */

#include "gtest/gtest.h"

#include <iostream>
#include <iomanip>
#include <fstream>
#include <chrono>

#include <Eigen/Eigenvalues>

#include "teaser/registration.h"
#include "test_utils.h"

TEST(TLSTest, TLSEstimate) {
  teaser::ScalarTLSEstimator tls;
  // No outlier
  {
    Eigen::RowVectorXd measurements(5);
    measurements << 0.5, 1, 0.6, 0.7, 1.2;
    Eigen::RowVectorXd ranges(5);
    ranges << 0.9, 0.9, 0.4, 0.5, 0.4;

    double ref_estimate = 0.8383;
    Eigen::Matrix<bool, 1, 5> ref_inliers;
    ref_inliers << true, true, true, true, true;

    double estimate_output;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> inliers_output;
    inliers_output.resize(1, ref_inliers.cols());
    tls.estimate(measurements, ranges, &estimate_output, &inliers_output);
    EXPECT_NEAR(estimate_output, ref_estimate, 0.001); // TODO tolerance seems quite large

    for (size_t i = 0; i < 5; ++i) {
      EXPECT_EQ(inliers_output(i), ref_inliers(i));
    }
  }
  // One outlier
  {
    Eigen::RowVectorXd measurements(6);
    measurements << 0.5, 1, 0.6, 0.7, 1.2, 10;
    Eigen::RowVectorXd ranges(6);
    ranges << 0.9, 0.9, 0.4, 0.5, 0.4, 0.5;

    double ref_estimate = 0.8383;
    Eigen::Matrix<bool, 1, 6> ref_inliers;
    ref_inliers << true, true, true, true, true, false;

    double estimate_output;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> inliers_output;
    inliers_output.resize(1, ref_inliers.cols());
    tls.estimate(measurements, ranges, &estimate_output, &inliers_output);
    EXPECT_NEAR(estimate_output, ref_estimate, 0.001);

    for (size_t i = 0; i < 6; ++i) {
      EXPECT_EQ(inliers_output(i), ref_inliers(i));
    }
  }
  // Three (out of six) outliers
  {
    Eigen::RowVectorXd measurements(6);
    measurements << 0.5, 1, 0.6, 20, 16, 10;
    Eigen::RowVectorXd ranges(6);
    ranges << 0.9, 0.9, 0.4, 0.5, 0.4, 0.5;

    double ref_estimate = 0.6425;
    Eigen::Matrix<bool, 1, 6> ref_inliers;
    ref_inliers << true, true, true, false, false, false;

    double estimate_output;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> inliers_output;
    inliers_output.resize(1, ref_inliers.cols());
    tls.estimate(measurements, ranges, &estimate_output, &inliers_output);
    EXPECT_NEAR(estimate_output, ref_estimate, 0.001);

    for (size_t i = 0; i < 6; ++i) {
      EXPECT_EQ(inliers_output(i), ref_inliers(i));
    }
  }
}

TEST(TLSTest, TLSEstimateTiled) {
  teaser::ScalarTLSEstimator tls;
  const int scale = 64;
  // No outlier
  {
    Eigen::RowVectorXd measurements(5);
    measurements << 0.5, 1, 0.6, 0.7, 1.2;
    Eigen::RowVectorXd ranges(5);
    ranges << 0.9, 0.9, 0.4, 0.5, 0.4;

    double ref_estimate = 0.8383;
    Eigen::Matrix<bool, 1, 5> ref_inliers;
    ref_inliers << true, true, true, true, true;

    double estimate_output;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> inliers_output;
    inliers_output.resize(1, ranges.cols());
    tls.estimate_tiled(measurements, ranges, scale, &estimate_output, &inliers_output);
    EXPECT_NEAR(estimate_output, ref_estimate, 0.001); // TODO tolerance seems quite large

    for (size_t i = 0; i < 5; ++i) {
      EXPECT_EQ(inliers_output(i), ref_inliers(i));
    }
  }
  // One outlier
  {
    Eigen::RowVectorXd measurements(6);
    measurements << 0.5, 1, 0.6, 0.7, 1.2, 10;
    Eigen::RowVectorXd ranges(6);
    ranges << 0.9, 0.9, 0.4, 0.5, 0.4, 0.5;

    double ref_estimate = 0.8383;
    Eigen::Matrix<bool, 1, 6> ref_inliers;
    ref_inliers << true, true, true, true, true, false;

    double estimate_output;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> inliers_output;
    inliers_output.resize(1, ranges.cols());
    tls.estimate_tiled(measurements, ranges, scale, &estimate_output, &inliers_output);
    EXPECT_NEAR(estimate_output, ref_estimate, 0.001);

    for (size_t i = 0; i < 6; ++i) {
      EXPECT_EQ(inliers_output(i), ref_inliers(i));
    }
  }
  // Three (out of six) outliers
  {
    Eigen::RowVectorXd measurements(6);
    measurements << 0.5, 1, 0.6, 20, 16, 10;
    Eigen::RowVectorXd ranges(6);
    ranges << 0.9, 0.9, 0.4, 0.5, 0.4, 0.5;

    double ref_estimate = 0.6425;
    Eigen::Matrix<bool, 1, 6> ref_inliers;
    ref_inliers << true, true, true, false, false, false;

    double estimate_output;
    Eigen::Matrix<bool, 1, Eigen::Dynamic> inliers_output;
    inliers_output.resize(1, ranges.cols());
    tls.estimate_tiled(measurements, ranges, scale, &estimate_output, &inliers_output);
    EXPECT_NEAR(estimate_output, ref_estimate, 0.001);

    for (size_t i = 0; i < 6; ++i) {
      EXPECT_EQ(inliers_output(i), ref_inliers(i));
    }
  }
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Siargey Kachanovich
 *
 *    Copyright (C) 2019 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "freudenthal_triangulation"
#include <boost/test/unit_test.hpp>

#include <gudhi/Unitary_tests_utils.h>
#include <gudhi/Freudenthal_triangulation.h>
#include <gudhi/Coxeter_triangulation.h>

BOOST_AUTO_TEST_CASE(freudenthal_triangulation) {
  // Point location check
  typedef std::vector<double> Point;
  typedef Gudhi::coxeter_triangulation::Freudenthal_triangulation<> FK_triangulation;
  typedef typename FK_triangulation::Simplex_handle Simplex_handle;
  typedef typename FK_triangulation::Vertex_handle Vertex_handle;
  typedef typename Simplex_handle::OrderedSetPartition Ordered_set_partition;
  typedef typename Ordered_set_partition::value_type Part;

  FK_triangulation tr(3);

  // Point location check
  {
    Point point({3, -1, 0});
    Simplex_handle s = tr.locate_point(point);
    BOOST_CHECK(s.vertex() == Vertex_handle({3, -1, 0}));
    BOOST_CHECK(s.partition() == Ordered_set_partition({Part({0, 1, 2, 3})}));
  }

  {
    Point point({3.5, -1.5, 0.5});
    Simplex_handle s = tr.locate_point(point);
    BOOST_CHECK(s.vertex() == Vertex_handle({3, -2, 0}));
    BOOST_CHECK(s.partition() == Ordered_set_partition({Part({0, 1, 2}), Part({3})}));
  }

  {
    Point point({3.5, -1.8, 0.5});
    Simplex_handle s = tr.locate_point(point);
    BOOST_CHECK(s.vertex() == Vertex_handle({3, -2, 0}));
    BOOST_CHECK(s.partition() == Ordered_set_partition({Part({0, 2}), Part({1}), Part({3})}));
  }

  {
    Point point({3.5, -1.8, 0.3});
    Simplex_handle s = tr.locate_point(point);
    BOOST_CHECK(s.vertex() == Vertex_handle({3, -2, 0}));
    BOOST_CHECK(s.partition() == Ordered_set_partition({Part({0}), Part({2}), Part({1}), Part({3})}));
  }

  // Dimension check
  BOOST_CHECK(tr.dimension() == 3);
  // Matrix check
  Eigen::MatrixXd default_matrix = Eigen::MatrixXd::Identity(3, 3);
  BOOST_CHECK(tr.matrix() == default_matrix);
  // Vector check
  Eigen::MatrixXd default_offset = Eigen::VectorXd::Zero(3);
  BOOST_CHECK(tr.offset() == default_offset);

  // Barycenter check
  Point point({3.5, -1.8, 0.3});
  Simplex_handle s = tr.locate_point(point);
  Eigen::Vector3d barycenter_cart = Eigen::Vector3d::Zero();
  for (auto v : s.vertex_range())
    for (std::size_t i = 0; i < v.size(); i++) barycenter_cart(i) += v[i];
  barycenter_cart /= 4.;  // simplex is three-dimensional
  Eigen::Vector3d barycenter = tr.barycenter(s);
  for (std::size_t i = 0; (long int)i < barycenter.size(); i++)
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(barycenter(i), barycenter_cart(i), 1e-7);

  // Barycenter check for twice the scale
  s = tr.locate_point(point, 2);
  barycenter_cart = Eigen::Vector3d::Zero();
  for (auto v : s.vertex_range())
    for (std::size_t i = 0; i < v.size(); i++) barycenter_cart(i) += v[i];
  barycenter_cart /= 3.;  // simplex is now a two-dimensional face
  barycenter_cart /= 2.;  // scale
  barycenter = tr.barycenter(s, 2);
  for (std::size_t i = 0; (long int)i < barycenter.size(); i++)
    GUDHI_TEST_FLOAT_EQUALITY_CHECK(barycenter(i), barycenter_cart(i), 1e-7);

  // Matrix and offset change check
  Eigen::MatrixXd new_matrix(3, 3);
  new_matrix << 1, 0, 0, -1, 1, 0, -1, 0, 1;
  Eigen::Vector3d new_offset(1.5, 1, 0.5);
  tr.change_matrix(new_matrix);
  tr.change_offset(new_offset);

  BOOST_CHECK(tr.matrix() == new_matrix);
  BOOST_CHECK(tr.offset() == new_offset);
}

#ifdef GUDHI_DEBUG
BOOST_AUTO_TEST_CASE(freudenthal_triangulation_exceptions_in_debug_mode) {
  // Point location check
  typedef Gudhi::coxeter_triangulation::Freudenthal_triangulation<> FK_triangulation;

  BOOST_CHECK_THROW (FK_triangulation tr(3, Eigen::MatrixXd::Identity(3, 3), Eigen::VectorXd::Zero(4)),
                     std::invalid_argument);

  FK_triangulation tr(3);
  // Point of dimension 4
  std::vector<double> point({3.5, -1.8, 0.3, 4.1});
  BOOST_CHECK_THROW (tr.locate_point(point), std::invalid_argument);
}
#endif


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "euclidean_simple_witness_complex"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <CGAL/Epick_d.h>

#include <gudhi/Simplex_tree.h>

#include <gudhi/Witness_complex.h>
#include <gudhi/Euclidean_witness_complex.h>
#include <gudhi/Strong_witness_complex.h>
#include <gudhi/Euclidean_strong_witness_complex.h>

#include <gudhi/Kd_tree_search.h>

#include <iostream>
#include <ctime>
#include <vector>

typedef Gudhi::Simplex_tree<> Simplex_tree;
typedef typename Simplex_tree::Vertex_handle Vertex_handle;
typedef std::vector< Vertex_handle > typeVectorVertex;
typedef CGAL::Epick_d<CGAL::Dynamic_dimension_tag> Kernel;
typedef typename Kernel::FT FT;
typedef typename Kernel::Point_d Point_d;
typedef Gudhi::witness_complex::Euclidean_witness_complex<Kernel> EuclideanWitnessComplex;
typedef Gudhi::witness_complex::Euclidean_strong_witness_complex<Kernel> EuclideanStrongWitnessComplex;

typedef std::vector<Point_d> Point_range;
typedef Gudhi::spatial_searching::Kd_tree_search<Kernel, Point_range> Kd_tree;
typedef Kd_tree::INS_range Nearest_landmark_range;
typedef std::vector<Nearest_landmark_range> Nearest_landmark_table;
typedef Gudhi::witness_complex::Witness_complex<Nearest_landmark_table> WitnessComplex;
typedef Gudhi::witness_complex::Strong_witness_complex<Nearest_landmark_table> StrongWitnessComplex;

/* All landmarks and witnesses are taken on the grid in the following manner.
   LWLWL
   WW.WW
   L...L
   WW.WW
   LWLWL

   Witness complex consists of 8 vertices, 12 edges and 4 triangles
 */

BOOST_AUTO_TEST_CASE(simple_witness_complex) {
  Simplex_tree complex, relaxed_complex, strong_relaxed_complex, strong_relaxed_complex2;
  Simplex_tree complex_ne, relaxed_complex_ne, strong_relaxed_complex_ne, strong_relaxed_complex2_ne;

  Point_range witnesses, landmarks;

  landmarks.push_back(Point_d(std::vector<FT>{-2,-2}));
  landmarks.push_back(Point_d(std::vector<FT>{-2, 0}));
  landmarks.push_back(Point_d(std::vector<FT>{-2, 2}));
  landmarks.push_back(Point_d(std::vector<FT>{ 0,-2}));
  landmarks.push_back(Point_d(std::vector<FT>{ 0, 2}));
  landmarks.push_back(Point_d(std::vector<FT>{ 2,-2}));
  landmarks.push_back(Point_d(std::vector<FT>{ 2, 0}));
  landmarks.push_back(Point_d(std::vector<FT>{ 2, 2}));
  witnesses.push_back(Point_d(std::vector<FT>{-2,-1}));
  witnesses.push_back(Point_d(std::vector<FT>{-2, 1}));
  witnesses.push_back(Point_d(std::vector<FT>{-1,-2}));
  witnesses.push_back(Point_d(std::vector<FT>{-1,-1}));
  witnesses.push_back(Point_d(std::vector<FT>{-1, 1}));
  witnesses.push_back(Point_d(std::vector<FT>{-1, 2}));
  witnesses.push_back(Point_d(std::vector<FT>{ 1,-2}));
  witnesses.push_back(Point_d(std::vector<FT>{ 1,-1}));
  witnesses.push_back(Point_d(std::vector<FT>{ 1, 1}));
  witnesses.push_back(Point_d(std::vector<FT>{ 1, 2}));
  witnesses.push_back(Point_d(std::vector<FT>{ 2,-1}));
  witnesses.push_back(Point_d(std::vector<FT>{ 2, 1}));

  Kd_tree landmark_tree(landmarks);
  Nearest_landmark_table nearest_landmark_table;
  for (auto w: witnesses)
    nearest_landmark_table.push_back(landmark_tree.incremental_nearest_neighbors(w));

  // Weak witness complex: Euclidean version
  EuclideanWitnessComplex eucl_witness_complex(landmarks,
                                               witnesses);
  eucl_witness_complex.create_complex(complex, 0);

  std::cout << "complex.num_simplices() = " << complex.num_simplices() << std::endl;
  BOOST_CHECK(complex.num_simplices() == 24);

  eucl_witness_complex.create_complex(relaxed_complex, 8.01);

  std::cout << "relaxed_complex.num_simplices() = " << relaxed_complex.num_simplices() << std::endl;
  BOOST_CHECK(relaxed_complex.num_simplices() == 239);
  // The corner simplex {0,2,5,7} and its cofaces are missing.

  // Weak witness complex: non-Euclidean version
  WitnessComplex witness_complex(nearest_landmark_table);
  witness_complex.create_complex(complex_ne, 0);

  std::cout << "complex.num_simplices() = " << complex_ne.num_simplices() << std::endl;
  BOOST_CHECK(complex_ne.num_simplices() == 24);

  witness_complex.create_complex(relaxed_complex_ne, 8.01);

  std::cout << "relaxed_complex.num_simplices() = " << relaxed_complex_ne.num_simplices() << std::endl;
  BOOST_CHECK(relaxed_complex_ne.num_simplices() == 239);

  // Strong complex : Euclidean version
  EuclideanStrongWitnessComplex eucl_strong_witness_complex(landmarks,
                                                            witnesses);

  eucl_strong_witness_complex.create_complex(strong_relaxed_complex, 9.1);
  eucl_strong_witness_complex.create_complex(strong_relaxed_complex2, 9.1, 2);

  std::cout << "strong_relaxed_complex.num_simplices() = " << strong_relaxed_complex.num_simplices() << std::endl;
  BOOST_CHECK(strong_relaxed_complex.num_simplices() == 239);

  std::cout << "strong_relaxed_complex2.num_simplices() = " << strong_relaxed_complex2.num_simplices() << std::endl;
  BOOST_CHECK(strong_relaxed_complex2.num_simplices() == 92);

  // Strong complex : non-Euclidean version
  StrongWitnessComplex strong_witness_complex(nearest_landmark_table);

  strong_witness_complex.create_complex(strong_relaxed_complex_ne, 9.1);
  strong_witness_complex.create_complex(strong_relaxed_complex2_ne, 9.1, 2);

  std::cout << "strong_relaxed_complex.num_simplices() = " << strong_relaxed_complex_ne.num_simplices() << std::endl;
  BOOST_CHECK(strong_relaxed_complex_ne.num_simplices() == 239);

  std::cout << "strong_relaxed_complex2.num_simplices() = " << strong_relaxed_complex2_ne.num_simplices() << std::endl;
  BOOST_CHECK(strong_relaxed_complex2_ne.num_simplices() == 92);

  // 8 vertices, 28 edges, 56 triangles
}


#include <Eigen/Core>
#include <iostream>
#include <random>

using namespace Eigen;

int main() {
  std::default_random_engine generator;
  std::poisson_distribution<int> distribution(4.1);
  auto poisson = [&] () {return distribution(generator);};

  RowVectorXi v = RowVectorXi::NullaryExpr(10, poisson );
  std::cout << v << "\n";
}


// Boost.Geometry
// Unit Test

// Copyright (c) 2016-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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 "test_formula.hpp"
#include "intersection_cases.hpp"

#include <boost/geometry/formulas/andoyer_inverse.hpp>
#include <boost/geometry/formulas/geographic.hpp>
#include <boost/geometry/formulas/gnomonic_intersection.hpp>
#include <boost/geometry/formulas/sjoberg_intersection.hpp>
#include <boost/geometry/formulas/thomas_direct.hpp>
#include <boost/geometry/formulas/thomas_inverse.hpp>
#include <boost/geometry/formulas/vincenty_direct.hpp>
#include <boost/geometry/formulas/vincenty_inverse.hpp>

#include <boost/geometry/srs/spheroid.hpp>

void check_result(expected_result const& result, expected_result const& expected,
                  expected_result const& reference, double reference_error,
                  bool check_reference_only)
{
    //BOOST_CHECK_MESSAGE((false), "(" << result.lon << " " << result.lat << ") vs (" << expected.lon << " " << expected.lat << ")");
    check_one(result.lon, expected.lon, reference.lon, reference_error, false, check_reference_only);
    check_one(result.lat, expected.lat, reference.lat, reference_error, false, check_reference_only);
}

void test_formulas(expected_results const& results, bool check_reference_only)
{
    // reference result
    if (results.sjoberg_vincenty.lon == ND)
    {
        return;
    }

    double const d2r = bg::math::d2r<double>();
    double const r2d = bg::math::r2d<double>();

    double lona1r = results.p1.lon * d2r;
    double lata1r = results.p1.lat * d2r;
    double lona2r = results.p2.lon * d2r;
    double lata2r = results.p2.lat * d2r;
    double lonb1r = results.q1.lon * d2r;
    double latb1r = results.q1.lat * d2r;
    double lonb2r = results.q2.lon * d2r;
    double latb2r = results.q2.lat * d2r;

    expected_result result;

    // WGS84
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);

    if (results.gnomonic_vincenty.lon != ND)
    {
        bg::formula::gnomonic_intersection<double, bg::formula::vincenty_inverse, bg::formula::vincenty_direct>
            ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
        result.lon *= r2d;
        result.lat *= r2d;
        check_result(result, results.gnomonic_vincenty, results.sjoberg_vincenty, 0.00000001, check_reference_only);
    }

    if (results.gnomonic_thomas.lon != ND)
    {
        bg::formula::gnomonic_intersection<double, bg::formula::thomas_inverse, bg::formula::thomas_direct>
            ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
        result.lon *= r2d;
        result.lat *= r2d;
        check_result(result, results.gnomonic_thomas, results.sjoberg_vincenty, 0.0000001, check_reference_only);
    }

    if (results.sjoberg_vincenty.lon != ND)
    {
        bg::formula::sjoberg_intersection<double, bg::formula::vincenty_inverse, 4>
            ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
        result.lon *= r2d;
        result.lat *= r2d;
        check_result(result, results.sjoberg_vincenty, results.sjoberg_vincenty, 0.00000001, check_reference_only);
    }

    if (results.sjoberg_thomas.lon != ND)
    {
        bg::formula::sjoberg_intersection<double, bg::formula::thomas_inverse, 2>
            ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
        result.lon *= r2d;
        result.lat *= r2d;
        check_result(result, results.sjoberg_thomas, results.sjoberg_vincenty, 0.0000001, check_reference_only);
    }

    if (results.sjoberg_andoyer.lon != ND)
    {
        bg::formula::sjoberg_intersection<double, bg::formula::andoyer_inverse, 1>
            ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
        result.lon *= r2d;
        result.lat *= r2d;
        check_result(result, results.sjoberg_andoyer, results.sjoberg_vincenty, 0.0001, check_reference_only);
    }

    if (results.great_elliptic.lon != ND)
    {
        typedef bg::model::point<double, 2, bg::cs::geographic<bg::degree> > point_geo;
        typedef bg::model::point<double, 3, bg::cs::cartesian> point_3d;
        point_geo a1(results.p1.lon, results.p1.lat);
        point_geo a2(results.p2.lon, results.p2.lat);
        point_geo b1(results.q1.lon, results.q1.lat);
        point_geo b2(results.q2.lon, results.q2.lat);
        point_3d a1v = bg::formula::geo_to_cart3d<point_3d>(a1, spheroid);
        point_3d a2v = bg::formula::geo_to_cart3d<point_3d>(a2, spheroid);
        point_3d b1v = bg::formula::geo_to_cart3d<point_3d>(b1, spheroid);
        point_3d b2v = bg::formula::geo_to_cart3d<point_3d>(b2, spheroid);
        point_3d resv(0, 0, 0);
        point_geo res(0, 0);
        bg::formula::great_elliptic_intersection(a1v, a2v, b1v, b2v, resv, spheroid);
        res = bg::formula::cart3d_to_geo<point_geo>(resv, spheroid);
        result.lon = bg::get<0>(res);
        result.lat = bg::get<1>(res);
        check_result(result, results.great_elliptic, results.sjoberg_vincenty, 0.01, check_reference_only);
    }
}

void test_4_input_combinations(expected_results const& results, bool check_reference_only)
{
    test_formulas(results, check_reference_only);

#ifdef BOOST_GEOMETRY_TEST_GEO_INTERSECTION_TEST_SIMILAR
    {
        expected_results results_alt = results;
        std::swap(results_alt.p1, results_alt.p2);
        test_formulas(results_alt, true);
    }
    {
        expected_results results_alt = results;
        std::swap(results_alt.q1, results_alt.q2);
        test_formulas(results_alt, true);
    }
    {
        expected_results results_alt = results;
        std::swap(results_alt.p1, results_alt.p2);
        std::swap(results_alt.q1, results_alt.q2);
        test_formulas(results_alt, true);
    }
#endif
}

void test_all(expected_results const& results)
{
    test_4_input_combinations(results, false);

#ifdef BOOST_GEOMETRY_TEST_GEO_INTERSECTION_TEST_SIMILAR
    expected_results results_alt = results;
    results_alt.p1.lat *= -1;
    results_alt.p2.lat *= -1;
    results_alt.q1.lat *= -1;
    results_alt.q2.lat *= -1;
    results_alt.gnomonic_vincenty.lat *= -1;
    results_alt.gnomonic_thomas.lat *= -1;
    results_alt.sjoberg_vincenty.lat *= -1;
    results_alt.sjoberg_thomas.lat *= -1;
    results_alt.sjoberg_andoyer.lat *= -1;
    results_alt.great_elliptic.lat *= -1;
    test_4_input_combinations(results_alt, true);
#endif
}

int test_main(int, char*[])
{
    for (size_t i = 0; i < expected_size; ++i)
    {
        test_all(expected[i]);
    }

    return 0;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                       */
/*  This file is part of the library KASKADE 7                 */
/*    see http://www.zib.de/en/numerik/software/kaskade-7.html         */
/*                                       */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin              */
/*                                       */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.  */
/*    see $KASKADE/academic.txt                        */
/*                                       */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <iostream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"

#include "fem/assemble.hh"
#include "fem/embedded_errorest.hh"
#include "fem/lagrangespace.hh"
//#include "fem/hierarchicspace.hh"   // ContinuousHierarchicMapper
#include "linalg/direct.hh"
#include "linalg/trivialpreconditioner.hh"
#include "linalg/iluprecond.hh"      // PrecondType::ILUT, PrecondType::ILUK, PrecondType::ARMS
#include "linalg/iccprecond.hh"
#include "linalg/icc0precond.hh"
#include "linalg/hyprecond.hh"       // BoomerAMG
#include "linalg/jacobiPreconditioner.hh"
#include "linalg/cg.hh"
#include "mg/hb.hh"
#include "utilities/enums.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare, createUnitCube
#include "io/vtk.hh"
//#include "io/amira.hh"
#include "utilities/kaskopt.hh"

//#include "cubus.hh"
using namespace Kaskade;
#include "peaksource.hh"

#ifndef SPACEDIM
#define SPACEDIM 2
#endif

#if SPACEDIM==2
#define DEFAULT_REFINEMENTS 5
#else
#define DEFAULT_REFINEMENTS 2
#endif

int main(int argc, char *argv[])
{
  using namespace boost::fusion;

  std::cout << "Start heat transfer tutorial program using embedded error estimation" << std::endl;

  boost::timer::cpu_timer totalTimer;

  int verbosityOpt = 1;
  bool dump = true;
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  int  refinements = getParameter(pt, "refinement", DEFAULT_REFINEMENTS),
       order       = getParameter(pt, "order", 2),
       verbosity   = getParameter(pt, "verbosity", 1);

  std::cout << "original mesh shall be refined : " << refinements << " times" << std::endl;
  std::cout << "discretization order         : " << order << std::endl;
  std::cout << "output level (verbosity)     : " << verbosity << std::endl;

  DirectType directType;
//  IterateType iterateType = IterateType::CG;
  MatrixProperties property;
  PrecondType precondType = PrecondType::NONE;
  std::string empty;

  int direct, onlyLowerTriangle = false;

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  direct = getParameter(pt, s, 0);

  // the user may select a value for solver.direct of
  // the enumeration class {UMFPACK, PARDISO, MUMPS, SUPERLU, UMFPACK3264, UMFPACK64}
  // Remark: DirectType::PARDISO not yet available
  s = "names.direct." + getParameter(pt, "solver.direct", empty);
  directType = static_cast<DirectType>(getParameter(pt, s, 2));

  // the user may select a value for solver.iterate of
  // the enumeration {CG, BICGSTAB, GMRES, PCG, APCG, SGS}
  // Remark: in this example only IterateType::CG is used.
//  s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
//  iterateType = static_cast<IterateType>(getParameter(pt, s, 0));

  // the user may select a value for solver.preconditioner of
  // the enumeration class {NONE, JACOBI, ILUT, ILUK, ARMS, ADDITIVESCHWARZ,
  //                  BOOMERAMG, EUCLID, SSOR, ICC0, ICC, ILUKS}
  // Remark: in this example only PrecondType::NONE,PrecondType::JACOBI, PrecondType::ICC, PrecondType::ICC0, PrecondType::BOOMERAMG are used.
  s = "names.preconditioner." + getParameter(pt, "solver.preconditioner", empty);
  precondType = static_cast<PrecondType>(getParameter(pt, s, 0));

  property = MatrixProperties::SYMMETRIC;
  std::cout << "discretization is symmetric" << std::endl;

  if ( (directType == DirectType::MUMPS)||(directType == DirectType::PARDISO) || ( (precondType == PrecondType::ICC) && !direct ) )
  {
    onlyLowerTriangle = true;
    std::cout <<
      "Note: direct solver MUMPS/PARADISO or PrecondType::ICC preconditioner ===> onlyLowerTriangle is set to true!"
      << std::endl;
  }

#if SPACEDIM==2
  //   two-dimensional space: dim=2
  constexpr int dim=2;
  using Grid = Dune::UGGrid<dim>;
  GridManager<Grid> gridManager( createUnitSquare<Grid>() );
  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " triangles, " << std::endl;
#else
  //  three-dimensional space: dim=3
  constexpr int dim=3;
  using Grid = Dune::UGGrid<dim>;
  GridManager<Grid> gridManager( createUnitCube<Grid>(0.5) );
  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " tetrahedra, " << std::endl;
  std::cout << "      " << gridManager.grid().size(1) << " triangles, " << std::endl;
#endif
  std::cout << "      " << gridManager.grid().size(dim-1) << " edges, " << std::endl;
  std::cout << "      " << gridManager.grid().size(dim) << " points" << std::endl;

  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  // alternative: using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<double,LeafView> >;
  using VariableDescriptions = boost::fusion::vector<VariableDescription<0,1,0> >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = PeaksourceFunctional<double,VariableSet>;
  using Assembler = VariationalFunctionalAssembler<LinearizationAt<Functional> >;
  constexpr int neq = Functional::TestVars::noOfVariables;
  using CoefficientVectors = VariableSet::CoefficientVectorRepresentation<0,neq>::type;
  using LinearSpace = VariableSet::CoefficientVectorRepresentation<0,neq>::type;

  gridManager.setVerbosity(verbosity);
  gridManager.enforceConcurrentReads(true);

  // construction of finite element space for the scalar solution T
  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),
               order);
  Spaces spaces(&temperatureSpace);
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable
  std::string varNames[1] = { "T" };
  VariableSet variableSet(spaces,varNames);

  Functional F;

    //construct Galerkin representation

  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  std::cout << "no of variables = " << nvars << std::endl;
  std::cout << "no of equations = " << neq   << std::endl;

  Assembler assembler(gridManager,spaces);
  VariableSet::VariableSet xx(variableSet);

  size_t nnz  = assembler.nnz(0,neq,0,nvars,onlyLowerTriangle);
  size_t size = variableSet.degreesOfFreedom(0,nvars);
  if ( verbosity>0) std::cout << "init mesh: nnz = " << nnz << ", dof = " << size << std::endl;

  std::vector<std::pair<double,double> > tol(1);
  double atol = getParameter(pt, "solver.atol", 1.0e-5);
  double rtol = getParameter(pt, "solver.rtol", 1.0e-5);
  tol[0] = std::make_pair(atol,rtol);
  std::cout << std::endl << "Accuracy: atol = " << atol << ",  rtol = " << rtol << std::endl;

  bool accurate = true;
  int refSteps = -1;
  int iter=0;

  do {
    refSteps++;

    boost::timer::cpu_timer assembTimer;
    VariableSet::VariableSet x(variableSet);
    assembler.assemble(linearization(F,x));
    CoefficientVectors solution(VariableSet::CoefficientVectorRepresentation<0,neq>::init(spaces));
    solution = 0;
    CoefficientVectors rhs(assembler.rhs());
    AssembledGalerkinOperator<Assembler,0,1,0,1> A(assembler, onlyLowerTriangle);
    MatrixAsTriplet<double> tri = A.get<MatrixAsTriplet<double> >();
    if ( verbosity>1) std::cout << "assemble: " << (double)assembTimer.elapsed().user/1e9 << "s\n";

    if (direct) {
      boost::timer::cpu_timer directTimer;
      directInverseOperator(A,directType,property).applyscaleadd(-1.0,rhs,solution);
      x.data = solution.data;

      if ( verbosity>1) std::cout << "direct solve: " << (double)(directTimer.elapsed().user)/1e9 << "s\n";
    }
    else {
      //if ( verbosity>0) std::cout << "iterative solver: steps = " << iteSteps << ", eps = " << iteEps << std::endl;
      boost::timer::cpu_timer iteTimer;
      Dune::InverseOperatorResult res;
      const DefaultDualPairing<LinearSpace,LinearSpace> defaultScalarProduct{};
      int iteSteps = getParameter(pt, "solver.iteMax", 2000);
      double iteEps = getParameter(pt, "solver.iteEps", 1.0e-10);
      StrakosTichyPTerminationCriterion<double> termination(iteEps,iteSteps);
      int lookAhead;
      switch (precondType)
      {
        case PrecondType::NONE:
        case PrecondType::HB:   lookAhead=50; break;
        default:                lookAhead=3; break;
      }
      lookAhead = getParameter(pt, "solver.lookAhead", lookAhead);
      termination.setLookAhead(lookAhead);

      switch (precondType)
      {
        case PrecondType::NONE:
        {
          TrivialPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > trivial;
          CG<LinearSpace,LinearSpace> cg(A,trivial,defaultScalarProduct,termination,verbosity);
          cg.apply(solution,rhs,res);
        }
        break;
        case PrecondType::ICC:
        {
          std::cout << "selected preconditioner: ICC" << std::endl;
          if (property != MatrixProperties::SYMMETRIC)
          {
            std::cout << "PrecondType::ICC preconditioner of TAUCS lib has to be used with matrix.property==MatrixProperties::SYMMETRIC\n";
            std::cout << "i.e., call the executable with option --solver.property MatrixProperties::SYMMETRIC\n\n";
          }
          double dropTol = getParameter(pt, "solver.ICC.dropTol", 0.01);;
          ICCPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > icc(A,dropTol);
          CG<LinearSpace,LinearSpace> cg(A,icc,defaultScalarProduct,termination,verbosity);
          cg.apply(solution,rhs,res);
        }
        break;
        case PrecondType::ICC0:
        {
          std::cout << "selected preconditioner: ICC0" << std::endl;
          ICC_0Preconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > icc0(A);
          CG<LinearSpace,LinearSpace> cg(A,icc0,defaultScalarProduct,termination,verbosity);
          cg.apply(solution,rhs,res);
        }
        break;
        case PrecondType::HB:
        {
          std::cout << "selected preconditioner: HB" << std::endl;
          HierarchicalBasisPreconditioner<Grid,AssembledGalerkinOperator<Assembler,0,1,0,1>::range_type, AssembledGalerkinOperator<Assembler,0,1,0,1>::range_type > hb(gridManager.grid());
          CG<LinearSpace,LinearSpace> cg(A,hb,defaultScalarProduct,termination,verbosity);
          cg.apply(solution,rhs,res);
        }
        break;
        case PrecondType::BOOMERAMG:
        {
          int steps = getParameter(pt, "solver.BOOMERAMG.steps", iteSteps);
          int coarsentype = getParameter(pt, "solver.BOOMERAMG.coarsentype", 21);
          int interpoltype = getParameter(pt, "solver.BOOMERAMG.interpoltype", 0);
          int cycleType = getParameter(pt, "solver.BOOMERAMG.cycleType", 1);
          int relaxType = getParameter(pt, "solver.BOOMERAMG.relaxType", 3);
          int variant = getParameter(pt, "solver.BOOMERAMG.variant", 0);
          int overlap = getParameter(pt, "solver.BOOMERAMG.overlap", 1);
          double tol = getParameter(pt, "solver.BOOMERAMG.tol", iteEps);
          double strongThreshold = getParameter(pt, "solver.BOOMERAMG.strongThreshold", (dim==2)?0.25:0.6);
          BoomerAMG<AssembledGalerkinOperator<Assembler,0,1,0,1> >
          BoomerAMGPrecon(A,steps,coarsentype,interpoltype,tol,cycleType,relaxType,
          strongThreshold,variant,overlap,1,verbosity);
          CG<LinearSpace,LinearSpace> cg(A,BoomerAMGPrecon,defaultScalarProduct,termination,verbosity);
//          Dune::LoopSolver<LinearSpace> cg(A,BoomerAMGPrecon,iteEps,iteSteps,verbosity);
          cg.apply(solution,rhs,res);
        }
        break;
        case PrecondType::JACOBI:
        default:
        {
          JacobiPreconditioner<AssembledGalerkinOperator<Assembler,0,1,0,1> > jacobi(A,1.0);
          CG<LinearSpace,LinearSpace> cg(A,jacobi,defaultScalarProduct,termination,verbosity);
          cg.apply(solution,rhs,res);
        }
        break;
      }
      solution *= -1.0;
      x.data = solution.data;

      if ( verbosity>0) std::cout << "iterative solve eps= " << iteEps << ": "
          << (res.converged?"converged":"failed") << " after "
          << res.iterations << " steps, rate="
          << res.conv_rate << ", time=" << (double)(iteTimer.elapsed().user)/1e9 << "s\n";
    }

	// graphical output of solution
    std::ostringstream fn;
    fn << "graph/peak-grid";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps;
    fn.flush();

    // output of solution in VTK format for visualization,
    // the data are written as ascii stream into file temperature.vtu,
    // possible is also binary
    writeVTKFile(x,fn.str(),IoOptions().setOrder(order));

  // output of solution for Amira visualization,
  // the data are written in binary format into file temperature.am,
  // possible is also ascii
  //    IoOptions options;
  //    options.outputType = IoOptions::ascii;
  //    LeafView leafGridView = gridManager.grid().leafGridView();
  //    writeAMIRAFile(leafGridView,variableSet,x,fn.str(),options);

    VariableSet::VariableSet e = x;
    projectHierarchically(variableSet,e);
    e -= x;

    accurate = embeddedErrorEstimator(variableSet,e,x,IdentityScaling(),tol,gridManager,verbosity);
    nnz = assembler.nnz(0,1,0,1,onlyLowerTriangle);;
    size_t size = variableSet.degreesOfFreedom(0,1);
    if ( verbosity>0) std::cout << "new mesh: nnz = " << nnz << ", dof = " << size << std::endl;

    //ridx.resize(nnz);
    //cidx.resize(nnz);
    //data.resize(nnz);
    //rhs.resize(size);
    //solution.resize(size);

    // VariableSet::VariableSet xx may be used beyond the do...while loop
    xx.data = x.data;
    iter++;
    if (iter>9)
    {
      std::cout << "*** Maximum number of iterations exceeded ***" << std::endl;
      break;
    }

  }  while (!accurate);

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End heat transfer (peak source) tutorial program" << std::endl;
}


#include <boost/test/unit_test.hpp>
#include <vector>
#include <numeric>
#include <limits>
#include <set>
#include <math.h>

using namespace std;

int FindMid( vector<int>& A )
{
    if ( A.size() == 0 )
        return -1;

    long long rsum = std::accumulate( A.begin()+1, A.end(), 0 );
    long long lsum = 0;

    if( lsum == rsum )
        return 0;

    for( size_t i=1; i<A.size(); ++i )
    {
        lsum += A[i-1];
        rsum -= A[i];

        if ( rsum == lsum )
            return i;
    }

    return -1;
}

BOOST_AUTO_TEST_CASE( TestCodilityDemo )
{
    {
        int A[] = { 1, -2, 1 };
        BOOST_CHECK_EQUAL( 1, FindMid( vector<int>( A, A+3 ) ) );
    }

    {
        int A[] = { 1, 2, -2, 1, 1 };
        BOOST_CHECK_EQUAL( 3, FindMid( vector<int>( A, A+(sizeof(A)/sizeof(A[0])) ) ) );
    }
}

int ClosestAscender( int A[], int length, int k )
{
    if( k < 0 || k >= length )
        return -1;

    vector<int> as;
    for( int i=0; i<length; ++i )
    {
        if( i == k ) continue;

        if( A[i] > A[k] )
            as.push_back( i );
    }

    int min = numeric_limits<int>::max();
    int minIndex = -1;
    for( int j = 0; j < (int)as.size(); ++j)
    {
        int dist = abs( as[j]-k );
        if( dist < min )
        {
            min = dist;
            minIndex = as[j];
        }
    }

    return minIndex;
}

BOOST_AUTO_TEST_CASE( TestClosestAscender )
{
    {
        int A[] = { 1, 2, 3, 4, 5 };
        BOOST_CHECK_EQUAL( 3, ClosestAscender( A, sizeof(A)/sizeof(A[0]), 2 ) );
    }

    {
        int A[] = { 1, 5, 3, 4, 4 };
        BOOST_CHECK_EQUAL( 1, ClosestAscender( A, sizeof(A)/sizeof(A[0]), 4 ) );
    }
}

long long Fib( int N )
{
    if( N >= 2 )
    {
        return Fib( N-1 ) + Fib( N -2 );
    }

    return N;
}

long long FibImp( int N )
{
    long long a = 0;
    long long b = 1;
    long long t;

    for( int i = 0; i<N; ++i )
    {
        t = a;
        a = b;
        b = b + t;
    }
    return a;
}

int power_fib( int N, int M )
{
    return FibImp( pow( (double)N, (double)M ) ) % 10000103;
}

BOOST_AUTO_TEST_CASE( TestPowerFib )
{
    BOOST_CHECK_EQUAL( 21, power_fib( 2, 3 ) );
}

int FirstCoveringPrefix( int A[], int length )
{
    if ( length == 0 )
        return -1;

    set<int> keys;
    int prefix = -1;

    for( int i = 0; i<length; ++i )
    {
        pair<set<int>::iterator,bool> ret = keys.insert ( A[i] );
        if( ret.second == true )
        {
            prefix = i;
        }
    }

    return prefix;
}

BOOST_AUTO_TEST_CASE( TestFirstCoveringPrefix )
{
    {
        int A[] = { 1, 5, 3, 4, 4 };
        BOOST_CHECK_EQUAL( 3, FirstCoveringPrefix( A, sizeof(A)/sizeof(A[0]) ) );
    }

    {
        int A[] = { 1 };
        BOOST_CHECK_EQUAL( 0, FirstCoveringPrefix( A, sizeof(A)/sizeof(A[0]) ) );
    }

    {
        int A[] = { 1, 1, 1, 1, 1 };
        BOOST_CHECK_EQUAL( 0, FirstCoveringPrefix( A, sizeof(A)/sizeof(A[0]) ) );
    }

    {
        int A[] = { 1 };
        BOOST_CHECK_EQUAL( -1, FirstCoveringPrefix( A, 0 ) );
    }

    {
        int A[] = { 1,2,3,4,5 };
        BOOST_CHECK_EQUAL( 4, FirstCoveringPrefix( A, sizeof(A)/sizeof(A[0]) ) );
    }

}


//---------------------------------------------------------------------------//
// Copyright (c) 2014 Roshan <thisisroshansmail@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestUniformIntDistribution
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/count_if.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/random/default_random_engine.hpp>
#include <boost/compute/random/uniform_int_distribution.hpp>
#include <boost/compute/lambda.hpp>

#include "context_setup.hpp"

namespace compute=boost::compute;

BOOST_AUTO_TEST_CASE(uniform_int_distribution_doctest)
{
    using boost::compute::uint_;
    using boost::compute::lambda::_1;

    boost::compute::vector<uint_> vec(128, context);

//! [generate]
// initialize the default random engine
boost::compute::default_random_engine engine(queue);

// setup the uniform distribution to produce integers 0 and 1
boost::compute::uniform_int_distribution<uint_> distribution(0, 1);

// generate the random values and store them to 'vec'
distribution.generate(vec.begin(), vec.end(), engine, queue);
//! [generate]

    BOOST_CHECK_EQUAL(
        boost::compute::count_if(
            vec.begin(), vec.end(), _1 > 1, queue
        ),
        size_t(0)
    );
}

BOOST_AUTO_TEST_CASE(issue159) {
    using boost::compute::lambda::_1;

    boost::compute::vector<int> input(10, context);

    // generate random numbers between 1 and 10
    compute::default_random_engine rng(queue);
    compute::uniform_int_distribution<int> d(1, 10);
    d.generate(input.begin(), input.end(), rng, queue);

    BOOST_CHECK_EQUAL(
        boost::compute::count_if(
            input.begin(), input.end(), _1 > 10, queue
        ),
        size_t(0)
    );

    BOOST_CHECK_EQUAL(
        boost::compute::count_if(
            input.begin(), input.end(), _1 < 1, queue
        ),
        size_t(0)
    );
}

BOOST_AUTO_TEST_SUITE_END()