//
//  TriMesh.hpp
//  OptCuts
//
//  Created by Minchen Li on 8/30/17.
//

#ifndef TriMesh_hpp
#define TriMesh_hpp

#include <Eigen/Eigen>

#include <set>
#include <array>

namespace OptCuts{

    enum Primitive
    {
        P_SQUARE,
        P_CYLINDER
    };
    class Scaffold;

    // duplicate the vertices and edges of a mesh to separate its triangles,
    // adjacent triangles in the original mesh will have a cohesive edge structure to
    // indicate the connectivity
    class TriMesh{
    public: // owned data
        Eigen::MatrixXd V_rest; // duplicated rest vertex coordinates in 3D
        Eigen::MatrixXd V; // duplicated vertex coordinates, the dimension depends on the search space
        Eigen::MatrixXi F; // reordered triangle draw list (0, 1, 2, ...), indices based on V
        Eigen::MatrixXi cohE; // cohesive edge pairs with the 4 end vertex indices based on V
        Eigen::MatrixXi initSeams; // initial cohesive edge pairs actually

    public:
        const Scaffold* scaffold = NULL;
        double areaThres_AM; // for preventing degeneracy of air mesh triangles

    public: // owned features
        Eigen::VectorXi boundaryEdge; // 1: boundary edge, 0: interior edge
        Eigen::VectorXd edgeLen; // cohesive edge rest length, used as weights
        Eigen::SparseMatrix<double> LaplacianMtr; // 2 * V.rows() wide
        Eigen::VectorXd triArea; // triangle rest area
        Eigen::MatrixXd triNormal;
        double surfaceArea;
        Eigen::VectorXd triAreaSq; // triangle rest squared area
        Eigen::VectorXd e0dote1; // triangle rest edge dot product
        Eigen::VectorXd e0SqLen, e1SqLen; // triangle edge rest squared length
        Eigen::VectorXd e0SqLen_div_dbAreaSq;
        Eigen::VectorXd e1SqLen_div_dbAreaSq;
        Eigen::VectorXd e0dote1_div_dbAreaSq;
        double avgEdgeLen;
        double virtualRadius;
        std::vector<std::set<std::pair<int, int>>> validSplit;
        std::set<int> fixedVert; // for linear solve
        Eigen::Matrix<double, 2, 3> bbox;
        Eigen::VectorXd vertWeight; // for regional seam placement

        // indices for fast access
        std::map<std::pair<int, int>, int> edge2Tri;
        std::vector<std::set<int>> vNeighbor;
        std::map<std::pair<int, int>, int> cohEIndex;

        std::set<int> fracTail;
        int curFracTail;
        std::pair<int, int> curInteriorFracTails;
        double initSeamLen;

    public: // constructor
        // default constructor that doesn't do anything
        TriMesh(void);

        // initialize from a triangle mesh, V will be constructed from UV_mesh in 2D,
        // V_mesh will be used to initialize restShape
        TriMesh(const Eigen::MatrixXd& V_mesh, const Eigen::MatrixXi& F_mesh,
                     const Eigen::MatrixXd& UV_mesh, const Eigen::MatrixXi& FUV_mesh = Eigen::MatrixXi(),
                     bool separateTri = true, double p_initSeamLen = 0.0, double p_areaThres_AM = 0.0);

        TriMesh(Primitive primitive, double size = 1.0, double spacing = 0.1, bool separateTri = true);

    public: // API
        void computeFeatures(bool multiComp = false, bool resetFixedV = false);
        void updateFeatures(void);
        void resetFixedVert(const std::set<int>& p_fixedVert);
        void buildCohEfromRecord(const Eigen::MatrixXi& cohERecord);

        void querySplit(double lambda_t, bool propagate, bool splitInterior,
                        double& EwDec_max, std::vector<int>& path_max, Eigen::MatrixXd& newVertPos_max,
                        std::pair<double, double>& energyChanges_max) const;
        bool splitEdge(double lambda_t, double EDecThres = 0.0, bool propagate = false, bool splitInterior = false);
        void queryMerge(double lambda, bool propagate,
                        double& EwDec_max, std::vector<int>& path_max, Eigen::MatrixXd& newVertPos_max,
                        std::pair<double, double>& energyChanges_max);
        bool mergeEdge(double lambda, double EDecThres, bool propagate);
        bool splitOrMerge(double lambda_t, double EDecThres, bool propagate, bool splitInterior,
                          bool& isMerge);

        void onePointCut(int vI = 0);
        void highCurvOnePointCut(void);
        void farthestPointCut(void);
        void geomImgCut(TriMesh& data_findExtrema);
        void cutPath(std::vector<int> path, bool makeCoh = false, int changePos = 0,
                     const Eigen::MatrixXd& newVertPos = Eigen::MatrixXd(), bool allowCutThrough = true);

        void computeSeamScore(Eigen::VectorXd& seamScore) const;
        void computeBoundaryLen(double& boundaryLen) const;
        void computeSeamSparsity(double& sparsity, bool triSoup = false) const;
        void computeStandardStretch(double& stretch_l2, double& stretch_inf, double& stretch_shear, double& compress_inf) const;
        void computeL2StretchPerElem(Eigen::VectorXd& L2StretchPerElem) const;
        void outputStandardStretch(std::ofstream& file) const;
        void computeAbsGaussianCurv(double& absGaussianCurv) const;

        void initRigidUV(void);

        bool checkInversion(int triI, bool mute) const;
        bool checkInversion(bool mute = false, const std::vector<int>& triangles = std::vector<int>()) const;

        void save(const std::string& filePath, const Eigen::MatrixXd& V, const Eigen::MatrixXi& F,
                  const Eigen::MatrixXd UV, const Eigen::MatrixXi& FUV = Eigen::MatrixXi()) const;
        void save(const std::string& filePath) const;

        void saveAsMesh(const std::string& filePath, bool scaleUV = false) const;

        void saveAsMesh(const std::string& filePath,
                        const Eigen::MatrixXi& F0,
                        bool scaleUV = false) const;

    public: // helper function
        void computeLaplacianMtr(void);

        bool findBoundaryEdge(int vI, const std::pair<int, int>& startEdge,
                              std::pair<int, int>& boundaryEdge);

        bool insideTri(int triI, const Eigen::RowVector2d& pos) const;
        bool insideUVRegion(const std::vector<int>& triangles, const Eigen::RowVector2d& pos) const;

        // toBound = false indicate counter-clockwise
        bool isBoundaryVert(int vI, int vI_neighbor,
                            std::vector<int>& tri_toSep, std::pair<int, int>& boundaryEdge, bool toBound = true) const;
        bool isBoundaryVert(int vI) const;

        void compute2DInwardNormal(int vI, Eigen::RowVector2d& normal) const;

        void splitEdgeOnBoundary(const std::pair<int, int>& edge, const Eigen::MatrixXd& newVertPos,
                                bool changeVertPos = true, bool allowCutThrough = true);
        void mergeBoundaryEdges(const std::pair<int, int>& edge0, const std::pair<int, int>& edge1,
                                const Eigen::RowVectorXd& mergedPos);

        // query vertex candidate for either split or merge
        double computeLocalLDec(int vI, double lambda_t,
                                std::vector<int>& path,
                                Eigen::MatrixXd& newVertPos,
                                std::pair<double, double>& energyChanges,
                                const std::vector<int>& incTris = std::vector<int>(),
                                const Eigen::RowVector2d& initMergedPos = Eigen::RowVector2d()) const;
        // query interior incident edge of a boundary vertex candidate
        double queryLocalEdDec_bSplit(const std::pair<int, int>& edge,
                                      Eigen::MatrixXd& newVertPos) const;

        // boundary split
        double computeLocalEdDec_bSplit(const std::vector<int>& triangles,
                                        const std::set<int>& freeVert,
                                        const std::vector<int>& splitPath,
                                        Eigen::MatrixXd& newVertPos,
                                        int maxIter = 100) const;
        // interior split
        double computeLocalEdDec_inSplit(const std::vector<int>& triangles,
                                         const std::set<int>& freeVert,
                                         const std::vector<int>& path,
                                         Eigen::MatrixXd& newVertPos,
                                         int maxIter = 100) const;
        // merge
        double computeLocalEdDec_merge(const std::vector<int>& path,
                                       const std::vector<int>& triangles,
                                       const std::set<int>& freeVert,
                                       std::map<int, Eigen::RowVector2d>& newVertPos,
                                       const std::map<int, int>& mergeVert,
                                       const Eigen::RowVector2d& initMergedPos,
                                       bool closeup = false, int maxIter = 100) const;
    };

}

#endif /* TriMesh_hpp */


#ifndef STAN_MATH_PRIM_FUN_GAMMA_Q_HPP
#define STAN_MATH_PRIM_FUN_GAMMA_Q_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/functor/apply_scalar_binary.hpp>
#include <boost/math/special_functions/gamma.hpp>

namespace stan {
namespace math {

/**
 *
   \f[
   \mbox{gamma\_q}(a, z) =
   \begin{cases}
     \textrm{error} & \mbox{if } a\leq 0 \textrm{ or } z < 0\\
     Q(a, z) & \mbox{if } a > 0, z \geq 0 \\[6pt]
     \textrm{NaN} & \mbox{if } a = \textrm{NaN or } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{gamma\_q}(a, z)}{\partial a} =
   \begin{cases}
     \textrm{error} & \mbox{if } a\leq 0 \textrm{ or } z < 0\\
     \frac{\partial\, Q(a, z)}{\partial a} & \mbox{if } a > 0, z \geq 0 \\[6pt]
     \textrm{NaN} & \mbox{if } a = \textrm{NaN or } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{gamma\_q}(a, z)}{\partial z} =
   \begin{cases}
     \textrm{error} & \mbox{if } a\leq 0 \textrm{ or } z < 0\\
     \frac{\partial\, Q(a, z)}{\partial z} & \mbox{if } a > 0, z \geq 0 \\[6pt]
     \textrm{NaN} & \mbox{if } a = \textrm{NaN or } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   Q(a, z)=\frac{1}{\Gamma(a)}\int_z^\infty t^{a-1}e^{-t}dt
   \f]

   \f[
   \frac{\partial \, Q(a, z)}{\partial a} =
 -\frac{\Psi(a)}{\Gamma^2(a)}\int_z^\infty t^{a-1}e^{-t}dt
   + \frac{1}{\Gamma(a)}\int_z^\infty (a-1)t^{a-2}e^{-t}dt
   \f]

   \f[
   \frac{\partial \, Q(a, z)}{\partial z} = -\frac{z^{a-1}e^{-z}}{\Gamma(a)}
   \f]
   * @throws domain_error if x is at pole
 */
inline double gamma_q(double x, double a) { return boost::math::gamma_q(x, a); }

/**
 * Enables the vectorised application of the gamma_q function,
 * when the first and/or second arguments are containers.
 *
 * @tparam T1 type of first input
 * @tparam T2 type of second input
 * @param a First input
 * @param b Second input
 * @return gamma_q function applied to the two inputs.
 */
template <typename T1, typename T2, require_any_container_t<T1, T2>* = nullptr>
inline auto gamma_q(const T1& a, const T2& b) {
  return apply_scalar_binary(
      a, b, [&](const auto& c, const auto& d) { return gamma_q(c, d); });
}

}  // namespace math
}  // namespace stan
#endif


#ifndef TEST_HELPER_HH
#define TEST_HELPER_HH

#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include <boost/test/included/unit_test.hpp>

using bezier_com_traj::Matrix3;
using bezier_com_traj::MatrixX3;
using bezier_com_traj::MatrixXX;
using bezier_com_traj::Vector3;
using bezier_com_traj::VectorX;

#define MASS 50.
#define MU 0.5
#define LX 0.2172  // contact surface size in x direction
#define LY 0.138   // contact surface size in y direction
#define KIN_X_MIN -0.7
#define KIN_X_MAX 0.7
#define KIN_Y_MIN -0.4
#define KIN_Y_MAX 0.4
#define KIN_Z_MIN 0
#define KIN_Z_MAX 0.9
#define EPSILON 1e-6

typedef std::pair<MatrixX3, VectorX> ConstraintsPair;

