// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Olga Diamanti <olga.diam@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include <igl/angle_bound_frame_fields.h>
#include <igl/edge_topology.h>
#include <igl/local_basis.h>
#include <igl/sparse.h>
#include <igl/speye.h>
#include <igl/slice.h>
#include <igl/polyroots.h>
#include <igl/colon.h>
#include <Eigen/Sparse>

#include <iostream>

namespace igl {

  template <typename DerivedV, typename DerivedF>
  class AngleBoundFFSolverData
  {
    public:
      const Eigen::PlainObjectBase<DerivedV> &V; int numV;
      const Eigen::PlainObjectBase<DerivedF> &F; int numF;

      Eigen::MatrixXi EV; int numE;
      Eigen::MatrixXi F2E;
      Eigen::MatrixXi E2F;
      Eigen::VectorXd K;

      Eigen::VectorXi isBorderEdge;
      int numInteriorEdges;
      Eigen::Matrix<int,Eigen::Dynamic,2> E2F_int;
      Eigen::VectorXi indInteriorToFull;
      Eigen::VectorXi indFullToInterior;

      DerivedV B1, B2, FN;

      //laplacians
      Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>> DDA, DDB;

  private:
    IGL_INLINE void computeLaplacians();
    IGL_INLINE void computek();
    IGL_INLINE void computeCoefficientLaplacian(int n, Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &D);
    IGL_INLINE void precomputeInteriorEdges();

public:
      IGL_INLINE AngleBoundFFSolverData(const Eigen::PlainObjectBase<DerivedV> &_V,
                                   const Eigen::PlainObjectBase<DerivedF> &_F);
  };

  template <typename DerivedV, typename DerivedF, typename DerivedO>
  class AngleBoundFFSolver
  {
  public:
    IGL_INLINE AngleBoundFFSolver(const AngleBoundFFSolverData<DerivedV, DerivedF> &_data,
                                  const typename DerivedV::Scalar &_thetaMin = 30,
                                 int _maxIter = 50,
                                 const typename DerivedV::Scalar &_lambdaInit = 100,
                                 const typename DerivedV::Scalar &_lambdaMultFactor = 1.01,
                                const bool _doHardConstraints = false);
    IGL_INLINE bool solve(const Eigen::VectorXi &isConstrained,
                          const Eigen::PlainObjectBase<DerivedO> &initialSolution,
                          Eigen::PlainObjectBase<DerivedO> &output,
                          typename DerivedV::Scalar *lambdaOut = NULL);

  private:

    const AngleBoundFFSolverData<DerivedV, DerivedF> &data;

    //polyVF data
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> Acoeff, Bcoeff;
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 2> pvU, pvV;
    typename DerivedV::Scalar lambda;

    //parameters
    typename DerivedV::Scalar lambdaInit,lambdaMultFactor;
    int maxIter;
    typename DerivedV::Scalar thetaMin;
    bool doHardConstraints;

    typename DerivedV::Scalar computeAngle(const std::complex<typename DerivedV::Scalar> &u,
                                           const std::complex<typename DerivedV::Scalar> &v);
//    IGL_INLINE void computeAngles(Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> &angles);

    IGL_INLINE int getNumOutOfBounds();

    IGL_INLINE void rotateAroundBisector(const std::complex<typename DerivedV::Scalar> &uin,
                         const std::complex<typename DerivedV::Scalar> &vin,
                         const typename DerivedV::Scalar theta,
                         std::complex<typename DerivedV::Scalar> &uout,
                         std::complex<typename DerivedV::Scalar> &vout);

    IGL_INLINE void localStep();

    IGL_INLINE void globalStep(const Eigen::Matrix<int, Eigen::Dynamic, 1>  &isConstrained,
                               const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Ak,
                               const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Bk);

    IGL_INLINE void minQuadWithKnownMini(const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &Q,
                         const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &f,
                         const Eigen::VectorXi isConstrained,
                         const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &xknown,
                                         Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &x);
    IGL_INLINE void setFieldFromCoefficients();
    IGL_INLINE void setCoefficientsFromField();

  };
}

//Implementation
/***************************** Data ***********************************/

template <typename DerivedV, typename DerivedF>
IGL_INLINE igl::AngleBoundFFSolverData<DerivedV, DerivedF>::
AngleBoundFFSolverData(const Eigen::PlainObjectBase<DerivedV> &_V,
                  const Eigen::PlainObjectBase<DerivedF> &_F):
V(_V),
numV(_V.rows()),
F(_F),
numF(_F.rows())
{
  igl::edge_topology(V,F,EV,F2E,E2F);
  numE = EV.rows();

  precomputeInteriorEdges();

  igl::local_basis(V,F,B1,B2,FN);

  computek();

  computeLaplacians();

};

template <typename DerivedV, typename DerivedF>
IGL_INLINE void igl::AngleBoundFFSolverData<DerivedV, DerivedF>::computeLaplacians()
{
  computeCoefficientLaplacian(2, DDA);

  computeCoefficientLaplacian(4, DDB);
}

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::AngleBoundFFSolverData<DerivedV, DerivedF>::
precomputeInteriorEdges()
{
  // Flag border edges
  numInteriorEdges = 0;
  isBorderEdge.setZero(numE,1);
  indFullToInterior = Eigen::VectorXi::Constant(numE,-1);

  for(unsigned i=0; i<numE; ++i)
  {
    if ((E2F(i,0) == -1) || ((E2F(i,1) == -1)))
      isBorderEdge[i] = 1;
    else
    {
      indFullToInterior[i] = numInteriorEdges;
      numInteriorEdges++;
    }
  }

  E2F_int.resize(numInteriorEdges, 2);
  indInteriorToFull.setZero(numInteriorEdges,1);
  int ii = 0;
  for (int k=0; k<numE; ++k)
  {
    if (isBorderEdge[k])
      continue;
    E2F_int.row(ii) = E2F.row(k);
    indInteriorToFull[ii] = k;
    ii++;
  }

}

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::AngleBoundFFSolverData<DerivedV, DerivedF>::
computeCoefficientLaplacian(int n, Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &D)
{
  std::vector<Eigen::Triplet<std::complex<typename DerivedV::Scalar> >> tripletList;

  // For every non-border edge
  for (unsigned eid=0; eid<numE; ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = E2F(eid,0);
      int fid1 = E2F(eid,1);

      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid0,
                                                                                fid0,
                                                                                std::complex<typename DerivedV::Scalar>(1.)));
      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid1,
                                                                                fid1,
                                                                                std::complex<typename DerivedV::Scalar>(1.)));
      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid0,
                                                                                fid1,
                                                                                -1.*std::polar(1.,-1.*n*K[eid])));
      tripletList.push_back(Eigen::Triplet<std::complex<typename DerivedV::Scalar> >(fid1,
                                                                                fid0,
                                                                                -1.*std::polar(1.,1.*n*K[eid])));

    }
  }
  D.resize(numF,numF);
  D.setFromTriplets(tripletList.begin(), tripletList.end());

}

