#include "utils/stl_to_string.hpp"
#include "trajopt/kinematic_constraints.hpp"
#include "trajopt/utils.hpp"
#include "trajopt/rave_utils.hpp"
#include "utils/logging.hpp"
#include "sco/expr_ops.hpp"
#include "sco/modeling_utils.hpp"
#include <boost/format.hpp>
#include <boost/bind.hpp>
#include <iostream>
#include <Eigen/Geometry>
#include <openrave/openrave.h>
#include "utils/eigen_conversions.hpp"
#include "utils/eigen_slicing.hpp"
using namespace std;
using namespace sco;
using namespace Eigen;
using namespace util;

namespace {

static MatrixXd diffAxis0(const MatrixXd& in) {
  return in.middleRows(1, in.rows()-1) - in.middleRows(0, in.rows()-1);
}
Vector3d rotVec(const Matrix3d& m) {
  Quaterniond q; q = m;
  return Vector3d(q.x(), q.y(), q.z());
}
Vector3d rotVec(const OpenRAVE::Vector& q) {
  return Vector3d(q[1], q[2], q[3]);
}

VectorXd concat(const VectorXd& a, const VectorXd& b) {
  VectorXd out(a.size()+b.size());
  out.topRows(a.size()) = a;
  out.middleRows(a.size(), b.size()) = b;
  return out;
}

template <typename T>
vector<T> concat(const vector<T>& a, const vector<T>& b) {
  vector<T> out;
  vector<int> x;
  out.insert(out.end(), a.begin(), a.end());
  out.insert(out.end(), b.begin(), b.end());
  return out;
}

}