std::pair<MatrixX3, MatrixX3> generateKinematicsConstraints() {
  // generate a simple polytgone  : faces aligned along x,y,z axis
  // size : x [-0.5,0.5] ; y = [-0.3,1] ; z [0,0.8]
  MatrixX3 N(6, 3);
  MatrixX3 V(6, 3);

  N.block<1, 3>(0, 0) = Vector3(-1, 0, 0);
  V.block<1, 3>(0, 0) = Vector3(KIN_X_MIN, KIN_Y_MAX, 0);
  N.block<1, 3>(1, 0) = Vector3(0, 1, 0);
  V.block<1, 3>(1, 0) = Vector3(KIN_X_MIN, KIN_Y_MAX, 0);
  N.block<1, 3>(2, 0) = Vector3(1, 0, 0);
  V.block<1, 3>(2, 0) = Vector3(KIN_X_MAX, KIN_Y_MIN, 0);
  N.block<1, 3>(3, 0) = Vector3(0, -1, 0);
  V.block<1, 3>(3, 0) = Vector3(KIN_X_MAX, KIN_Y_MIN, 0);
  N.block<1, 3>(4, 0) = Vector3(0, 0, -1);
  V.block<1, 3>(4, 0) = Vector3(0, 0, KIN_Z_MIN);
  N.block<1, 3>(5, 0) = Vector3(0, 0, 1);
  V.block<1, 3>(5, 0) = Vector3(0, 0, KIN_Z_MAX);

  return std::make_pair(N, V);
}

std::pair<MatrixXX, VectorX> generateKinematicsConstraints(Matrix3 endEffRotation, Vector3 endEffTranslation) {
  std::pair<MatrixX3, MatrixX3> NV = generateKinematicsConstraints();
  MatrixX3 N = NV.first;
  MatrixX3 V = NV.second;
  size_t numFaces = N.rows();
  MatrixX3 A(numFaces, 3);
  VectorX b(numFaces);
  VectorX n, v;

  for (size_t i = 0; i < numFaces; ++i) {
    n = endEffRotation * (N.block<1, 3>(i, 0).transpose());
    v = endEffRotation * (V.block<1, 3>(i, 0).transpose()) + endEffTranslation;
    A.block<1, 3>(i, 0) = n;
    b[i] = v.dot(n);
  }

  return std::make_pair(A, b);
}

std::pair<MatrixX3, MatrixX3> computeRectangularContacts(MatrixX3 normals, MatrixX3 positions, double size_X,
                                                         double size_Y) {
  // TODO : consider normal != z (see code in rbprm :: stability.cc (or add it as dependency ?)

  BOOST_CHECK(normals.rows() == positions.rows());
  MatrixX3 rec_normals(normals.rows() * 4, 3);
  MatrixX3 rec_positions(normals.rows() * 4, 3);

  double lx = size_X / 2.;
  double ly = size_Y / 2.;
  MatrixX3 p(4, 3);
  p << lx, ly, 0, lx, -ly, 0, -lx, -ly, 0, -lx, ly, 0;

  for (long int ic = 0; ic < normals.rows(); ++ic) {
    for (long int i = 0; i < 4; ++i) {
      rec_normals.block<1, 3>(ic * 4 + i, 0) = normals.block<1, 3>(ic, 0);
      rec_positions.block<1, 3>(ic * 4 + i, 0) = positions.block<1, 3>(ic, 0) + p.block<1, 3>(i, 0);
    }
  }
  return std::make_pair(rec_normals, rec_positions);
}

centroidal_dynamics::Equilibrium ComputeContactCone(
    MatrixX3 normals, MatrixX3 positions,
    const centroidal_dynamics::EquilibriumAlgorithm algo = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
  centroidal_dynamics::Equilibrium contactCone("test-quasiStatic", MASS, 4, centroidal_dynamics::SOLVER_LP_QPOASES,
                                               true, 10, false);
  // centroidal_dynamics::EquilibriumAlgorithm alg = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP;
  contactCone.setNewContacts(positions, normals, MU, algo);
  return contactCone;
}

std::pair<MatrixXX, VectorX> generateStabilityConstraints(centroidal_dynamics::Equilibrium contactPhase,
                                                          Vector3 acc = Vector3::Zero()) {
  const Vector3& g = contactPhase.m_gravity;
  const Matrix3 gSkew = bezier_com_traj::skew(g);
  const Matrix3 accSkew = bezier_com_traj::skew(acc);
  // compute GIWC
  centroidal_dynamics::MatrixXX Hrow;
  VectorX h;
  contactPhase.getPolytopeInequalities(Hrow, h);
  MatrixXX H = -Hrow;
  H.rowwise().normalize();
  int dimH = (int)(H.rows());
  MatrixXX mH = contactPhase.m_mass * H;
  // constraints : mH[:,3:6] g^  x <= h + mH[:,0:3]g
  // A = mH g^
  // b = h + mHg
  MatrixX3 A = mH.block(0, 3, dimH, 3) * (gSkew - accSkew);
  VectorX b = h + mH.block(0, 0, dimH, 3) * (g - acc);
  return std::make_pair(A, b);
}

std::pair<MatrixXX, VectorX> generateStabilityConstraints(
    MatrixX3 normals, MatrixX3 positions, Vector3 acc = Vector3::Zero(),
    const centroidal_dynamics::EquilibriumAlgorithm algo = centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP) {
  std::pair<MatrixX3, MatrixX3> contacts = computeRectangularContacts(normals, positions, LX, LY);
  centroidal_dynamics::Equilibrium contactPhase = ComputeContactCone(contacts.first, contacts.second, algo);
  return generateStabilityConstraints(contactPhase, acc);
}

std::pair<Matrix3, Vector3> computeCost() {
  Matrix3 H = Matrix3::Identity();
  Vector3 g = Vector3::Zero();
  return std::make_pair(H, g);
}

std::pair<MatrixX3, VectorX> generateConstraints(MatrixX3 normals, MatrixX3 positions, Matrix3 endEffRotation,
                                                 Vector3 endEffTranslation) {
  std::pair<MatrixX3, VectorX> Ab = generateKinematicsConstraints(endEffRotation, endEffTranslation);
  std::pair<MatrixX3, VectorX> Cd = generateStabilityConstraints(normals, positions);
  size_t numIneq = Ab.first.rows() + Cd.first.rows();
  MatrixXX M(numIneq, 3);
  VectorX n(numIneq);
  M.block(0, 0, Ab.first.rows(), 3) = Ab.first;
  M.block(Ab.first.rows(), 0, Cd.first.rows(), 3) = Cd.first;
  n.segment(0, Ab.first.rows()) = Ab.second;
  n.segment(Ab.first.rows(), Cd.first.rows()) = Cd.second;
  return std::make_pair(M, n);
}

double fRandom(double fMin, double fMax) {
  double f = (double)std::rand() / RAND_MAX;
  return fMin + f * (fMax - fMin);
}

ConstraintsPair stackConstraints(const ConstraintsPair& Ab, const ConstraintsPair& Cd) {
  size_t numIneq = Ab.first.rows() + Cd.first.rows();
  MatrixX3 M(numIneq, 3);
  VectorX n(numIneq);
  M.block(0, 0, Ab.first.rows(), 3) = Ab.first;
  M.block(Ab.first.rows(), 0, Cd.first.rows(), 3) = Cd.first;
  n.segment(0, Ab.first.rows()) = Ab.second;
  n.segment(Ab.first.rows(), Cd.first.rows()) = Cd.second;
  return std::make_pair(M, n);
}

bool verifyKinematicConstraints(const ConstraintsPair& Ab, const Vector3& point) {
  for (long int i = 0; i < Ab.second.size(); ++i) {
    if (Ab.first.block<1, 3>(i, 0).dot(point) > Ab.second[i]) {
      return false;
    }
  }
  return true;
}

bool verifyStabilityConstraintsDLP(centroidal_dynamics::Equilibrium contactPhase, Vector3 c, Vector3 /*dc*/,
                                   Vector3 ddc) {
  bool success(false);
  double res;
  centroidal_dynamics::Equilibrium contactPhaseDLP(contactPhase);
  contactPhaseDLP.setAlgorithm(centroidal_dynamics::EQUILIBRIUM_ALGORITHM_DLP);
  centroidal_dynamics::LP_status status = contactPhaseDLP.computeEquilibriumRobustness(c, ddc, res);
  success = (status == centroidal_dynamics::LP_STATUS_OPTIMAL || status == centroidal_dynamics::LP_STATUS_UNBOUNDED);
  if (success) success = res >= -EPSILON;
  if (!success) std::cout << "fail level " << res << std::endl;
  return success;
}

bool verifyStabilityConstraintsPP(centroidal_dynamics::Equilibrium contactPhase, Vector3 c, Vector3 /*dc*/,
                                  Vector3 acc) {
  // compute inequalities :
  const Vector3& g = contactPhase.m_gravity;
  const Matrix3 gSkew = bezier_com_traj::skew(g);
  const Matrix3 accSkew = bezier_com_traj::skew(acc);
  // compute GIWC
  centroidal_dynamics::MatrixXX Hrow;
  VectorX h;
  contactPhase.getPolytopeInequalities(Hrow, h);
  MatrixXX H = -Hrow;
  H.rowwise().normalize();
  int dimH = (int)(H.rows());
  MatrixXX mH = contactPhase.m_mass * H;
  // constraints : mH[:,3:6] g^  x <= h + mH[:,0:3]g
  // A = mH g^
  // b = h + mHg
  MatrixX3 A = mH.block(0, 3, dimH, 3) * (gSkew - accSkew);
  VectorX b = h + mH.block(0, 0, dimH, 3) * (g - acc);

  // verify inequalities with c :
  for (long int i = 0; i < b.size(); ++i) {
    if (A.block<1, 3>(i, 0).dot(c) - EPSILON > b[i]) {
      return false;
    }
  }
  return true;
}

#endif  // TEST_HELPER_HH


#include <NTL/ZZ_pX.h>
#include <NTL/FFT.h>

#include <cstdio>

NTL_CLIENT

double clean_data(double *t)
{
   double x, y, z;
   long i, ix, iy, n;

   x = t[0]; ix = 0;
   y = t[0]; iy = 0;

   for (i = 1; i < 5; i++) {
      if (t[i] < x) {
         x = t[i];
         ix = i;
      }
      if (t[i] > y) {
         y = t[i];
         iy = i;
      }
   }

   z = 0; n = 0;
   for (i = 0; i < 5; i++) {
      if (i != ix && i != iy) z+= t[i], n++;
   }

   z = z/n;

   return z;
}

void print_flag()
{

#if defined(NTL_FFT_LAZYMUL)
printf("FFT_LAZYMUL ");
#endif

#if defined(NTL_SPMM_ULL)
printf("SPMM_ULL ");
#endif

#if defined(NTL_AVOID_BRANCHING)
printf("AVOID_BRANCHING ");
#endif

#if defined(NTL_FFT_BIGTAB)
printf("FFT_BIGTAB ");
#endif

printf("\n");

}