template<typename DerivedV, typename DerivedF>
IGL_INLINE void igl::AngleBoundFFSolverData<DerivedV, DerivedF>::
computek()
{
  K.setZero(numE);
  // For every non-border edge
  for (unsigned eid=0; eid<numE; ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = E2F(eid,0);
      int fid1 = E2F(eid,1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N0 = FN.row(fid0);
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N1 = FN.row(fid1);

      // find common edge on triangle 0 and 1
      int fid0_vc = -1;
      int fid1_vc = -1;
      for (unsigned i=0;i<3;++i)
      {
        if (F2E(fid0,i) == eid)
          fid0_vc = i;
        if (F2E(fid1,i) == eid)
          fid1_vc = i;
      }
      assert(fid0_vc != -1);
      assert(fid1_vc != -1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> common_edge = V.row(F(fid0,(fid0_vc+1)%3)) - V.row(F(fid0,fid0_vc));
      common_edge.normalize();

      // Map the two triangles in a new space where the common edge is the x axis and the N0 the z axis
      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> P;
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> o = V.row(F(fid0,fid0_vc));
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> tmp = -N0.cross(common_edge);
      P << common_edge, tmp, N0;
      //      P.transposeInPlace();

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V0;
      V0.row(0) = V.row(F(fid0,0)) -o;
      V0.row(1) = V.row(F(fid0,1)) -o;
      V0.row(2) = V.row(F(fid0,2)) -o;

      V0 = (P*V0.transpose()).transpose();

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V1;
      V1.row(0) = V.row(F(fid1,0)) -o;
      V1.row(1) = V.row(F(fid1,1)) -o;
      V1.row(2) = V.row(F(fid1,2)) -o;
      V1 = (P*V1.transpose()).transpose();

      // compute rotation R such that R * N1 = N0
      // i.e. map both triangles to the same plane
      double alpha = -atan2(V1((fid1_vc+2)%3,2),V1((fid1_vc+2)%3,1));

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> R;
      R << 1,          0,            0,
      0, cos(alpha), -sin(alpha) ,
      0, sin(alpha),  cos(alpha);
      V1 = (R*V1.transpose()).transpose();

      // measure the angle between the reference frames
      // k_ij is the angle between the triangle on the left and the one on the right
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref0 = V0.row(1) - V0.row(0);
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref1 = V1.row(1) - V1.row(0);

      ref0.normalize();
      ref1.normalize();

      double ktemp = atan2(ref1(1),ref1(0)) - atan2(ref0(1),ref0(0));

      // just to be sure, rotate ref0 using angle ktemp...
      Eigen::Matrix<typename DerivedV::Scalar, 2, 2> R2;
      R2 << cos(ktemp), -sin(ktemp), sin(ktemp), cos(ktemp);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 2> tmp1 = R2*(ref0.head(2)).transpose();

      K[eid] = ktemp;
    }
  }

}

/***************************** Solver ***********************************/
template <typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
AngleBoundFFSolver(const AngleBoundFFSolverData<DerivedV, DerivedF> &_data,
                   const typename DerivedV::Scalar &_thetaMin,
                  int _maxIter,
                  const typename DerivedV::Scalar &_lambdaInit,
                  const typename DerivedV::Scalar &_lambdaMultFactor,
                   const bool _doHardConstraints):
data(_data),
lambdaInit(_lambdaInit),
maxIter(_maxIter),
lambdaMultFactor(_lambdaMultFactor),
doHardConstraints(_doHardConstraints),
thetaMin(_thetaMin)
{
  Acoeff.resize(data.numF,1);
  Bcoeff.resize(data.numF,1);
  pvU.setZero(data.numF, 2);
  pvV.setZero(data.numF, 2);
};

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
rotateAroundBisector(const std::complex<typename DerivedV::Scalar> &uin,
                          const std::complex<typename DerivedV::Scalar> &vin,
                          const typename DerivedV::Scalar diff,
                          std::complex<typename DerivedV::Scalar> &uout,
                          std::complex<typename DerivedV::Scalar> &vout)
{
  //rotate 2D complex vectors u and v around their bisector so that their
  //angle is at least theta

  uout = uin;
  vout = vin;
  typename DerivedV::Scalar au = arg(uin);
  typename DerivedV::Scalar av = arg(vin);
  if (au<av)
  {
    uout = std::polar (1.0,-.5*diff)*uin;
    vout = std::polar (1.0, .5*diff)*vin;
  }
  else
  {
    uout = std::polar (1.0, .5*diff)*uin;
    vout = std::polar (1.0,-.5*diff)*vin;
  }

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
localStep()
{
  for (int j =0; j<data.numF; ++j)
  {

    std::complex<typename DerivedV::Scalar> u(pvU(j,0),pvU(j,1));
    std::complex<typename DerivedV::Scalar> v(pvV(j,0),pvV(j,1));

    typename DerivedV::Scalar current_angle = computeAngle(u, v);
    if (current_angle<thetaMin*M_PI/180)
    {
      // bring all to 1st or 4th quarter plane
      if ((arg(u)>=0.5*M_PI || arg(u)<-0.5*M_PI ))
        u = -u;
      if ((arg(v)>=0.5*M_PI || arg(v)<-0.5*M_PI ))
        v = -v;
      assert(fabs(computeAngle(u, v) - current_angle)<1e-5);

      if ( fabs(arg(u) - arg(v)) >0.5*M_PI )
        v = -v;
      assert(fabs(computeAngle(u, v) - current_angle)<1e-5);

      std::complex<typename DerivedV::Scalar> u1, v1;
      typename DerivedV::Scalar diff = thetaMin*M_PI/180 - current_angle + 1e-6;
      rotateAroundBisector(u, v, diff, u1, v1);

//      if (computeAngle(u1, v1)<thetaMin*M_PI/180)
//      {
//        std::cerr<<"u = ["<<real(u)<<","<<imag(u)<< "]; v= ["<<real(v)<<","<<imag(v)<<"];"<<std::endl;
//        std::cerr<<"u1 = ["<<real(u1)<<","<<imag(u1)<< "]; v1= ["<<real(v1)<<","<<imag(v1)<<"];"<<std::endl;
//        std::cerr<<"current_angle = "<<current_angle<<std::endl;
//        std::cerr<<"aout = "<<computeAngle(u1, v1)<< "; theta= "<<thetaMin*M_PI/180<<";"<<std::endl;
//      }
//      assert(computeAngle(u1, v1)>=thetaMin*M_PI/180);

      pvU.row(j) << real(u1),imag(u1);
      pvV.row(j) << real(v1),imag(v1);
    }
  }

}

//
//template<typename DerivedV, typename DerivedF, typename DerivedO>
//IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
//computeAngles(Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> &angles)
//{
//  angles.resize(data.numF,1);
//  for (int i =0; i<data.numF; ++i)
//  {
//    std::complex<typename DerivedV::Scalar> u(pvU(i,0),pvU(i,1));
//    std::complex<typename DerivedV::Scalar> v(pvV(i,0),pvV(i,1));
//    angles[i] = fabs(arg(u) - arg(v));
//    if (angles[i]>M_PI)
//      angles[i] = 2*M_PI-angles[i];
//    if (angles[i]>.5*M_PI)
//      angles[i] = M_PI-angles[i];
//  }
//}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE typename DerivedV::Scalar igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
computeAngle(const std::complex<typename DerivedV::Scalar> &u,
             const std::complex<typename DerivedV::Scalar> &v)
{
  typename DerivedV::Scalar angle = std::min(fabs(arg(u*conj(v))), fabs(arg(u*conj(-v))));

//  typename DerivedV::Scalar angle;
//  typename DerivedV::Scalar a1 = fabs(arg(u*conj(v)));
//  typename DerivedV::Scalar a2 = fabs(arg(u*conj(-v)));
//  if (a1 < a2)
//    angle = a1;
//  else
//  {
//    angle = a2; v = -v;
//  }

//  typename DerivedV::Scalar angle = fabs(arg(u) - arg(v));
//  if (angle>M_PI)
//  {
//    u = -u;
//    angle = fabs(arg(u) - arg(v));
//  };
//
//  if (angle>.5*M_PI)
//  {
//    v = -v;
//    angle = fabs(arg(u) - arg(v));
//  };
//
//  assert(fabs(angle-angle1)<1e-6);

//  if (angle>M_PI)
//    angle = 2*M_PI-angle;
//  if (angle>.5*M_PI)
//    angle = M_PI-angle;

//  typename DerivedV::Scalar angle = fabs(arg(u) - arg(v));
//    if (angle>M_PI)
//      angle = 2*M_PI-angle;
//    if (angle>.5*M_PI)
//      angle = M_PI-angle;

  assert(angle <= .5*M_PI && angle >0);

  return angle;
}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE int igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
getNumOutOfBounds()
{
  Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> angles;
//  computeAngles(angles);
  int numOoB = 0;
  for (int i =0; i<data.numF; ++i)
  {
    std::complex<typename DerivedV::Scalar> u(pvU(i,0),pvU(i,1));
    std::complex<typename DerivedV::Scalar> v(pvV(i,0),pvV(i,1));
    typename DerivedV::Scalar angle = computeAngle(u,v);
//    if (angles[i] <thetaMin*M_PI/180)
    if (angle <thetaMin*M_PI/180)
      numOoB ++;
  }
  return numOoB;
}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
setCoefficientsFromField()
{
  for (int i = 0; i <data.numF; ++i)
  {
    std::complex<typename DerivedV::Scalar> u(pvU(i,0),pvU(i,1));
    std::complex<typename DerivedV::Scalar> v(pvV(i,0),pvV(i,1));
    Acoeff(i) = u*u+v*v;
    Bcoeff(i) = u*u*v*v;
  }
}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
globalStep(const Eigen::Matrix<int, Eigen::Dynamic, 1>  &isConstrained,
           const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Ak,
           const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1>  &Bk)
{
  setCoefficientsFromField();

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > I;
  igl::speye(data.numF, data.numF, I);
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > QA = data.DDA+lambda*I;
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fA = (-2*lambda*Acoeff).sparseView();

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > QB = data.DDB+lambda*I;
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fB = (-2*lambda*I*Bcoeff).sparseView();

  if(doHardConstraints)
  {
    minQuadWithKnownMini(QA, fA, isConstrained, Ak, Acoeff);
    minQuadWithKnownMini(QB, fB, isConstrained, Bk, Bcoeff);
  }
  else
  {
    Eigen::Matrix<int, Eigen::Dynamic, 1>isknown_; isknown_.setZero(data.numF,1);
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> xknown_; xknown_.setZero(0,1);
    minQuadWithKnownMini(QA, fA, isknown_, xknown_, Acoeff);
    minQuadWithKnownMini(QB, fB, isknown_, xknown_, Bcoeff);
  }
  setFieldFromCoefficients();

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
setFieldFromCoefficients()
{
  for (int i = 0; i <data.numF; ++i)
  {
    //    poly coefficients: 1, 0, -Acoeff, 0, Bcoeff
    //    matlab code from roots (given there are no trailing zeros in the polynomial coefficients)
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> polyCoeff(5,1);
    polyCoeff<<1., 0., -Acoeff(i), 0., Bcoeff(i);

    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> roots;
    polyRoots<std::complex<typename DerivedV::Scalar>>(polyCoeff,roots);

    std::complex<typename DerivedV::Scalar> u = roots[0];
    int maxi = -1;
    float maxd = -1;
    for (int k =1; k<4; ++k)
    {
      float dist = abs(roots[k]+u);
      if (dist>maxd)
      {
        maxd = dist;
        maxi = k;
      }
    }
    std::complex<typename DerivedV::Scalar> v = roots[maxi];
    pvU(i,0) = real(u); pvU(i,1) = imag(u);
    pvV(i,0) = real(v); pvV(i,1) = imag(v);
  }

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE void igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
minQuadWithKnownMini(const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &Q,
                     const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &f,
                     const Eigen::VectorXi isConstrained,
                     const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &xknown,
                     Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &x)
{
  int N = Q.rows();

  int nc = xknown.rows();
  Eigen::VectorXi known; known.setZero(nc,1);
  Eigen::VectorXi unknown; unknown.setZero(N-nc,1);

  int indk = 0, indu = 0;
  for (int i = 0; i<N; ++i)
    if (isConstrained[i])
    {
      known[indk] = i;
      indk++;
    }
    else
    {
      unknown[indu] = i;
      indu++;
    }

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>> Quu, Quk;

  igl::slice(Q,unknown, unknown, Quu);
  igl::slice(Q,unknown, known, Quk);

  std::vector<typename Eigen::Triplet<std::complex<typename DerivedV::Scalar> > > tripletList;

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fu(N-nc,1);

  igl::slice(f,unknown, Eigen::VectorXi::Zero(1,1), fu);

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > rhs = (Quk*xknown).sparseView()+.5*fu;

  Eigen::SparseLU< Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>>> solver;
  solver.compute(-Quu);
  if(solver.info()!=Eigen::Success)
  {
    std::cerr<<"Decomposition failed!"<<std::endl;
    return;
  }
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>>  b  = solver.solve(rhs);
  if(solver.info()!=Eigen::Success)
  {
    std::cerr<<"Solving failed!"<<std::endl;
    return;
  }

  indk = 0, indu = 0;
  x.setZero(N,1);
  for (int i = 0; i<N; ++i)
    if (isConstrained[i])
      x[i] = xknown[indk++];
    else
      x[i] = b.coeff(indu++,0);

}

template<typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE bool igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO>::
solve(const Eigen::VectorXi &isConstrained,
      const Eigen::PlainObjectBase<DerivedO> &initialSolution,
      Eigen::PlainObjectBase<DerivedO> &output,
      typename DerivedV::Scalar *lambdaOut)
{
  int numConstrained = isConstrained.sum();
  // coefficient values
  Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> Ak, Bk;

  pvU.resize(data.numF,2);
  pvV.resize(data.numF,2);
  for (int fi = 0; fi <data.numF; ++fi)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b1 = data.B1.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b2 = data.B2.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &u3 = initialSolution.block(fi,0,1,3);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &v3 = initialSolution.block(fi,3,1,3);
    pvU.row(fi)<< u3.dot(b1), u3.dot(b2);
    pvV.row(fi)<< v3.dot(b1), v3.dot(b2);
  }
  setCoefficientsFromField();
  Ak.resize(numConstrained,1);
  Bk.resize(numConstrained,1);
  int ind = 0;
  for (int i = 0; i <data.numF; ++i)
  {
    if(isConstrained[i])
    {
      Ak(ind) = Acoeff[i];
      Bk(ind) = Bcoeff[i];
      ind ++;
    }
  }

  typename DerivedV::Scalar smoothnessValue;
  int oob;

  smoothnessValue = (Acoeff.adjoint()*data.DDA*Acoeff + Bcoeff.adjoint()*data.DDB*Bcoeff).real()[0];
  printf("\n\nInitial smoothness: %.5g\n",smoothnessValue);
  oob = getNumOutOfBounds();
  printf("\n\nInitial out-of-bounds: %d\n",oob);
  printf(" %d %.5g %d\n",-1, smoothnessValue, oob);

  lambda = lambdaInit;
  for (int iter = 0; iter<maxIter; ++iter)
  {
    printf("\n\n--- Iteration %d ---\n",iter);

    localStep();
    globalStep(isConstrained, Ak, Bk);

    smoothnessValue = (Acoeff.adjoint()*data.DDA*Acoeff + Bcoeff.adjoint()*data.DDB*Bcoeff).real()[0];

    printf("Smoothness: %.5g\n",smoothnessValue);

    oob = getNumOutOfBounds();

    bool stoppingCriterion = (oob == 0) ;
    if (stoppingCriterion)
      break;
    lambda = lambda*lambdaMultFactor;
//    printf(" %d %.5g %d\n",iter, smoothnessValue, oob);

  }

  output.setZero(data.numF,6);
  for (int fi=0; fi<data.numF; ++fi)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b1 = data.B1.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b2 = data.B2.row(fi);
    output.block(fi,0, 1, 3) = pvU(fi,0)*b1 + pvU(fi,1)*b2;
    output.block(fi,3, 1, 3) = pvV(fi,0)*b1 + pvV(fi,1)*b2;
  }

  if (lambdaOut)
    *lambdaOut = lambda;

  return (oob==0);
}

template <typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE bool igl::angle_bound_frame_fields(const Eigen::PlainObjectBase<DerivedV> &V,
                                            const Eigen::PlainObjectBase<DerivedF> &F,
                                              const typename DerivedV::Scalar &thetaMin,
                                            const Eigen::VectorXi &isConstrained,
                                            const Eigen::PlainObjectBase<DerivedO> &initialSolution,
                                            Eigen::PlainObjectBase<DerivedO> &output,
                                            int maxIter,
                                            const typename DerivedV::Scalar &lambdaInit,
                                            const typename DerivedV::Scalar &lambdaMultFactor,
                                              const bool doHardConstraints)
{
  igl::AngleBoundFFSolverData<DerivedV, DerivedF> csdata(V, F);
  igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO> cs(csdata, thetaMin, maxIter, lambdaInit, lambdaMultFactor, doHardConstraints);
  return (cs.solve(isConstrained, initialSolution, output));
}

template <typename DerivedV, typename DerivedF, typename DerivedO>
IGL_INLINE bool igl::angle_bound_frame_fields(const igl::AngleBoundFFSolverData<DerivedV, DerivedF> &csdata,
                                              const typename DerivedV::Scalar &thetaMin,
                                            const Eigen::VectorXi &isConstrained,
                                            const Eigen::PlainObjectBase<DerivedO> &initialSolution,
                                            Eigen::PlainObjectBase<DerivedO> &output,
                                            int maxIter,
                                            const typename DerivedV::Scalar &lambdaInit,
                                            const typename DerivedV::Scalar &lambdaMultFactor,
                                              const bool doHardConstraints,
                                            typename DerivedV::Scalar *lambdaOut)
{
  igl::AngleBoundFFSolver<DerivedV, DerivedF, DerivedO> cs(csdata, thetaMin, maxIter, lambdaInit, lambdaMultFactor, doHardConstraints);
  return (cs.solve(isConstrained, initialSolution, output, lambdaOut));
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
#endif


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

  @copyright 2012-2015 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_CONSTANT_DEFINITION_MAXLEFTSHIFT_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_DEFINITION_MAXLEFTSHIFT_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    struct maxleftshift_ : boost::dispatch::constant_value_<maxleftshift_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,maxleftshift_,boost::dispatch::constant_value_<maxleftshift_>);
      struct value_map
      {
        template<typename X>
        static std::integral_constant<typename boost::dispatch::as_integer_t<X>,sizeof(X)*CHAR_BIT-1> value(boost::dispatch::arithmetic_<X> const&);
      };
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,maxleftshift_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::maxleftshift_,maxleftshift);
  }

  template<typename T> BOOST_FORCEINLINE auto Maxleftshift()
  BOOST_NOEXCEPT_DECLTYPE(detail::maxleftshift( boost::dispatch::as_<T>{}))
  {
    return detail::maxleftshift( boost::dispatch::as_<T>{} );
  }
} }