namespace trajopt {

//////////////////////

void makeTrajVariablesAndBounds(int n_steps, RobotAndDOFPtr manip, OptProb& prob_out, VarArray& vars_out) {
  int n_dof = manip->GetDOF();
  DblVec lower, upper;
  manip->GetDOFLimits(lower, upper);
  LOG_INFO("Dof limits: %s, %s", CSTR(lower), CSTR(upper));
  vector<double> vlower, vupper;
  vector<string> names;
  for (int i=0; i < n_steps; ++i) {
    vlower.insert(vlower.end(), lower.data(), lower.data()+lower.size());
    vupper.insert(vupper.end(), upper.data(), upper.data()+upper.size());
    for (unsigned j=0; j < n_dof; ++j) {
      names.push_back( (boost::format("j_%i_%i")%i%j).str() );
    }
  }

  prob_out.createVariables(names, vlower, vupper);
  vars_out = VarArray(n_steps, n_dof, prob_out.getVars().data());

}

JointPosCost::JointPosCost(const VarVector& vars, const VectorXd& vals, const VectorXd& coeffs) :
    Cost("JointVel"), vars_(vars), vals_(vals), coeffs_(coeffs) {
    for (int i=0; i < vars.size(); ++i) {
      if (coeffs[i] > 0) {
        AffExpr diff = exprSub(AffExpr(vars[i]), AffExpr(vals[i]));
        exprInc(expr_, exprMult(exprSquare(diff), coeffs[i]));
      }
    }
}
double JointPosCost::value(const vector<double>& xvec) {
  VectorXd dofs = getVec(xvec, vars_);
  return ((dofs - vals_).array().square() * coeffs_.array()).sum();
}
ConvexObjectivePtr JointPosCost::convex(const vector<double>& x, Model* model) {
  ConvexObjectivePtr out(new ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

JointVelCost::JointVelCost(const VarArray& vars, const VectorXd& coeffs) :
    Cost("JointVel"), vars_(vars), coeffs_(coeffs) {
  for (int i=0; i < vars.rows()-1; ++i) {
    for (int j=0; j < vars.cols(); ++j) {
      AffExpr vel;
      exprInc(vel, exprMult(vars(i,j), -1));
      exprInc(vel, exprMult(vars(i+1,j), 1));
      exprInc(expr_, exprMult(exprSquare(vel),coeffs_[j]));
    }
  }
}
double JointVelCost::value(const vector<double>& xvec) {
  MatrixXd traj = getTraj(xvec, vars_);
  return (diffAxis0(traj).array().square().matrix() * coeffs_.asDiagonal()).sum();
}
ConvexObjectivePtr JointVelCost::convex(const vector<double>& x, Model* model) {
  ConvexObjectivePtr out(new ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

JointAccCost::JointAccCost(const VarArray& vars, const VectorXd& coeffs) :
    Cost("JointAcc"), vars_(vars), coeffs_(coeffs) {
  for (int i=0; i < vars.rows()-2; ++i) {
    for (int j=0; j < vars.cols(); ++j) {
      AffExpr acc;
      exprInc(acc, exprMult(vars(i,j), -1));
      exprInc(acc, exprMult(vars(i+1,j), 2));
      exprInc(acc, exprMult(vars(i+2,j), -1));
      exprInc(expr_, exprMult(exprSquare(acc), coeffs_[j]));
    }
  }
}
double JointAccCost::value(const vector<double>& xvec) {
  MatrixXd traj = getTraj(xvec, vars_);
  return (diffAxis0(diffAxis0(traj)).array().square().matrix() * coeffs_.asDiagonal()).sum();
}
ConvexObjectivePtr JointAccCost::convex(const vector<double>& x, Model* model) {
  ConvexObjectivePtr out(new ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

struct CartPoseErrCalculator : public VectorOfVector {
  OR::Transform pose_inv_;
  RobotAndDOFPtr manip_;
  OR::KinBody::LinkPtr link_;
  VectorXd coeffs_;
  CartPoseErrCalculator(const OR::Transform& pose, RobotAndDOFPtr manip, OR::KinBody::LinkPtr link, const VectorXd& coeffs) :
  pose_inv_(pose.inverse()),
  manip_(manip),
  link_(link),
  coeffs_(coeffs)
  {}
//  CartPoseCostCalculator(const CartPoseCostCalculator& other) : pose_(other.pose_), manip_(other.manip_), rs_(other.rs_) {}
  VectorXd operator()(const VectorXd& dof_vals) const {
    manip_->SetDOFValues(toDblVec(dof_vals));
    OR::Transform newpose = link_->GetTransform();

    OR::Transform pose_err = pose_inv_ * newpose;
    VectorXd err = coeffs_.cwiseProduct(concat(rotVec(pose_err.rot), toVector3d(pose_err.trans)));
    return err;
  }
};

CartPoseCost::CartPoseCost(const VarVector& vars, const OR::Transform& pose, const Vector3d& rot_coeffs,
    const Vector3d& pos_coeffs, RobotAndDOFPtr manip, KinBody::LinkPtr link) :
    CostFromErrFunc(VectorOfVectorPtr(new CartPoseErrCalculator(pose, manip, link, VectorXd::Ones(6))),
        vars, concat(rot_coeffs, pos_coeffs), ABS, "CartPose")
{}
void CartPoseCost::Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  CartPoseErrCalculator* calc = static_cast<CartPoseErrCalculator*>(f_.get());
  DblVec dof_vals = getDblVec(x, vars_);
  calc->manip_->SetDOFValues(dof_vals);
  OR::Transform target = calc->pose_inv_.inverse(), cur = calc->link_->GetTransform();
  PlotAxes(env, cur, .1,  handles);
  PlotAxes(env, target, .1,  handles);
  handles.push_back(env.drawarrow(cur.trans, target.trans, .01, OR::Vector(1,0,1,1)));
}

CartPoseConstraint::CartPoseConstraint(const VarVector& vars, const OR::Transform& pose,
    RobotAndDOFPtr manip, KinBody::LinkPtr link, const VectorXd& coeffs) :
    ConstraintFromFunc(VectorOfVectorPtr(new CartPoseErrCalculator(pose, manip, link, coeffs)),
        vars, EQ, "CartPose")
{
}

void CartPoseConstraint::Plot(const DblVec& x, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
  // IDENTITCAL TO CartPoseCost::Plot
  CartPoseErrCalculator* calc = static_cast<CartPoseErrCalculator*>(f_.get());
  DblVec dof_vals = getDblVec(x, vars_);
  calc->manip_->SetDOFValues(dof_vals);
  OR::Transform target = calc->pose_inv_.inverse(), cur = calc->link_->GetTransform();
  PlotAxes(env, cur, .1,  handles);
  PlotAxes(env, target, .1,  handles);
  handles.push_back(env.drawarrow(cur.trans, target.trans, .01, OR::Vector(1,0,1,1)));
}

struct CartPositionErrCalculator {
  Vector3d pt_world_;
  RobotAndDOFPtr manip_;
  OR::KinBody::LinkPtr link_;
  CartPositionErrCalculator(const Vector3d& pt_world, RobotAndDOFPtr manip, OR::KinBody::LinkPtr link) :
  pt_world_(pt_world),
  manip_(manip),
  link_(link)
  {}
  VectorXd operator()(const VectorXd& dof_vals) {
    manip_->SetDOFValues(toDblVec(dof_vals));
    OR::Transform newpose = link_->GetTransform();
    return pt_world_ - toVector3d(newpose.trans);
  }
};

struct CartVelJacCalculator : MatrixOfVector {
  RobotAndDOFPtr manip_;
  KinBody::LinkPtr link_;
  double limit_;
  CartVelJacCalculator(RobotAndDOFPtr manip, KinBody::LinkPtr link, double limit) :
    manip_(manip), link_(link), limit_(limit) {}

  MatrixXd operator()(const VectorXd& dof_vals) const {
    int n_dof = manip_->GetDOF();
    MatrixXd out(6, 2*n_dof);
    manip_->SetDOFValues(toDblVec(dof_vals.topRows(n_dof)));
    OR::Transform pose0 = link_->GetTransform();
    MatrixXd jac0 = manip_->PositionJacobian(link_->GetIndex(), pose0.trans);
    manip_->SetDOFValues(toDblVec(dof_vals.bottomRows(n_dof)));
    OR::Transform pose1 = link_->GetTransform();
    MatrixXd jac1 = manip_->PositionJacobian(link_->GetIndex(), pose1.trans);
    out.block(0,0,3,n_dof) = -jac0;
    out.block(0,n_dof,3,n_dof) = jac1;
    out.block(3,0,3,n_dof) = jac0;
    out.block(3,n_dof,3,n_dof) = -jac1;
    return out;
  }
};

struct CartVelCalculator : VectorOfVector {
  RobotAndDOFPtr manip_;
  KinBody::LinkPtr link_;
  double limit_;
  CartVelCalculator(RobotAndDOFPtr manip, KinBody::LinkPtr link, double limit) :
    manip_(manip), link_(link), limit_(limit) {}

  VectorXd operator()(const VectorXd& dof_vals) const {
    int n_dof = manip_->GetDOF();
    manip_->SetDOFValues(toDblVec(dof_vals.topRows(n_dof)));
    OR::Transform pose0 = link_->GetTransform();
    manip_->SetDOFValues(toDblVec(dof_vals.bottomRows(n_dof)));
    OR::Transform pose1 = link_->GetTransform();
    VectorXd out(6);
    out.topRows(3) = toVector3d(pose1.trans - pose0.trans - OR::Vector(limit_,limit_,limit_));
    out.bottomRows(3) = toVector3d( - pose1.trans + pose0.trans - OR::Vector(limit_, limit_, limit_));
    return out;
  }
};

CartVelConstraint::CartVelConstraint(const VarVector& step0vars, const VarVector& step1vars, RobotAndDOFPtr manip, KinBody::LinkPtr link, double distlimit) :
        ConstraintFromFunc(VectorOfVectorPtr(new CartVelCalculator(manip, link, distlimit)),
                           MatrixOfVectorPtr(new CartVelJacCalculator(manip, link, distlimit)),
                          concat(step0vars, step1vars), INEQ, "CartVel")
{}

struct UpErrorCalculator {
  Vector3d dir_local_;
  Vector3d goal_dir_world_;
  RobotAndDOFPtr manip_;
  OR::KinBody::LinkPtr link_;
  MatrixXd perp_basis_; // 2x3 matrix perpendicular to goal_dir_world
  UpErrorCalculator(const Vector3d& dir_local, const Vector3d& goal_dir_world, RobotAndDOFPtr manip, KinBody::LinkPtr link) :
    dir_local_(dir_local),
    goal_dir_world_(goal_dir_world),
    manip_(manip),
    link_(link)
  {
    Vector3d perp0 = goal_dir_world_.cross(Vector3d::Random()).normalized();
    Vector3d perp1 = goal_dir_world_.cross(perp0);
    perp_basis_.resize(2,3);
    perp_basis_.row(0) = perp0.transpose();
    perp_basis_.row(1) = perp1.transpose();
  }
  VectorXd operator()(const VectorXd& dof_vals) {
    manip_->SetDOFValues(toDblVec(dof_vals));
    OR::Transform newpose = link_->GetTransform();
    return perp_basis_*(toRot(newpose.rot) * dir_local_ - goal_dir_world_);
  }
};
}


// Copyright (c) by respective owners including Yahoo!, Microsoft, and
// individual contributors. All rights reserved. Released under a BSD (revised)
// license as described in the file LICENSE.

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

#include "cats.h"
#include "memory.h"
#include "vw.h"
#include "test_common.h"

using namespace VW::continuous_action::cats;

BOOST_AUTO_TEST_CASE(cats_test_get_loss_zero_for_bad_prediction)
{
  auto data = VW::make_unique<cats>(nullptr);
  data->min_value = 0;
  data->max_value = 32;
  data->num_actions = 8;
  data->bandwidth = 3;

  VW::cb_continuous::continuous_label cont_label;
  float action = 10.0f;
  float cost = 1.0f;
  float pdf_value = 0.166666672f;
  cont_label.costs.push_back({action, cost, pdf_value});

  // predicted action not near label action, plus bandwidth too small to provide overlap after smoothing
  float predicted_action = 32.0f;
  auto loss = data->get_loss(cont_label, predicted_action);

  BOOST_CHECK_EQUAL(loss, 0.0f);
}

BOOST_AUTO_TEST_CASE(cats_test_get_loss_not_zero_for_bad_prediction_and_large_b)
{
  auto data = VW::make_unique<cats>(nullptr);
  data->min_value = 0;
  data->max_value = 32;
  data->num_actions = 8;
  data->bandwidth = 30;

  VW::cb_continuous::continuous_label cont_label;
  float action = 10.0f;
  float cost = 1.0f;
  float pdf_value = 0.166666672f;
  cont_label.costs.push_back({action, cost, pdf_value});

  // predicted action not near label action, but the large bandwidth provides overlap with the logged action
  float predicted_action = 32.0f;
  // centre of predicted action will be 30.0f
  // label is 10.0f and bandiwdth is 30.0f
  // so if: action - bandwidth <= centre <= action + bandwidth then we calculate the loss
  // bandwidth_range = min(max_value, centre + bandwidth) - max(min_value, centre - bandwidth)
  // bandiwdth_range = min(32, 33) - max(0, 0) <=> bandwidth_range = 32 - 0 <=> bandwidth_range = 32
  // loss = cost / (pdf_value * bandwidth_range) <=> loss = 1.0f / (0.166666672f * 32) <=> loss = 0.1875
  auto loss = data->get_loss(cont_label, predicted_action);

  BOOST_CHECK_CLOSE(loss, 0.1875, FLOAT_TOL);
}

BOOST_AUTO_TEST_CASE(cats_test_get_loss_for_good_prediction_and_small_b_not_close_to_range_edges)
{
  auto data = VW::make_unique<cats>(nullptr);
  data->min_value = 1;
  data->max_value = 34;
  data->num_actions = 4;
  data->bandwidth = 4;

  // continous_range = 34 - 1 = 33
  // unit_range = continuous_range / num_action = 33 / 4 = 8.25

  VW::cb_continuous::continuous_label cont_label;
  float action = 17.0f;
  float cost = 1.0f;
  float pdf_value = 0.125f;
  cont_label.costs.push_back({action, cost, pdf_value});

  // predicted action not near label action, but the large bandwidth provides overlap with the logged action
  float predicted_action = 15.1f;
  // centre of predicted action will be 13.3f
  // label/action is 17.0f and bandiwdth is 2.0f
  // so if: action - bandwidth <= centre <= action + bandwidth then we calculate the loss (13 <= 13.3 <= 21)
  // bandwidth_range = min(max_value, centre + bandwidth) - max(min_value, centre - bandwidth)
  // bandiwdth_range = min(34, 17) - max(1, 9) <=> bandwidth_range = 17 - 9 <=> bandwidth_range = 8
  // loss = cost / (pdf_value * bandwidth_range) <=> loss = 1.0f / (0.125f * 8) <=> loss = 1
  auto loss = data->get_loss(cont_label, predicted_action);

  BOOST_CHECK_EQUAL(loss, 1.0f);
}

BOOST_AUTO_TEST_CASE(cats_test_get_loss_for_good_prediction_and_small_b_close_to_range_edges)
{
  auto data = VW::make_unique<cats>(nullptr);
  data->min_value = 1;
  data->max_value = 34;
  data->num_actions = 4;
  data->bandwidth = 4;

  // continous_range = 34 - 1 = 33
  // unit_range = continuous_range / num_action = 33 / 4 = 8.25

  VW::cb_continuous::continuous_label cont_label;
  float action = 33.0f;
  float cost = 1.0f;
  float pdf_value = 0.125f;
  cont_label.costs.push_back({action, cost, pdf_value});

  // predicted action not near label action, but the large bandwidth provides overlap with the logged action
  float predicted_action = 26.1f;
  // centre of predicted action will be 29.875
  // label/action is 33.0f and bandiwdth is 4.0f
  // so if: action - bandwidth <= centre <= action + bandwidth then we calculate the loss (29 <= 29.875 <= 37)
  // bandwidth_range = min(max_value, centre + bandwidth) - max(min_value, centre - bandwidth)
  // bandiwdth_range = min(34, 33.875) - max(1, 25.875) <=> bandwidth_range = 33.875 - 25.875 <=> bandwidth_range = 8 (2
  // * bandwidth) loss = cost / (pdf_value * bandwidth_range) <=> loss = 1.0f / (0.125f * 8) <=> loss = 1
  auto loss = data->get_loss(cont_label, predicted_action);

  BOOST_CHECK_CLOSE(loss, 1.0f, FLOAT_TOL);
}

BOOST_AUTO_TEST_CASE(cats_test_get_loss_with_default_bandwidth)
{
  auto data = VW::make_unique<cats>(nullptr);
  data->min_value = 0;
  data->max_value = 32;
  data->num_actions = 8;

  float leaf_width = (data->max_value - data->min_value) / (data->num_actions);  // aka unit range
  float half_leaf_width = leaf_width / 2.f;
  data->bandwidth = half_leaf_width;

  // continous_range = 32 - 0 = 32
  // unit_range = continuous_range / num_action = 32 / 8 = 4

  VW::cb_continuous::continuous_label cont_label;
  float action = 32.0f;
  float cost = 1.0f;
  float pdf_value = 0.25f;
  cont_label.costs.push_back({action, cost, pdf_value});

  // predicted action in corner leaf
  float predicted_action = 32.0f;
  // centre of predicted action will be 30.0
  // label/action is 32.0f and bandiwdth is 2.0f
  // so if: action - bandwidth <= centre <= action + bandwidth then we calculate the loss (30 <= 30 <= 34)
  // bandwidth_range = min(max_value, centre + bandwidth) - max(min_value, centre - bandwidth)
  // bandiwdth_range = min(32, 32) - max(0, 28) <=> bandwidth_range = 32 - 28 <=> bandwidth_range = 4 (i.e. 2 *
  // bandwidth) loss = cost / (pdf_value * bandwidth_range) <=> loss = 1.0f / (0.25f * 4) <=> loss = 1
  auto loss = data->get_loss(cont_label, predicted_action);

  BOOST_CHECK_EQUAL(loss, 1.0f);
}


#include "outer_hull.h"
#include "order_facets_around_edges.h"
#include "../outer_facet.h"
#include "../sortrows.h"
#include "../facet_components.h"
#include "../winding_number.h"
#include "../triangle_triangle_adjacency.h"
#include "../unique_edge_map.h"
#include "../barycenter.h"
#include "../per_face_normals.h"
#include "../writePLY.h"
#include "../sort_angles.h"

#include <Eigen/Geometry>
#include <vector>
#include <map>
#include <queue>
#include <iostream>
#include <CGAL/number_utils.h>
//#define IGL_OUTER_HULL_DEBUG

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedN,
  typename DerivedG,
  typename DerivedJ,
  typename Derivedflip>
IGL_INLINE void igl::outer_hull(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  const Eigen::PlainObjectBase<DerivedN> & N,
  Eigen::PlainObjectBase<DerivedG> & G,
  Eigen::PlainObjectBase<DerivedJ> & J,
  Eigen::PlainObjectBase<Derivedflip> & flip)
{
#ifdef IGL_OUTER_HULL_DEBUG
  std::cerr << "Extracting outer hull" << std::endl;
#endif
  using namespace Eigen;
  using namespace std;
  typedef typename DerivedF::Index Index;
  Matrix<Index,DerivedF::RowsAtCompileTime,1> C;
  typedef Matrix<typename DerivedV::Scalar,Dynamic,DerivedV::ColsAtCompileTime> MatrixXV;
  typedef Matrix<typename DerivedF::Scalar,Dynamic,DerivedF::ColsAtCompileTime> MatrixXF;
  typedef Matrix<typename DerivedG::Scalar,Dynamic,DerivedG::ColsAtCompileTime> MatrixXG;
  typedef Matrix<typename DerivedJ::Scalar,Dynamic,DerivedJ::ColsAtCompileTime> MatrixXJ;
  typedef Matrix<typename DerivedN::Scalar,1,3> RowVector3N;
  const Index m = F.rows();

  // UNUSED:
  //const auto & duplicate_simplex = [&F](const int f, const int g)->bool
  //{
  //  return
  //    (F(f,0) == F(g,0) && F(f,1) == F(g,1) && F(f,2) == F(g,2)) ||
  //    (F(f,1) == F(g,0) && F(f,2) == F(g,1) && F(f,0) == F(g,2)) ||
  //    (F(f,2) == F(g,0) && F(f,0) == F(g,1) && F(f,1) == F(g,2)) ||
  //    (F(f,0) == F(g,2) && F(f,1) == F(g,1) && F(f,2) == F(g,0)) ||
  //    (F(f,1) == F(g,2) && F(f,2) == F(g,1) && F(f,0) == F(g,0)) ||
  //    (F(f,2) == F(g,2) && F(f,0) == F(g,1) && F(f,1) == F(g,0));
  //};

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer hull..."<<endl;
#endif

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"edge map..."<<endl;
#endif
  typedef Matrix<typename DerivedF::Scalar,Dynamic,2> MatrixX2I;
  typedef Matrix<typename DerivedF::Index,Dynamic,1> VectorXI;
  typedef Matrix<typename DerivedV::Scalar, 3, 1> Vector3F;
  MatrixX2I E,uE;
  VectorXI EMAP;
  vector<vector<typename DerivedF::Index> > uE2E;
  unique_edge_map(F,E,uE,EMAP,uE2E);
#ifdef IGL_OUTER_HULL_DEBUG
  for (size_t ui=0; ui<uE.rows(); ui++) {
      std::cout << ui << ": " << uE2E[ui].size() << " -- (";
      for (size_t i=0; i<uE2E[ui].size(); i++) {
          std::cout << uE2E[ui][i] << ", ";
      }
      std::cout << ")" << std::endl;
  }
#endif

  std::vector<std::vector<typename DerivedF::Index> > uE2oE;
  std::vector<std::vector<bool> > uE2C;
  order_facets_around_edges(V, F, N, E, uE, EMAP, uE2E, uE2oE, uE2C);
  uE2E = uE2oE;
  VectorXI diIM(3*m);
  for (auto ue : uE2E) {
      for (size_t i=0; i<ue.size(); i++) {
          auto fe = ue[i];
          diIM[fe] = i;
      }
  }

  vector<vector<vector<Index > > > TT,_1;
  triangle_triangle_adjacency(E,EMAP,uE2E,false,TT,_1);
  VectorXI counts;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"facet components..."<<endl;
#endif
  facet_components(TT,C,counts);
  assert(C.maxCoeff()+1 == counts.rows());
  const size_t ncc = counts.rows();
  G.resize(0,F.cols());
  J.resize(0,1);
  flip.setConstant(m,1,false);

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"reindex..."<<endl;
#endif
  // H contains list of faces on outer hull;
  vector<bool> FH(m,false);
  vector<bool> EH(3*m,false);
  vector<MatrixXG> vG(ncc);
  vector<MatrixXJ> vJ(ncc);
  vector<MatrixXJ> vIM(ncc);
  //size_t face_count = 0;
  for(size_t id = 0;id<ncc;id++)
  {
    vIM[id].resize(counts[id],1);
  }
  // current index into each IM
  vector<size_t> g(ncc,0);
  // place order of each face in its respective component
  for(Index f = 0;f<m;f++)
  {
    vIM[C(f)](g[C(f)]++) = f;
  }

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"barycenters..."<<endl;
#endif
  // assumes that "resolve" has handled any coplanar cases correctly and nearly
  // coplanar cases can be sorted based on barycenter.
  MatrixXV BC;
  barycenter(V,F,BC);

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"loop over CCs (="<<ncc<<")..."<<endl;
#endif
  for(Index id = 0;id<(Index)ncc;id++)
  {
    auto & IM = vIM[id];
    // starting face that's guaranteed to be on the outer hull and in this
    // component
    int f;
    bool f_flip;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer facet..."<<endl;
#endif
    outer_facet(V,F,N,IM,f,f_flip);
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer facet: "<<f<<endl;
  cout << V.row(F(f, 0)) << std::endl;
  cout << V.row(F(f, 1)) << std::endl;
  cout << V.row(F(f, 2)) << std::endl;
#endif
    int FHcount = 1;
    FH[f] = true;
    // Q contains list of face edges to continue traversing upong
    queue<int> Q;
    Q.push(f+0*m);
    Q.push(f+1*m);
    Q.push(f+2*m);
    flip(f) = f_flip;
    //std::cout << "face " << face_count++ << ": " << f << std::endl;
    //std::cout << "f " << F.row(f).array()+1 << std::endl;
    //cout<<"flip("<<f<<") = "<<(flip(f)?"true":"false")<<endl;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"BFS..."<<endl;
#endif
    while(!Q.empty())
    {
      // face-edge
      const int e = Q.front();
      Q.pop();
      // face
      const int f = e%m;
      // corner
      const int c = e/m;
#ifdef IGL_OUTER_HULL_DEBUG
      std::cout << "edge: " << e << ", ue: " << EMAP(e) << std::endl;
      std::cout << "face: " << f << std::endl;
      std::cout << "corner: " << c << std::endl;
      std::cout << "consistent: " << uE2C[EMAP(e)][diIM[e]] << std::endl;
#endif
      // Should never see edge again...
      if(EH[e] == true)
      {
        continue;
      }
      EH[e] = true;
      // source of edge according to f
      const int fs = flip(f)?F(f,(c+2)%3):F(f,(c+1)%3);
      // destination of edge according to f
      const int fd = flip(f)?F(f,(c+1)%3):F(f,(c+2)%3);
      // edge valence
      const size_t val = uE2E[EMAP(e)].size();
#ifdef IGL_OUTER_HULL_DEBUG
      std::cout << "vd: " << V.row(fd) << std::endl;
      std::cout << "vs: " << V.row(fs) << std::endl;
      std::cout << "edge: " << V.row(fd) - V.row(fs) << std::endl;
      for (size_t i=0; i<val; i++) {
          if (i == diIM(e)) {
              std::cout << "* ";
          } else {
              std::cout << "  ";
          }
          std::cout << i << ": "
              << " (e: " << uE2E[EMAP(e)][i] << ", f: "
              << uE2E[EMAP(e)][i] % m * (uE2C[EMAP(e)][i] ? 1:-1) << ")" << std::endl;
      }
#endif
      //// find overlapping face-edges
      //const auto & neighbors = uE2E[EMAP(e)];
      //// normal after possible flipping
      //const auto & fN = (flip(f)?-1.:1.)*N.row(f);
      //// Edge vector according to f's (flipped) orientation.
      ////const auto & eV = (V.row(fd)-V.row(fs)).normalized();

//#warning "EXPERIMENTAL, DO NOT USE"
      //// THIS IS WRONG! The first face is---after sorting---no longer the face
      //// used for orienting the sort.
      //const auto ui = EMAP(e);
      //const auto fe0 = uE2E[ui][0];
      //const auto es = F(fe0%m,((fe0/m)+1)%3);

      // is edge consistent with edge of face used for sorting
      const int e_cons = (uE2C[EMAP(e)][diIM(e)] ? 1: -1);
      int nfei = -1;
      // Loop once around trying to find suitable next face
      for(size_t step = 1; step<val+2;step++)
      {
        const int nfei_new = (diIM(e) + 2*val + e_cons*step*(flip(f)?-1:1))%val;
        const int nf = uE2E[EMAP(e)][nfei_new] % m;
        // Don't consider faces with identical dihedral angles
        //if ((di[EMAP(e)][diIM(e)].array() != di[EMAP(e)][nfei_new].array()).any())
        //if((di[EMAP(e)][diIM(e)] != di[EMAP(e)][nfei_new]))
//#warning "THIS IS HACK, FIX ME"
//        if( abs(di[EMAP(e)][diIM(e)] - di[EMAP(e)][nfei_new]) < 1e-15 )
        {
#ifdef IGL_OUTER_HULL_DEBUG
        //cout<<"Next facet: "<<(f+1)<<" --> "<<(nf+1)<<", |"<<
        //  di[EMAP(e)][diIM(e)]<<" - "<<di[EMAP(e)][nfei_new]<<"| = "<<
        //    abs(di[EMAP(e)][diIM(e)] - di[EMAP(e)][nfei_new])
        //    <<endl;
#endif

          // Only use this face if not already seen
          if(!FH[nf])
          {
            nfei = nfei_new;
          //} else {
          //    std::cout << "skipping face " << nfei_new << " because it is seen before"
          //        << std::endl;
          }
          break;
        //} else {
        //    std::cout << di[EMAP(e)][diIM(e)].transpose() << std::endl;
        //    std::cout << di[EMAP(e)][diIM(nfei_new)].transpose() << std::endl;
        //    std::cout << "skipping face " << nfei_new << " with identical dihedral angle"
        //        << std::endl;
        }
//#ifdef IGL_OUTER_HULL_DEBUG
//        cout<<"Skipping co-planar facet: "<<(f+1)<<" --> "<<(nf+1)<<endl;
//#endif
      }

      int max_ne = -1;
      //// Loop over and find max dihedral angle
      //typename DerivedV::Scalar max_di = -1;
      //for(const auto & ne : neighbors)
      //{
      //  const int nf = ne%m;
      //  if(nf == f)
      //  {
      //    continue;
      //  }
      //  // Corner of neighbor
      //  const int nc = ne/m;
      //  // Is neighbor oriented consistently with (flipped) f?
      //  //const int ns = F(nf,(nc+1)%3);
      //  const int nd = F(nf,(nc+2)%3);
      //  const bool cons = (flip(f)?fd:fs) == nd;
      //  // Normal after possibly flipping to match flip or orientation of f
      //  const auto & nN = (cons? (flip(f)?-1:1.) : (flip(f)?1.:-1.) )*N.row(nf);
      //  // Angle between n and f
      //  const auto & ndi = M_PI - atan2( fN.cross(nN).dot(eV), fN.dot(nN));
      //  if(ndi>=max_di)
      //  {
      //    max_ne = ne;
      //    max_di = ndi;
      //  }
      //}

      ////cout<<(max_ne != max_ne_2)<<" =?= "<<e_cons<<endl;
      //if(max_ne != max_ne_2)
      //{
      //  cout<<(f+1)<<" ---> "<<(max_ne%m)+1<<" != "<<(max_ne_2%m)+1<<" ... "<<e_cons<<" "<<flip(f)<<endl;
      //  typename DerivedV::Scalar max_di = -1;
      //  for(size_t nei = 0;nei<neighbors.size();nei++)
      //  {
      //    const auto & ne = neighbors[nei];
      //    const int nf = ne%m;
      //    if(nf == f)
      //    {
      //      cout<<"  "<<(ne%m)+1<<":\t"<<0<<"\t"<<di[EMAP[e]][nei]<<" "<<diIM(ne)<<endl;
      //      continue;
      //    }
      //    // Corner of neighbor
      //    const int nc = ne/m;
      //    // Is neighbor oriented consistently with (flipped) f?
      //    //const int ns = F(nf,(nc+1)%3);
      //    const int nd = F(nf,(nc+2)%3);
      //    const bool cons = (flip(f)?fd:fs) == nd;
      //    // Normal after possibly flipping to match flip or orientation of f
      //    const auto & nN = (cons? (flip(f)?-1:1.) : (flip(f)?1.:-1.) )*N.row(nf);
      //    // Angle between n and f
      //    const auto & ndi = M_PI - atan2( fN.cross(nN).dot(eV), fN.dot(nN));
      //    cout<<"  "<<(ne%m)+1<<":\t"<<ndi<<"\t"<<di[EMAP[e]][nei]<<" "<<diIM(ne)<<endl;
      //    if(ndi>=max_di)
      //    {
      //      max_ne = ne;
      //      max_di = ndi;
      //    }
      //  }
      //}
      if(nfei >= 0)
      {
        max_ne = uE2E[EMAP(e)][nfei];
      }

      if(max_ne>=0)
      {
        // face of neighbor
        const int nf = max_ne%m;
#ifdef IGL_OUTER_HULL_DEBUG
        if(!FH[nf])
        {
          // first time seeing face
          cout<<(f+1)<<" --> "<<(nf+1)<<endl;
        }
#endif
        FH[nf] = true;
        //std::cout << "face " << face_count++ << ": " << nf << std::endl;
        //std::cout << "f " << F.row(nf).array()+1 << std::endl;
        FHcount++;
        // corner of neighbor
        const int nc = max_ne/m;
        const int nd = F(nf,(nc+2)%3);
        const bool cons = (flip(f)?fd:fs) == nd;
        flip(nf) = (cons ? flip(f) : !flip(f));
        //cout<<"flip("<<nf<<") = "<<(flip(nf)?"true":"false")<<endl;
        const int ne1 = nf+((nc+1)%3)*m;
        const int ne2 = nf+((nc+2)%3)*m;
        if(!EH[ne1])
        {
          Q.push(ne1);
        }
        if(!EH[ne2])
        {
          Q.push(ne2);
        }
      }
    }

    {
      vG[id].resize(FHcount,3);
      vJ[id].resize(FHcount,1);
      //nG += FHcount;
      size_t h = 0;
      assert(counts(id) == IM.rows());
      for(int i = 0;i<counts(id);i++)
      {
        const size_t f = IM(i);
        //if(f_flip)
        //{
        //  flip(f) = !flip(f);
        //}
        if(FH[f])
        {
          vG[id].row(h) = (flip(f)?F.row(f).reverse().eval():F.row(f));
          vJ[id](h,0) = f;
          h++;
        }
      }
      assert((int)h == FHcount);
    }
  }

  // Is A inside B? Assuming A and B are consistently oriented but closed and
  // non-intersecting.
  const auto & is_component_inside_other = [](
    const Eigen::MatrixXd & V,
    const MatrixXV & BC,
    const MatrixXG & A,
    const MatrixXJ & AJ,
    const MatrixXG & B)->bool
  {
    const auto & bounding_box = [](
      const Eigen::MatrixXd & V,
      const MatrixXG & F)->
      Eigen::MatrixXd
    {
      Eigen::MatrixXd BB(2,3);
      BB<<
         1e26,1e26,1e26,
        -1e26,-1e26,-1e26;
      const size_t m = F.rows();
      for(size_t f = 0;f<m;f++)
      {
        for(size_t c = 0;c<3;c++)
        {
          const auto & vfc = V.row(F(f,c));
          BB.row(0) = BB.row(0).array().min(vfc.array()).eval();
          BB.row(1) = BB.row(1).array().max(vfc.array()).eval();
        }
      }
      return BB;
    };
    // A lot of the time we're dealing with unrelated, distant components: cull
    // them.
    Eigen::MatrixXd ABB = bounding_box(V,A);
    Eigen::MatrixXd BBB = bounding_box(V,B);
    if( (BBB.row(0)-ABB.row(1)).maxCoeff()>0  ||
        (ABB.row(0)-BBB.row(1)).maxCoeff()>0 )
    {
      // bounding boxes do not overlap
      return false;
    }
    ////////////////////////////////////////////////////////////////////////
    // POTENTIAL ROBUSTNESS WEAK AREA
    ////////////////////////////////////////////////////////////////////////
    //

    // winding_number_3 expects colmajor
    // q could be so close (<~1e-15) to B that the winding number is not a robust way to
    // determine inside/outsideness. We could try to find a _better_ q which is
    // farther away, but couldn't they all be bad?
    double q[3] = {
        CGAL::to_double(BC(AJ(0), 0)),
        CGAL::to_double(BC(AJ(0), 1)),
        CGAL::to_double(BC(AJ(0), 2)) };
    // In a perfect world, it's enough to test a single point.
    double w;
    winding_number_3(
      V.data(),V.rows(),
      B.data(),B.rows(),
      q,1,&w);
    return w > 0.5 || w < -0.5;
  };

  Eigen::MatrixXd Vcol(V.rows(), V.cols());
  for (size_t i=0; i<(size_t)V.rows(); i++) {
      for (size_t j=0; j<(size_t)V.cols(); j++) {
          Vcol(i, j) = CGAL::to_double(V(i, j));
      }
  }

  // Reject components which are completely inside other components
  vector<bool> keep(ncc,true);
  size_t nG = 0;
  // This is O( ncc * ncc * m)
  for(size_t id = 0;id<ncc;id++)
  {
    for(size_t oid = 0;oid<ncc;oid++)
    {
      if(id == oid)
      {
        continue;
      }
      const bool inside = is_component_inside_other(Vcol,BC,vG[id],vJ[id],vG[oid]);
#ifdef IGL_OUTER_HULL_DEBUG
      cout<<id<<" is inside "<<oid<<" ? "<<inside<<endl;
#endif
      keep[id] = keep[id] && !inside;
    }
    if(keep[id])
    {
      nG += vJ[id].rows();
    }
  }

  // collect G and J across components
  G.resize(nG,3);
  J.resize(nG,1);
  {
    size_t off = 0;
    for(Index id = 0;id<(Index)ncc;id++)
    {
      if(keep[id])
      {
        assert(vG[id].rows() == vJ[id].rows());
        G.block(off,0,vG[id].rows(),vG[id].cols()) = vG[id];
        J.block(off,0,vJ[id].rows(),vJ[id].cols()) = vJ[id];
        off += vG[id].rows();
      }
    }
  }
}

template <
  typename DerivedV,
  typename DerivedF,
  typename DerivedG,
  typename DerivedJ,
  typename Derivedflip>
IGL_INLINE void igl::outer_hull(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedG> & G,
  Eigen::PlainObjectBase<DerivedJ> & J,
  Eigen::PlainObjectBase<Derivedflip> & flip)
{
  Eigen::Matrix<typename DerivedV::Scalar,DerivedF::RowsAtCompileTime,3> N;
  per_face_normals_stable(V,F,N);
  return outer_hull(V,F,N,G,J,flip);
}

#ifdef IGL_STATIC_LIBRARY

#include <igl/barycenter.cpp>
template void igl::barycenter<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> >(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> >&);

#include <igl/outer_facet.cpp>
template void igl::outer_facet<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<long, -1, 1, 0, -1, 1>, int>(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<long, -1, 1, 0, -1, 1> > const&, int&, bool&);

#include <igl/cgal/order_facets_around_edges.cpp>
template std::__1::enable_if<std::is_same<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3>::Scalar, CGAL::Lazy_exact_nt<CGAL::Gmpq> >::value, void>::type igl::order_facets_around_edges<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 2, 0, -1, 2>, Eigen::Matrix<int, -1, 2, 0, -1, 2>, Eigen::Matrix<long, -1, 1, 0, -1, 1>, long, long, bool>(Eigen::PlainObjectBase<Eigen::Matrix<CGAL::Lazy_exact_nt<CGAL::Gmpq>, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 2, 0, -1, 2> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 2, 0, -1, 2> > const&, Eigen::PlainObjectBase<Eigen::Matrix<long, -1, 1, 0, -1, 1> > const&, std::__1::vector<std::__1::vector<long, std::__1::allocator<long> >, std::__1::allocator<std::__1::vector<long, std::__1::allocator<long> > > > const&, std::__1::vector<std::__1::vector<long, std::__1::allocator<long> >, std::__1::allocator<std::__1::vector<long, std::__1::allocator<long> > > >&, std::__1::vector<std::__1::vector<bool, std::__1::allocator<bool> >, std::__1::allocator<std::__1::vector<bool, std::__1::allocator<bool> > > >&);

// Explicit template specialization
template void igl::outer_hull<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<long, -1, 1, 0, -1, 1>, Eigen::Matrix<bool, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<long, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<bool, -1, 1, 0, -1, 1> >&);
template void igl::outer_hull<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
template void igl::outer_hull<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<bool, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, Eigen::PlainObjectBase<Eigen::Matrix<bool, -1, 1, 0, -1, 1> >&);
#endif


#include "IntersectionPoints.h"
#include <Eigen/Dense>
#include <iostream>
#include "Face.h"
#include "Log.h"

namespace Geotree
{
  IntersectionPoints::IntersectionPoints(const Mesh &mesh0, const Mesh &mesh1)
  : mesh0(mesh0), mesh1(mesh1)
  {
    findPoints();
  }

  bool IntersectionPoints::contains(Point &point)
  {
    for (Point p : points)
    {
      if (p == point)
      {
        return true;
      }
    }
    return false;
  }

  const Mesh &IntersectionPoints::getMesh(MeshID meshID)
  {
    switch (meshID)
      {
      case MESH0: return this->mesh0;
      case MESH1: return this->mesh1;
      }
    throw;
  }

  const Mesh &IntersectionPoints::getOtherMesh(MeshID meshID)
  {
    switch (meshID)
      {
      case MESH1: return this->mesh0;
      case MESH0: return this->mesh1;
      }
    throw;
  }

  void IntersectionPoints::getIntersectionPoints(MeshID meshID, std::vector <Point> &points)
  {
    std::vector <Segment> segments;
    std::vector <Face> faces;

    const Mesh &mesh = getMesh(meshID);
    const Mesh &othermesh = getOtherMesh(meshID);

    mesh.getFaces(faces);
    othermesh.getSegments(segments);

    for (Face face : faces)
      {
	for (Segment segment : segments)
	  {
	    face.intersect(segment, points);
	  }
      }
  }

  void IntersectionPoints::add(Point point)
  {

    if (this->contains(point))
      {
	for (Point &p : points)
	  {
	    if (p == point)
	      {
		p.facesMesh0.insert(point.facesMesh0.begin(), point.facesMesh0.end());
		p.facesMesh1.insert(point.facesMesh1.begin(), point.facesMesh1.end());
		Log().debug() << point << " MODIFY";
	      }
	  }
      }
      else
      {
	Log().debug() << point << " INSERT";
	this->points.push_back(point);
      }
  }

  void IntersectionPoints::findPoints()
  {
    std::vector <Point> points0;
    std::vector <Point> points1;

    getIntersectionPoints(MESH0, points0);
    getIntersectionPoints(MESH1, points1);

    for (Point &point : points0)
      {
	this->add(point);
      }

    for (Point &point : points1)
      {
	point.flip();
	this->add(point);
      }
  }

  std::ostream& operator<< (std::ostream& stream, const IntersectionPoints& points)
  {
    for (Point p : points.points)
    {
      stream << p << std::endl;
    }
    return stream;
  }
}


#include <boost/geometry/algorithms/relation.hpp>


//
// Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH.
// Copyright (c) 2015-2017, University of Bremen
// 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.
//
// 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.
//

#ifndef TEST_TOOLS_GENERATEPOINTCLOUDS_HPP_
#define TEST_TOOLS_GENERATEPOINTCLOUDS_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace maps {

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

/**
 * LIDARSimulator simulates a 3d laser range finder, with laser beams at
 */
class LIDARSimulator
{
public:
    /**
     * Constructor taking vectors of all possible latitude and longitude angles.
     * Both ranges must have at least one value. Latitude must be in [-PI/2, +PI/2], Longitude must be in [-PI, +PI].
     * For a 2D Scanner, choose latitudeRange Eigen::Matrix<double, 1,1>(0.0),
     * for a range of angles use the Eigen::VectorXd::LinSpaced function.
     */
    template<class Derived1, class Derived2>
    LIDARSimulator(const Eigen::MatrixBase<Derived1>& latitudeRange_, const Eigen::MatrixBase<Derived2>& longitudeRange_) : latitudeRange(latitudeRange_), longitudeRange(longitudeRange_)
    {
        assert(latitudeRange.size()>=1    && latitudeRange.abs().maxCoeff() <= M_PI/2.0+0.001 );
        assert(longitudeRange.size() >= 1 && longitudeRange.abs().maxCoeff() <= M_PI+0.001);
    }

    /**
     * getRanges computes ranges to the nearest plane of the scene, given a pose of the LIDAR relative to the scene.
     * Optionally, it also computes the corresponding pointcloud (in the coordinate system of the LIDAR).
     */
    void getRanges(Eigen::Ref<Eigen::ArrayXXd> ranges, const std::vector<Eigen::Hyperplane<double, 3> >& scene, const Eigen::Affine3d & pose, PointCloud *pc = 0)
    {
        Eigen::DenseIndex height = latitudeRange.size();
        Eigen::DenseIndex width = longitudeRange.size();
        assert(ranges.rows()==height && ranges.cols() == width);
        if(pc)
        {
            pc->height = height;
            pc->width = width;
            pc->resize(height * width);
        }
        for(Eigen::DenseIndex i=0; i<latitudeRange.size(); ++i)
        {
            double sinTheta = std::sin(latitudeRange(i)), cosTheta=std::cos(latitudeRange(i));
            for(Eigen::DenseIndex j=0; j<longitudeRange.size(); ++j)
            {
                Eigen::Vector3d dir(cosTheta*std::cos(longitudeRange(j)), cosTheta*std::sin(longitudeRange(j)), sinTheta);
                Eigen::ParametrizedLine<double, 3> line(pose.translation(), pose.linear()*dir);
                double minDist = 1e99;
//                std::cout << "Line = [" << line.origin().transpose() << "] + l*[" << line.direction().transpose() << "]: ";
                for(size_t k=0; k<scene.size(); ++k)
                {
                    double dist = line.intersectionParameter(scene[k]);
//                    std::cout << dist << " ";
                    if(dist > 0.0 && dist < minDist)
                        minDist = dist;
                }
//                std::cout << std::endl;
                ranges(i, j) = minDist;
                if(pc) // PCL first wants the column, then the row:
                    pc->at(j, i).getVector3fMap() = (dir*minDist).cast<float>();
            }
        }
    }

    void getRanges(Eigen::ArrayXXd& ranges, const std::vector<Eigen::Hyperplane<double, 3> >& scene, const Eigen::Affine3d & pose, PointCloud * pc = 0)
    {
        ranges.resize(latitudeRange.size(), longitudeRange.size());
        getRanges(Eigen::Ref<Eigen::ArrayXXd>(ranges), scene, pose, pc);
    }

protected:
    Eigen::ArrayXd latitudeRange, longitudeRange;
};

}  // namespace maps

#endif /* TEST_TOOLS_GENERATEPOINTCLOUDS_HPP_ */


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2012 Dominic Marcello
//  Copyright (c) 2012 Bryce Adelstein-Lelbach
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
////////////////////////////////////////////////////////////////////////////////

#if !defined(OCTOPUS_0CBAD952_ED85_4D22_84DF_6AB16A34E518)
#define OCTOPUS_0CBAD952_ED85_4D22_84DF_6AB16A34E518

#include <octopus/assert.hpp>
#include <octopus/face.hpp>
#include <octopus/array.hpp>

#include <boost/serialization/access.hpp>
#include <boost/cstdint.hpp>

#include <ostream>

namespace octopus
{

struct child_index
{
    child_index() : packed_(0) {}

    child_index(boost::uint64_t packed)
      : packed_(static_cast<boost::uint8_t>(packed))
    {
        OCTOPUS_ASSERT_MSG(7 >= packed, "packed index is out of range");
    }

    child_index(boost::uint64_t x, boost::uint64_t y, boost::uint64_t z)
      : packed_(static_cast<boost::uint8_t>(x | (y << 1) | (z << 2)))
    {
        OCTOPUS_ASSERT_MSG(1 >= x, "x-index is out of range");
        OCTOPUS_ASSERT_MSG(1 >= y, "y-index is out of range");
        OCTOPUS_ASSERT_MSG(1 >= z, "z-index is out of range");
    }

    child_index(child_index const& other)
      : packed_(other.packed_)
    {}

    child_index operator=(child_index const& other)
    {
        packed_ = other.packed_;
        return *this;
    }

    operator boost::uint64_t() const
    {
        return packed_;
    }

    bool x() const
    {
        return (packed_ >> 0) & 1;
    }

    bool y() const
    {
        return (packed_ >> 1) & 1;
    }

    bool z() const
    {
        return (packed_ >> 2) & 1;
    }

    void set_x(boost::uint64_t x_)
    {
        OCTOPUS_ASSERT_MSG(1 >= x_, "x-index is out of range");
        if (0 == x_)
            packed_ &= ~1;
        else
            packed_ |= 1;
    }

    void set_y(boost::uint64_t y_)
    {
        OCTOPUS_ASSERT_MSG(1 >= y_, "y-indey is out of range");
        if (0 == y_)
            packed_ &= ~2;
        else
            packed_ |= 2;
    }

    void set_z(boost::uint64_t z_)
    {
        OCTOPUS_ASSERT_MSG(1 >= z_, "z-indez is out of range");
        if (0 == z_)
            packed_ &= ~4;
        else
            packed_ |= 4;
    }

    template <typename T>
    array<T, 3> get_array() const
    {
        array<T, 3> a;
        a[0] = x();
        a[1] = y();
        a[2] = z();
        return a;
    }

  private:
    friend class boost::serialization::access;

    template <typename Archive>
    void serialize(Archive& ar, const unsigned int)
    {
        ar & packed_;
    }

    boost::uint8_t packed_;
};

inline std::ostream& operator<<(std::ostream& os, child_index idx)
{
    os << "(" << idx.x() << ", " << idx.y() << ", " << idx.z() << ")";
    return os;
}

OCTOPUS_EXPORT child_index invert(face f, child_index idx);

OCTOPUS_EXPORT child_index invert(child_index idx);

}

#endif // OCTOPUS_0CBAD952_ED85_4D22_84DF_6AB16A34E518


// MIT License
//
// Copyright (c) 2021 Oliver Schick
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef MPO19_PCA_17072020
#define MPO19_PCA_17072020

#include <mpo19/cluster.hpp>
#include <mpo19/point.hpp>
#include <mpo19/utility.hpp>
#include <mpo19/typedefs.hpp>
#include <mpo19/config.hpp>
#include <mpo19/pcure.hpp>
#include <tuple>
#include <vector>
#include <string>
#include <experimental/propagate_const>
#include <boost/range/algorithm.hpp>

struct P1;
struct P2;

template<typename Party>
struct basic_party {
private:
    using self_ = basic_party;
    using derived_ = Party;

public:
    using output_t = std::tuple<
                     std::vector<cluster>
                     , std::vector<point<double>>
                     , std::vector<size_t>>;

    void print_sigma() const;

    output_t execute_pca(array_t<point<int64_t>>& input, size_t t);
    output_t execute_opt(array_t<point<int64_t>>& input, size_t t);

    template<typename Range>
    output_t execute_pca(Range const& range, size_t t)
    {
        array_t<point<int64_t>> in{boost::extents[range.size()]};
        boost::copy(range, in.begin());
        return execute_pca(in, t);
    }

    template<typename Range>
    output_t execute_opt(Range const& range, size_t t)
    {
        array_t<point<int64_t>> in{boost::extents[range.size()]};
        boost::copy(range, in.begin());
        return execute_opt(in, t);
    }

    std::vector<cluster> pcure0(std::vector<point<int64_t>> const& input
                                , size_t s, size_t p, size_t q, size_t t1, size_t t2, size_t t);
    std::vector<cluster> pcure0_opt(std::vector<point<int64_t>> const& input
                                    , size_t s, size_t p, size_t q, size_t t1, size_t t2, size_t t);

    template<typename Range>
    std::vector<cluster> pcure0(Range const& input
                                , size_t s, size_t p, size_t q, size_t t1, size_t t2, size_t t)
    {
        std::vector<point<int64_t>> in{input.begin(), input.end()};
        return pcure0(in, s, p, q, t1, t2, t);
    }

    template<typename Range>
    std::vector<cluster> pcure0_opt(Range const& input
                                    , size_t s, size_t p, size_t q, size_t t1, size_t t2, size_t t)
    {
        std::vector<point<int64_t>> in{input.begin(), input.end()};
        return pcure0_opt(in, s, p, q, t1, t2, t);
    }

    std::vector<cluster> pcure1(std::vector<point<int64_t>> const& input
                                , size_t s, size_t p, size_t q, size_t t1, size_t t2, size_t t);
    std::vector<cluster> pcure1_opt(std::vector<point<int64_t>> const& input
                                    , size_t s, size_t p, size_t q, size_t t1, size_t t2, size_t t);

    template<typename Range>
    std::vector<cluster> pcure1(Range const& input
                                , size_t s, size_t p, size_t q, size_t t1, size_t t2, size_t t)
    {
        std::vector<point<int64_t>> in{input.begin(), input.end()};
        return pcure1(in, s, p, q, t1, t2, t);
    }

    template<typename Range>
    std::vector<cluster> pcure1_opt(Range const& input
                                    , size_t s, size_t p, size_t q, size_t t1, size_t t2, size_t t)
    {
        std::vector<point<int64_t>> in{input.begin(), input.end()};
        return pcure1_opt(in, s, p, q, t1, t2, t);
    }

    std::vector<cluster> pcure2(array_t<point<int64_t>>& input
                                , size_t s, size_t q, size_t t1, size_t t2, size_t t);
    std::vector<cluster> pcure2_opt(array_t<point<int64_t>>& input
                                    , size_t s, size_t q, size_t t1, size_t t2, size_t t);

    template<typename Range>
    std::vector<cluster> pcure2(Range const& input
                                , size_t s, size_t q, size_t t1, size_t t2, size_t t)
    {
        array_t<point<int64_t>> in{boost::extents[input.size()]};
        boost::copy(input, in.begin());
        return pcure2(in, s, q, t1, t2, t);
    }

    template<typename Range>
    std::vector<cluster> pcure2_opt(Range const& input
                                    , size_t s, size_t q, size_t t1, size_t t2, size_t t)
    {
        array_t<point<int64_t>> in{boost::extents[input.size()]};
        boost::copy(input, in.begin());
        return pcure2(in, s, q, t1, t2, t);
    }

protected:

    ~basic_party() = default;

private:

    void clustering(size_t t);
    void clustering_opt(size_t t);
    void insert_other_parties_centroids(std::vector<point<double>>& centroids);
    std::vector<point<double>> pca_on_A_clusters(mpo19::pcure_clusters<int64_t>& clusters, size_t t2, size_t t);
    std::vector<point<double>> opt_on_A_clusters(mpo19::pcure_clusters<int64_t>& clusters, size_t t2, size_t t);
    std::vector<point<double>> remove_outliers(output_t& out, mpo19::pcure_clusters<int64_t>& clusters, size_t t2);
};

struct P1 : basic_party<P1> {
private:
    using self_ = P1;
    using base_ = basic_party<self_>;
    friend base_;

public:
    P1(unsigned short port);

    void print_L();

    ~P1();

private:

    struct impl_;
    std::experimental::propagate_const<
    std::unique_ptr<impl_>> pimpl_;
};

struct P2  : basic_party<P2> {
private:
    using self_ = P2;
    using base_ = basic_party<self_>;
    friend base_;

public:

    P2(std::string const& destination, unsigned short port);

    ~P2();

    void print_L();

private:

    struct impl_;
    std::experimental::propagate_const<
    std::unique_ptr<impl_>> pimpl_;
};

#endif //MPO19_PCA_17072020


#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

#include <aruco_mapping.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Pose.h>
#include <boost/lexical_cast.hpp>

void poseCallback_aruco(){
  static tf::TransformBroadcaster br;
  static tf::TransformListener listener;

  tf::StampedTransform aruco_camera;
  // listener.waitForTransform("/camera_position", "/world", ros::Time(0), ros::Duration(1));
  try{
    listener.lookupTransform("/camera_position", "/world", ros::Time(0), aruco_camera); // B
  }
  catch (tf::TransformException &ex) {
    ROS_ERROR("%s",ex.what());
  }

  tf::StampedTransform lizi_camera;
  // listener.waitForTransform( "/camera_rgb_optical_frame", "/map", ros::Time(0), ros::Duration(1));
  try{
    listener.lookupTransform("/camera_rgb_optical_frame", "/map", ros::Time(0), lizi_camera); // A
  }
  catch (tf::TransformException &ex) {
    ROS_ERROR("%s",ex.what());
  }

  // tf::Transform final_pub = lizi_camera.inverse() * aruco_camera;
  tf::Transform final_pub = aruco_camera.inverse() * lizi_camera;

  std::cout << "Publishing..." << std::endl;
  br.sendTransform(tf::StampedTransform(final_pub, ros::Time::now(), "/world", "/map"));
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;

  tf::TransformBroadcaster br;
  tf::TransformListener listener;

  tf::StampedTransform aruco_camera;
  tf::StampedTransform lizi_camera;

  listener.waitForTransform("/camera_position", "/world", ros::Time(0), ros::Duration(1));
  listener.waitForTransform( "/camera_rgb_optical_frame", "/map", ros::Time(0), ros::Duration(1));

  ros::Rate rate(50.0);
  while(node.ok()){
    // poseCallback_aruco();

    // std::cout << "Publishing..." << std::endl;

    try{
      listener.lookupTransform("/camera_position", "/world", ros::Time(0), aruco_camera); // B
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
    }

    try{
      listener.lookupTransform("/camera_rgb_optical_frame", "/map", ros::Time(0), lizi_camera); // A
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
    }

    tf::Transform final_pub = aruco_camera.inverse() * lizi_camera;
    br.sendTransform(tf::StampedTransform(final_pub, ros::Time::now(), "/world", "/map"));

    rate.sleep();
  }

  ros::spin();
  return 0;

}


#define BOOST_TEST_MODULE pcraster model_engine field
#include <boost/test/unit_test.hpp>
#include "calc_nonspatial.h"
#include "calc_spatial.h"

BOOST_AUTO_TEST_CASE(testCtorNonSpatial)
{
  using namespace calc;

  {
    REAL4 v;
    NonSpatial ns(VS_S,v);
    BOOST_CHECK(ns.cr()==CR_REAL4);
  }
  {
    INT4 vb[3]={2,MV_INT4,6};
    Spatial  sb(VS_N,vb,3);

    BOOST_CHECK(sb.cr()==CR_INT4);
    BOOST_CHECK(sb.nrValues()==3);
    double v;
    BOOST_CHECK(sb.getCell(v,0));
    BOOST_CHECK(v==2);
    BOOST_CHECK(!sb.getCell(v,1));
    BOOST_CHECK(sb.getCell(v,2));
    BOOST_CHECK(v==6);
  }
}


/*ckwg +29
 * Copyright 2014 by Kitware, Inc.
 * 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 name of Kitware, Inc. nor the names of any 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 AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * \file
 * \brief test core camera class
 */

#include <test_common.h>

#include <iostream>
#include <boost/foreach.hpp>
#include <boost/math/constants/constants.hpp>
#include <maptk/core/camera.h>

#include <maptk/core/camera_io.h>

#define TEST_ARGS ()

DECLARE_TEST_MAP();

int
main(int argc, char* argv[])
{
  CHECK_ARGS(1);

  testname_t const testname = argv[1];

  RUN_TEST(testname);
}

IMPLEMENT_TEST(look_at)
{
  using namespace maptk;
  vector_2d pp(300,400);
  camera_intrinsics_d K(1000, pp);
  vector_3d focus(0, 1, -2);
  maptk::camera_d cam(vector_3d(3, -4, 7), rotation_d(), K);
  cam.look_at(focus);

  vector_2d ifocus = cam.project(focus);
  TEST_NEAR("look_at focus projects to origin",
            (ifocus-pp).magnitude(), 0.0, 1e-12);

  vector_2d ifocus_up = cam.project(focus + vector_3d(0,0,2));
  vector_2d tmp = ifocus_up - pp;
  TEST_NEAR("look_at vertical projects vertical",
            tmp.x(), 0.0, 1e-12);
  // "up" in image space is actually negative Y because the
  // Y axis is inverted
  TEST_EQUAL("look_at up projects up", tmp.y() < 0.0, true);
}

IMPLEMENT_TEST(projection)
{
  using namespace maptk;
  vector_2d pp(300,400);
  camera_intrinsics_d K(1000, pp);
  vector_3d focus(0, 1, -2);
  maptk::camera_d cam(vector_3d(3, -4, 7), rotation_d(), K);
  cam.look_at(focus);

  matrix_3x4d P(cam);
  vector_3d test_pt(1,2,3);
  vector_4d test_hpt(test_pt.x(), test_pt.y(), test_pt.z(), 1.0);

  vector_3d proj_hpt = P * test_hpt;
  vector_2d proj_pt(proj_hpt.x()/proj_hpt.z(), proj_hpt.y()/proj_hpt.z());

  TEST_NEAR("camera projection = matrix multiplication",
             (cam.project(test_pt) - proj_pt).magnitude(), 0.0, 1e-12);
}

IMPLEMENT_TEST(interpolation)
{
  using namespace maptk;
  using namespace std;

  double pi = boost::math::constants::pi<double>();
  camera_d a(vector_3d(-1, -1, -1),
             rotation_d(vector_4d(0, 0, 0, 1))),  // no rotation
           b(vector_3d(3, 3, 3),
             rotation_d(-pi / 2, vector_3d(0, 0, 1))),  // rotated around z-axis 90 degrees
           c;
  c = interpolate_camera(a, b, 0.5);

  cerr << "a.rotation: " << a.rotation().axis() << ' '  << a.rotation().angle() << endl;
  cerr << "b.rotation: " << b.rotation().axis() << ' '  << b.rotation().angle() << endl;

  cerr << "c.center  : " << c.center() << endl;
  TEST_NEAR("c.center.x", c.center().x(), 1, 1e-16);
  TEST_NEAR("c.center.y", c.center().y(), 1, 1e-16);
  TEST_NEAR("c.center.z", c.center().z(), 1, 1e-16);

  cerr << "c.rotation (aa): " << c.rotation().axis() << ' ' << c.rotation().angle() << endl;
  cerr << "c.rotation  (q): " << c.rotation() << endl;
  TEST_NEAR("c.rotation.axis.x", c.rotation().axis().x(),  0, 1e-15);
  TEST_NEAR("c.rotation.axis.y", c.rotation().axis().y(),  0, 1e-15);
  TEST_NEAR("c.rotation.axis.z", c.rotation().axis().z(), -1, 1e-15);
  TEST_NEAR("c.rotation.angle" , c.rotation().angle()   , pi / 4, 1e-15);
}

IMPLEMENT_TEST(multiple_interpolations)
{
  using namespace maptk;
  using namespace std;

  double pi = boost::math::constants::pi<double>();
  camera_d a(vector_3d(-1, -1, -1),
             rotation_d(vector_4d(0, 0, 0, 1))),        // no rotation
           b(vector_3d(3, 3, 3),
             rotation_d(-pi / 2, vector_3d(0, 0, 1)));  // rotated around z-axis 90 degrees
  vector<camera_d> cams;

  cams.push_back(a);
  interpolated_cameras(a, b, 3, cams);
  cams.push_back(b);

  cerr << "Vector size: " << cams.size() << endl;
  TEST_EQUAL("vector size", cams.size(), 5);
  BOOST_FOREACH(camera_d cam, cams)
  {
    cerr << "\t" << cam.center() << " :: " << cam.rotation().axis() << " " << cam.rotation().angle() << endl;
  }

  camera_d i1 = cams[1],
           i2 = cams[2],
           i3 = cams[3];
  cerr << "i1 .25 c : " << i1.center() << " :: " << i1.rotation().axis() << ' ' << i1.rotation().angle() << endl;
  cerr << "i2 .25 c : " << i2.center() << " :: " << i2.rotation().axis() << ' ' << i2.rotation().angle() << endl;
  cerr << "i3 .25 c : " << i3.center() << " :: " << i3.rotation().axis() << ' ' << i3.rotation().angle() << endl;

  TEST_NEAR("i1 center.x", i1.center().x(), 0, 1e-15);
  TEST_NEAR("i1 center.y", i1.center().y(), 0, 1e-15);
  TEST_NEAR("i1 center.z", i1.center().z(), 0, 1e-15);
  TEST_NEAR("i1 r.axis.x", i1.rotation().axis().x(), 0, 1e-15);
  TEST_NEAR("i1 r.axis.y", i1.rotation().axis().y(), 0, 1e-15);
  TEST_NEAR("i1 r.axis.z", i1.rotation().axis().z(), -1, 1e-15);
  TEST_NEAR("i1 r.angle",  i1.rotation().angle(), pi / 8, 1e-15);

  TEST_NEAR("i2 center.x", i2.center().x(), 1, 1e-15);
  TEST_NEAR("i2 center.y", i2.center().y(), 1, 1e-15);
  TEST_NEAR("i2 center.z", i2.center().z(), 1, 1e-15);
  TEST_NEAR("i2 r.axis.x", i2.rotation().axis().x(), 0, 1e-15);
  TEST_NEAR("i2 r.axis.y", i2.rotation().axis().y(), 0, 1e-15);
  TEST_NEAR("i2 r.axis.z", i2.rotation().axis().z(), -1, 1e-15);
  TEST_NEAR("i2 r.angle",  i2.rotation().angle(), pi / 4, 1e-15);

  TEST_NEAR("i3 center.x", i3.center().x(), 2, 1e-15);
  TEST_NEAR("i3 center.y", i3.center().y(), 2, 1e-15);
  TEST_NEAR("i3 center.z", i3.center().z(), 2, 1e-15);
  TEST_NEAR("i3 r.axis.x", i3.rotation().axis().x(), 0, 1e-15);
  TEST_NEAR("i3 r.axis.y", i3.rotation().axis().y(), 0, 1e-15);
  TEST_NEAR("i3 r.axis.z", i3.rotation().axis().z(), -1, 1e-15);
  TEST_NEAR("i3 r.angle",  i3.rotation().angle(), 3*pi / 8, 1e-15);
}

// TODO: Full test case for camera interpolation would be to create a full,
// connected ring of cameras looking at a point (or even multiple loops), and
// checking that the rotation angle returned by the getter function of the
// rotation between each cameras is less than pi. It would be even more
// detailed to do this for camera rings along each major axis plane, as well
// as for a stare-point that is not at along the axis of rotation for the
// camera ring.


/*
 * Copyright 2015 David A. Boyuka II
 *
 * 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.
 */

/*
 * test-index-io.cpp
 *
 *  Created on: May 10, 2014
 *      Author: David A. Boyuka II
 */

#include <cstdint>
#include <cassert>
#include <cstdlib>
#include <vector>
#include <boost/smart_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/dynamic_bitset.hpp>

#include "pique/region/region-encoding.hpp"
#include "pique/setops/setops.hpp"

#include "pique/region/ii/ii.hpp"
#include "pique/region/ii/ii-encode.hpp"
#include "pique/setops/ii/ii-setops.hpp"

#include "pique/region/cii/cii.hpp"
#include "pique/region/cii/cii-encode.hpp"
#include "pique/setops/cii/cii-setops.hpp"

using bitmaps_t = std::vector< std::reference_wrapper< boost::dynamic_bitset<> > >;
using ciis_t = std::vector< boost::shared_ptr< CIIRegionEncoding > >;

// Note: bitmap specified in big-endian order (least significant bit is the rightmost character in the string)
boost::dynamic_bitset<> bitmap1(std::string("1111000010111000"));
boost::dynamic_bitset<> bitmap2(std::string("1000100111110011"));
boost::dynamic_bitset<> bitmap3(std::string("1111111100000000"));

static boost::dynamic_bitset<> make_big_bitmap(uint64_t domain_size) {
	boost::dynamic_bitset<> bitmap(domain_size, 0);
	for (uint64_t i = 0; i < domain_size; i++)
		bitmap[i] = (rand() & 1);
	return bitmap;
}

boost::dynamic_bitset<> bigbitmap1 = make_big_bitmap(1ULL<<10);
boost::dynamic_bitset<> bigbitmap2 = make_big_bitmap(1ULL<<10);
boost::dynamic_bitset<> bigbitmap3 = make_big_bitmap(1ULL<<10);

boost::dynamic_bitset<> rlybigbitmap1 = make_big_bitmap(1ULL<<14);
boost::dynamic_bitset<> rlybigbitmap2 = make_big_bitmap(1ULL<<14);
boost::dynamic_bitset<> rlybigbitmap3 = make_big_bitmap(1ULL<<14);

//std::vector<int> bitmap1 = {0, 0, 0, 1, 1, 1, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1};
//	std::vector<int> bitmap2 = {1, 1, 0, 0, 1, 1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 1};

template<typename RegionEncoderT>
static boost::shared_ptr< typename RegionEncoderT::RegionEncodingOutT > make_region(typename RegionEncoderT::RegionEncoderConfig conf, const boost::dynamic_bitset<> &bitmap) {
	RegionEncoderT encoder(conf, bitmap.size());

	for (uint64_t i = 0; i < bitmap.size(); ++i)
		encoder.push_bits(1, bitmap[i]);

	encoder.finalize();
	return encoder.to_region_encoding();
}

template<typename SetOps>
static inline void do_test_with_setops(ciis_t all_regions, const bitmaps_t &all_bitmaps) {
	SetOps setops(CIISetOperationsConfig(false));

	std::vector< uint32_t > rids;

	// A | B | C | ...
	boost::shared_ptr< CIIRegionEncoding > runion = setops.nary_set_op(all_regions.begin(), all_regions.end(), NArySetOperation::UNION);
	runion->convert_to_rids(rids, true, true);
	auto it = rids.begin(), end_it = rids.end();

	for (size_t i = 0; i < all_bitmaps[0].get().size(); ++i) {
		bool expected = false;
		for (boost::dynamic_bitset<> bitmap : all_bitmaps) {
			if (bitmap[i]) {
				expected = true;
				break;
			}
		}

		if (expected) {
			assert(it != end_it && *it == i);
			++it;
		} else {
			assert(it == end_it || *it != i);
		}
	}

	// A & B & C & ...
	boost::shared_ptr< CIIRegionEncoding > rinter = setops.nary_set_op(all_regions.begin(), all_regions.end(), NArySetOperation::INTERSECTION);
	rinter->convert_to_rids(rids, true, true);
	it = rids.begin(); end_it = rids.end();

	for (size_t i = 0; i < all_bitmaps[0].get().size(); ++i) {
		bool expected = true;
		for (boost::dynamic_bitset<> bitmap : all_bitmaps) {
			if (!bitmap[i]) {
				expected = false;
				break;
			}
		}

		if (expected) {
			assert(it != end_it && *it == i);
			++it;
		} else {
			assert(it == end_it || *it != i);
		}
	}

	// Invert all operands
	for (auto region : all_regions) setops.inplace_unary_set_op(region, UnarySetOperation::COMPLEMENT);

	// ~A | ~B | ~C | ...

	boost::shared_ptr< CIIRegionEncoding > rnegunion = setops.nary_set_op(all_regions.begin(), all_regions.end(), NArySetOperation::UNION);
	rnegunion->convert_to_rids(rids, true, true);
	it = rids.begin(); end_it = rids.end();

	for (size_t i = 0; i < all_bitmaps[0].get().size(); ++i) {
		bool expected = false;
		for (boost::dynamic_bitset<> bitmap : all_bitmaps) {
			if (!bitmap[i]) {
				expected = true;
				break;
			}
		}

		if (expected) {
			assert(it != end_it && *it == i);
			++it;
		} else {
			assert(it == end_it || *it != i);
		}
	}

	// Re-invert all operands
	for (auto region : all_regions) setops.inplace_unary_set_op(region, UnarySetOperation::COMPLEMENT);
}

static inline void do_test_vary_setops(ciis_t all_regions, const bitmaps_t &all_bitmaps) {
	do_test_with_setops< CIISetOperations >(all_regions, all_bitmaps);
	do_test_with_setops< CIISetOperationsNAry >(all_regions, all_bitmaps);
}

static inline void do_test(const bitmaps_t &all_bitmaps) {
	std::vector< boost::shared_ptr< CIIRegionEncoding > > all_regions;
	for (boost::dynamic_bitset<> &bitmap : all_bitmaps) {
		all_regions.push_back(make_region< CIIRegionEncoder >(CIIRegionEncoderConfig(), bitmap));
	}

	for (int i = -1; i < (int)all_regions.size(); ++i) {
		if (i >= 0)
			all_regions[i]->decompress();
		do_test_vary_setops(all_regions, all_bitmaps);
	}
}

int main(int argc, char **argv) {
	do_test(bitmaps_t { bitmap1, bitmap2, bitmap3 });
	do_test(bitmaps_t { bigbitmap1, bigbitmap2, bigbitmap3 });
	do_test(bitmaps_t { rlybigbitmap1, rlybigbitmap2, rlybigbitmap3 });
}


#include <boost/test/unit_test.hpp>
#include <sudoku/grid.h>
#include <sudoku/standard.h>

struct GridTestSuiteFixture
{
    sudoku::square::Dimensions dims{2};
    sudoku::Grid grid{dims};

    void fillGrid()
    {
        // Fill grid with a valid solution
        std::vector<sudoku::CellValue> cellValues{
            1, 2, 3, 4,
            3, 4, 1, 2,
            2, 1, 4, 3,
            4, 3, 2, 1
        };
        for (sudoku::CellCount cellPos = 0; cellPos < dims.getCellCount(); ++cellPos) {
            grid.setCellValue(cellPos, cellValues[cellPos]);
        }
    }
};

BOOST_FIXTURE_TEST_SUITE(GridTestSuite, GridTestSuiteFixture)

    BOOST_AUTO_TEST_CASE(initialPotentialsAllUnblocked)
    {
        // Each value of each cell should be available.
        for (sudoku::CellCount cellPos = 0; cellPos < dims.getCellCount(); ++cellPos) {
            for (sudoku::CellValue cellValue = 1; cellValue <= dims.getMaxCellValue(); ++cellValue) {
                BOOST_REQUIRE(!grid.getCellPotential(cellPos).isBlocked(cellValue));
            }
        }
    }

    BOOST_AUTO_TEST_CASE(setAndGetValue)
    {
        grid.setCellValue(3, 3);
        BOOST_REQUIRE_EQUAL(grid.getCellValue(3), 3);
    }

    BOOST_AUTO_TEST_CASE(setValues_valuesBlockedInGroup)
    {
        // 1 2 | 0 0
        // 0 0 | 0 0
        // ----+----
        // 0 0 | 0 0
        // 0 0 | 0 0
        grid.setCellValue(0, 1);
        grid.setCellValue(1, 2);

        // Do some checks to make sure the values are blocked.
        BOOST_CHECK(grid.getCellPotential(3).isBlocked(1)); // cell 8 blocked by cell 0
        BOOST_CHECK(grid.getCellPotential(3).isBlocked(2)); // cell 8 blocked by cell 1
        BOOST_CHECK(grid.getCellPotential(8).isBlocked(1)); // cell 27 blocked by cell 0
        BOOST_CHECK(!grid.getCellPotential(8).isBlocked(2)); // cell 27 not blocked by cell 1
    }

    BOOST_AUTO_TEST_CASE(setAndClearValue)
    {
        grid.setCellValue(0, 1); // 1 x 0 0 <-- x blocked on value 1
                                // 0 0 0 0
                                // 0 0 0 0
                                // 0 0 0 0

        grid.setCellValue(13, 1); // 1 x 0 0 <-- x blocked on value 1
                                // 0 0 0 0
                                // 0 0 0 0
                                // 0 1 0 0

        BOOST_REQUIRE(grid.getCellPotential(1).isBlocked(1));

        // Remove the 1 at position 0. The cell at 'x' should still
        // be blocked by the 1 at position 13.
        grid.clearCellValue(0);
        BOOST_REQUIRE(grid.getCellPotential(1).isBlocked(1));

        // Remove the 1 at position 13. The cell at 'x' should now be
        // free again.
        grid.clearCellValue(13);
        BOOST_REQUIRE(!grid.getCellPotential(1).isBlocked(1));
    }

    BOOST_AUTO_TEST_CASE(restrictValue)
    {
        BOOST_REQUIRE(!grid.getCellPotential(0).isBlocked(3));
        grid.restrictCellValue(0, 3);
        BOOST_REQUIRE(grid.getCellPotential(0).isBlocked(3));
    }

    BOOST_AUTO_TEST_CASE(getMaxBlockEmptyCell_emptyGrid)
    {
        auto cellPos = grid.getMaxBlockEmptyCell();
        BOOST_REQUIRE_LT(cellPos, dims.getCellCount());
    }

    BOOST_AUTO_TEST_CASE(getMaxBlockEmptyCell_fullGrid)
    {
        fillGrid();
        BOOST_REQUIRE_EQUAL(grid.getMaxBlockEmptyCell(), dims.getCellCount());
    }

    BOOST_AUTO_TEST_CASE(getMaxBlockEmptyCell_EmptyCellCompletelyBlocked)
    {
        // 1 x 0 0  (x is completely blocked)
        // 0 2 0 0
        // 0 3 0 0
        // 0 4 0 0
        grid.setCellValue(0, 1);
        grid.setCellValue(5, 2);
        grid.setCellValue(9, 3);
        grid.setCellValue(13, 4);
        BOOST_REQUIRE_EQUAL(grid.getMaxBlockEmptyCell(), 1);
    }

    BOOST_AUTO_TEST_CASE(isFull_emptyGrid)
    {
        BOOST_REQUIRE(!grid.isFull());
    }

    BOOST_AUTO_TEST_CASE(isFull_fullGrid)
    {
        fillGrid();
        BOOST_REQUIRE(grid.isFull());
    }

    BOOST_AUTO_TEST_CASE(isFull_almostFull)
    {
        fillGrid();
        grid.clearCellValue(0);
        BOOST_REQUIRE(!grid.isFull());
    }

BOOST_AUTO_TEST_SUITE_END()


/**
 * @author Alessandro Bianco
 */

/**
 * @addtogroup DFNs
 * @{
 */

#ifndef FEATURESDESCRIPTION3D_PFHDESCRIPTOR3D_HPP
#define FEATURESDESCRIPTION3D_PFHDESCRIPTOR3D_HPP

#include "FeaturesDescription3DInterface.hpp"

#include <Types/CPP/PointCloud.hpp>
#include <Types/CPP/VisualPointFeatureVector3D.hpp>
#include <Converters/PointCloudToPclPointCloudConverter.hpp>
#include <Converters/PointCloudToPclNormalsCloudConverter.hpp>
#include <Helpers/ParametersListHelper.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <boost/make_shared.hpp>
#include <yaml-cpp/yaml.h>

#include <stdlib.h>
#include <string>

namespace CDFF
{
namespace DFN
{
namespace FeaturesDescription3D
{
	/**
	 * Computation of descriptors for input 3D keypoints using PFH (provided
	 * by PCL).
	 *
	 * @param searchRadius
	 *        neighbor points within this distance of the keypoint are used
	 *        in the computation of its descriptor
	 *
	 * @param forceNormalsEstimation
	 *        compute surface normals regardless of whether estimates are
	 *        provided as input
	 * @param enableNormalsEstimation, this
	 *        determines whether the absence of input estimates of the surface
	 *        normals causes the class to throw an error or compute estimates
	 *
	 * @param outputFormat
	 *        Format of the returned pointcloud of keypoints:
	 *        * Positions:
	 *          The pointcloud contains the 3D coordinates of the keypoints
	 *        * References:
	 *          The pointcloud contains the indices of the 3D points,
	 *          as they appear in the input pointcloud
	 *
	 * @param normalEstimationOptions.searchRadius
	 *        neighbor points within this distance of a point are used
	 *        in the computation of the surface normal at the point
	 * @param normalEstimationOptions.neighboursSetSize
	 *        how many closest neighbors to a point are used
	 *        in the computation of the surface normal at the point
	 */
	class PfhDescriptor3D : public FeaturesDescription3DInterface
	{
		public:

			PfhDescriptor3D();
			virtual ~PfhDescriptor3D();

			virtual void configure() override;
			virtual void process() override;

		private:

			//DFN Parameters
			enum OutputFormat
			{
				POSITIONS_OUTPUT,
				REFERENCES_OUTPUT
			};

			class OutputFormatHelper : public Helpers::ParameterHelper<OutputFormat, std::string>
			{
				public:
					OutputFormatHelper(const std::string& parameterName, OutputFormat& boundVariable, const OutputFormat& defaultValue);
				private:
					OutputFormat Convert(const std::string& value) override;
			};

			struct BaseOptionsSet
			{
				double searchRadius;
				OutputFormat outputFormat;
				bool enableNormalsEstimation;
				bool forceNormalsEstimation;
			};

			struct NormalEstimationOptionsSet
			{
				double searchRadius;
				int neighboursSetSize;
			};

			struct PfhOptionsSet
			{
				BaseOptionsSet baseOptions;
				NormalEstimationOptionsSet normalEstimationOptions;
			};

			Helpers::ParametersListHelper parametersHelper;
			PfhOptionsSet parameters;
			static const PfhOptionsSet DEFAULT_PARAMETERS;

			//External conversion helpers
			Converters::PointCloudToPclPointCloudConverter pointCloudToPclPointCloud;
			Converters::PointCloudToPclNormalsCloudConverter pointCloudToPclNormalsCloud;

			//Type conversion methods
			pcl::IndicesConstPtr Convert(const VisualPointFeatureVector3DWrapper::VisualPointFeatureVector3DConstPtr featuresVector);
			VisualPointFeatureVector3DWrapper::VisualPointFeatureVector3DConstPtr Convert(
				const pcl::PointCloud<pcl::PointXYZ>::ConstPtr inputCloud,
				const pcl::IndicesConstPtr indicesList,
				const pcl::PointCloud<pcl::PFHSignature125>::ConstPtr pfhPointCloud
			);

			//Core computation methods
			pcl::PointCloud<pcl::PFHSignature125>::ConstPtr ComputePfhDescriptors(
				pcl::PointCloud<pcl::PointXYZ>::ConstPtr pointCloud,
				pcl::IndicesConstPtr indicesList,
				pcl::PointCloud<pcl::Normal>::ConstPtr optionalNormalsCloud
			);
			pcl::PointCloud<pcl::Normal>::ConstPtr EstimateNormals(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pointCloud, pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree);

			//Input Validation methods
			void ValidateParameters();
			void ValidateMandatoryInputs(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pointCloud, pcl::IndicesConstPtr indicesList);
			bool IsNormalsCloudValid(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pointCloud, pcl::PointCloud<pcl::Normal>::ConstPtr normalsCloud);
	};
}
}
}

#endif // FEATURESDESCRIPTION3D_PfhDescriptor3D_HPP

/** @} */


#include <val_matrix.h>
#include <boost/python.hpp>
#include <boost/numeric/ublas/matrix.hpp>

using namespace boost::python;
using namespace boost::numeric::ublas;

void export_val_matrix()
{
  class_<Val_Matrix, bases< Value >, boost::shared_ptr<Val_Matrix> >
        ("Val_Matrix", init<uint64_t &,uint64_t &>())
    .def("toConfString", &Val_Matrix::toConfString)
	.def("insert", &Val_Matrix::insert)
	.def("erase", &Val_Matrix::erase)
    .def("mk2",  &Val_Matrix::mk2)
    .staticmethod("mk2")
  ;
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ored/marketdata/fxtriangulation.hpp>
#include <oret/toplevelfixture.hpp>
#include <ql/quotes/simplequote.hpp>

using namespace ore::data;
using namespace QuantLib;
using namespace std;

using ore::test::TopLevelFixture;

namespace {

// Test data from ECB as of 8 Jan 2016
vector<pair<string, Real>> data() {

    // clang-format off
    vector<pair<string, Real>> testData{
        { "EURUSD", 1.0861 },
        { "EURJPY", 128.51 },
        { "EURCZK", 27.022 },
        { "EURDKK", 7.4598 },
        { "EURGBP", 0.74519 },
        { "EURHUF", 315.53 },
        { "EURPLN", 4.3523 },
        { "EURSEK", 9.2640 },
        { "EURCHF", 1.0860 },
        { "EURNOK", 9.6810 },
        { "EURAUD", 1.5495 },
        { "ZZZEUR", 3.141 }, // just to test reverse quotes
        { "AUDNZD", 1.0616327848 } // Should be enough for USDNZD (value = 1.645 / 1.5495)
    };
    // clang-format on

    return testData;
}

// Provide the FXTriangulation object for the tests
class FxTriFixture : public TopLevelFixture {
public:
    FXTriangulation fx;

    FxTriFixture() {
        // Initialise FX data
        for (const auto& p : data()) {
            Handle<Quote> q(boost::make_shared<SimpleQuote>(p.second));
            fx.addQuote(p.first, q);
        }
    }

    ~FxTriFixture() {}
};

} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, TopLevelFixture)

BOOST_FIXTURE_TEST_SUITE(FXTriangulationTests, FxTriFixture)

BOOST_AUTO_TEST_CASE(testDataLoaded) {
    for (const auto& p : data()) {
        BOOST_CHECK_EQUAL(fx.getQuote(p.first)->value(), p.second);
    }
}

BOOST_AUTO_TEST_CASE(testUnity) {
    BOOST_CHECK_EQUAL(fx.getQuote("EUREUR")->value(), 1.0);
    BOOST_CHECK_EQUAL(fx.getQuote("USDUSD")->value(), 1.0);
}

BOOST_AUTO_TEST_CASE(testValues) {

    // Tolerance for comparisons
    Real tol = 1e-12;

    // Check inverse
    BOOST_CHECK_CLOSE(fx.getQuote("USDEUR")->value(), 1.0 / 1.0861, tol);
    BOOST_CHECK_CLOSE(fx.getQuote("JPYEUR")->value(), 1.0 / 128.51, tol);

    // Check Triangulation
    BOOST_CHECK_CLOSE(fx.getQuote("USDJPY")->value(), 128.51 / 1.0861, tol);
    BOOST_CHECK_CLOSE(fx.getQuote("JPYUSD")->value(), 1.0861 / 128.51, tol);
    BOOST_CHECK_CLOSE(fx.getQuote("USDGBP")->value(), 0.74519 / 1.0861, tol);
    BOOST_CHECK_CLOSE(fx.getQuote("GBPUSD")->value(), 1.0861 / 0.74519, tol);
    BOOST_CHECK_CLOSE(fx.getQuote("NOKSEK")->value(), 9.2640 / 9.6810, tol);

    // Check Triangulation where the EUR quote is reversed
    BOOST_CHECK_CLOSE(fx.getQuote("ZZZUSD")->value(), 3.141 * 1.0861, tol);
    BOOST_CHECK_CLOSE(fx.getQuote("USDZZZ")->value(), 1 / (3.141 * 1.0861), tol);
}

BOOST_AUTO_TEST_CASE(testMoreThanOneStep) {

    // Larger tolerance for multiple steps
    Real tol = 1e-8;

    // Check that we don't handle more than one step
    // EURUSD + EURAUD + AUDNZD => USDNZD
    BOOST_CHECK_THROW(fx.getQuote("USDNZD"), QuantLib::Error);
    // but if we cache EURNZD first....
    BOOST_CHECK_CLOSE(fx.getQuote("EURNZD")->value(), 1.6450, tol);
    // then we should be able to get it with one step
    BOOST_CHECK_CLOSE(fx.getQuote("USDNZD")->value(), 1.6450 / 1.0861, tol);
}

BOOST_AUTO_TEST_CASE(testBadInputsThrow) {
    BOOST_CHECK_THROW(fx.getQuote("BadInput"), QuantLib::Error);
    BOOST_CHECK_THROW(fx.getQuote(""), QuantLib::Error);
    BOOST_CHECK_THROW(fx.getQuote("MXNZAR"), QuantLib::Error);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#include <boost/heap/d_ary_heap.hpp>


/*
 * #%L
 * OME-FILES C++ library for image IO.
 * Copyright © 2016 Open Microscopy Environment:
 *   - Massachusetts Institute of Technology
 *   - National Institutes of Health
 *   - University of Dundee
 *   - Board of Regents of the University of Wisconsin-Madison
 *   - Glencoe Software, Inc.
 * %%
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. 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.
 *
 * 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 HOLDERS 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.
 *
 * The views and conclusions contained in the software and documentation are
 * those of the authors and should not be interpreted as representing official
 * policies, either expressed or implied, of any organization.
 * #L%
 */

#include <iostream>

#include <boost/units/io.hpp>
#include <ome/common/units/length.h>

#include <ome/xml/model/enums/UnitsLength.h>
#include <ome/xml/model/primitives/Quantity.h>

using namespace ome::common::units;
using ome::xml::model::primitives::convert;
using ome::xml::model::primitives::Quantity;
using ome::xml::model::enums::UnitsLength;

namespace
{

  void
  basicUnits()
  {
    std::cout << "Basic unit usage\n";

    /* basic-example-start */
    // Micrometre units.
    micrometre_quantity a(micrometre_quantity::from_value(50.0));
    micrometre_quantity b(micrometre_quantity::from_value(25.3));
    std::cout << "a=" << a << '\n';
    std::cout << "b=" << b << '\n';

    // Arithmetic operations.
    micrometre_quantity c(a + b);
    micrometre_quantity d(a * 8.0);
    micrometre_quantity e(a / 4.0);
    std::cout << "c=" << c << '\n';
    std::cout << "d=" << d << '\n';
    std::cout << "e=" << e << '\n';

    // Unit conversion to SI and Imperial units.
    nanometre_quantity f(c);
    thou_quantity g(c);
    inch_quantity h(c);
    std::cout << "f=" << f << '\n';
    std::cout << "g=" << g << '\n';
    std::cout << "h=" << h << '\n';

    // Unit sytems which do not permit interconversion.
    pixel_quantity i(pixel_quantity::from_value(34.8));
    reference_frame_quantity j(reference_frame_quantity::from_value(2.922));
    std::cout << "i=" << i << '\n';
    std::cout << "j=" << j << '\n';

    // Compilation will fail if uncommented since conversion is
    // impossible.
    // micrometre_quantity k(i);
    // micrometre_quantity l(j);
    /* basic-example-end */
  }

  void
  modelUnits()
  {
    std::cout << "Model unit usage\n";

    /* model-example-start */
    typedef Quantity<UnitsLength, double> Length;

    // Micrometre units.
    Length a(50.0, UnitsLength::MICROMETER);
    Length b(25.3, UnitsLength::MICROMETER);
    std::cout << "a=" << a << '\n';
    std::cout << "b=" << b << '\n';

    // Arithmetic operations.
    Length c(a + b);
    Length d(a * 8.0);
    Length e(a / 4.0);
    std::cout << "c=" << c << '\n';
    std::cout << "d=" << d << '\n';
    std::cout << "e=" << e << '\n';

    // Unit conversion to SI and Imperial units.
    Length f(convert(c, UnitsLength::NANOMETER));
    Length g(convert(c, UnitsLength::THOU));
    Length h(convert(c, UnitsLength::INCH));
    std::cout << "f=" << f << '\n';
    std::cout << "g=" << g << '\n';
    std::cout << "h=" << h << '\n';

    // Unit sytems which do not permit interconversion.
    Length i(34.8, UnitsLength::PIXEL);
    Length j(2.922, UnitsLength::REFERENCEFRAME);
    std::cout << "i=" << i << '\n';
    std::cout << "j=" << j << '\n';

    // Will throw since conversion is impossible.
    try
      {
        Length k(convert(i, UnitsLength::MICROMETER));
      }
    catch (const std::logic_error&)
      {
        std::cout << "i not convertible to µm\n";
      }
    try
      {
        Length l(convert(j, UnitsLength::MICROMETER));
      }
    catch (const std::logic_error&)
      {
        std::cout << "j not convertible to µm\n";
      }
    /* model-example-end */
  }
}

int
main()
{
  basicUnits();
  modelUnits();
}


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. 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 ANU 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 ANU OR THE 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.                                                               *
 ******************************************************************************/

#ifndef OPENGV_ABSOLUTE_POSE_MODULES_GPNP4_MODULES_HPP_
#define OPENGV_ABSOLUTE_POSE_MODULES_GPNP4_MODULES_HPP_

#include <stdlib.h>
#include <Eigen/Eigen>
#include <vector>

namespace opengv
{
namespace absolute_pose
{
namespace modules
{
namespace gpnp4
{

void init(
    Eigen::Matrix<double,25,37> & groebnerMatrix,
    const Eigen::Matrix<double,12,1> & a,
    Eigen::Matrix<double,12,1> & n,
    Eigen::Matrix<double,12,1> & m,
    Eigen::Matrix<double,12,1> & k,
    Eigen::Matrix<double,12,1> & l,
    Eigen::Vector3d & c0,
    Eigen::Vector3d & c1,
    Eigen::Vector3d & c2,
    Eigen::Vector3d & c3 );
void compute( Eigen::Matrix<double,25,37> & groebnerMatrix );
void sPolynomial5( Eigen::Matrix<double,25,37> & groebnerMatrix );
void sPolynomial6( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow5_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial7( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow6_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial8( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow7_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial9( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow7_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow8_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow5_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow6_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow7_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow8_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow8_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial10( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow6_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow9_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial11( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow4_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow10_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow4_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial12( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow5_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow11_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial13( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow6_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow4_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow12_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial14( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow5_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow13_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial15( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow14_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow14_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial16( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow13_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow15_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow15_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow15_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial17( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow12_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow16_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow16_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial18( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow14_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow14_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow17_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow17_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial19( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow18_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial20( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow18_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial21( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow20_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial22( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow19_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0001_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_0010_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow20_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial23( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow20_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow21_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_1100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow19_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_0100_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_1000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void groebnerRow22_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );
void sPolynomial24( Eigen::Matrix<double,25,37> & groebnerMatrix );
void groebnerRow23_0000_f( Eigen::Matrix<double,25,37> & groebnerMatrix, int targetRow );

}
}
}
}

#endif /* OPENGV_ABSOLUTE_POSE_MODULES_GPNP4_MODULES_HPP_ */