int main()
{

   SetSeed(ZZ(0));

   long n, k;

   n = 200;
   k = 10*NTL_ZZ_NBITS;

   ZZ p;

   RandomLen(p, k);
   if (!IsOdd(p)) p++;

   ZZ_p::init(p);         // initialization

   ZZ_pX f, g, h, r1, r2, r3;

   random(g, n);    // g = random polynomial of degree < n
   random(h, n);    // h =             "   "
   random(f, n);    // f =             "   "

   SetCoeff(f, n);  // Sets coefficient of X^n to 1

   // For doing arithmetic mod f quickly, one must pre-compute
   // some information.

   ZZ_pXModulus F;
   build(F, f);

   PlainMul(r1, g, h);  // this uses classical arithmetic
   PlainRem(r1, r1, f);

   MulMod(r2, g, h, F);  // this uses the FFT

   MulMod(r3, g, h, f);  // uses FFT, but slower

   // compare the results...

   if (r1 != r2) {
      printf("999999999999999 ");
      print_flag();
      return 0;
   }
   else if (r1 != r3) {
      printf("999999999999999 ");
      print_flag();
      return 0;
   }

   double t;
   long i, j;
   long iter;

   const int nprimes = 30;
   const long L = 12;
   const long N = 1L << L;
   long r;

   for (r = 0; r < nprimes; r++) UseFFTPrime(r);

   vec_long A1[nprimes], A2[nprimes];
   vec_long B1[nprimes], B2[nprimes];

   for (r = 0; r < nprimes; r++) {
      A1[r].SetLength(N);
      A2[r].SetLength(N);
      B1[r].SetLength(N);
      B2[r].SetLength(N);

      for (i = 0; i < N; i++) {
         A1[r][i] = RandomBnd(GetFFTPrime(r));
         A2[r][i] = RandomBnd(GetFFTPrime(r));
      }
   }

   for (r = 0; r < nprimes; r++) {
      long *A1p = A1[r].elts();
      long *A2p = A2[r].elts();
      long *B1p = B1[r].elts();
      long *B2p = B2[r].elts();
      long q = GetFFTPrime(r);
      mulmod_t qinv = GetFFTPrimeInv(r);

      FFTFwd(B1p, A1p, L, r);
      FFTFwd(B2p, A2p, L, r);
      for (i = 0; i < N; i++) B1p[i] = NormalizedMulMod(B1p[i], B2p[i], q, qinv);
      FFTRev1(B1p, B1p, L, r);
   }

   iter = 1;

   do {
     t = GetTime();
     for (j = 0; j < iter; j++) {
        for (r = 0; r < nprimes; r++) {
           long *A1p = A1[r].elts();
           long *A2p = A2[r].elts();
           long *B1p = B1[r].elts();
           long *B2p = B2[r].elts();
           long q = GetFFTPrime(r);
           mulmod_t qinv = GetFFTPrimeInv(r);

           FFTFwd(B1p, A1p, L, r);
           FFTFwd(B2p, A2p, L, r);
           for (i = 0; i < N; i++) B1p[i] = NormalizedMulMod(B1p[i], B2p[i], q, qinv);
           FFTRev1(B1p, B1p, L, r);
        }
     }
     t = GetTime() - t;
     iter = 2*iter;
   } while(t < 1);

   iter = iter/2;

   iter = long((3/t)*iter) + 1;

   double tvec[5];
   long w;

   for (w = 0; w < 5; w++) {
     t = GetTime();
     for (j = 0; j < iter; j++) {
        for (r = 0; r < nprimes; r++) {
           long *A1p = A1[r].elts();
           long *A2p = A2[r].elts();
           long *B1p = B1[r].elts();
           long *B2p = B2[r].elts();
           long q = GetFFTPrime(r);
           mulmod_t qinv = GetFFTPrimeInv(r);

           FFTFwd(B1p, A1p, L, r);
           FFTFwd(B2p, A2p, L, r);
           for (i = 0; i < N; i++) B1p[i] = NormalizedMulMod(B1p[i], B2p[i], q, qinv);
           FFTRev1(B1p, B1p, L, r);
        }
     }
     t = GetTime() - t;
     tvec[w] = t;
   }

   t = clean_data(tvec);

   t = floor((t/iter)*1e13);

   if (t < 0 || t >= 1e15)
      printf("999999999999999 ");
   else
      printf("%015.0f ", t);

   printf(" [%ld] ", iter);

   print_flag();

   return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file EigenOptimizer.cpp
 *
 * @brief optimizer linear factor graph using eigen solver as backend
 *
 * @date Aug 2019
 * @author Mandy Xie
 */

#include <gtsam/base/timing.h>
#include <gtsam/linear/EigenOptimizer.h>
#include <Eigen/MetisSupport>
#include <vector>

using namespace std;

namespace gtsam {
using SpMat = Eigen::SparseMatrix<double>;
/// obtain sparse matrix for eigen sparse solver
std::pair<SpMat, Eigen::VectorXd> obtainSparseMatrix(
    const GaussianFactorGraph &gfg,
    boost::optional<const Ordering &> ordering = boost::none) {
  gttic_(EigenOptimizer_obtainSparseMatrix);
  // Get sparse entries of Jacobian [A|b] augmented with RHS b.
  auto entries = gfg.sparseJacobian(ordering);
  // Convert boost tuples to Eigen triplets
  vector<Eigen::Triplet<double>> triplets;
  triplets.reserve(entries.size());
  size_t rows = 0, cols = 0;
  for (const auto &e : entries) {
    size_t temp_rows = e.get<0>(), temp_cols = e.get<1>();
    triplets.emplace_back(temp_rows, temp_cols, e.get<2>());
    rows = std::max(rows, temp_rows);
    cols = std::max(cols, temp_cols);
  }
  // ...and make a sparse matrix with it.
  SpMat Ab(rows + 1, cols + 1);
  Ab.setFromTriplets(triplets.begin(), triplets.end());
  Ab.makeCompressed();
  return make_pair<SpMat, Eigen::VectorXd>(Ab.block(0, 0, rows + 1, cols),
                                           Ab.col(cols));
}

/* ************************************************************************* */
VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg,
                             const Ordering &ordering) {
  gttic_(EigenOptimizer_optimizeEigenQR);
  auto Ab_pair = obtainSparseMatrix(gfg, ordering);
  // Solve A*x = b using sparse QR from Eigen
  gttic_(EigenOptimizer_optimizeEigenQR_create_solver);
  Eigen::SparseQR<SpMat, Eigen::NaturalOrdering<int>> solver(Ab_pair.first);
  gttoc_(EigenOptimizer_optimizeEigenQR_create_solver);
  gttic_(EigenOptimizer_optimizeEigenQR_solve);
  Eigen::VectorXd x = solver.solve(Ab_pair.second);
  gttoc_(EigenOptimizer_optimizeEigenQR_solve);
  return VectorValues(x, gfg.getKeyDimMap());
}

VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg,
                             const std::string &orderingType) {
  if (orderingType == "COLAMD") {
    return optimizeEigenQR(gfg, Ordering::Colamd(gfg));
  } else if (orderingType == "METIS") {
    return optimizeEigenQR(gfg, Ordering::Metis(gfg));
  } else if (orderingType == "NATURAL") {
    return optimizeEigenQR(gfg, Ordering::Natural(gfg));
  } else {
    std::cout
        << "No applicable ordering method, use COLAMD ordering by default !!"
        << std::endl;
    return optimizeEigenQR(gfg, Ordering::Colamd(gfg));
  }
}

/* *************************************************************************/
VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg,
                                   const Ordering &ordering) {
  gttic_(EigenOptimizer_optimizeEigenCholesky);
  auto Ab_pair = obtainSparseMatrix(gfg, ordering);
  auto A = Ab_pair.first;
  gttic_(EigenOptimizer_optimizeEigenCholesky_Atranspose);
  auto At = A.transpose();
  gttoc_(EigenOptimizer_optimizeEigenCholesky_Atranspose);
  gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver);
  // Solve A*x = b using sparse QR from Eigen
  Eigen::SimplicialLDLT<SpMat, Eigen::Lower, Eigen::NaturalOrdering<int>>
      solver(At * A);
  gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver);
  gttic_(EigenOptimizer_optimizeEigenCholesky_solve);
  Eigen::VectorXd x = solver.solve(At * Ab_pair.second);
  gttoc_(EigenOptimizer_optimizeEigenCholesky_solve);
  return VectorValues(x, gfg.getKeyDimMap());
}

VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg,
                                   const std::string &orderingType) {
  if (orderingType == "COLAMD") {
    return optimizeEigenCholesky(gfg, Ordering::Colamd(gfg));
  } else if (orderingType == "METIS") {
    return optimizeEigenCholesky(gfg, Ordering::Metis(gfg));
  } else if (orderingType == "NATURAL") {
    return optimizeEigenCholesky(gfg, Ordering::Natural(gfg));
  } else {
    std::cout
        << "No applicable ordering method, use COLAMD ordering by default !!"
        << std::endl;
    return optimizeEigenCholesky(gfg, Ordering::Colamd(gfg));
  }
}

}  // namespace gtsam


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

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

#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/is_lez.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/is_odd.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/nan.hpp>

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( signgam_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
#ifdef BOOST_SIMD_NO_INVALIDS
      if (is_lez(a0))
      {
        if (is_flint(a0))
          return Nan<A0>();
        else
          return One<A0>()-bool(is_odd(floor(a0)))*Two<A0>();
      }
#else
      if (is_lez(a0))
      {
        if (is_flint(a0)||is_inf(a0))
          return Nan<A0>();
        else
          return One<A0>()-bool(is_odd(floor(a0)))*Two<A0>();
      }
      else if (is_nan(a0)) return a0;
#endif
      return One<A0>();
    }
  };
} } }

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 reduction toolbox - sum/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of reduction components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 24/02/2011
///
#include <nt2/reduction/include/functions/sum.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/simd/logical.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/include/functions/arith.hpp>

NT2_TEST_CASE_TPL ( sum_real__1_0,  NT2_SIMD_REAL_TYPES)
{
  using nt2::sum;
  using nt2::tag::sum_;
  using boost::simd::native;
  using nt2::meta::cardinal_of;

  typedef NT2_SIMD_DEFAULT_EXTENSION                ext_t;
  typedef std::complex<T>                           cT;
  typedef native<T ,ext_t>                          vT;
  typedef native<cT ,ext_t>                         vcT;
  typedef typename nt2::meta::call<sum_(vcT)>::type r_t;

  // specific values tests
  vT r =  nt2::arith<vT>(0, 1);
  vT i =  nt2::arith<vT>(0, 1);
  vcT z = vcT(r, i);
  const size_t N = cardinal_of<vT>::value;

  NT2_TEST_EQUAL(sum(z), cT((N*(N-1))/2, (N*(N-1))/2));
  i = nt2::arith<vT>(T(cardinal_of<vT>::value-1), T(-1));
  z = vcT(r, i);

  NT2_TEST_EQUAL(sum(z), cT((N*(N-1))/2,(N*(N-1))/2));

} // end of test for floating_


#ifndef SHIFT_MATH_SERIALIZATION2_HPP
#define SHIFT_MATH_SERIALIZATION2_HPP

#include <cstdint>
#include <boost/endian/conversion.hpp>
#include <shift/serialization2/types.hpp>
#include "shift/math/vector.hpp"
#include "shift/math/matrix.hpp"
#include "shift/math/aabb.hpp"

namespace shift::serialization2
{
///
template <boost::endian::order Order, std::size_t N, typename T>
compact_input_archive<Order>& operator>>(compact_input_archive<Order>& archive,
                                         math::vector<N, T>& vector)
{
  for (std::size_t i = 0; i < N; ++i)
    archive >> vector(i);
  return archive;
}

///
template <boost::endian::order Order, std::size_t N, typename T>
compact_output_archive<Order>& operator<<(
  compact_output_archive<Order>& archive, const math::vector<N, T>& vector)
{
  for (std::size_t i = 0; i < N; ++i)
    archive << vector(i);
  return archive;
}

///
template <boost::endian::order Order, std::size_t Rows, std::size_t Columns,
          typename T>
compact_input_archive<Order>& operator>>(compact_input_archive<Order>& archive,
                                         math::matrix<Rows, Columns, T>& matrix)
{
  for (std::size_t i = 0; i < Columns; ++i)
    archive >> matrix.column_vector(i);
  return archive;
}

///
template <boost::endian::order Order, std::size_t Rows, std::size_t Columns,
          typename T>
compact_output_archive<Order>& operator<<(
  compact_output_archive<Order>& archive,
  const math::matrix<Rows, Columns, T>& matrix)
{
  for (std::size_t i = 0; i < Columns; ++i)
    archive << matrix.column_vector(i);
  return archive;
}

///
template <boost::endian::order Order, std::size_t N, typename T>
compact_input_archive<Order>& operator>>(compact_input_archive<Order>& archive,
                                         math::aabb<N, T>& box)
{
  archive >> box.center >> box.extent;
  return archive;
}

///
template <boost::endian::order Order, std::size_t N, typename T>
compact_output_archive<Order>& operator<<(
  compact_output_archive<Order>& archive, const math::aabb<N, T>& box)
{
  archive << box.center << box.extent;
  return archive;
}
}

#endif