#endif


// Copyright 2020 Advanced Remanufacturing and Technology Centre
// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef EMD__GRASP_PLANNER__COMMON__MATH_FUNCTIONS_HPP_
#define EMD__GRASP_PLANNER__COMMON__MATH_FUNCTIONS_HPP_

// Other Libraries
#include <Eigen/Core>

// For uuid
#include "boost/uuid/uuid.hpp"
#include "boost/uuid/random_generator.hpp"
#include "boost/uuid/uuid_io.hpp"

#include <pcl/point_types.h>
namespace MathFunctions
{

struct Point
{
  Eigen::Vector3f reference_vector;
  Eigen::Vector3f reference_point;
  float projection_distance;
  float perpendicular_distance;
  int index;
  pcl::PointNormal pcl_npoint;
  Point(
    Eigen::Vector3f reference_vector_,
    Eigen::Vector3f reference_point_,
    float projection_distance_,
    float perpendicular_distance_)
  : reference_vector(reference_vector_),
    reference_point(reference_point_),
    projection_distance(projection_distance_),
    perpendicular_distance(perpendicular_distance_)
  {}
};
float normalize(
  const float & target,
  const float & min,
  const float & max);

float normalizeInt(
  const int & target,
  const int & min,
  const int & max);

float getAngleBetweenVectors(
  const Eigen::Vector3f & vector_1,
  const Eigen::Vector3f & vector_2);

Eigen::Vector3f getPointInDirection(
  const Eigen::Vector3f & base_point,
  const Eigen::Vector3f & vector_direction,
  const float & distance);

Eigen::Vector3f getRotatedVector(
  const Eigen::Vector3f & target_vector,
  const float & angle,
  const char & axis);

MathFunctions::Point getPointInfo(
  const Eigen::Vector3f & target_point,
  const Eigen::Vector3f & vector_direction,
  const Eigen::Vector3f & point_on_vector);

std::string generate_task_id();

}  // namespace MathFunctions

#endif  // EMD__GRASP_PLANNER__COMMON__MATH_FUNCTIONS_HPP_


/// \file   test_order_book.cpp
///
/// \brief
///
/// \authors    Maarten P. Scholl
/// \date       2020-06-27
/// \copyright  Copyright 2017-2020 The Institute for New Economic Thinking,
///             Oxford Martin School, University of Oxford
///
///             Licensed under the Apache License, Version 2.0 (the "License");
///             you may not use this file except in compliance with the License.
///             You may obtain a copy of the License at
///
///                 http://www.apache.org/licenses/LICENSE-2.0
///
///             Unless required by applicable law or agreed to in writing,
///             software distributed under the License is distributed on an "AS
///             IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
///             express or implied. See the License for the specific language
///             governing permissions and limitations under the License.
///
///             You may obtain instructions to fulfill the attribution
///             requirements in CITATION.cff
///
#include <set>
#include <cmath>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE order_book

#include <boost/test/included/unit_test.hpp>

#define private public
#define protected public
#include <esl/economics/currencies.hpp>
#include <esl/economics/markets/order_book/binary_tree_order_book.hpp>
#include <esl/economics/markets/order_book/static_order_book.hpp>
#undef private
#undef protected
#include <esl/data/representation.hpp>

using namespace esl;
using namespace esl::economics;
using namespace esl::economics::markets::order_book;
using esl::economics::markets::quote;
using esl::economics::markets::order_book::limit_order;