//=======================================================================
// Copyright (c) 2013 Maciej Andrejczuk
//               2014 Piotr Wygocki
//
// 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)
//=======================================================================
/**
 * @file steiner_component.hpp
 * @brief
 * @author Maciej Andrejczuk, Piotr Wygocki
 * @version 1.0
 * @date 2013-08-01
 */
#ifndef PAAL_STEINER_COMPONENT_HPP
#define PAAL_STEINER_COMPONENT_HPP

#include "paal/data_structures/metric/basic_metrics.hpp"
#include "paal/steiner_tree/dreyfus_wagner.hpp"
#include "paal/utils/irange.hpp"

#include <boost/range/algorithm/transform.hpp>
#include <boost/range/join.hpp>

#include <iosfwd>
#include <set>

namespace paal {
namespace ir {

/**
 * @class steiner_component
 * @brief Class represents k-components of Steiner Tree.
 * Component is a subtree whose terminals coincide with leaves.
 */
template <typename Vertex, typename Dist>
class steiner_component {
public:
    using Edge = typename std::pair<Vertex, Vertex>;
    using Vertices = typename std::vector<Vertex>;

    ///constructor
    template<typename Metric, typename Terminals>
    steiner_component(const Metric & cost_map, Vertices terminals, const Terminals& steiner_vertices) :
        m_terminals(std::move(terminals)), m_size(m_terminals.size()) {

        auto all_elements = boost::join(m_terminals, steiner_vertices);
        data_structures::array_metric<typename data_structures::metric_traits<Metric>::DistanceType>
            fast_metric(cost_map, all_elements);
        auto term_nr = boost::distance(m_terminals);
        auto all_elements_nr = boost::distance(all_elements);
        auto dw = paal::make_dreyfus_wagner(fast_metric,
                    irange(term_nr),
                    irange(int(term_nr), int(all_elements_nr)));
        dw.solve();
        m_cost = dw.get_cost();
        auto &steiner = dw.get_steiner_elements();
        m_steiner_elements.resize(steiner.size());
        auto id_to_elem = [&](int i){
                if(i < term_nr) {
                    return m_terminals[i];
                } else {
                    return steiner_vertices[i - term_nr];
                }
        };
        boost::transform(steiner, m_steiner_elements.begin(), id_to_elem);
        m_edges.resize(dw.get_edges().size());
        boost::transform(dw.get_edges(), m_edges.begin(), [=](std::pair<int, int> e) {
            return std::make_pair(id_to_elem(e.first), id_to_elem(e.second));
        });
    }

    /**
     * @brief Each component has versions, where sink is chosen from its
     * terminals
     */
    Vertex get_sink(int version) const {
        assert(version < count_terminals());
        return m_terminals[version];
    }

    /**
     * Returns vector composed of component's terminals.
     */
    const Vertices &get_terminals() const { return m_terminals; }

    /**
     * Returns vector composed of component's nonterminals, i.e. Steiner
     * elements.
     */
    const Vertices &get_steiner_elements() const {
        return m_steiner_elements;
    }

    /**
     * Returns edges spanning the component.
     */
    const std::vector<Edge> &get_edges() const { return m_edges; }

    /**
     * Returns degree of component, i.e. number of terminals.
     */
    int count_terminals() const { return m_size; }

    /**
     * Returns minimal cost of spanning a component.
     */
    Dist get_cost() const { return m_cost; }

    /**
     * Prints the component.
     */
    friend std::ostream &operator<<(std::ostream &stream,
                                    const steiner_component &component) {
        for (int i = 0; i < component.m_size; i++) {
            stream << component.m_terminals[i] << " ";
        }
        stream << ": ";
        for (auto edge : component.m_edges) {
            stream << "(" << edge.first << "," << edge.second << ") ";
        }
        stream << component.m_cost;
        return stream;
    }

  private:
    const Vertices m_terminals; // terminals of the component
    int m_size;                           // m_terminals.size()
    Dist m_cost; // minimal cost of spanning the component
    Vertices m_steiner_elements; // non-terminals selected for
                                            // spanning tree
    std::vector<Edge> m_edges;              // edges spanning the component
};

} // ir
} // paal

#endif // PAAL_STEINER_COMPONENT_HPP


//Link to Boost
#define BOOST_TEST_DYN_LINK

//Define our Module name (prints at testing)
#define BOOST_TEST_MODULE BetheSolver UnitTests

//VERY IMPORTANT - include this last
#include <boost/test/included/unit_test.hpp>
//#include <boost/test/unit_test.hpp>

#include "test.h"
#include "../HomotopyContinuation.h"

bool isInteger(elem_t n) {
    return std::abs(floor(n + eps) - n) < eps;
}

// test suite
BOOST_FIXTURE_TEST_SUITE(UnityEquation_suite, SimpleTestFixture, * utf::label("UnityEquation"))

BOOST_DATA_TEST_CASE(numberOfRoots_test, bdata::random(1, 4) ^ bdata::random(1, 7) ^ bdata::xrange(20), nEq, order, index)
{
    UnityEquation uss(nEq, order);
    int expected = 1;
    for (int i = 0; i < nEq; i++) {
        expected *= order;
    }

    BOOST_TEST(uss.numberOfRoots() == expected);
}

BOOST_DATA_TEST_CASE(getRoot_test, bdata::random(1, 4) ^ bdata::random(1, 7) ^ bdata::xrange(20), nEq, order, index)
{
    UnityEquation uss(nEq, order);
    std::vector<int> phases;
    for (int i = 0; i < uss.numberOfRoots(); i++) {
        Vector root = uss.getRoot(i);
        BOOST_TEST(root.size() == nEq);

        int index = 0;
        for (int j = 0; j < root.size(); j++) {
            BOOST_TEST(std::abs(std::abs(root[j]) - (elem_t)1.0) < eps);
            elem_t phi = std::arg(root[j]);
            BOOST_TEST(isInteger(phi/(2 * Pi/order)));
            int m = (int)floor(phi/(2 * Pi/order) + eps);
            if (m < 0) m += order;
            BOOST_TEST(m < order);
            index = index * order + m;
        }
        phases.push_back(index);
    }
    std::sort(phases.begin(), phases.end());
    std::vector<int>::iterator it = std::unique(phases.begin(), phases.end());
    phases.resize(std::distance(phases.begin(), it));
    BOOST_TEST(uss.numberOfRoots() == phases.size());
}

BOOST_DATA_TEST_CASE(eval_test, bdata::random(1, 4) ^ bdata::random(1, 7) ^ bdata::xrange(20), nEq, order, index)
{
    UnityEquation uss(nEq, order);
    Vector root = uss.getRoot(random64(uss.numberOfRoots()));
    Solution sol(root);
    Vector res = uss.eval(sol);
    for (int i = 0; i < res.size(); i++) {
        BOOST_TEST(abs(res[i]) < eps);
    }
}