BOOST_AUTO_TEST_SUITE(ESL)
    ///
    /// \brief This tests whether memory is initialized correctly
    ///
    BOOST_AUTO_TEST_CASE(statically_allocated_book_construction)
    {
        auto  min_ = quote(price::approximate(0.01, currencies::USD), 100 *  currencies::USD.denominator);
        auto  max_ = quote(price::approximate(1'0.00, currencies::USD), 100 *  currencies::USD.denominator);
        auto book_ = markets::order_book::static_order_book(min_, max_);

        BOOST_CHECK_EQUAL(book_.ticks,10'000);

        BOOST_CHECK_EQUAL(book_.limits_.size(),999 * 100 + 1);

        markets::order_book::static_order_book::limit l_;

        book_.default_encode(book_.valid_limits, quote(min_), l_);
        BOOST_CHECK_EQUAL(l_,0);

        markets::order_book::static_order_book::limit u_;

        book_.default_encode(book_.valid_limits, quote(max_), u_);
        BOOST_CHECK_EQUAL(u_,999 * 100 + 1);
    }

    ///
    /// \brief Here we tests whether incorrect orders are rejected
    ///         as they should be
    ///
    BOOST_AUTO_TEST_CASE(statically_allocated_book_invalidated)
    {
        auto  min_ = quote(price::approximate(0.01, currencies::USD), 100 *  currencies::USD.denominator);
        auto  max_ = quote(price::approximate(1'0.00, currencies::USD), 100 *  currencies::USD.denominator);
        auto book_ = markets::order_book::static_order_book(min_, max_);

        esl::economics::markets::ticker ticker_dummy_;
        identity<agent> owner_dummy_;

        auto bid_toosmall_ = limit_order
            (ticker_dummy_
                , owner_dummy_
                , limit_order::side_t::buy
                , quote(price::approximate(0.001, currencies::USD), 100 *  currencies::USD.denominator)
                , 1000
                , limit_order::lifetime_t::good_until_cancelled
            );

        book_.insert(bid_toosmall_);

        BOOST_CHECK_EQUAL(book_.reports.size(), 1);
        BOOST_CHECK_EQUAL(book_.reports.front().state,  execution_report::invalid);

        auto bid_toolarge_ = limit_order
            (ticker_dummy_
                , owner_dummy_
                , limit_order::side_t::buy
                , quote(price::approximate(10.01, currencies::USD), 100 *  currencies::USD.denominator)
                , 1000
                , limit_order::lifetime_t::good_until_cancelled
            );
        book_.insert(bid_toolarge_);

        BOOST_CHECK_EQUAL(book_.reports.size(), 2);
        BOOST_CHECK_EQUAL(book_.reports.back().state,  execution_report::invalid);
    }

    limit_order create_bid(double p, size_t q = 1000)
    {
        esl::economics::markets::ticker ticker_dummy_;
        identity<agent> owner_dummy_;

        auto bid_ = limit_order
            (ticker_dummy_
                , owner_dummy_
                , limit_order::side_t::buy
                , quote(price::approximate(p, currencies::USD), 100 *  currencies::USD.denominator)
                , q
                , limit_order::lifetime_t::good_until_cancelled
            );

        return bid_;
    }

    limit_order create_ask(double p, size_t q = 1000)
    {
        esl::economics::markets::ticker ticker_dummy_;
        identity<agent> owner_dummy_;

        auto bid_ = limit_order
            (ticker_dummy_
                , owner_dummy_
                , limit_order::side_t::sell
                , quote(price::approximate(p, currencies::USD), 100 *  currencies::USD.denominator)
                , q
                , limit_order::lifetime_t::good_until_cancelled
            );

        return bid_;
    }

    ///
    /// \brief  This test is to see whether the function that maps quotes of
    ///         any type to an array index yields unique results and that the
    ///         the array index is converted back to the right quote.
    ///
    BOOST_AUTO_TEST_CASE(statically_allocated_book_encode_decode)
    {
        auto  min_ = quote(price::approximate(0.01, currencies::USD), 100 *  currencies::USD.denominator);
        auto  max_ = quote(price::approximate(10.00, currencies::USD), 100 *  currencies::USD.denominator);
        auto book_ = markets::order_book::static_order_book(min_, max_);

        std::set<markets::order_book::static_order_book::limit> unique_;

        for(double p: std::vector<double>{
            0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09,
            4.96, 4.97, 4.98, 4.99, 5.00, 5.01, 5.02, 5.03, 5.04,
            9.92, 9.93, 9.94, 9.95, 9.96, 9.97, 9.98, 9.99, 10.00
        }){
            markets::order_book::static_order_book::limit l;
            // use nextafter, because otherwise we make floating point errors
            auto q = quote(price::approximate(std::nextafter(p, p+0.01), currencies::USD)
                                   , 100 *  currencies::USD.denominator);
            BOOST_CHECK(book_.encode(book_.valid_limits, q, l));
            auto array_index_ = l;

            // testing this seems somewhat redundant, but this can
            // detect a broken comparison operator in the quote class
            // or bad floating point conversions

            BOOST_CHECK(unique_.find(array_index_) == unique_.end());
            unique_.insert(array_index_);

            auto rtq_ = book_.decode(book_.valid_limits, l);    // round trip value
            BOOST_CHECK_EQUAL(q, rtq_);

        }

    }

    BOOST_AUTO_TEST_CASE(statically_allocated_book_placement)
    {
        auto  min_ = quote(price::approximate(0.01, currencies::USD), 100 *  currencies::USD.denominator);
        auto  max_ = quote(price::approximate(10.00, currencies::USD), 100 *  currencies::USD.denominator);
        auto book_ = markets::order_book::static_order_book(min_, max_);

        book_.insert(create_bid(4.75));
        BOOST_CHECK_EQUAL(book_.reports.front().state,  execution_report::placement);

        book_.insert(create_bid(4.75));
        book_.insert(create_bid(4.76));
        book_.insert(create_bid(4.74));
        BOOST_CHECK_EQUAL(book_.reports.size(), 4);
        BOOST_CHECK_EQUAL(book_.reports.back().state,  execution_report::placement);

        BOOST_CHECK(book_.bid());
        BOOST_CHECK_EQUAL(book_.bid().value(), quote(price::approximate(4.76, currencies::USD), 100 *  currencies::USD.denominator));
    }

    BOOST_AUTO_TEST_CASE(statically_allocated_book_match)
    {
        auto  min_ = quote(price::approximate(0.01, currencies::USD), 100 *  currencies::USD.denominator);
        auto  max_ = quote(price::approximate(10.00, currencies::USD), 100 *  currencies::USD.denominator);
        auto book_ = markets::order_book::static_order_book(min_, max_);

        book_.insert(create_bid(4.75, 500));
        book_.insert(create_bid(4.75, 500));
        book_.insert(create_bid(4.74));
        book_.insert(create_bid(4.76, 500));

        book_.display(); std::cout << std::endl;

        book_.insert(create_ask(4.69, 750));    // expect: execute 500 at 4.76 and remainder of 250 at 4.75, leaving 750

        book_.display(); std::cout << std::endl;

        BOOST_CHECK_EQUAL(book_.bid().value(), quote(price::approximate(4.75, currencies::USD), 100 *  currencies::USD.denominator));

        book_.insert(create_ask(4.75, 1000));   //  expect: execute 750 at 4.75 and then place 250 in book at 4.75, improving best ask

        book_.display(); std::cout << std::endl;

        BOOST_CHECK_EQUAL(book_.bid().value(), quote(price::approximate(4.74, currencies::USD), 100 *  currencies::USD.denominator));
        BOOST_CHECK_EQUAL(book_.ask().value(), quote(price::approximate(4.75, currencies::USD), 100 *  currencies::USD.denominator));

        book_.insert(create_bid(4.75, 200));   //  expect: execute 200 at 4.75
        BOOST_CHECK_EQUAL(book_.ask().value(), quote(price::approximate(4.75, currencies::USD), 100 *  currencies::USD.denominator));
        BOOST_CHECK_EQUAL(book_.reports.back().quantity, 200);

        book_.display(); std::cout << std::endl;

        book_.insert(create_bid(4.75, 51));   //  expect: execute 750 at 4.75 and then place 250 in book at 4.75, improving best ask

        book_.display(); std::cout << std::endl;

        BOOST_CHECK(!book_.ask());
        BOOST_CHECK_EQUAL(book_.bid().value(), quote(price::approximate(4.75, currencies::USD), 100 *  currencies::USD.denominator));
    }

    BOOST_AUTO_TEST_CASE(statically_allocated_book_match2)
    {
        auto  min_ = quote(price::approximate(0.01, currencies::USD), 100 *  currencies::USD.denominator);
        auto  max_ = quote(price::approximate(10.00, currencies::USD), 100 *  currencies::USD.denominator);
        auto book_ = markets::order_book::static_order_book(min_, max_);

        book_.insert(create_bid(5.00, 500));
        book_.insert(create_bid(4.75, 500));
        book_.insert(create_bid(4.74));
        book_.insert(create_bid(4.76, 500));

        book_.display(); std::cout << std::endl;

        book_.insert(create_ask(4.69, 750));    // expect: execute 500 at 4.76 and remainder of 250 at 4.75, leaving 750

        book_.display(); std::cout << std::endl;

        BOOST_CHECK_EQUAL(book_.bid().value(), quote(price::approximate(4.75, currencies::USD), 100 *  currencies::USD.denominator));

        book_.insert(create_ask(4.75, 1000));   //  expect: execute 750 at 4.75 and then place 250 in book at 4.75, improving best ask

        book_.display(); std::cout << std::endl;

        BOOST_CHECK_EQUAL(book_.bid().value(), quote(price::approximate(4.74, currencies::USD), 100 *  currencies::USD.denominator));
        BOOST_CHECK_EQUAL(book_.ask().value(), quote(price::approximate(4.75, currencies::USD), 100 *  currencies::USD.denominator));

        book_.insert(create_bid(4.75, 200));   //  expect: execute 200 at 4.75
        BOOST_CHECK_EQUAL(book_.ask().value(), quote(price::approximate(4.75, currencies::USD), 100 *  currencies::USD.denominator));
        BOOST_CHECK_EQUAL(book_.reports.back().quantity, 200);

        book_.display(); std::cout << std::endl;

        book_.insert(create_bid(4.75, 51));   //  expect: execute 750 at 4.75 and then place 250 in book at 4.75, improving best ask

        book_.display(); std::cout << std::endl;

        BOOST_CHECK(!book_.ask());
        BOOST_CHECK_EQUAL(book_.bid().value(), quote(price::approximate(4.75, currencies::USD), 100 *  currencies::USD.denominator));
    }

    limit_order create(double p, size_t q = 1000, limit_order::side_t side = limit_order::side_t::sell)
    {
        esl::economics::markets::ticker ticker_dummy_;
        identity<agent> owner_dummy_;

        auto bid_ = limit_order
            (ticker_dummy_
                , owner_dummy_
                , side
                , quote(price::approximate(p, currencies::USD), 100 *  currencies::USD.denominator)
                , q
                , limit_order::lifetime_t::good_until_cancelled
            );

        return bid_;
    }

#ifdef ESL_RELEASE
    BOOST_AUTO_TEST_CASE(statically_allocated_book_performance)
    {
        auto  min_ = quote(price::approximate( 80.00, currencies::USD), 100 *  currencies::USD.denominator);
        auto  max_ = quote(price::approximate(120.00, currencies::USD), 100 *  currencies::USD.denominator);
        auto book_ = new static_order_book(min_, max_, 1024*1024);

        std::vector<limit_order> messages_;

        std::minstd_rand generator_;
        std::normal_distribution<> order_limit_(100.0, 1.5);

        std::exponential_distribution<double> order_size_(5.0);

        for(size_t i = 0; i < 1000'000; ++i){
            auto limit_ = order_limit_(generator_);
            auto size_ = static_cast<std::uint32_t>(10000 * (order_size_(generator_)) );

            if(generator_() & 0x1){
                messages_.emplace_back(create(limit_, size_, limit_order::side_t::sell));
            }else{
                messages_.emplace_back(create(limit_, size_, limit_order::side_t::buy));
            }
        }

        auto t1 = std::chrono::high_resolution_clock::now();
        for(const auto &o: messages_){

            if(0 == generator_() % 5 ){
                // cancel random order
            }

            book_->insert(o);
            book_->reports.clear();
        }
        auto t2 = std::chrono::high_resolution_clock::now();
        auto time_span = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);

        std::cout << messages_.size() << " orders in " << time_span.count() << " seconds.";
        std::cout << std::endl;

        book_->display(5);

        std::cout << book_->pool_.size() << std::endl;
    }
#endif

BOOST_AUTO_TEST_SUITE_END()  // ESL


//
// g++ async_example.cpp -o async_example -I.. -lboost_program_options -lhiredis -lev
//

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <string>
#include <iostream>
#include <hiredis/async.h>

#include <hiredispp/hiredispp.h>
#include <hiredispp/hiredispp_async.h>

#include <boost/bind.hpp>
#include <boost/random.hpp>
#include <boost/program_options.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

using namespace std;
using namespace hiredispp;
using namespace boost::posix_time;
using namespace boost::accumulators;

namespace po = boost::program_options;

class Main
{
    typedef accumulator_set<double, stats <tag::median(with_p_square_quantile),
                                           tag::skewness, // add mean, moment<2>, moment<3>
                                           tag::count, tag::min, tag::max, tag::mean
                                           > > AccuType;

public:
    Main(const string& host, int port, int total, int batch, const string& key, bool is_set, int vsize) :
        _host(host), _port(port), _connected(false),
        _ac(_host, _port), _counter(0), _done(0), _total(total),
        _batch(batch), _key(key), _set(is_set), _vsize(vsize),
        _dist(1, (vsize >0 ? vsize : 1)), _rand(_rng, _dist)
    {
        connect();
        _start=microsec_clock::universal_time();
        _cstart=_start;
    }

    ~Main()
    {
        if (_connected) {
            _ac.disconnect();
        }
        boost::posix_time::ptime stop=microsec_clock::universal_time();
        cout << "FINAL:" << endl;
        print_stats(cout, _start, stop, _lat) << endl;
    }

    ostream& print_stats(ostream& out,
                         boost::posix_time::ptime start,
                         boost::posix_time::ptime stop,
                         const AccuType& acc) const
    {
        long long int per = time_period(start, stop).length().total_microseconds();
        return out
            << extract_result< tag::count >(acc) << " "
            << extract_result< tag::min >(acc) << " "
            << mean(acc) << " "
            << extract_result< tag::max >(acc) << " "
            << extract_result< tag::median >(acc) << " "
            << sqrt(extract_result< tag::moment<2> > (acc)) << " "
            << extract_result< tag::skewness > (acc)
            << "\ttime: "<< per
            << "\tRPS "  << 1e6 * extract_result< tag::count >(acc) / per;
    }

    void connect()
    {
        _ac.connect(boost::bind(&Main::onConnected, this, _1),
                    boost::bind(&Main::onDisconnected, this, _1));
    }

    void setDone(RedisConnectionAsync& ac, Redis::Element *reply, int id, boost::posix_time::ptime start)
    {
        boost::posix_time::ptime stop=microsec_clock::universal_time();
        try {
            if (reply) {
                ++_done;
                reply->checkError();
            }
            else {
                throw RedisException(std::string("disconnected"));
            }

            _lat(time_period(start, stop).length().total_microseconds());
            _cur(time_period(start, stop).length().total_microseconds());

            if (_done==_counter && (_batch!=1 || (_done % (_total/10)==0))) {
                cout << "exec.done "<<_counter
                     <<(reply ? " REPLY " : " NULL ");
                print_stats(cout, _cstart, stop, _cur) << endl;
                _cstart = stop;
                _cur=AccuType();
            }

            if (_done==_counter) {
                executeNext();
            }
        }
        catch (const RedisException& ex) {
            cout<<"Main::execCmd exception "<<ex.what()<<endl;
            if (_connected)
                _ac.disconnect();
        }
    }

    void onConnected(boost::shared_ptr<RedisException> &ex)
    {
        cout << "Main::onConnected: "
             << (ex ? ex->what() : "OK")
             << endl;

        if (ex==NULL) {
            _connected=true;
            executeNext();
        }
    }

    void onDisconnected(boost::shared_ptr<RedisException> &ex)
    {
        cout << "Main::onDisconnected: "
             << (ex ? ex->what() : "OK")
             << endl;

        _connected=false;
    }

    void executeNext()
    {
        if (_counter<_total) {
            try {
                for (int i=0; i<_batch && _counter<_total; ++i) {
                    boost::posix_time::ptime start=microsec_clock::universal_time();
                    RedisCommandBase<char> cmd;
                    if (_set) {
                        cmd<<"set"
                           <<(_key+boost::lexical_cast<string> (_counter));
                        if (_vsize==0) {
                            cmd<<((string)"myvalue" + boost::lexical_cast<string> (_counter));
                        }
                        else {
                            cmd<<string(_rand(),'v');
                        }
                    }
                    else {
                        cmd<<"get"
                           <<(_key+boost::lexical_cast<string> (_counter));
                    }
                    _ac.execAsyncCommand(cmd, boost::bind(&Main::setDone, this, _1, _2, _counter, start));
                    ++_counter;
                }
            }
            catch (const hiredispp::RedisException& ex) {
                cout<<ex.what()<<endl;
                _ac.disconnect();
            }
        }
        else {
            if (_done==_total) {
                _ac.disconnect();
            }
        }
    }

    string _host;
    int    _port;
    bool   _connected;
    int    _counter;
    int    _done;

    boost::posix_time::ptime _start;
    boost::posix_time::ptime _cstart;

    const string _key;
    const int _batch;
    const int _total;
    const int _vsize;
    const bool _set;

    boost::mt19937        _rng;
    boost::uniform_int<> _dist;
    boost::variate_generator<boost::mt19937&, boost::uniform_int<> > _rand;

    RedisConnectionAsync _ac;

    AccuType _cur;
    AccuType _lat;
};

namespace std {
    std::ostream& operator<< (std::ostream& out, const std::pair<std::string, std::string> &p) {
        return out << p.first << ":" << p.second;
    }
}

int main(int argc, char** argv)
{
    string host;
    string key;
    int    port;
    int    count;
    int    batch;
    int    vsize;
    bool   is_set;

    po::options_description desc("options");

    desc.add_options()
        ("help", "produce this help message")
        ("host", po::value<string>(&host)->default_value("localhost"), "host")
        ("port", po::value<int>(&port)->default_value(6379), "port")
        ("count", po::value<int>(&count)->default_value(1000), "number of requests")
        ("batch", po::value<int>(&batch)->default_value(100), "batch size")
        ("is_set", po::value<bool>(&is_set)->default_value(true), "set/get command")
        ("key", po::value<string>(&key)->default_value("mykey"), "key prefix")
        ("value_size", po::value<int>(&vsize)->default_value(0), "maximum value size, 0 - fixed.")
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }

    signal(SIGPIPE, SIG_IGN);
    ev_default_loop(0);
    Main main(host, port, count, batch, key, is_set, vsize);
    ev_loop(EV_DEFAULT, 0);
    return 0;
}


#define BOOST_TEST_MODULE PotentialScaleReductionFactorTestSuite

#include <boost/test/included/unit_test.hpp>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <hops/hops.hpp>

BOOST_AUTO_TEST_SUITE(PotentialScaleReductionFactorTestSuite)
    BOOST_AUTO_TEST_CASE(ComputeAllDraws) {
        // Expected results were evaluated using stan implementation
        size_t numChains = 4;
        auto expectedResult = std::vector<double>{3.87441, 4.94213, 3.52473, 6.43039, 4.06456};
        std::vector<std::vector<Eigen::VectorXd>> chains(numChains);
        for (size_t i = 0; i < numChains; ++i) {
            auto statesMatrix = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
                    std::string("../../resources/test_states/states") + std::to_string(i) + ".csv");

            std::vector<Eigen::VectorXd> draws(statesMatrix.rows());

            for (long j = 0; j < statesMatrix.rows(); ++j) {
                draws[j] = statesMatrix.row(j);
            }

            chains[i] = draws;
        }

        std::vector<double> actualResult = hops::computePotentialScaleReductionFactor(chains);

        for (size_t i = 0; i < expectedResult.size(); ++i) {
            BOOST_CHECK_CLOSE(expectedResult[i], actualResult[i], 0.01);
        }
    }

    BOOST_AUTO_TEST_CASE(ComputeAllDrawsPointerArray) {
        // Expected results were evaluated using stan implementation
        size_t numChains = 4;
        auto expectedResult = std::vector<double>{3.87441, 4.94213, 3.52473, 6.43039, 4.06456};
        std::vector<std::vector<Eigen::VectorXd>> chains(numChains);
        std::vector<const std::vector<Eigen::VectorXd>*> chainsPtrArray(numChains);
        for (size_t i = 0; i < numChains; ++i) {
            auto statesMatrix = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
                    std::string("../../resources/test_states/states") + std::to_string(i) + ".csv");

            std::vector<Eigen::VectorXd> draws(statesMatrix.rows());

            for (long j = 0; j < statesMatrix.rows(); ++j) {
                draws[j] = statesMatrix.row(j);
            }

            chains[i] = draws;
            chainsPtrArray[i] = &chains[i];
        }

        std::vector<double> actualResult = hops::computePotentialScaleReductionFactor(chains);

        for (size_t i = 0; i < expectedResult.size(); ++i) {
            BOOST_CHECK_CLOSE(expectedResult[i], actualResult[i], 0.01);
        }
    }

    BOOST_AUTO_TEST_CASE(ComputeIncrementally) {
        // Expected results were evaluated using stan implementation
        size_t numChains = 4;
        auto expectedResult = std::vector<double>{8.18767, 7.33072, 5.33744, 9.95099, 6.34195};
        std::vector<std::vector<Eigen::VectorXd>> chains(numChains);
        for (size_t i = 0; i < numChains; ++i) {
            auto statesMatrix = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
                    std::string("../../resources/test_states/states") + std::to_string(i) + ".csv");

            std::vector<Eigen::VectorXd> draws(static_cast<unsigned long>(statesMatrix.rows() / 2));

            for (long j = 0; j < statesMatrix.rows() / 2; ++j) {
                draws[j] = statesMatrix.row(j);
            }

            chains[i] = draws;
        }

        unsigned long numUnseen = static_cast<unsigned long>(chains[0].size());
        std::vector<std::vector<double>> sampleVariances;
        std::vector<std::vector<double>> intraChainExpectations;
        std::vector<double> interChainExpectation;
        unsigned long numSeen = 0;

        std::vector<double> actualResult = hops::computePotentialScaleReductionFactor<Eigen::VectorXd>(
                chains, numUnseen, sampleVariances, intraChainExpectations, interChainExpectation, numSeen);

        for (size_t i = 0; i < expectedResult.size(); ++i) {
            BOOST_CHECK_CLOSE(expectedResult[i], actualResult[i], 0.01);
        }

        expectedResult = std::vector<double>{3.87441, 4.94213, 3.52473, 6.43039, 4.06456};
        for (size_t i = 0; i < numChains; ++i) {
            auto statesMatrix = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
                    std::string("../../resources/test_states/states") + std::to_string(i) + ".csv");

            for (long j = statesMatrix.rows() / 2; j < statesMatrix.rows(); ++j) {
                chains[i].push_back(statesMatrix.row(j));
            }
        }

        actualResult = hops::computePotentialScaleReductionFactor(
                chains, numUnseen, sampleVariances, intraChainExpectations, interChainExpectation, numSeen);

        for (size_t i = 0; i < expectedResult.size(); ++i) {
            BOOST_CHECK_CLOSE(expectedResult[i], actualResult[i], 0.01);
        }
    }

    BOOST_AUTO_TEST_CASE(ComputeIncrementallyPointerArray) {
        // Expected results were evaluated using stan implementation
        size_t numChains = 4;
        auto expectedResult = std::vector<double>{8.18767, 7.33072, 5.33744, 9.95099, 6.34195};
        std::vector<std::vector<Eigen::VectorXd>> chains(numChains);
        std::vector<const std::vector<Eigen::VectorXd>*> chainsPtrArray(numChains);
        for (size_t i = 0; i < numChains; ++i) {
            auto statesMatrix = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
                    std::string("../../resources/test_states/states") + std::to_string(i) + ".csv");

            std::vector<Eigen::VectorXd> draws(static_cast<unsigned long>(statesMatrix.rows() / 2));

            for (long j = 0; j < statesMatrix.rows() / 2; ++j) {
                draws[j] = statesMatrix.row(j);
            }

            chains[i] = draws;
            chainsPtrArray[i] = &chains[i];
        }

        unsigned long numUnseen = static_cast<unsigned long>(chains[0].size());
        std::vector<std::vector<double>> sampleVariances;
        std::vector<std::vector<double>> intraChainExpectations;
        std::vector<double> interChainExpectation;
        unsigned long numSeen = 0;

        std::vector<double> actualResult = hops::computePotentialScaleReductionFactor<Eigen::VectorXd>(
                chains, numUnseen, sampleVariances, intraChainExpectations, interChainExpectation, numSeen);

        for (size_t i = 0; i < expectedResult.size(); ++i) {
            BOOST_CHECK_CLOSE(expectedResult[i], actualResult[i], 0.01);
        }

        // Expected results were evaluated using stan implementation
        expectedResult = std::vector<double>{3.87441, 4.94213, 3.52473, 6.43039, 4.06456};
        for (size_t i = 0; i < numChains; ++i) {
            auto statesMatrix = hops::CsvReader::readMatrix<Eigen::MatrixXd>(
                    std::string("../../resources/test_states/states") + std::to_string(i) + ".csv");

            for (long j = statesMatrix.rows() / 2; j < statesMatrix.rows(); ++j) {
                chains[i].push_back(statesMatrix.row(j));
            }
        }

        actualResult = hops::computePotentialScaleReductionFactor(
                chains, numUnseen, sampleVariances, intraChainExpectations, interChainExpectation, numSeen);

        for (size_t i = 0; i < expectedResult.size(); ++i) {
            BOOST_CHECK_CLOSE(expectedResult[i], actualResult[i], 0.01);
        }
    }

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry

// Copyright (c) 2017 Adam Wulkiewicz, Lodz, Poland.

// 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)

#ifndef BOOST_GEOMETRY_UTIL_SELECT_SEQUENCE_ELEMENT
#define BOOST_GEOMETRY_UTIL_SELECT_SEQUENCE_ELEMENT

#include <boost/mpl/if.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/size.hpp>

#include <boost/type_traits/is_same.hpp>

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace util
{

template <typename Curr, typename Next>
struct pred_more_precise_coordinate_type
{
    typedef typename geometry::coordinate_type<Curr>::type curr_coord_t;
    typedef typename geometry::coordinate_type<Next>::type next_coord_t;

    typedef typename boost::mpl::if_c
        <
            boost::is_same
                <
                    curr_coord_t,
                    typename select_most_precise
                        <
                            curr_coord_t,
                            next_coord_t
                        >::type
                >::value,
            Curr,
            Next
        >::type type;
};

template
<
    typename Seq,
    template<typename, typename> class Pred = pred_more_precise_coordinate_type,
    int I = 0,
    int N = boost::mpl::size<Seq>::value
>
struct select_sequence_element
{
    typedef typename boost::mpl::at<Seq, boost::mpl::int_<I> >::type curr_t;
    typedef typename select_sequence_element<Seq, Pred, I+1, N>::type next_t;

    typedef typename Pred<curr_t, next_t>::type type;
};

template <typename Seq, template<typename, typename> class Pred, int N>
struct select_sequence_element<Seq, Pred, N, N>
{
    typedef typename boost::mpl::at<Seq, boost::mpl::int_<N-1> >::type type;
};

} // namespace util

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_UTIL_SELECT_SEQUENCE_ELEMENT


#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <iostream>
#include <string>
#include <utility>
#include <vector>
#include <unordered_set>

struct Op {
    enum { SPIN, SWAP_PROG, SWAP_POS } op;
    union POS {
        unsigned char c;
        int pos;
    };
    POS a, b;
};

std::vector<Op> ops;

void spin(std::string& data, const Op& op)
{
    int dist = data.size() - op.a.pos;
    for (int j = dist; j < data.size(); ++j) {
        int pos = j;
        for (int i = 0; i < dist; ++i, --pos) {
            std::swap(data[pos-1], data[pos]);
        }
    }
}