BOOST_DATA_TEST_CASE(diff_test, bdata::random(1, 4) ^ bdata::random(1, 7) ^ bdata::xrange(20), nEq, order, index)
{
    UnityEquation uss(nEq, order);
    Vector root = uss.getRoot(random64(uss.numberOfRoots()));
    Solution sol(root);
    Matrix res = uss.diff(sol);
    for (int i = 0; i < res.rows(); i++) {
        for (int j = 0; j < res.cols(); j++) {
            if (i != j) {
                BOOST_TEST(std::abs(res(i, j)) < eps);
            } else {
                BOOST_TEST(abs(std::abs(res(i, j) * root[i]) - order) < eps);
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()

class QuardraticEquations : public Function
{
private:
    std::vector<var_t> _coefs1;
    std::vector<var_t> _coefs2;
public:
    QuardraticEquations() {
        _nEq = 2;
    }

    void setCoefs(std::vector<var_t> &coefs1, std::vector<var_t> &coefs2) {
        _coefs1 = coefs1;
        _coefs2 = coefs2;
    }

    static var_t eval(const Solution &sol, std::vector<var_t> coefs) {
        const Vector &vars = sol.root();
        return coefs[0] + coefs[1] * vars[0] + coefs[2] * vars[1] +
        coefs[3] * vars[0] * vars[0] + coefs[4] * vars[0] * vars[1] +
        coefs[5] * vars[1] * vars[1];
    }

    virtual Vector eval(const Solution &sol) {
        Vector ret(2);
        ret[0] = eval(sol, _coefs1);
        ret[1] = eval(sol, _coefs2);
        return ret;
    }

    virtual Matrix diff(const Solution &sol) {
        Matrix ret(2, 2);
        const Vector &vars = sol.root();
        ret(0, 0) = _coefs1[1] + _coefs1[3] * vars[0] * (elem_t)2.0 + _coefs1[4] * vars[1];
        ret(0, 1) = _coefs1[2] + _coefs1[4] * vars[0] + _coefs1[5] * vars[1] * (elem_t)2.0;
        ret(1, 0) = _coefs2[1] + _coefs2[3] * vars[0] * (elem_t)2.0 + _coefs2[4] * vars[1];
        ret(1, 1) = _coefs2[2] + _coefs2[4] * vars[0] + _coefs2[5] * vars[1] * (elem_t)2.0;
        return ret;
    }
};

// test suite
BOOST_FIXTURE_TEST_SUITE(MultiVariables_suite, SimpleTestFixture, * utf::label("MultiVariables"))

BOOST_AUTO_TEST_CASE(Demo1)
{
    UnityEquation start(2, 2);
    QuardraticEquations target;
    // construct equations with one solution (x=3,y=-2).
    // first equation is -12+3x-4y+x^2+xy-2y^2==0
    std::vector<var_t> coefs1({(elem_t)-12.0, (elem_t)3.0, (elem_t)-4.0, (elem_t)1.0, (elem_t)1.0, (elem_t)-2.0});
    // second equation is -32-2x+y+2x^2-3xy+y^2==0
    std::vector<var_t> coefs2({(elem_t)-32.0, (elem_t)-2.0, (elem_t)1.0, (elem_t)2.0, (elem_t)-3.0, (elem_t)1.0});
    target.setCoefs(coefs1, coefs2);
    SimpleHomotopy sh(&start, &target);
    sh.setSteps(1000);
    SimpleHomotopyContinuation hc;

    // there should be 3 solutions
    //.. todo

    Vector expected(2);
    expected << var_t(3.0), var_t(-2.0);
    bool found = false;
    for (int i = 0; i < start.numberOfRoots(); i++) {
        Vector startRoot = start.getRoot(i);
        Solution sol(startRoot);
        hc.solve(sh, sol);
        BOOST_TEST_INFO("root=" << sol.root() << " norm=" << sh.evalTarget(sol).norm());
        BOOST_TEST(sh.evalTarget(sol).norm() == 0, tt::tolerance(eps));
        if ((sol.root() - expected).norm() < eps) {
            found = true;
        }
//        std::cout << "solution " << i << ": " << target.getRoot(i) << std::endl;
//        std::cout << "verify: " << target.eval(target.getRoot(i)) << std::endl;
    }

    BOOST_TEST(found);
}

BOOST_AUTO_TEST_SUITE_END()


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2013-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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)
*
*****************************************************************************/

#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>

#include <rokko/grid.hpp>
#include <rokko/solver.hpp>
#include <rokko/distributed_matrix.hpp>
#include <rokko/localized_matrix.hpp>

#include <rokko/utility/frank_matrix.hpp>

#define BOOST_TEST_MODULE test_distributed_matrix
#ifndef BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

template<typename T, typename MATRIX_MAJOR>
void localized_2_distributed(const rokko::localized_matrix<T, MATRIX_MAJOR>& lmat, rokko::distributed_matrix<MATRIX_MAJOR>& mat) {
  if (mat.get_m_global() != mat.get_n_global())
    BOOST_THROW_EXCEPTION(std::invalid_argument("frank_matrix::generate() : non-square matrix"));
  for(int local_i = 0; local_i < mat.get_m_local(); ++local_i) {
    for(int local_j = 0; local_j < mat.get_n_local(); ++local_j) {
      int global_i = mat.translate_l2g_row(local_i);
      int global_j = mat.translate_l2g_col(local_j);
      mat.set_local(local_i, local_j, lmat(global_i, global_j));
    }
  }
}

BOOST_AUTO_TEST_CASE(test_distributed_matrix) {
  int argc = boost::unit_test::framework::master_test_suite().argc;
  char** argv = boost::unit_test::framework::master_test_suite().argv;
  unsigned int dim = 10;
  int provided;
  MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
  MPI_Comm comm = MPI_COMM_WORLD;
  rokko::grid g(comm);
  BOOST_FOREACH(std::string name, rokko::parallel_dense_solver::solvers()) {
    rokko::parallel_dense_solver solver(name);
    solver.initialize(argc, argv);
    rokko::distributed_matrix<rokko::matrix_col_major> mat(dim, dim, g, solver);
    rokko::localized_matrix<double, rokko::matrix_col_major> lmat(dim, dim);
    rokko::frank_matrix::generate(lmat);
    localized_2_distributed(lmat, mat);
    rokko::frank_matrix::generate(mat);
    mat.print();
    //BOOST_CHECK_CLOSE(mat.get_global(0, 0), dim, 1e-14);
    solver.finalize();
  }
  MPI_Finalize();
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2018 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include "nthorderderivativeop.hpp"
#include "utilities.hpp"

#include <ql/math/comparison.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/math/interpolations/bicubicsplineinterpolation.hpp>
#include <ql/math/matrixutilities/bicgstab.hpp>
#include <ql/math/optimization/levenbergmarquardt.hpp>
#include <ql/math/richardsonextrapolation.hpp>
#include <ql/methods/finitedifferences/meshers/concentrating1dmesher.hpp>
#include <ql/methods/finitedifferences/meshers/fdmmeshercomposite.hpp>
#include <ql/methods/finitedifferences/meshers/fdmhestonvariancemesher.hpp>
#include <ql/methods/finitedifferences/meshers/predefined1dmesher.hpp>
#include <ql/methods/finitedifferences/meshers/uniform1dmesher.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearopcomposite.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearoplayout.hpp>
#include <ql/methods/finitedifferences/operators/nthorderderivativeop.hpp>
#include <ql/methods/finitedifferences/operators/firstderivativeop.hpp>
#include <ql/methods/finitedifferences/operators/secondderivativeop.hpp>
#include <ql/methods/finitedifferences/operators/secondordermixedderivativeop.hpp>
#include <ql/methods/finitedifferences/utilities/fdminnervaluecalculator.hpp>
#include <ql/methods/finitedifferences/solvers/fdm2dimsolver.hpp>
#include <ql/methods/finitedifferences/solvers/fdmbackwardsolver.hpp>
#include <ql/pricingengines/vanilla/analytichestonengine.hpp>

#include <numeric>
#include <utility>

using namespace QuantLib;
using namespace boost::unit_test_framework;

#ifndef QL_NO_UBLAS_SUPPORT

#include <boost/numeric/ublas/banded.hpp>

#if defined(__clang__) || defined(__GNUC__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-local-typedef"
#endif

#include <boost/numeric/ublas/operation_sparse.hpp>

#if defined(__clang__) || defined(__GNUC__)
#pragma clang diagnostic pop
#endif

#include <boost/numeric/ublas/matrix_proxy.hpp>

void NthOrderDerivativeOpTest::testSparseMatrixApply() {
    BOOST_TEST_MESSAGE("Testing sparse matrix apply...");

    SparseMatrix sm(5,7);

    BOOST_CHECK(sm.size1() == SparseMatrix::size_type(5));
    BOOST_CHECK(sm.size2() == SparseMatrix::size_type(7));

    sm(1,3) = 3.0;

    const Array x(7,0.0,1.0);
    const Array y = prod(sm, x);

    BOOST_CHECK(close_enough(y[0], 0.0));
    BOOST_CHECK(close_enough(y[1], 3.0*3.0));
    BOOST_CHECK(close_enough(y[2], 0.0));
    BOOST_CHECK(close_enough(y[3], 0.0));
    BOOST_CHECK(close_enough(y[4], 0.0));
}

void NthOrderDerivativeOpTest::testFirstOrder2PointsApply() {
    BOOST_TEST_MESSAGE("Testing two points first order "
            "derivative operator apply on an uniform grid...");

    const Real dx = 1/5.0;

    const NthOrderDerivativeOp op(0, 1, 3,
        ext::make_shared<FdmMesherComposite>(
            ext::make_shared<Uniform1dMesher>(0.0, 1.0, 6)));

    const Array x(6,0.0, 1.0);
    const Array y = op.apply(x);

    for (Size i=0; i < x.size(); ++i)
        BOOST_CHECK(close_enough(y[i], 1/dx));
}

void NthOrderDerivativeOpTest::testFirstOrder3PointsOnUniformGrid() {
    BOOST_TEST_MESSAGE("Testing three points first order "
            "derivative operator on an uniform grid...");

    const Real ddx = 1.0/0.2;

    const SparseMatrix m =
        NthOrderDerivativeOp(0, 1, 3,
            ext::make_shared<FdmMesherComposite>(
                ext::make_shared<Uniform1dMesher>(0.0, 1.0, 6))).toMatrix();

    // to reproduce the reference results use
    // http://web.media.mit.edu/~crtaylor/calculator.html

    BOOST_CHECK(close_enough(m(2,0), 0.0));
    BOOST_CHECK(close_enough(m(2,1), -0.5*ddx));
    BOOST_CHECK(m(2,2) < 42*QL_EPSILON);
    BOOST_CHECK(close_enough(m(2,3), 0.5*ddx));
    BOOST_CHECK(close_enough(m(2,4), 0.0));
    BOOST_CHECK(close_enough(m(2,5), 0.0));

    BOOST_CHECK(close_enough(m(0,0), -3.0/2.0*ddx));
    BOOST_CHECK(close_enough(m(0,1), 2.0*ddx));
    BOOST_CHECK(close_enough(m(0,2), -0.5*ddx));
    BOOST_CHECK(close_enough(m(0,3), 0.0));
    BOOST_CHECK(close_enough(m(0,4), 0.0));
    BOOST_CHECK(close_enough(m(0,5), 0.0));

    BOOST_CHECK(close_enough(m(5,0), 0.0));
    BOOST_CHECK(close_enough(m(5,1), 0.0));
    BOOST_CHECK(close_enough(m(5,2), 0.0));
    BOOST_CHECK(close_enough(m(5,3), 0.5*ddx));
    BOOST_CHECK(close_enough(m(5,4), -2.0*ddx));
    BOOST_CHECK(close_enough(m(5,5), 3.0/2.0*ddx));
}

void NthOrderDerivativeOpTest::testFirstOrder5PointsOnUniformGrid() {
    BOOST_TEST_MESSAGE("Testing five points first order "
            "derivative operator on an uniform grid...");

    const Real ddx = 1.0/0.4;

    const SparseMatrix m =
        NthOrderDerivativeOp(0, 1, 5,
            ext::make_shared<FdmMesherComposite>(
                ext::make_shared<Uniform1dMesher>(0.0, 2.0, 6))).toMatrix();

    BOOST_CHECK(close_enough(m(2,0), 1.0/12.0*ddx));
    BOOST_CHECK(close_enough(m(2,1), -2.0/3.0*ddx));
    BOOST_CHECK(m(2,2) < 42*QL_EPSILON);
    BOOST_CHECK(close_enough(m(2,3), 2.0/3.0*ddx));
    BOOST_CHECK(close_enough(m(2,4), -1.0/12.0*ddx));
    BOOST_CHECK(close_enough(m(2,5), 0.0));

    BOOST_CHECK(close_enough(m(0,0), -25.0/12.0*ddx));
    BOOST_CHECK(close_enough(m(0,1), 4.0*ddx));
    BOOST_CHECK(close_enough(m(0,2), -3.0*ddx));
    BOOST_CHECK(close_enough(m(0,3), 4.0/3.0*ddx));
    BOOST_CHECK(close_enough(m(0,4), -0.25*ddx));
    BOOST_CHECK(close_enough(m(0,5), 0.0));

    BOOST_CHECK(close_enough(m(1,0), -0.25*ddx));
    BOOST_CHECK(close_enough(m(1,1), -5.0/6.0*ddx));
    BOOST_CHECK(close_enough(m(1,2), 3.0/2.0*ddx));
    BOOST_CHECK(close_enough(m(1,3), -0.5*ddx));
    BOOST_CHECK(close_enough(m(1,4), 1.0/12.0*ddx));
    BOOST_CHECK(close_enough(m(1,5), 0.0));

    BOOST_CHECK(close_enough(m(4,5), 0.25*ddx));
    BOOST_CHECK(close_enough(m(4,4), 5.0/6.0*ddx));
    BOOST_CHECK(close_enough(m(4,3), -3.0/2.0*ddx));
    BOOST_CHECK(close_enough(m(4,2), 0.5*ddx));
    BOOST_CHECK(close_enough(m(4,1), -1.0/12.0*ddx));
    BOOST_CHECK(close_enough(m(4,0), 0.0));

    BOOST_CHECK(close_enough(m(5,0), 0.0));
    BOOST_CHECK(close_enough(m(5,1), 0.25*ddx));
    BOOST_CHECK(close_enough(m(5,2), -4.0/3.0*ddx));
    BOOST_CHECK(close_enough(m(5,3), 3.0*ddx));
    BOOST_CHECK(close_enough(m(5,4), -4.0*ddx));
    BOOST_CHECK(close_enough(m(5,5), 25.0/12.0*ddx));
}

void NthOrderDerivativeOpTest::testFirstOrder2PointsOnUniformGrid() {
    BOOST_TEST_MESSAGE("Testing two points first order "
            "derivative operator on an uniform grid...");

    const Real ddx = 1.0/0.2;

    const SparseMatrix m =
        NthOrderDerivativeOp(0, 1, 2,
            ext::make_shared<FdmMesherComposite>(
                ext::make_shared<Uniform1dMesher>(0.0, 0.6, 4))).toMatrix();

    BOOST_CHECK(close_enough(m(0,0), -ddx));
    BOOST_CHECK(close_enough(m(0,1), ddx));
    BOOST_CHECK(close_enough(m(0,2), 0.0));
    BOOST_CHECK(close_enough(m(0,3), 0.0));

    BOOST_CHECK(close_enough(m(1,0), -ddx));
    BOOST_CHECK(close_enough(m(1,1), ddx));
    BOOST_CHECK(close_enough(m(1,2), 0.0));
    BOOST_CHECK(close_enough(m(1,3), 0.0));

    BOOST_CHECK(close_enough(m(2,0), 0.0));
    BOOST_CHECK(close_enough(m(2,1), -ddx));
    BOOST_CHECK(close_enough(m(2,2), ddx));
    BOOST_CHECK(close_enough(m(2,3), 0.0));

    BOOST_CHECK(close_enough(m(3,0), 0.0));
    BOOST_CHECK(close_enough(m(3,1), 0.0));
    BOOST_CHECK(close_enough(m(3,2), -ddx));
    BOOST_CHECK(close_enough(m(3,3), ddx));
}

void NthOrderDerivativeOpTest::testFirstOrder4PointsOnUniformGrid() {
    BOOST_TEST_MESSAGE("Testing four points first order "
            "derivative operator on an uniform grid...");

    const Real ddx = 1.0/0.2;

    const SparseMatrix m =
        NthOrderDerivativeOp(0, 1, 4,
            ext::make_shared<FdmMesherComposite>(
                ext::make_shared<Uniform1dMesher>(0.0, 0.6, 4))).toMatrix();

    BOOST_CHECK(close_enough(m(0,0), -11.0/6.0*ddx));
    BOOST_CHECK(close_enough(m(0,1), 3.0*ddx));
    BOOST_CHECK(close_enough(m(0,2), -1.5*ddx));
    BOOST_CHECK(close_enough(m(2,3), 1.0/3.0*ddx));

    BOOST_CHECK(close_enough(m(1,0), -1.0/3.0*ddx));
    BOOST_CHECK(close_enough(m(1,1), -0.5*ddx));
    BOOST_CHECK(close_enough(m(1,2), ddx));
    BOOST_CHECK(close_enough(m(1,3), -1.0/6.0*ddx));

    BOOST_CHECK(close_enough(m(2,0), 1.0/6.0*ddx));
    BOOST_CHECK(close_enough(m(2,1), -ddx));
    BOOST_CHECK(close_enough(m(2,2), 0.5*ddx));
    BOOST_CHECK(close_enough(m(2,3), 1.0/3.0*ddx));

    BOOST_CHECK(close_enough(m(3,0), -1.0/3.0*ddx));
    BOOST_CHECK(close_enough(m(3,1), 1.5*ddx));
    BOOST_CHECK(close_enough(m(3,2), -3.0*ddx));
    BOOST_CHECK(close_enough(m(3,3), 11.0/6.0*ddx));
}

void NthOrderDerivativeOpTest::testFirstOrder2PointsOn2DimUniformGrid() {
    BOOST_TEST_MESSAGE("Testing two points first order "
            "derivative operator on a 2 dimensional uniform grid...");

    const Real ddx = 1.0/0.2;

    const Size xGrid=4;
    const ext::shared_ptr<FdmMesher> mesher =
        ext::make_shared<FdmMesherComposite>(
            ext::make_shared<Uniform1dMesher>(0.0, 1, xGrid),
            ext::make_shared<Uniform1dMesher>(0.0, 0.4, 3));

    const SparseMatrix m = NthOrderDerivativeOp(1, 1, 2, mesher).toMatrix();

    const ext::shared_ptr<FdmLinearOpLayout> layout = mesher->layout();
    const FdmLinearOpIterator endIter = layout->end();

    for (FdmLinearOpIterator iter = layout->begin(); iter!=endIter; ++iter) {
        const Size i = iter.index();
        const Size ix = iter.coordinates()[1];

        switch (ix) {
          case 0:
            BOOST_CHECK(close_enough(m(i, i        ),-ddx));
            BOOST_CHECK(close_enough(m(i, i+  xGrid), ddx));
            BOOST_CHECK(close_enough(m(i, i+2*xGrid), 0.0));
            break;
          case 1:
            BOOST_CHECK(close_enough(m(i, i-  xGrid),-ddx));
            BOOST_CHECK(close_enough(m(i, i        ), ddx));
            BOOST_CHECK(close_enough(m(i, i+2*xGrid), 0.0));
            break;
          case 2:
            BOOST_CHECK(close_enough(m(i, i-2*xGrid), 0.0));
            BOOST_CHECK(close_enough(m(i, i-  xGrid),-ddx));
            BOOST_CHECK(close_enough(m(i, i        ), ddx));
            break;
          default:
            QL_FAIL("inconsistent coordinate");
        }
    }
}

void NthOrderDerivativeOpTest::testSecondOrder3PointsNonUniformGrid() {
    BOOST_TEST_MESSAGE("Testing three points second order "
            "derivative operator on a non-uniform grid...");

    std::vector<Real> xValues(4);
    xValues[0] = 0.5;
    xValues[1] = 1.0;
    xValues[2] = 2.0;
    xValues[3] = 4.0;

    const SparseMatrix m =
        NthOrderDerivativeOp(0, 2, 3,
            ext::make_shared<FdmMesherComposite>(
                ext::make_shared<Predefined1dMesher>(xValues))).toMatrix();

    BOOST_CHECK(close_enough(m(0,0), 8.0/3.0));
    BOOST_CHECK(close_enough(m(0,1), -4.0));
    BOOST_CHECK(close_enough(m(0,2), 4.0/3.0));
    BOOST_CHECK(close_enough(m(0,3), 0.0));

    BOOST_CHECK(close_enough(m(1,0), 8.0/3.0));
    BOOST_CHECK(close_enough(m(1,1), -4.0));
    BOOST_CHECK(close_enough(m(1,2), 4.0/3.0));
    BOOST_CHECK(close_enough(m(1,3), 0.0));

    BOOST_CHECK(close_enough(m(2,0), 0.0));
    BOOST_CHECK(close_enough(m(2,1), 2.0/3.0));
    BOOST_CHECK(close_enough(m(2,2), -1.0));
    BOOST_CHECK(close_enough(m(2,3), 1.0/3.0));

    BOOST_CHECK(close_enough(m(3,0), 0.0));
    BOOST_CHECK(close_enough(m(3,1), 2.0/3.0));
    BOOST_CHECK(close_enough(m(3,2), -1.0));
    BOOST_CHECK(close_enough(m(3,3), 1.0/3.0));
}

void NthOrderDerivativeOpTest::testSecondOrder4PointsNonUniformGrid() {
    BOOST_TEST_MESSAGE("Testing four points second order "
            "derivative operator on a non-uniform grid...");

    std::vector<Real> xValues(5);
    xValues[0] = 0.5;
    xValues[1] = 1.0;
    xValues[2] = 2.0;
    xValues[3] = 4.0;
    xValues[4] = 8.0;

    const SparseMatrix m =
        NthOrderDerivativeOp(0, 2, 4,
            ext::make_shared<FdmMesherComposite>(
                ext::make_shared<Predefined1dMesher>(xValues))).toMatrix();

    BOOST_CHECK(close_enough(m(0,0), 88.0/21.0));
    BOOST_CHECK(close_enough(m(0,1), -140.0/21.0));
    BOOST_CHECK(close_enough(m(0,2), 56.0/21.0));
    BOOST_CHECK(close_enough(m(0,3), -4.0/21.0));
    BOOST_CHECK(close_enough(m(0,4), 0.0));

    BOOST_CHECK(close_enough(m(1,0), 64.0/21.0));
    BOOST_CHECK(close_enough(m(1,1), -98.0/21.0));
    BOOST_CHECK(close_enough(m(1,2), 35.0/21.0));
    BOOST_CHECK(close_enough(m(1,3), -1.0/21.0));
    BOOST_CHECK(close_enough(m(1,4), 0.0));

    BOOST_CHECK(close_enough(m(2,0), 16.0/21.0));
    BOOST_CHECK(close_enough(m(2,1), -2.0/3.0));
    BOOST_CHECK(close_enough(m(2,2), -1.0/3.0));
    BOOST_CHECK(close_enough(m(2,3), 5.0/21.0));
    BOOST_CHECK(close_enough(m(2,4), 0.0));

    BOOST_CHECK(close_enough(m(3,0), 0.0));
    BOOST_CHECK(close_enough(m(3,1), 4.0/21.0));
    BOOST_CHECK(close_enough(m(3,2), -1.0/6.0));
    BOOST_CHECK(close_enough(m(3,3), -1.0/12.0));
    BOOST_CHECK(close_enough(m(3,4), 5.0/84.0));

    BOOST_CHECK(close_enough(m(4,0), 0.0));
    BOOST_CHECK(close_enough(m(4,1), -20.0/21.0));
    BOOST_CHECK(close_enough(m(4,2), 11.0/6.0));
    BOOST_CHECK(close_enough(m(4,3), -13.0/12.0));
    BOOST_CHECK(close_enough(m(4,4), 17.0/84.0));
}

void NthOrderDerivativeOpTest::testThirdOrder4PointsUniformGrid() {
    BOOST_TEST_MESSAGE("Testing four points third order "
            "derivative operator on a uniform grid...");

    const SparseMatrix m =
        NthOrderDerivativeOp(0, 3, 4,
            ext::make_shared<FdmMesherComposite>(
                ext::make_shared<Uniform1dMesher>(0.0, 0.6, 4))).toMatrix();

    for (Size i=0; i < 4; ++i) {
        BOOST_CHECK(close_enough(m(i,0), -125.0));
        BOOST_CHECK(close_enough(m(i,1), 375.0));
        BOOST_CHECK(close_enough(m(i,2), -375.0));
        BOOST_CHECK(close_enough(m(i,3), 125));
    }
}

namespace {
    struct GridSetup {
        Real alpha;
        Real density;
        bool cellAvg;
        bool midPoint;
        Size nPoints;
        Size tGrid;
        Size yGrid;
        Size vGrid;
        FdmSchemeDesc scheme;
    };

    class FdmHestonNthOrderOp : public FdmLinearOpComposite {
      public:
        FdmHestonNthOrderOp(Size nPoints,
                            const ext::shared_ptr<HestonProcess>& hestonProcess,
                            const ext::shared_ptr<FdmMesher>& mesher,
                            Size direction = 0)
        : vol2_(0.5 * hestonProcess->theta()),
          preconditioner_(
              SecondDerivativeOp(direction, mesher).mult(Array(mesher->layout()->size(), vol2_))) {

            const Array vv(mesher->locations(1));
            Array varianceValues(0.5*vv);

            ext::shared_ptr<FdmLinearOpLayout> layout = mesher->layout();
            FdmLinearOpIterator endIter = layout->end();
            for (FdmLinearOpIterator iter = layout->begin(); iter != endIter;
                ++iter) {
                if (   iter.coordinates()[0] == 0
                    || iter.coordinates()[0] == layout->dim()[0]-1) {
                    varianceValues[iter.index()] = 0.0;
                }
            }
            const Size n = mesher->layout()->size();

            using namespace boost::numeric::ublas;
            banded_matrix<Real> v(n, n), u(n, n), rV(n, n);
            for (Size i=0; i < n; ++i) {
                v(i, i) = varianceValues[i];
                u(i, i) = vv[i];
                rV(i, i) = varianceValues[i] - 0.5*hestonProcess->theta();
            }

            const SparseMatrix dx = NthOrderDerivativeOp(0, 1, nPoints, mesher).toMatrix();
            const SparseMatrix dxx = NthOrderDerivativeOp(0, 2, nPoints, mesher).toMatrix();
            const SparseMatrix dv = NthOrderDerivativeOp(1, 1, nPoints, mesher).toMatrix();
            const SparseMatrix dvv = NthOrderDerivativeOp(1, 2, nPoints, mesher).toMatrix();

            const Real kappa = hestonProcess->kappa();
            const Real theta = hestonProcess->theta();
            const Real sigma = hestonProcess->sigma();
            const Real rho = hestonProcess->rho();

            map_ = sparse_prod<SparseMatrix>(-rV, dx)
                + sparse_prod<SparseMatrix>(v, dxx)
                + (0.5*rho*sigma)*sparse_prod<SparseMatrix>(
                    u, sparse_prod<SparseMatrix>(dx, dv)
                        + sparse_prod<SparseMatrix>(dv, dx))
                + (0.5*sigma*sigma)*sparse_prod<SparseMatrix>(u, dvv)
                + kappa*sparse_prod<SparseMatrix>(
                    theta*identity_matrix<Real>(n) - u, dv);
        }

        Disposable<SparseMatrix> toMatrix() const override {
            SparseMatrix tmp(map_);
            return tmp;
        }

        Size size() const override { return 2; }
        void setTime(Time t1, Time t2) override { }

        Disposable<Array> apply(const Array& r) const override {
            return prod(map_, r);
        }

        Disposable<Array> apply_mixed(const Array& r) const override {
            QL_FAIL("operator splitting is not supported");
        }

        Disposable<Array> apply_direction(
            Size direction, const Array& r) const override {
            QL_FAIL("operator splitting is not supported");
        }

        Disposable<Array> solve_splitting(
            Size direction, const Array& r, Real dt) const override {
            QL_FAIL("operator splitting is not supported");
        }

        Disposable<Array> preconditioner(const Array& r, Real dt) const override {
            return preconditioner_.solve_splitting(r, dt, 1.0);
        }

      private:
        Disposable<Array> solve_apply(const Array& r, Real dt) const {
            return r - dt*apply(r);
        }

        const Volatility vol2_;
        SparseMatrix map_;
        TripleBandLinearOp preconditioner_;
    };

    class AvgPayoffFct {
      public:
        AvgPayoffFct(ext::shared_ptr<PlainVanillaPayoff> payoff,
                     Volatility vol, Time T, Real growthFactor)
        : payoff_(std::move(payoff)),
          vol2_(0.5*vol*vol*T),
          growthFactor_(growthFactor) { }

        Real operator()(Real x) const {
            return (*payoff_)(std::exp(x - vol2_)*growthFactor_);
        }

       private:
        const ext::shared_ptr<PlainVanillaPayoff> payoff_;
        const Volatility vol2_;
        const Real growthFactor_;
    };

   class MyInnerValueCalculator : public FdmInnerValueCalculator {
      public:
        MyInnerValueCalculator(ext::shared_ptr<Payoff> payoff,
                               ext::shared_ptr<FdmMesher> mesher,
                               ext::shared_ptr<YieldTermStructure> rTS,
                               ext::shared_ptr<YieldTermStructure> qTS,
                               Volatility vol,
                               Size direction)
        : payoff_(std::move(std::move(payoff))), mesher_(std::move(std::move(mesher))),
          rTS_(std::move(std::move(rTS))), qTS_(std::move(std::move(qTS))), vol_(vol),
          direction_(direction) {}

        Real innerValue(const FdmLinearOpIterator& iter, Time t)  override {
            const Real g = mesher_->location(iter, direction_);
            const Real sT = std::exp(g - 0.5*vol_*vol_*t);

            return (*payoff_)(sT);
        }

        Real avgInnerValue(const FdmLinearOpIterator& iter, Time t) override {
            return innerValue(iter, t);
        }

      private:
        const ext::shared_ptr<Payoff> payoff_;
        const ext::shared_ptr<FdmMesher> mesher_;
        const ext::shared_ptr<YieldTermStructure> rTS_, qTS_;
        const Volatility vol_;
        const Size direction_;
    };

    Disposable<Array> priceReport(
        const GridSetup& setup, const Array& strikes) {

        const Date today(2, May, 2018);
        const DayCounter dc = Actual365Fixed();
        const Date maturity = today + Period(1, Years);
        const Time T = dc.yearFraction(today, maturity);

        const ext::shared_ptr<YieldTermStructure> rTS
            = flatRate(today, 0.05, dc);
        const ext::shared_ptr<YieldTermStructure> qTS
            = flatRate(today, 0.0, dc);

        const Real S = 100.0;
        const Volatility vol = 0.2;
        const Real v0 = vol*vol;
        const Real kappa = 1.0;
        const Real theta = vol*vol;
        const Real sig = 0.2;
        const Real rho = -0.75;

        const ext::shared_ptr<HestonProcess> hestonProcess
            = ext::make_shared<HestonProcess>(
                  Handle<YieldTermStructure>(rTS),
                  Handle<YieldTermStructure>(qTS),
                  Handle<Quote>(ext::make_shared<SimpleQuote>(S)),
                  v0, kappa, theta, sig, rho);

        const Real stdDev = vol * std::sqrt(T);
        const DiscountFactor df
            = qTS->discount(maturity)/rTS->discount(maturity);

        const Real y = std::log(S);

        const Real ymin = y - setup.alpha*stdDev;
        const Real ymax = y + setup.alpha*stdDev;

        const Size yGrid = setup.yGrid;
        const Size vGrid = setup.vGrid;

        Array diffs(strikes.size()), fdmPrices(strikes.size());
        for (Size k=0; k < strikes.size(); ++k) {
            const Real strike = strikes[k];
            const Real specialPoint = std::log(strike/df) + 0.5*vol*vol*T;

            const ext::shared_ptr<Fdm1dMesher> mesher1d =
                ext::make_shared<Concentrating1dMesher>(
                    ymin, ymax, yGrid,
                    std::pair<Real, Real>(specialPoint, setup.density));

            std::vector<Real> loc = mesher1d->locations();
            for (Size i = 0; setup.midPoint && i < loc.size()-1; ++i)
                if (loc[i] < specialPoint && loc[i+1]>= specialPoint) {
                    const Real d = loc[i+1] - loc[i];

                    const Real offset = (specialPoint - 0.5*d) - loc[i];

                    for (double& l : loc)
                        l += offset;

                    break;
                }

            const ext::shared_ptr<FdmMesherComposite> mesher =
                ext::make_shared<FdmMesherComposite>(
                    ext::make_shared<Predefined1dMesher>(loc),
                    ext::make_shared<FdmHestonVarianceMesher>(
                        vGrid, hestonProcess, 1.0)
            );

            const Array g = mesher->locations(0);
            const Array sT = Exp(g - 0.5*vol*vol*T)*df;

            Array rhs(mesher->layout()->size());

            const ext::shared_ptr<PlainVanillaPayoff> payoff =
                ext::make_shared<PlainVanillaPayoff>(Option::Put, strike);

            const ext::shared_ptr<FdmLinearOpLayout> layout = mesher->layout();
            const FdmLinearOpIterator endIter = layout->end();

            for (FdmLinearOpIterator iter = layout->begin();
                 iter!=endIter; ++iter) {
                const Size idx = iter.index();
                const Size idxm1 = layout->neighbourhood(iter,  0,-1);
                const Size idxp1 = layout->neighbourhood(iter,  0, 1);

                const Size nx = iter.coordinates()[0];

                if (nx != 0 && nx != yGrid-1
                    && setup.cellAvg && (
                        (sT[idx] < strike && sT[idxp1] >= strike)
                     || (sT[idxm1] < strike && sT[idx] >= strike))) {

                    const Real gMin = 0.5*(g[idxm1] + g[idx]);
                    const Real gMax = 0.5*(g[idxp1] + g[idx]);

                    const AvgPayoffFct f(payoff, vol, T, df);

                    rhs[idx] = GaussLobattoIntegral(1000, 1e-12)(
                        f, gMin, gMax)/(gMax - gMin);
                }
                else
                    rhs[idx] = (*payoff)(sT[idx]);
            }

            const ext::shared_ptr<FdmHestonNthOrderOp> heatEqn =
                ext::make_shared<FdmHestonNthOrderOp>(
                    setup.nPoints, hestonProcess, mesher);

            FdmBackwardSolver solver(
              heatEqn,
              FdmBoundaryConditionSet(),
              ext::shared_ptr<FdmStepConditionComposite>(),
              setup.scheme);

            solver.rollback(rhs, T, 0.0, setup.tGrid, 1);

            rhs *= rTS->discount(maturity);

            const std::vector<Real>& x =
                mesher->getFdm1dMeshers()[0]->locations();
            const std::vector<Real>& v =
                mesher->getFdm1dMeshers()[1]->locations();

            Matrix resultValues_(layout->dim()[1], layout->dim()[0]);
            std::copy(rhs.begin(), rhs.end(), resultValues_.begin());

            const ext::shared_ptr<BicubicSpline> interpolation =
                ext::make_shared<BicubicSpline>(
                    x.begin(), x.end(), v.begin(), v.end(), resultValues_);

            const Real fdmPrice = (*interpolation)(y, hestonProcess->v0());

            VanillaOption option(
                ext::make_shared<PlainVanillaPayoff>(Option::Put, strike),
                ext::make_shared<EuropeanExercise>(maturity)
            );
            option.setPricingEngine(
                ext::make_shared<AnalyticHestonEngine>(
                    ext::make_shared<HestonModel>(hestonProcess), 192)
            );
            const Real npv = option.NPV();

            diffs[k] = npv - fdmPrice;
        }

        return diffs;
    }

    class FdmMispricingCostFunction : public CostFunction {
      public:
        FdmMispricingCostFunction(
            const GridSetup& setup, Array strikes)
        : setup_(setup), strikes_(std::move(strikes)) { }

        Disposable<Array> values(const Array& x) const override {
            const GridSetup g = {
                x[0], x[1],
                setup_.cellAvg, setup_.midPoint,
                setup_.nPoints,
                setup_.tGrid, setup_.yGrid, setup_.vGrid,
                setup_.scheme
            };

            try {
                return priceReport(g, strikes_);
            }
            catch (std::exception const&) {
                Array q(2, 1000);
                return q;
            }
        }

      private:
        const GridSetup setup_;
        const Array strikes_;
    };
}

void NthOrderDerivativeOpTest::testHigherOrderHestonOptionPricing() {
    BOOST_TEST_MESSAGE("Testing Heston model option pricing convergence with "
            "higher order finite difference operators...");

    SavedSettings backup;

    const Array strikes = {50, 75, 90, 100, 110, 125, 150, 200};

    const GridSetup initSetup = {
        3.87773, 0.043847, true, false,
        5, 21, 20, 11, FdmSchemeDesc::CrankNicolson()
    };

    const Array initialValues = {initSetup.alpha, initSetup.density};

    FdmMispricingCostFunction costFct(initSetup, strikes);
    NoConstraint noConstraint;

    Problem prob(costFct, noConstraint, initialValues);

    LevenbergMarquardt().minimize(
        prob, EndCriteria(400, 40, 1.0e-4, 1.0e-4, 1.0e-4));

    const GridSetup optimalSetup = {
        prob.currentValue()[0], prob.currentValue()[1],
        initSetup.cellAvg, initSetup.midPoint,
        initSetup.nPoints,
        initSetup.tGrid,
        initSetup.yGrid/2,
        initSetup.vGrid,
        initSetup.scheme
    };

    const Array q = priceReport(optimalSetup, strikes);
    const Real ac = std::sqrt(DotProduct(q, q)/q.size());

    const Array p = priceReport(initSetup, strikes);
    const Real ap = std::sqrt(DotProduct(p, p)/p.size());

    const Real convergence = std::log(ac/ap)*M_LOG2E;

    if (convergence < 3.6) {
        BOOST_ERROR("convergence order is too low"
                << "\n expected convergence: 4.0"
                << "\n measured convergence: " << convergence
                << "\n tolerance           : 0.4");
    }
}

namespace {
    Real priceQuality(Real h) {

        const Array strikes = {100};

        const Size yGrid = Size(1/h);
        const GridSetup setup = {
              5.50966, 0.0130581,
             true, false,
             5, 401, yGrid, 21,
             FdmSchemeDesc::CrankNicolson()
        };

        return  std::fabs(priceReport(setup, strikes)[0]);
    }
}

void NthOrderDerivativeOpTest::testHigherOrderAndRichardsonExtrapolation() {
    BOOST_TEST_MESSAGE(
            "Testing Heston option pricing convergence with "
            "higher order FDM operators and Richardson Extrapolation...");

    SavedSettings backup;

    const Real n1 = priceQuality(1.0/25);
    const Real n3
        = std::fabs(RichardsonExtrapolation(priceQuality, 1.0/25, 4.0)(2.0));

    const Real r2 = std::log(n1/n3)*M_LOG2E;

    if (r2 < 4.9) {
        BOOST_ERROR("convergence order is too low using Richardson extrapolation"
                << "\n expected convergence: 5.0"
                << "\n measured convergence: " << r2
                << "\n tolerance           : 0.1");
    }
}

void NthOrderDerivativeOpTest::testCompareFirstDerivativeOpNonUniformGrid() {
    BOOST_TEST_MESSAGE(
        "Testing with FirstDerivativeOp on a non-uniform grid...");

    Array xValues = Exp(Array(7, 0, 0.1));

    const ext::shared_ptr<Fdm1dMesher> m
        = ext::make_shared<Predefined1dMesher>(
            std::vector<Real>(xValues.begin(), xValues.end()));

    const ext::shared_ptr<FdmMesher> m1d(
        ext::make_shared<FdmMesherComposite>(m));

    const ext::shared_ptr<FirstDerivativeOp> fx
        = ext::make_shared<FirstDerivativeOp>(0, m1d);

    const ext::shared_ptr<NthOrderDerivativeOp> dx
        = ext::make_shared<NthOrderDerivativeOp>(0, 1, 3, m1d);

    const SparseMatrix fm = fx->toMatrix();
    const SparseMatrix dm = dx->toMatrix();

    for (Size i=1; i < m->size()-1; ++i) // different boundary conditions
        for (Size j=0; j < m->size(); ++j)
            BOOST_CHECK(std::fabs(fm(i, j)- dm(i, j)) < 1e-12);
}

void NthOrderDerivativeOpTest::testCompareFirstDerivativeOp2dUniformGrid() {
    BOOST_TEST_MESSAGE(
        "Testing with FirstDerivativeOp on a 2d uniform grid...");

    const ext::shared_ptr<Fdm1dMesher> m1
        = ext::make_shared<Uniform1dMesher>(0.0, 0.6, 5);
    const ext::shared_ptr<Fdm1dMesher> m2
        = ext::make_shared<Uniform1dMesher>(0.0, 1.6, 6);

    const ext::shared_ptr<FdmMesher> mc(
        ext::make_shared<FdmMesherComposite>(m1, m2));

    const ext::shared_ptr<FdmLinearOpLayout> layout = mc->layout();

    const Size n = layout->dim()[0];
    const Size m = layout->dim()[1];

    SparseMatrix fm = FirstDerivativeOp(0, mc).toMatrix();
    SparseMatrix dm = NthOrderDerivativeOp(0, 1, 3, mc).toMatrix();

    for (Size k=0; k < m; ++k) {
        const Size idx = k*n;
        for (Size i=1; i < n-1; ++i)
            for (Size j=0; j < n*m; ++j)
                BOOST_CHECK(std::fabs(fm(idx + i, j) - dm(idx + i, j)) < 1e-12);
    }

    fm = FirstDerivativeOp(1, mc).toMatrix();
    dm = NthOrderDerivativeOp(1, 1, 3, mc).toMatrix();

    for (Size i=n; i < n*(m-1); ++i)
        for (Size j=0; j < n*m; ++j)
            BOOST_CHECK(std::fabs(fm(i, j) - dm(i, j)) < 1e-12);
}

void NthOrderDerivativeOpTest::testMixedSecondOrder9PointsOnUniformGrid() {
    BOOST_TEST_MESSAGE(
            "Testing nine points mixed second order "
            "derivative operator on a uniform grid...");

    const ext::shared_ptr<Fdm1dMesher> m
        = ext::make_shared<Uniform1dMesher>(0.0, 0.6, 5);

    const ext::shared_ptr<FdmMesher> mc(
        ext::make_shared<FdmMesherComposite>(m, m));

    const SparseMatrix cc =
            prod(NthOrderDerivativeOp(0, 1, 3, mc).toMatrix(),
                 NthOrderDerivativeOp(1, 1, 3, mc).toMatrix());

    const SparseMatrix mm = SecondOrderMixedDerivativeOp(0,1,mc).toMatrix();

    const Size n = m->size();

    for (Size i=1; i < n-1; ++i)
        for (Size j=1; j < n-1; ++j) {
            const Size idx = i*n+j;
            for (Size k=1; k < n-1; ++k)
                for (Size l=1; l < n-1; ++l) {
                    const Size kdx = k*n+l;
                    BOOST_CHECK(std::fabs(mm(idx,kdx) - cc(idx,kdx)) < 1e-12);
                }
        }
}
#endif

test_suite* NthOrderDerivativeOpTest::suite(SpeedLevel speed) {
    auto* suite = BOOST_TEST_SUITE("NthOrderDerivativeOp tests");

#ifndef QL_NO_UBLAS_SUPPORT

    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testSparseMatrixApply));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testFirstOrder2PointsApply));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testFirstOrder3PointsOnUniformGrid));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testFirstOrder5PointsOnUniformGrid));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testFirstOrder2PointsOnUniformGrid));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testFirstOrder4PointsOnUniformGrid));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testFirstOrder2PointsOn2DimUniformGrid));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testSecondOrder3PointsNonUniformGrid));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testSecondOrder4PointsNonUniformGrid));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testThirdOrder4PointsUniformGrid));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testHigherOrderAndRichardsonExtrapolation));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testCompareFirstDerivativeOpNonUniformGrid));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testCompareFirstDerivativeOp2dUniformGrid));
    suite->add(QUANTLIB_TEST_CASE(
        &NthOrderDerivativeOpTest::testMixedSecondOrder9PointsOnUniformGrid));

    if (speed <= Fast) {
        suite->add(QUANTLIB_TEST_CASE(&NthOrderDerivativeOpTest::testHigherOrderHestonOptionPricing));
    }

#endif

    return suite;
}


#ifndef _MATHEMATICS_HPP_
#define _MATHEMATICS_HPP_

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>

void calc_rpy(Eigen::Matrix4f a,double &roll,double &pitch,double &yaw);
float distance(float x,float y);

#endif


#include <csapex/model/node.h>
#include <csapex/msg/io.h>
#include <csapex/utility/register_apex_plugin.h>

#include <csapex/msg/generic_value_message.hpp>
#include <csapex_scan_2d/scan_message.h>

#include <csapex/model/node_modifier.h>
#include <csapex/param/parameter_factory.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <cslibs_laser_processing/common/yaml-io.hpp>

namespace
{
double normalize(double angle)
{
    while (angle <= -M_PI)
        angle += 2 * M_PI;
    while (angle > M_PI)
        angle -= 2 * M_PI;
    return angle;
}
}  // namespace

namespace csapex
{
using namespace lib_laser_processing;
using namespace boost::accumulators;
using namespace connection_types;

class PitchAngle : public csapex::Node
{
public:
    PitchAngle()
    {
    }

    virtual void setup(NodeModifier& node_modifier) override
    {
        input_ = node_modifier.addInput<ScanMessage>("filtered scan");
        output_ = node_modifier.addOutput<double>("pitch");
    }