void swap_prog(std::string& data, const Op& op)
{
    std::swap(data[op.a.pos], data[op.b.pos]);
}

void swap_pos(std::string& data, const Op& op)
{
    std::swap(data[data.find(op.a.c)], data[data.find(op.b.c)]);
}

int main()
{
    std::string data{"abcdefghijklmnop"};

    std::string line;
    std::getline(std::cin, line);
    std::vector<std::string> words;
    boost::algorithm::split(
        words, line, boost::is_any_of(","), boost::algorithm::token_compress_on);
    for (const auto& cmd : words) {
        if (cmd[0] == 's') {
            Op op;
            op.op = Op::SPIN;
            op.a.pos = atoi(&cmd[1]);
            ops.push_back(op);
        } else if (cmd[0] == 'x') {
            Op op;
            op.op = Op::SWAP_PROG;
            op.a.pos = atoi(&cmd[1]);
            op.b.pos = atoi(&cmd[cmd.find('/')+1]);
            ops.push_back(op);
        } else if (cmd[0] == 'p') {
            Op op;
            op.op = Op::SWAP_POS;
            op.a.c = cmd[1];
            op.b.c = cmd[3];
            ops.push_back(op);
        } else {
            std::cerr << "ERROR: " << cmd << "\n";
            return 1;
        }
    }

    std::cout << "Ops: " << ops.size() << "\n";

    std::unordered_set<std::string> results;
    std::string lastR;

    for (int i = 0; i < 1000000000; ++i) {
        for (const auto& op : ops) {
            if (op.op == Op::SPIN) {
                spin(data, op);
            } else if (op.op == Op::SWAP_PROG) {
                swap_prog(data, op);
            } else if (op.op == Op::SWAP_POS) {
                swap_pos(data, op);
            }
        }
        if (!results.insert(data).second) {
            std::cout << "Found loop at " << i << ", current result " << data
                      << ", last result: " << lastR << "\n";
        }
        if (i % 10000 == 0) {
            std::cout << i / 10000000. << "%\n";
        }
        lastR = data;
    }
    std::cout << "data: " << data << "\n";
    return 0;
}


// System
#include <ctime>
#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <string>
#include <fstream>
#include <vector>
#include <chrono>
#include <random>
#include <Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/crop_box.h>

// Custom
#include <gpg/candidates_generator.h>
#include <gpg/hand_search.h>
#include <gpg/finger_hand.h>
#include <gpg/config_file.h>
#include <gpg/plot.h>

const double PI = M_PI;
const double PI_2 = M_PI*2;

// function to read in a double array from a single line of a configuration file
std::vector<double> stringToDouble(const std::string& str)
{
  std::vector<double> values;
  std::stringstream ss(str);
  double v;

  while (ss >> v)
  {
    values.push_back(v);
    if (ss.peek() == ' ')
    {
      ss.ignore();
    }
  }

  return values;
}

// function to load registrated object pose between mesh and point cloud
Eigen::Affine3d loadRegistration(const std::string& filepath)
{
  std::ifstream ifs(filepath, std::ifstream::in);
  std::string s;
  ifs >> s;
  Eigen::Matrix4d mesh2cloud;
  mesh2cloud.row(3) << 0,0,0,1;
  std::stringstream ss;
  ss << s;
  std::string substr;
  for (int i=0; i<12; ++i) {
      std::getline(ss, substr, ',');
      mesh2cloud.col(i/3).row(i%3) << std::stod(substr);
  }
  return Eigen::Affine3d(mesh2cloud);
}

int main(int argc, char* argv[])
{
  // Read parameters from configuration file.
  if (argc<5) exit(1);
  ConfigFile config_file(argv[1]);

  double finger_width = config_file.getValueOfKey<double>("finger_width", 0.01);
  double hand_outer_diameter  = config_file.getValueOfKey<double>("hand_outer_diameter", 0.12);
  double hand_depth = config_file.getValueOfKey<double>("hand_depth", 0.06);
  double hand_height  = config_file.getValueOfKey<double>("hand_height", 0.02);
  double init_bite  = config_file.getValueOfKey<double>("init_bite", 0.01);
  double friction_coeff = config_file.getValueOfKey<double>("friction_coeff", 20.0);
  int viable_thresh = config_file.getValueOfKey<int>("viable_thresh", 6);
  double noise_std_cm = config_file.getValueOfKey<double>("noise_std_cm", -1.0);

  std::cout << "finger_width: " << finger_width << "\n";
  std::cout << "hand_outer_diameter: " << hand_outer_diameter << "\n";
  std::cout << "hand_depth: " << hand_depth << "\n";
  std::cout << "hand_height: " << hand_height << "\n";
  std::cout << "init_bite: " << init_bite << "\n";
  std::cout << "friction_coeff: " << friction_coeff << "\n";
  std::cout << "viable_thresh: " << viable_thresh << "\n";
  std::cout << "noise_std_cm: " << noise_std_cm << "\n";

  bool voxelize = config_file.getValueOfKey<bool>("voxelize", true);
  bool remove_outliers = config_file.getValueOfKey<bool>("remove_outliers", false);
  std::string workspace_str = config_file.getValueOfKeyAsString("workspace", "");
  std::string camera_pose_str = config_file.getValueOfKeyAsString("camera_pose", "");
  std::vector<double> workspace = stringToDouble(workspace_str);
  std::vector<double> camera_pose = stringToDouble(camera_pose_str);
  std::cout << "voxelize: " << voxelize << "\n";
  std::cout << "remove_outliers: " << remove_outliers << "\n";
  std::cout << "workspace: " << workspace_str << "\n";
  std::cout << "camera_pose: " << camera_pose_str << "\n";

  int num_samples = config_file.getValueOfKey<int>("num_samples", 1000);
  int num_threads = config_file.getValueOfKey<int>("num_threads", 1);
  double nn_radius = config_file.getValueOfKey<double>("nn_radius", 0.01);
  int num_orientations = config_file.getValueOfKey<int>("num_orientations", 8);
  int rotation_axis = config_file.getValueOfKey<int>("rotation_axis", 2);
  std::cout << "num_samples: " << num_samples << "\n";
  std::cout << "num_threads: " << num_threads << "\n";
  std::cout << "nn_radius: " << nn_radius << "\n";
  std::cout << "num_orientations: " << num_orientations << "\n";
  std::cout << "rotation_axis: " << rotation_axis << "\n";

  bool plot_candidates = config_file.getValueOfKey<bool>("plot_candidates", true);
  bool plot_normals = config_file.getValueOfKey<bool>("plot_normals", false);
  bool output_merged_pcd = config_file.getValueOfKey<bool>("output_merged_pcd", false);
  int max_samples = config_file.getValueOfKey<int>("max_samples", 10);
  std::cout << "plot_candidates: " << plot_candidates << "\n";
  std::cout << "plot_normals: " << plot_normals << "\n";
  std::cout << "max_samples: " << max_samples << "\n";
  std::cout << "output_merged_pcd: " << output_merged_pcd << "\n";

  if (output_merged_pcd && argc<6) exit(2);

  // Create object to generate grasp candidates.
  CandidatesGenerator::Parameters generator_params;
  generator_params.num_samples_ = num_samples;
  generator_params.num_threads_ = num_threads;
  generator_params.plot_normals_ = plot_normals;
  generator_params.plot_grasps_ = plot_candidates;
  generator_params.remove_statistical_outliers_ = remove_outliers;
  generator_params.voxelize_ = false;
  generator_params.workspace_ = workspace;
  HandSearch::Parameters hand_search_params;
  hand_search_params.finger_width_ = finger_width;
  hand_search_params.hand_outer_diameter_ = hand_outer_diameter;
  hand_search_params.hand_depth_ = hand_depth;
  hand_search_params.hand_height_ = hand_height;
  hand_search_params.init_bite_ = init_bite;
  hand_search_params.nn_radius_frames_ = nn_radius;
  hand_search_params.num_orientations_ = num_orientations;
  hand_search_params.num_samples_ = num_samples;
  hand_search_params.num_threads_ = num_threads;
  hand_search_params.rotation_axis_ = rotation_axis;
  hand_search_params.friction_coeff_ = friction_coeff;
  hand_search_params.viable_thresh_ = viable_thresh;
  CandidatesGenerator candidates_generator(generator_params, hand_search_params);

  std::default_random_engine randn_generator;
  std::normal_distribution<double> randn(0.0, 1.0);

  // Set the camera pose.
  Eigen::Matrix3Xd view_points(3,1);
  view_points << camera_pose[3], camera_pose[6], camera_pose[9];

  // Create object to load point cloud from file.
  PointCloudRGB::Ptr mesh_cam_pts(new PointCloudRGB);
  pcl::StatisticalOutlierRemoval<pcl::PointXYZRGBA> sor; //filter to remove outliers
  sor.setStddevMulThresh (2.0);
  sor.setMeanK(30);
  int num_point_clouds = argc-(output_merged_pcd?5:4);
  for (int i=0; i<num_point_clouds; ++i) {
    PointCloudRGB::Ptr mesh_cam_to_merge = CloudCamera::loadPointCloudFromFile(argv[i+2]);
    // Remove outliers
    sor.setInputCloud (mesh_cam_to_merge);
    sor.filter (*mesh_cam_to_merge);
    *mesh_cam_pts += *mesh_cam_to_merge;
  }

  if (voxelize) {
      std::cout << "Voxelizing point cloud..." << std::endl;
      pcl::VoxelGrid<pcl::PointXYZRGBA> grid;
      grid.setInputCloud(mesh_cam_pts);
      grid.setLeafSize(0.003f, 0.003f, 0.003f);
      grid.filter(*mesh_cam_pts);
  }

  // Add random noise to point cloud (for robustness testing)
  if (noise_std_cm>0) {
    for (int i=0; i<mesh_cam_pts->width; ++i) {
      // Generate a random unit vector. Sampled from unite ball
      double vx = randn(randn_generator);
      double vy = randn(randn_generator);
      double vz = randn(randn_generator);
      double  v = std::max(std::sqrt(vx*vx+vy*vy+vz*vz), 1e-10); // magnitude of the random vector
      double  a = randn(randn_generator) * noise_std_cm * 0.01; // in centimeter
      if (a<0) a = -a;
      // Multiply the magnitude of the noise on the unit vector
      vx = vx / v * a;
      vy = vy / v * a;
      vz = vz / v * a;
      // Add the noise to point cloud
      mesh_cam_pts->points[i].x += vx;
      mesh_cam_pts->points[i].y += vy;
      mesh_cam_pts->points[i].z += vz;
    }
  }

  Eigen::Affine3d mesh2cloud(loadRegistration(argv[num_point_clouds+2]));
  pcl::transformPointCloud(*mesh_cam_pts, *mesh_cam_pts, mesh2cloud);
  CloudCamera mesh_cam(mesh_cam_pts, 0, view_points);
  if (mesh_cam.getCloudOriginal()->size() == 0)
  {
    std::cout << "mesh_cam: Input point cloud is empty or does not exist!\n";
    return (-1);
  }

  std::chrono::steady_clock::time_point total_begin = std::chrono::steady_clock::now();

  // Point cloud preprocessing: voxelize, remove statistical outliers, workspace filter, compute normals, subsample.
  candidates_generator.preprocessPointCloud(mesh_cam);

  // Generate a list of grasp candidates.
  std::vector<Grasp> candidates = candidates_generator.generateGraspCandidates(mesh_cam);
  std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
  std::cout << "Total => " << std::chrono::duration_cast<std::chrono::microseconds>(end - total_begin).count()*1e-6 << " seconds" << std::endl;

  std::ofstream output_fs(argv[num_point_clouds+3]);

  for (const auto &single_grasp : candidates) {
      std::vector<Grasp> grasp_vec, grasp_vec_centered;
      Eigen::Vector3d trans = single_grasp.getGraspBottom().cast<double>();
      Eigen::Vector3d top_vec = single_grasp.getGraspTop();
      Eigen::Matrix3d rot   = single_grasp.getFrame().cast<double>();
      double top = single_grasp.getTop();
      double bottom = single_grasp.getBottom();
      grasp_vec.push_back(single_grasp);

      Eigen::Matrix4d Trans_m, InvTrans_m;
      Eigen::Affine3d Trans, InvTrans;
      Eigen::Vector3d euler;
      if (rot(2,2)<0) // up-side-down
      {
        rot = rot * Eigen::AngleAxisd(PI, Eigen::Vector3d::UnitX());
      }
      euler = rot.eulerAngles(0, 1, 2); // roll, pitch, yaw
      double roll = euler(0);
      double pitch = euler(1);
      double yaw = euler(2);
      Trans_m.setIdentity();
      Trans_m.block<3,3>(0,0) = rot;
      Trans_m.block<3,1>(0,3) = trans;
      Trans.matrix() = Trans_m;
      InvTrans = Trans.inverse();
      InvTrans_m = InvTrans.matrix();

      if (roll<0) roll = std::min(PI_2, std::max(0.0, roll+PI_2));
      if (pitch<0) pitch = std::min(PI_2, std::max(0.0, pitch+PI_2));
      if (yaw<0) yaw = std::min(PI_2, std::max(0.0, yaw+PI_2));

      output_fs << std::setprecision(8) \
                << Trans_m.row(0)         << ' ' \
                << Trans_m.row(1)         << ' ' \
                << Trans_m.row(2)         << ' ' \
                << InvTrans_m.row(0)      << ' ' \
                << InvTrans_m.row(1)      << ' ' \
                << InvTrans_m.row(2)      << std::endl;
  }

  output_fs.close();

  if (output_merged_pcd) {
    pcl::io::savePCDFileBinary<pcl::PointXYZRGBA>(argv[num_point_clouds+4], *mesh_cam_pts);
  }

  return 0;
}


#include <boost/qvm/mat.hpp>


/*
 * SPDX-FileCopyrightText: 2014 University of Edinburgh, Imperial College London, University of Manchester.
 * SPDX-FileCopyrightText: 2020 Smart Robotics Lab, Imperial College London
 * SPDX-FileCopyrightText: 2020 Sotiris Papatheodorou
 * SPDX-License-Identifier: MIT
 * Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1
 */

#ifndef __READER_ICLNUIM_HPP
#define __READER_ICLNUIM_HPP

#include <cstdint>
#include <fstream>
#include <string>

#include <Eigen/Dense>

#include "se/image/image.hpp"
#include "reader_base.hpp"

namespace se {

  /** Reader for ICL-NUIM datasets. */
  class ICLNUIMReader: public Reader {
    public:
      /** Construct an ICLNUIMReader from a ReaderConfig.
       *
       * \param[in] c The configuration struct to use.
       */
      ICLNUIMReader(const ReaderConfig& c);

      /** Restart reading from the beginning. */
      void restart();

      /** The name of the reader.
       *
       * \return The string `"ICLNUIMReader"`.
       */
      std::string name() const;

      /** Convert a Euclidean distance to the camera centre to a depth value.
       * The conversion is shown in the diagram in
       * http://www.doc.ic.ac.uk/~ahanda/VaFRIC/codes.html
       *
       * \param[in] dist The distance to convert (d in the diagram).
       * \param[in] x    The horizontal pixel coordinate of dist.
       * \param[in] y    The vertical pixel coordinate of dist.
       * \return The respective depth value of dist (z in the diagram).
       */
      static float distanceToDepth(float dist, int x, int y);

      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    private:
      static constexpr float fx_ =  481.2f;
      static constexpr float fy_ = -480.0f;
      static constexpr float cx_ =  319.5f;
      static constexpr float cy_ =  239.5f;

      ReaderStatus nextDepth(Image<float>& depth_image);

      ReaderStatus nextRGBA(Image<uint32_t>& rgba_image);

      /** Return the number of depth images in the supplied directory.
       * Depth images are considered those whose name conforms to the pattern
       * scene_00_XXXX.depth where X is a digit 0-9.
       *
       * \param[in] dir The directory inside which to look for depth images.
       * \return The number of depth images found.
       */
      size_t numDepthImages(const std::string& dir) const;
  };

} // namespace se

#endif