    void setupParameters(Parameterizable& parameters) override
    {
        parameters.addParameter(param::factory::declareRange("height", 0.05, 1.0, 0.1, 0.001));
        parameters.addParameter(param::factory::declareTrigger("reset"), std::bind(&PitchAngle::reset, this));
        parameters.addParameter(param::factory::declareBool("degrees", false));
    }

    virtual void process() override
    {
        ScanMessage::ConstPtr in = msg::getMessage<ScanMessage>(input_);
        const Scan& scan = in->value;
        double out = 0.0;
        double height = readParameter<double>("height");
        bool deg = readParameter<bool>("degrees");

        for (const LaserBeam& b : scan.rays) {
            if (b.range() > 0.f)
                mean_dist_(b.posX());
        }

        out = normalize(asin(height / mean(mean_dist_)));
        if (deg)
            out *= 180.0 / M_PI;

        msg::publish(output_, out);
    }

private:
    Input* input_;
    Output* output_;

    accumulator_set<double, stats<tag::mean>> mean_dist_;

    void reset() override
    {
        mean_dist_ = accumulator_set<double, stats<tag::mean>>();
    }
};
}  // namespace csapex

CSAPEX_REGISTER_CLASS(csapex::PitchAngle, csapex::Node)


#include <QtCore/qglobal.h>