/*
 * Copyright (c) 2015 University of Basel
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * Neither the name of the project's author nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <boost/program_options.hpp>

#include <itkCommand.h>
#include <itkCompositeTransform.h>
#include <itkImageFileReader.h>
#include <itkImageRegistrationMethod.h>
#include <itkInterpolatingStatisticalDeformationModelTransform.h>
#include <itkLBFGSOptimizer.h>
#include <itkLinearInterpolateImageFunction.h>
#include <itkNormalizedCorrelationImageToImageMetric.h>
#include <itkRigid2DTransform.h>
#include <itkStandardImageRepresenter.h>
#include <itkStatismoIO.h>
#include <itkStatisticalModel.h>
#if (ITK_VERSION_MAJOR >= 4 && ITK_VERSION_MINOR >= 6)
#include <itkTransformToDisplacementFieldFilter.h>
#else
#include <itkTransformToDisplacementFieldSource.h>
#endif
#include <itkVersorRigid3DTransform.h>
#include <itkWarpImageFilter.h>

#include "utils/itkPenalizingMeanSquaresImageToImageMetric.h"
#include "utils/statismo-fitting-utils.h"

const unsigned Dimensionality2D = 2;
const unsigned Dimensionality3D = 3;

namespace po = boost::program_options;
using namespace std;

struct programOptions {
    bool bDisplayHelp;

    string strInputModelFileName;
    string strInputMovingImageFileName;
    string strInputFixedImageFileName;

    string strOutputFittedImageFileName;
    string strOutputModelTransformFileName;
    string strOutputEntireTransformFileName;

    string strInputFixedLandmarksFileName;
    string strInputMovingLandmarksFileName;
    double dLandmarksVariance;

    unsigned uNumberOfDimensions;
    unsigned uNumberOfIterations;
    double dRegularizationWeight;

    bool bPrintFittingInformation;
};

po::options_description initializeProgramOptions(programOptions& poParameters);
bool isOptionsConflictPresent(programOptions& opt);
template<unsigned Dimensions, class RotationAndTranslationTransformType>
void fitImage(programOptions opt, ConsoleOutputSilencer* pCOSilencer);

int main(int argc, char** argv) {
    po::positional_options_description optPositional;
    optPositional.add("output-fit", 1);

    programOptions poParameters;
    po::options_description optAllOptions = initializeProgramOptions(poParameters);

    po::variables_map vm;
    try {
        po::parsed_options parsedOptions = po::command_line_parser(argc, argv).options(optAllOptions).positional(optPositional).run();
        po::store(parsedOptions, vm);
        po::notify(vm);
    } catch (po::error& e) {
        cerr << "An exception occurred while parsing the Command line:"<<endl;
        cerr << e.what() << endl << endl;
        cout << optAllOptions << endl;
        return EXIT_FAILURE;
    }

    if (poParameters.bDisplayHelp == true) {
        cout << optAllOptions << endl;
        return EXIT_SUCCESS;
    }
    if (isOptionsConflictPresent(poParameters) == true)	{
        cerr << "A conflict in the options exists or insufficient options were set." << endl;
        cout << optAllOptions << endl;
        return EXIT_FAILURE;
    }

    ConsoleOutputSilencer coSilencer;
    try {
        if (poParameters.uNumberOfDimensions == Dimensionality2D) {
            typedef itk::Rigid2DTransform<double> RotationAndTranslationTransformType;
            fitImage<Dimensionality2D, RotationAndTranslationTransformType>(poParameters, &coSilencer);
        } else {
            typedef itk::VersorRigid3DTransform<double> RotationAndTranslationTransformType;
            fitImage<Dimensionality3D, RotationAndTranslationTransformType>(poParameters, &coSilencer);
        }
    } catch (ifstream::failure & e) {
        coSilencer.enableOutput();
        cerr << "Could not read a file:" << endl;
        cerr << e.what() << endl;
        return EXIT_FAILURE;
    } catch (itk::ExceptionObject & e) {
        coSilencer.enableOutput();
        cerr << "Could not fit the model:" << endl;
        cerr << e.what() << endl;
        return EXIT_FAILURE;
    }

    return EXIT_SUCCESS;
}

bool isOptionsConflictPresent(programOptions& opt) {
    //if one set of the landmarks-file is provided, then both have to be provided (-> use XOR)
    if (((opt.strInputFixedLandmarksFileName != "") ^ (opt.strInputMovingLandmarksFileName != "")) == true) {
        return true;
    }

    if (opt.strInputFixedImageFileName == "" || opt.strInputModelFileName == "" || opt.strInputMovingImageFileName == "") {
        return true;
    }

    if (opt.uNumberOfDimensions != 2 && opt.uNumberOfDimensions != 3) {
        return true;
    }

    //at least one thing has to be saved/displayed, otherwise running it is pointless
    if (opt.strOutputEntireTransformFileName == "" && opt.strOutputFittedImageFileName == "" && opt.strOutputModelTransformFileName == "") {
        return true;
    }

    return false;
}

template<class ImageType>
void saveImage(typename ImageType::Pointer pImage, const std::string& strOutputFileName) {
    typedef itk::ImageFileWriter<ImageType>  ImageWriter;
    typename ImageWriter::Pointer pImageWriter = ImageWriter::New();
    pImageWriter->SetInput(pImage);
    pImageWriter->SetFileName(strOutputFileName.c_str());
    pImageWriter->Update();
}

template<class DisplacementFieldImageType, class ReferenceImageType, class TransformType>
typename DisplacementFieldImageType::Pointer generateAndSaveDisplacementField(typename ReferenceImageType::Pointer pReferenceImage, typename TransformType::Pointer pTransform, const std::string& strOutputFileName) {
#if (ITK_VERSION_MAJOR >= 4 && ITK_VERSION_MINOR >= 6)
    typedef itk::TransformToDisplacementFieldFilter<DisplacementFieldImageType,	double>  DisplacementFieldGeneratorType;
    typename DisplacementFieldGeneratorType::Pointer pDispfieldGenerator = DisplacementFieldGeneratorType::New();
    pDispfieldGenerator->UseReferenceImageOn();
    pDispfieldGenerator->SetReferenceImage(pReferenceImage);
#else
    typedef typename itk::TransformToDisplacementFieldSource<DisplacementFieldImageType, double> DisplacementFieldGeneratorType;
    typename DisplacementFieldGeneratorType::Pointer pDispfieldGenerator = DisplacementFieldGeneratorType::New();
    pDispfieldGenerator->SetOutputParametersFromImage(pReferenceImage);
#endif

    pDispfieldGenerator->SetTransform(pTransform);
    pDispfieldGenerator->Update();

    typename DisplacementFieldImageType::Pointer pDisplacementField = pDispfieldGenerator->GetOutput();

    if (strOutputFileName != "") {
        saveImage<DisplacementFieldImageType>(pDisplacementField, strOutputFileName);
    }

    return pDisplacementField;
}

template<unsigned Dimensions, class RotationAndTranslationTransformType>
void fitImage(programOptions opt, ConsoleOutputSilencer* pCOSilencer) {
    typedef itk::Image<float, Dimensions> ImageType;
    typedef itk::ImageFileReader<ImageType> ImageReaderType;
    typename ImageReaderType::Pointer pFixedImageReader = ImageReaderType::New();
    pFixedImageReader->SetFileName(opt.strInputFixedImageFileName.c_str());
    pFixedImageReader->Update();
    typename ImageType::Pointer pFixedImage = pFixedImageReader->GetOutput();

    typename ImageReaderType::Pointer pMovingImageReader = ImageReaderType::New();
    pMovingImageReader->SetFileName(opt.strInputMovingImageFileName.c_str());
    pMovingImageReader->Update();
    typename ImageType::Pointer pMovingImage = pMovingImageReader->GetOutput();

    typedef itk::Vector<float, Dimensions> VectorPixelType;
    typedef itk::Image<VectorPixelType, Dimensions> VectorImageType;
    typedef itk::StandardImageRepresenter<VectorPixelType, Dimensions> RepresenterType;
    typename RepresenterType::Pointer pRepresenter = RepresenterType::New();

    typedef itk::StatisticalModel<VectorImageType> StatisticalModelType;
    typename StatisticalModelType::Pointer pModel = StatisticalModelType::New();
    pModel = itk::StatismoIO<VectorImageType>::LoadStatisticalModel(pRepresenter, opt.strInputModelFileName.c_str());

    typedef itk::Transform<double, Dimensions, Dimensions> TransformType;
    typename TransformType::Pointer pTransform;

    typedef itk::InterpolatingStatisticalDeformationModelTransform<VectorImageType, double, Dimensions> ModelTransformType;
    typename ModelTransformType::Pointer pModelTransform = ModelTransformType::New();

    if (opt.strInputMovingLandmarksFileName != "") {
        pModel = buildPosteriorDeformationModel<VectorImageType, StatisticalModelType>(pModel, opt.strInputFixedLandmarksFileName, opt.strInputMovingLandmarksFileName, opt.dLandmarksVariance);
        pTransform = pModelTransform;
    } else {
        //No Landmarks are available: we also have to allow rotation and translation.
        typename RotationAndTranslationTransformType::Pointer pRotationAndTranslationTransform = RotationAndTranslationTransformType::New();
        pRotationAndTranslationTransform->SetIdentity();

        typedef itk::CompositeTransform<double, Dimensions> CompositeTransformType;
        typename CompositeTransformType::Pointer pCompositeTransform = CompositeTransformType::New();
        pCompositeTransform->AddTransform(pRotationAndTranslationTransform);
        pCompositeTransform->AddTransform(pModelTransform);
        pCompositeTransform->SetAllTransformsToOptimizeOn();

        pTransform = pCompositeTransform;
    }

    pModelTransform->SetStatisticalModel(pModel);
    pModelTransform->SetIdentity();

    typedef itk::LBFGSOptimizer OptimizerType;
    OptimizerType::Pointer pOptimizer = OptimizerType::New();
    initializeOptimizer<OptimizerType>(pOptimizer, opt.uNumberOfIterations, pModel->GetNumberOfPrincipalComponents(), pTransform->GetNumberOfParameters(), opt.bPrintFittingInformation, pCOSilencer);

    typedef itk::PenalizingMeanSquaresImageToImageMetric<ImageType, ImageType> MetricType;
    typename MetricType::Pointer pMetric = MetricType::New();
    pMetric->SetRegularizationParameter(opt.dRegularizationWeight);
    pMetric->SetNumberOfModelComponents(pModel->GetNumberOfPrincipalComponents());

    typedef itk::LinearInterpolateImageFunction<ImageType, double> InterpolatorType;
    typename InterpolatorType::Pointer pInterpolator = InterpolatorType::New();

    typedef itk::ImageRegistrationMethod<ImageType, ImageType> RegistrationFilterType;
    typename RegistrationFilterType::Pointer pRegistration = RegistrationFilterType::New();
    pRegistration->SetInitialTransformParameters(pTransform->GetParameters());
    pRegistration->SetMetric(pMetric);
    pRegistration->SetOptimizer(pOptimizer);
    pRegistration->SetTransform(pTransform);
    pRegistration->SetInterpolator(pInterpolator);
    pRegistration->SetFixedImage(pFixedImage);
    pRegistration->SetFixedImageRegion(pFixedImage->GetBufferedRegion());
    pRegistration->SetMovingImage(pMovingImage);

    pCOSilencer->disableOutput();
    pRegistration->Update();
    pCOSilencer->enableOutput();

    typename VectorImageType::Pointer pDisplacementField = generateAndSaveDisplacementField<VectorImageType, ImageType, TransformType>(
                pFixedImage, pTransform, opt.strOutputEntireTransformFileName);
    generateAndSaveDisplacementField<VectorImageType, ImageType, TransformType>(pFixedImage, (typename TransformType::Pointer) pModelTransform, opt.strOutputModelTransformFileName);

    if (opt.strOutputFittedImageFileName != "") {
        typedef itk::WarpImageFilter<ImageType, ImageType, VectorImageType> WarpFilterType;
        typename WarpFilterType::Pointer pWarper = WarpFilterType::New();
        pWarper->SetInput(pMovingImage);
        pWarper->SetInterpolator(pInterpolator);
        pWarper->SetOutputSpacing(pFixedImage->GetSpacing());
        pWarper->SetOutputOrigin(pFixedImage->GetOrigin());
        pWarper->SetOutputDirection(pFixedImage->GetDirection());
        pWarper->SetDisplacementField(pDisplacementField);
        pWarper->Update();

        saveImage<ImageType>(pWarper->GetOutput(), opt.strOutputFittedImageFileName);
    }
}

po::options_description initializeProgramOptions(programOptions& poParameters) {
    po::options_description optMandatory("Mandatory options");
    optMandatory.add_options()

    ("input-model,i", po::value<string>(&poParameters.strInputModelFileName), "The path to the model file.")
    ("moving-image,m", po::value<string>(&poParameters.strInputMovingImageFileName), "The path to the moving image.")
    ("fixed-image,f", po::value<string>(&poParameters.strInputFixedImageFileName), "The path to the fixed image.")
    ("dimensionality,d", po::value<unsigned>(&poParameters.uNumberOfDimensions)->default_value(3), "Dimensionality of the input images & model")
    ("number-of-iterations,n", po::value<unsigned>(&poParameters.uNumberOfIterations)->default_value(100), "Number of iterations")
    ("regularization-weight,w", po::value<double>(&poParameters.dRegularizationWeight), "This is the regularization weight to make sure the model parameters don't don't get too big while fitting.")
    ;

    po::options_description optOutput("Mandatory Output options (set at least one)");
    optOutput.add_options()
    ("output-fit,o", po::value<string>(&poParameters.strOutputFittedImageFileName), "Name of the output file where the fitted image will be written to.")
    ("output-model-deformationfield,a", po::value<string>(&poParameters.strOutputModelTransformFileName), "Name of the output file where the model deformation field will be written to.")
    ("output-deformationfield,e", po::value<string>(&poParameters.strOutputEntireTransformFileName), "Name of the output file where the entire deformation field will be written to. This includes the rotation and translation (Only use this when NOT using landmarks).")
    ;

    po::options_description optLandmarks("Landmarks (optional - if you set one you have to set all)");
    optLandmarks.add_options()
    ("fixed-landmarks", po::value<string>(&poParameters.strInputFixedLandmarksFileName), "Name of the file where the fixed Landmarks are saved.")
    ("moving-landmarks", po::value<string>(&poParameters.strInputMovingLandmarksFileName), "Name of the file where the moving Landmarks are saved.")
    ("landmarks-variance,v", po::value<double>(&poParameters.dLandmarksVariance)->default_value(1), "The variance that will be used to build the posterior model.")
    ;

    po::options_description optAdditional("Optional options");
    optAdditional.add_options()
    ("print-fitting-information,p", po::bool_switch(&poParameters.bPrintFittingInformation), "Prints information (the parameters, metric score and the iteration count) with each iteration while fitting.")
    ("help,h", po::bool_switch(&poParameters.bDisplayHelp), "Display this help message")
    ;

    po::options_description optAllOptions;
    optAllOptions.add(optMandatory).add(optOutput).add(optLandmarks).add(optAdditional);
    return optAllOptions;
}


#define BOOST_TEST_MODULE TaggedTerminal
#include <boost/test/unit_test.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/multivector.hpp>
#include <vexcl/element_index.hpp>
#include <vexcl/tagged_terminal.hpp>
#include <vexcl/temporary.hpp>
#include <vexcl/reductor.hpp>
#include <vexcl/vector_view.hpp>
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(tagged_terminal)
{
    using vex::tag;

    const size_t n = 1024;

    std::vector<double> x = random_vector<double>(n);

    vex::vector<double> X(ctx, x);
    vex::vector<double> Y = tag<1>(X) * tag<1>(X);

    check_sample(Y, [&](size_t idx, double v) {
            BOOST_CHECK_CLOSE(v, x[idx] * x[idx], 1e-8); });

    vex::Reductor<double, vex::SUM> sum(ctx);

    BOOST_CHECK_CLOSE(
            sum(tag<3>(2) * tag<1>(X) * tag<1>(X) + tag<3>(2) * tag<2>(Y) * tag<2>(Y)),
            std::accumulate(x.begin(), x.end(), 0.0, [](double sum, double v) -> double {
                double y = v * v;
                return sum + 2 * (y + y * y);
                }),
            1e-6
            );

    tag<1>(X) = 1;
    check_sample(X, [&](size_t, double v) { BOOST_CHECK_CLOSE(v, 1, 1e-8); });

    tag<1>(X) = tag<1>(X) + vex::element_index();
    check_sample(X, [&](size_t idx, double v) { BOOST_CHECK_CLOSE(v, 1.0 + idx, 1e-8); });
}

BOOST_AUTO_TEST_CASE(tagged_slice)
{
    using vex::tag;
    using vex::range;

    const size_t n = 1024;

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    std::vector<double> x = random_vector<double>(n);

    vex::vector<double> X(queue, x);

    vex::slicer<1> slice(&n);

    vex::vector<double> Y = tag<1>(41) * tag<2>(X) + tag<3>(slice[range()](X));

    check_sample(Y, [&](size_t idx, double v) {
            BOOST_CHECK_CLOSE(v, 42 * x[idx], 1e-8); });
}

BOOST_AUTO_TEST_CASE(temporary_inside_tag)
{
    using vex::tag;

    const size_t n = 1024;

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    std::vector<double> x = random_vector<double>(n);

    vex::vector<double> X(queue, x);

    auto i = vex::make_temp<1>(vex::element_index() + 1);
    auto reverse = vex::permutation(n - i);
    vex::vector<double> Y(queue, n);

    Y = vex::tag<1>(reverse(X));

    check_sample(Y, [&](size_t idx, double v) {
            BOOST_CHECK_EQUAL(v, x[n - (idx + 1)]); });
}

BOOST_AUTO_TEST_CASE(tied_tagged_terminals)
{
    using vex::tag;

    const size_t n = 1024;

    vex::multivector<double, 2> X(ctx, random_vector<double>(2 * n));
    vex::multivector<double, 2> Y(ctx, n);

#define X0 tag<0>(X(0))
#define X1 tag<1>(X(1))

    Y = std::tie(X1, X0);

    check_sample(Y(1), X(0), [&](size_t, double y, double x) {
            BOOST_CHECK_EQUAL(y, x); });

    check_sample(Y(0), X(1), [&](size_t, double y, double x) {
            BOOST_CHECK_EQUAL(y, x); });
}

BOOST_AUTO_TEST_SUITE_END()