#include "Messages_interface.h"
#include "Scene_polyhedron_item.h"
#include "Scene_polylines_item.h"
#include <CGAL/Three/Scene_interface.h>

#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>
#include "Polyhedron_type.h"

#include <CGAL/Polygon_mesh_processing/triangulate_hole.h>
#include <CGAL/Polygon_mesh_processing/refine.h>
#include <CGAL/Polygon_mesh_processing/internal/named_function_params.h>
#include <CGAL/Polyhedron_incremental_builder_3.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/Timer.h>

#include <QAction>
#include <QMainWindow>
#include <QApplication>
#include <QInputDialog>
#include <QMessageBox>

#include <vector>
#include <algorithm>

#include <boost/function_output_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>

namespace PMP = CGAL::Polygon_mesh_processing;

template<class HDS>
class Polyhedron_builder : public CGAL::Modifier_base<HDS> {
public:
  Polyhedron_builder(std::vector<CGAL::Triple<int, int, int> >* triangles,
    Scene_polylines_item::Polyline* polyline)
    : triangles(triangles), polyline(polyline)
  { }

  void operator()(HDS& hds) {
    CGAL::Polyhedron_incremental_builder_3<HDS> B(hds, true);
    B.begin_surface(polyline->size() -1, triangles->size());

    for(Scene_polylines_item::Polyline::iterator it = polyline->begin();
      it != --polyline->end(); ++it) {
      B.add_vertex(*it);
    }

    for(std::vector<CGAL::Triple<int, int, int> >::iterator it = triangles->begin();
      it != triangles->end(); ++it) {
      B.begin_facet();
      B.add_vertex_to_facet(it->first);
      B.add_vertex_to_facet(it->second);
      B.add_vertex_to_facet(it->third);
      B.end_facet();
    }

    B.end_surface();
  }

private:
  std::vector<CGAL::Triple<int, int, int> >* triangles;
  Scene_polylines_item::Polyline* polyline;
};
using namespace CGAL::Three;
class Polyhedron_demo_hole_filling_polyline_plugin :
  public QObject,
  public Polyhedron_demo_plugin_interface
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")
public:
  bool applicable(QAction *) const { return qobject_cast<Scene_polylines_item*>(scene->item(scene->mainSelectionIndex())); }
  void print_message(QString message) { messages->information(message); }
  QList<QAction*> actions() const { return QList<QAction*>() << actionHoleFillingPolyline; }
  void init(QMainWindow* mainWindow, CGAL::Three::Scene_interface* scene_interface, Messages_interface* m){
    mw = mainWindow;
    scene = scene_interface;
    messages = m;
    actionHoleFillingPolyline = new QAction(tr("Polyline Hole Filling"), mw);
    connect(actionHoleFillingPolyline, SIGNAL(triggered()),
      this, SLOT(hole_filling_polyline_action()));
  }
private:
  struct Nop_functor {
    template<class T>
    void operator()(const T & /*t*/) const {}
  };
  typedef boost::function_output_iterator<Nop_functor> Nop_out;

  struct Get_handle {
    typedef Polyhedron::Facet_handle result_type;
    result_type operator()(Polyhedron::Facet& f) const
    { return f.halfedge()->facet(); }
  };

public Q_SLOTS:
  void hole_filling_polyline_action() {
    Scene_polylines_item* polylines_item = qobject_cast<Scene_polylines_item*>(scene->item(scene->mainSelectionIndex()));
    if(!polylines_item) {
      print_message("Error: there is no selected polyline item!");
      return;
    }

    bool also_refine;
    const double density_control_factor =
      QInputDialog::getDouble(mw, tr("Density Control Factor"),
      tr("Density Control Factor (Cancel for not Refine): "), 1.41, 0.0, 100.0, 2, &also_refine);

    bool use_DT =
      QMessageBox::Yes == QMessageBox::question(
      NULL, "Use Delaunay Triangulation", "Use Delaunay Triangulation ?", QMessageBox::Yes|QMessageBox::No);

    QApplication::setOverrideCursor(Qt::WaitCursor);
    std::size_t counter = 0;
    for(Scene_polylines_item::Polylines_container::iterator it = polylines_item->polylines.begin();
      it != polylines_item->polylines.end(); ++it, ++counter)
    {
      if(it->front() != it->back()) { //not closed, skip it
        print_message("Warning: skipping not closed polyline!");
        continue;
      }
      if(it->size() < 4) { // no triangle, skip it (needs at least 3 + 1 repeat)
        print_message("Warning: skipping polyline which has less than 4 points!");
        continue;
      }

      CGAL::Timer timer; timer.start();
      std::vector<CGAL::Triple<int, int, int> > patch;
      CGAL::Polygon_mesh_processing::triangulate_hole_polyline(*it,
        std::back_inserter(patch),
        PMP::parameters::use_delaunay_triangulation(use_DT));
      print_message(QString("Triangulated in %1 sec.").arg(timer.time()));

      if(patch.empty()) {
        print_message("Warning: generating patch is not successful, please try it without 'Delaunay Triangulation'!");
        continue;
      }
      Polyhedron* poly = new Polyhedron;
      Polyhedron_builder<Polyhedron::HalfedgeDS> patch_builder(&patch, &(*it));
      poly->delegate(patch_builder);

      if(also_refine) {
        timer.reset();
        CGAL::Polygon_mesh_processing::refine(*poly, faces(*poly),
          Nop_out(), Nop_out(),
          CGAL::Polygon_mesh_processing::parameters::density_control_factor(density_control_factor));
        print_message(QString("Refined in %1 sec.").arg(timer.time()));
      }

      Scene_polyhedron_item* poly_item = new Scene_polyhedron_item(poly);
      poly_item->setName(tr("%1-filled-%2").arg(polylines_item->name()).arg(counter));
      poly_item->setRenderingMode(FlatPlusEdges);
      scene->setSelectedItem(scene->addItem(poly_item));
    }
    QApplication::restoreOverrideCursor();
  }

private:
  QMainWindow* mw;
  CGAL::Three::Scene_interface* scene;
  Messages_interface* messages;
  QAction* actionHoleFillingPolyline;

}; // end Polyhedron_demo_hole_filling_polyline_plugin

// Q_EXPORT_PLUGIN2(Polyhedron_demo_hole_filling_polyline_plugin, Polyhedron_demo_hole_filling_polyline_plugin)

#include "Hole_filling_polyline_plugin.moc"