/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// includes
// std
#include <fstream>
#include <iostream>
#include <tuple>

// boost
#define BOOST_TEST_MODULE TasksTest
#include <boost/math/constants/constants.hpp>
#include <boost/test/unit_test.hpp>

// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>

// RBDyn
#include <RBDyn/EulerIntegration.h>
#include <RBDyn/FK.h>
#include <RBDyn/FV.h>

// Tasks
#include "Tasks/Tasks.h"

// Arms
#include "arms.h"

template<typename Task>
struct TanAccel
{
  Eigen::VectorXd tanAcc(Task & task, const std::vector<Eigen::VectorXd> & alphaD)
  {
    return Eigen::VectorXd(task.jac() * alphaD[0]);
  }
};

template<typename Task>
struct MRTanAccel
{
  MRTanAccel(int taskDim) : tanAccV(taskDim) {}

  Eigen::VectorXd tanAcc(Task & task, const std::vector<Eigen::VectorXd> & alphaD)
  {
    tanAccV.setZero();
    for(std::size_t i = 0; i < alphaD.size(); ++i)
    {
      tanAccV += task.jac(int(i)) * alphaD[i];
    }
    return tanAccV;
  }

  Eigen::VectorXd tanAccV;
};

/// run the task update(mb, mbc) method
template<typename Task>
struct ClassicUpdater : public TanAccel<Task>
{
  void operator()(Task & task, const std::vector<rbd::MultiBody> & mbs, const std::vector<rbd::MultiBodyConfig> & mbcs)
  {
    task.update(mbs[0], mbcs[0]);
  }
};

/// run the task update(mbs, mbcs) method
template<typename Task>
struct MRClassicUpdater : public MRTanAccel<Task>
{
  MRClassicUpdater(int taskDim) : MRTanAccel<Task>(taskDim) {}

  void operator()(Task & task, const std::vector<rbd::MultiBody> & mbs, const std::vector<rbd::MultiBodyConfig> & mbcs)
  {
    task.update(mbs, mbcs);
  }
};

/// Compute normal acceleration (like QPSolverData::computeNormalAccB)
void computeNormalAccB(const rbd::MultiBody & mb,
                       const rbd::MultiBodyConfig & mbc,
                       std::vector<sva::MotionVecd> & normalAccB)
{
  const std::vector<int> & pred = mb.predecessors();
  const std::vector<int> & succ = mb.successors();

  for(int i = 0; i < mb.nrJoints(); ++i)
  {
    const sva::PTransformd & X_p_i = mbc.parentToSon[i];
    const sva::MotionVecd & vj_i = mbc.jointVelocity[i];
    const sva::MotionVecd & vb_i = mbc.bodyVelB[i];

    if(pred[i] != -1)
      normalAccB[succ[i]] = X_p_i * normalAccB[pred[i]] + vb_i.cross(vj_i);
    else
      normalAccB[succ[i]] = vb_i.cross(vj_i);
  }
}

/// Multi-robot version of computeNormalAccB
void computeNormalAccB(const std::vector<rbd::MultiBody> & mbs,
                       const std::vector<rbd::MultiBodyConfig> & mbcs,
                       std::vector<std::vector<sva::MotionVecd>> & normalAccB)
{
  for(std::size_t i = 0; i < mbs.size(); ++i)
  {
    computeNormalAccB(mbs[i], mbcs[i], normalAccB[i]);
  }
}

/// run the task update(mb, mbc, bodyNormalAcc) method
template<typename Task>
struct NormalAccUpdater : public TanAccel<Task>
{
  NormalAccUpdater(const rbd::MultiBody & mb) : normalAccB(mb.nrBodies()) {}

  void operator()(Task & task, const std::vector<rbd::MultiBody> & mbs, const std::vector<rbd::MultiBodyConfig> & mbcs)
  {
    computeNormalAccB(mbs[0], mbcs[0], normalAccB);
    task.update(mbs[0], mbcs[0], normalAccB);
  }

  std::vector<sva::MotionVecd> normalAccB;
};

/// run the task update(mbs, mbcs, bodyNormalAccs) method
template<typename Task>
struct MRNormalAccUpdater : public MRTanAccel<Task>
{
  MRNormalAccUpdater(const std::vector<rbd::MultiBody> & mbs, int taskDim)
  : MRTanAccel<Task>(taskDim), normalAccBs(mbs.size())
  {
    for(std::size_t i = 0; i < mbs.size(); ++i)
    {
      normalAccBs[i].resize(mbs[i].nrBodies());
    }
  }

  void operator()(Task & task, const std::vector<rbd::MultiBody> & mbs, const std::vector<rbd::MultiBodyConfig> & mbcs)
  {
    computeNormalAccB(mbs, mbcs, normalAccBs);
    task.update(mbs, mbcs, normalAccBs);
  }

  std::vector<std::vector<sva::MotionVecd>> normalAccBs;
};

/// run the task update(mb, mbc, com, bodyNormalAcc) method
template<typename Task>
struct NormalAccCoMUpdater : public TanAccel<Task>
{
  NormalAccCoMUpdater(const rbd::MultiBody & mb) : normalAccB(mb.nrBodies()) {}

  void operator()(Task & task, const std::vector<rbd::MultiBody> & mbs, const std::vector<rbd::MultiBodyConfig> & mbcs)
  {
    computeNormalAccB(mbs[0], mbcs[0], normalAccB);
    Eigen::Vector3d com = rbd::computeCoM(mbs[0], mbcs[0]);
    task.update(mbs[0], mbcs[0], com, normalAccB);
  }

  std::vector<sva::MotionVecd> normalAccB;
};

/// run the task update(mbs, mbcs, coms, bodyNormalAccs) method
template<typename Task>
struct MRNormalAccCoMUpdater : public MRTanAccel<Task>
{
  MRNormalAccCoMUpdater(const std::vector<rbd::MultiBody> & mbs, int taskDim)
  : MRTanAccel<Task>(taskDim), normalAccBs(mbs.size()), coms(mbs.size())
  {
    for(std::size_t i = 0; i < mbs.size(); ++i)
    {
      normalAccBs[i].resize(mbs[i].nrBodies());
    }
  }

  void operator()(Task & task, const std::vector<rbd::MultiBody> & mbs, const std::vector<rbd::MultiBodyConfig> & mbcs)
  {
    computeNormalAccB(mbs, mbcs, normalAccBs);
    for(std::size_t i = 0; i < mbs.size(); ++i)
    {
      coms[i] = rbd::computeCoM(mbs[i], mbcs[i]);
    }
    task.update(mbs, mbcs, coms, normalAccBs);
  }

  std::vector<std::vector<sva::MotionVecd>> normalAccBs;
  std::vector<Eigen::Vector3d> coms;
};

/// Test position task (eval, speed and acc are defined)
struct PosTester
{
  void operator()(const Eigen::VectorXd & speedCur,
                  const Eigen::VectorXd & accCur,
                  const Eigen::VectorXd & speedDiff,
                  const Eigen::VectorXd & accDiff,
                  double tol)
  {
    BOOST_CHECK_SMALL((speedCur - speedDiff).norm(), tol);
    BOOST_CHECK_SMALL((accCur - accDiff).norm(), tol);
  }
};

/// Test TransformTask, position is reliable but only rotational acceleration
/// is reliable
struct PosTTTester
{
  void operator()(const Eigen::VectorXd & speedCur,
                  const Eigen::VectorXd & accCur,
                  const Eigen::VectorXd & speedDiff,
                  const Eigen::VectorXd & accDiff,
                  double tol)
  {
    BOOST_CHECK_SMALL((speedCur.tail<3>() - speedDiff.tail<3>()).norm(), tol);
    BOOST_CHECK_SMALL((accCur - accDiff).norm(), tol);
  }
};

/// Test MultiRobotTransformTask, the rotation part is not reliable since
/// he use rotationVelocity
struct PosMRTTTester
{
  void operator()(const Eigen::VectorXd & speedCur,
                  const Eigen::VectorXd & accCur,
                  const Eigen::VectorXd & speedDiff,
                  const Eigen::VectorXd & accDiff,
                  double tol)
  {
    BOOST_CHECK_SMALL((speedCur.tail<3>() - speedDiff.tail<3>()).norm(), tol);
    BOOST_CHECK_SMALL((accCur.tail<3>() - accDiff.tail<3>()).norm(), tol);
  }
};

/// Test position task with orientation error (speedDiff is not realiable)
struct OriTaskTester
{
  void operator()(const Eigen::VectorXd & /* speedCur */,
                  const Eigen::VectorXd & accCur,
                  const Eigen::VectorXd & /* speedDiff */,
                  const Eigen::VectorXd & accDiff,
                  double tol)
  {
    BOOST_CHECK_SMALL((accCur - accDiff).norm(), tol);
  }
};

/// Test velocity task (eval, and acc are defined)
struct VelTester
{
  void operator()(const Eigen::VectorXd & /* speedCur */,
                  const Eigen::VectorXd & accCur,
                  const Eigen::VectorXd & speedDiff,
                  const Eigen::VectorXd & /* accDiff */,
                  double tol)
  {
    BOOST_CHECK_SMALL((accCur - speedDiff).norm(), tol);
  }
};

/// Test position task (eval, speed and acc are defined)
struct VectOriTester
{
  void operator()(const Eigen::VectorXd & speedCur,
                  const Eigen::VectorXd & accCur,
                  const Eigen::VectorXd & speedDiff,
                  const Eigen::VectorXd & accDiff,
                  double tol)
  {
    BOOST_CHECK_SMALL((speedCur - speedDiff).norm(), tol);
    BOOST_CHECK_SMALL((accCur - accDiff).norm(), tol);
  }
};

/**
 * Use finite difference to test a task.
 * Task is the task to test, Updater is a function to call the Task::update
 * method, Tester test the task speed and acceleration against the finite
 * difference speed and accelartion.
 */
template<typename Task, typename Updater, typename Tester>
void testTaskNumDiff(const std::vector<rbd::MultiBody> & mbs,
                     const std::vector<rbd::MultiBodyConfig> & mbcs,
                     Task & task,
                     Updater updater,
                     Tester tester,
                     int nrIter = 100,
                     double diffStep = 1e-6,
                     double tol = 1e-4)
{
  using namespace Eigen;
  using namespace sva;
  using namespace rbd;

  std::vector<MultiBodyConfig> mbcsPost(mbcs), mbcsCur(mbcs);

  std::vector<Eigen::VectorXd> q(mbs.size());
  std::vector<Eigen::VectorXd> alpha(mbs.size());
  std::vector<Eigen::VectorXd> alphaD(mbs.size());

  for(int i = 0; i < nrIter; ++i)
  {
    for(std::size_t r = 0; r < mbs.size(); ++r)
    {
      const rbd::MultiBody & mb = mbs[r];
      const rbd::MultiBodyConfig & mbc = mbcs[r];
      rbd::MultiBodyConfig & mbcPost = mbcsPost[r];
      rbd::MultiBodyConfig & mbcCur = mbcsCur[r];

      q[r].setRandom(mb.nrParams());
      alpha[r].setRandom(mb.nrDof());
      alphaD[r].setRandom(mb.nrDof());

      mbcCur = mbc;
      vectorToParam(q[r], mbcCur.q);
      vectorToParam(alpha[r], mbcCur.alpha);
      vectorToParam(alphaD[r], mbcCur.alphaD);

      mbcPost = mbcCur;

      eulerIntegration(mb, mbcPost, diffStep);

      forwardKinematics(mb, mbcCur);
      forwardKinematics(mb, mbcPost);
      forwardVelocity(mb, mbcCur);
      forwardVelocity(mb, mbcPost);
    }

    updater(task, mbs, mbcsCur);
    VectorXd evalCur = task.eval();
    VectorXd speedCur = -task.speed();
    VectorXd accCur = -task.normalAcc();
    accCur -= updater.tanAcc(task, alphaD);

    updater(task, mbs, mbcsPost);
    VectorXd evalPost = task.eval();
    VectorXd speedPost = -task.speed();

    VectorXd speedDiff = (evalPost - evalCur) / diffStep;
    VectorXd accDiff = (speedPost - speedCur) / diffStep;

    tester(speedCur, accCur, speedDiff, accDiff, tol);
  }
}

template<typename Task, typename Updater, typename Tester>
void testTaskNumDiff(const rbd::MultiBody & mb,
                     const rbd::MultiBodyConfig & mbc,
                     Task & task,
                     Updater updater,
                     Tester tester,
                     int nrIter = 100,
                     double diffStep = 1e-6,
                     double tol = 1e-4)
{
  testTaskNumDiff(std::vector<rbd::MultiBody>{mb}, std::vector<rbd::MultiBodyConfig>{mbc}, task, updater, tester,
                  nrIter, diffStep, tol);
}

BOOST_AUTO_TEST_CASE(PositionTaskTest)
{
  using namespace Eigen;
  using namespace rbd;

  MultiBody mb;
  MultiBodyConfig mbc;

  std::tie(mb, mbc) = makeZXZArm();

  tasks::PositionTask pt(mb, "b3", Vector3d::Random(), Vector3d::Random());

  testTaskNumDiff(mb, mbc, pt, ClassicUpdater<tasks::PositionTask>(), PosTester());
  testTaskNumDiff(mb, mbc, pt, NormalAccUpdater<tasks::PositionTask>(mb), PosTester());
}

BOOST_AUTO_TEST_CASE(OrientationTaskTest)
{
  using namespace Eigen;
  using namespace rbd;

  MultiBody mb;
  MultiBodyConfig mbc;

  std::tie(mb, mbc) = makeZXZArm();

  tasks::OrientationTask ot(mb, "b3", Quaterniond(Vector4d::Random().normalized()));

  testTaskNumDiff(mb, mbc, ot, ClassicUpdater<tasks::OrientationTask>(), OriTaskTester());
  testTaskNumDiff(mb, mbc, ot, NormalAccUpdater<tasks::OrientationTask>(mb), OriTaskTester());
}

BOOST_AUTO_TEST_CASE(TransformTaskTest)
{
  using namespace Eigen;
  using namespace rbd;

  MultiBody mb;
  MultiBodyConfig mbc;

  std::tie(mb, mbc) = makeZXZArm();

  sva::PTransformd X_b_s(Quaterniond(Vector4d::Random().normalized()), Vector3d::Random());
  sva::PTransformd X_0_t(Quaterniond(Vector4d::Random().normalized()), Vector3d::Random());
  Eigen::Quaterniond E_0_c(Vector4d::Random().normalized());

  tasks::TransformTask tt(mb, "b3", X_0_t, X_b_s, E_0_c.matrix());

  testTaskNumDiff(mb, mbc, tt, NormalAccUpdater<tasks::TransformTask>(mb), PosTTTester(), 100);
}

BOOST_AUTO_TEST_CASE(SurfaceTransformTaskTest)
{
  using namespace Eigen;
  using namespace rbd;

  MultiBody mb;
  MultiBodyConfig mbc;

  std::tie(mb, mbc) = makeZXZArm();

  sva::PTransformd X_b_s(Quaterniond(Vector4d::Random().normalized()), Vector3d::Random());
  sva::PTransformd X_0_t(Quaterniond(Vector4d::Random().normalized()), Vector3d::Random());

  tasks::SurfaceTransformTask tt(mb, "b3", X_0_t, X_b_s);

  testTaskNumDiff(mb, mbc, tt, NormalAccUpdater<tasks::SurfaceTransformTask>(mb), PosMRTTTester(), 100);
}

BOOST_AUTO_TEST_CASE(MultiRobotTransformTaskTest)
{
  using namespace Eigen;
  using namespace rbd;

  MultiBody mb1, mb2;
  MultiBodyConfig mbc1, mbc2;

  std::tie(mb1, mbc1) = makeZXZArm();
  std::tie(mb2, mbc2) = makeZXZArm();

  std::vector<MultiBody> mbs{mb1, mb2};
  std::vector<MultiBodyConfig> mbcs{mbc1, mbc2};

  sva::PTransformd X_r1b_r1s(Quaterniond(Vector4d::Random().normalized()), Vector3d::Random());
  sva::PTransformd X_r2b_r2s(Quaterniond(Vector4d::Random().normalized()), Vector3d::Random());

  tasks::MultiRobotTransformTask mrtt(mbs, 0, 1, "b3", "b3", X_r1b_r1s, X_r2b_r2s);

  testTaskNumDiff(mbs, mbcs, mrtt, MRNormalAccUpdater<tasks::MultiRobotTransformTask>(mbs, 6), PosMRTTTester());
}

BOOST_AUTO_TEST_CASE(SurfaceOrientationTaskTest)
{
  using namespace Eigen;
  using namespace rbd;

  MultiBody mb;
  MultiBodyConfig mbc;

  std::tie(mb, mbc) = makeZXZArm();

  tasks::SurfaceOrientationTask sot(mb, "b3", Quaterniond(Vector4d::Random().normalized()),
                                    sva::PTransformd(Quaterniond(Vector4d::Random().normalized()), Vector3d::Random()));

  testTaskNumDiff(mb, mbc, sot, ClassicUpdater<tasks::SurfaceOrientationTask>(), OriTaskTester());
  testTaskNumDiff(mb, mbc, sot, NormalAccUpdater<tasks::SurfaceOrientationTask>(mb), OriTaskTester());
}

BOOST_AUTO_TEST_CASE(CoMTaskTest)
{
  using namespace Eigen;
  using namespace rbd;

  MultiBody mb;
  MultiBodyConfig mbc;

  std::tie(mb, mbc) = makeZXZArm();

  tasks::CoMTask ct(mb, Vector3d::Random());

  testTaskNumDiff(mb, mbc, ct, ClassicUpdater<tasks::CoMTask>(), PosTester());
  testTaskNumDiff(mb, mbc, ct, NormalAccCoMUpdater<tasks::CoMTask>(mb), PosTester());
}

BOOST_AUTO_TEST_CASE(MultiCoMTaskTest)
{
  using namespace Eigen;
  using namespace rbd;

  MultiBody mb1, mb2;
  MultiBodyConfig mbc1, mbc2;

  std::tie(mb1, mbc1) = makeZXZArm();
  std::tie(mb2, mbc2) = makeZXZArm();

  std::vector<MultiBody> mbs{mb1, mb2};
  std::vector<MultiBodyConfig> mbcs{mbc1, mbc2};

  tasks::MultiCoMTask mct(mbs, {0, 1}, Vector3d::Random());

  testTaskNumDiff(mbs, mbcs, mct, MRClassicUpdater<tasks::MultiCoMTask>(3), PosTester());
  testTaskNumDiff(mbs, mbcs, mct, MRNormalAccUpdater<tasks::MultiCoMTask>(mbs, 3), PosTester());
  testTaskNumDiff(mbs, mbcs, mct, MRNormalAccCoMUpdater<tasks::MultiCoMTask>(mbs, 3), PosTester());
}

BOOST_AUTO_TEST_CASE(MomentumTaskTest)
{
  using namespace Eigen;
  using namespace rbd;

  MultiBody mb;
  MultiBodyConfig mbc;

  std::tie(mb, mbc) = makeZXZArm();

  tasks::MomentumTask mt(mb, sva::ForceVecd(Vector6d::Random()));

  testTaskNumDiff(mb, mbc, mt, ClassicUpdater<tasks::MomentumTask>(), VelTester());
  testTaskNumDiff(mb, mbc, mt, NormalAccUpdater<tasks::MomentumTask>(mb), VelTester());
}

BOOST_AUTO_TEST_CASE(LinVelocityTaskTest)
{
  using namespace Eigen;
  using namespace rbd;

  MultiBody mb;
  MultiBodyConfig mbc;

  std::tie(mb, mbc) = makeZXZArm();

  tasks::LinVelocityTask lvt(mb, "b3", Vector3d::Random());

  testTaskNumDiff(mb, mbc, lvt, ClassicUpdater<tasks::LinVelocityTask>(), VelTester());
  testTaskNumDiff(mb, mbc, lvt, NormalAccUpdater<tasks::LinVelocityTask>(mb), VelTester());
}

BOOST_AUTO_TEST_CASE(VectorOrientationTaskTest)
{
  using namespace Eigen;
  using namespace rbd;

  MultiBody mb;
  MultiBodyConfig mbc;

  std::tie(mb, mbc) = makeZXZArm();

  tasks::VectorOrientationTask vot(mb, "b3", Vector3d::Random(), Vector3d::Random());

  testTaskNumDiff(mb, mbc, vot, NormalAccUpdater<tasks::VectorOrientationTask>(mb), VectOriTester());
}


#ifndef JHMI_LIVER_CONSTANTS_HPP_NRC_20150903
#define JHMI_LIVER_CONSTANTS_HPP_NRC_20150903

#include "utility/units.hpp"
#include <boost/math/constants/constants.hpp>

namespace jhmi {
//  using gamma = boost::units::static_rational<27, 10>::type;
//  using inv_gamma = boost::units::static_rational<gamma::Denominator, gamma::Numerator>::type;

  static double const pi = boost::math::double_constants::pi;
  static Pa const input_pressure = 98_mmHg;//1999 Bezy-Wendling
#if 0
#if 0
  inline Pa_s blood_viscosity(m radius) {
    return std::min(4. * pascals * seconds, Pa_s::from_value(1.8 * .6913 * (220 * std::exp(-1.3 * 1e6 * 2. * radius.value())
      + 3.2 - 2.44 * std::exp(-.06 * std::pow(1e6 * 2. * radius.value(), .645)))));
  }
#else
  inline Pa_s blood_viscosity(m) {
    return 3.5e-3 * pascals * seconds;
  }
#endif
#endif
  static Pa_s const blood_viscosity = 3.5e-3 * pascals * seconds;//Fung
  //static Pa const cell_pressure = 25_mmHg;//volmar
  //static auto const proper_ha_flow = 400. * mL / minutes;//1999 Bezy-Wendling
}
#endif


#ifndef INCLUDE_PARAMS_HPP_
#define INCLUDE_PARAMS_HPP_

#include <Eigen/Dense>
#include <iostream>
#include "mc_weights.h"
#include "mc_bias.h"
#include "mc_mean.h"
#include "mc_std.h"
using namespace Eigen;

class weights
{
public:
	Eigen::Matrix<float,WEIGHTS_1_ROWS, WEIGHTS_1_COLS> layer_1_weight;
	Eigen::Matrix<float,WEIGHTS_2_ROWS, WEIGHTS_2_COLS> layer_2_weight;
	Eigen::Matrix<float,WEIGHTS_3_ROWS, WEIGHTS_3_COLS> layer_3_weight;

	Eigen::Matrix<float,BIAS_1_ROWS, 1> bias_1;
	Eigen::Matrix<float,BIAS_2_ROWS, 1> bias_2;
	Eigen::Matrix<float,BIAS_3_ROWS, 1> bias_3;

	Eigen::Matrix<float,MEAN_ROWS,1> mean;
	Eigen::Matrix<float,STD_ROWS,1> std;
	// Eigen::VectorXf mean(6);
	// Eigen::VectorXf std(6);

	weights (){
		layer_1_weight << WEIGHTS_1;
		layer_2_weight << WEIGHTS_2;
		layer_3_weight << WEIGHTS_3;

		bias_1 << BIAS_1;
		bias_2 << BIAS_2;
		bias_3 << BIAS_3;

		mean << MEAN;
		std << STD;
	}
};

#endif


#include "descriptor.h"
#include "structure.h"
#include "squared_exponential.h"
#include "three_body.h"
#include "two_body.h"
#include "gtest/gtest.h"
#include <Eigen/Dense>
#include <chrono>
#include <cmath>
#include <iostream>
#include <stdlib.h>

class NBodyTest : public ::testing::Test {
public:
  int n_atoms = 5;
  int n_species = 3;
  Eigen::MatrixXd cell;
  std::vector<int> species;
  Eigen::MatrixXd positions;

  std::string cutoff_name = "cosine";
  std::vector<double> cutoff_hyps;

  TwoBody desc;
  ThreeBody three_body_desc;
  std::vector<Descriptor *> dc;
  Structure test_struc;
  DescriptorValues struc_desc;
  ClusterDescriptor cluster_desc;

  double sigma = 2.0;
  double ls = 1.0;
  SquaredExponential kernel;

  double cell_size = 10;
  double cutoff = cell_size / 2;

  NBodyTest() {
    // Make positions.
    cell = Eigen::MatrixXd::Identity(3, 3) * cell_size;
    // positions = Eigen::MatrixXd::Random(n_atoms, 3) * cell_size / 2;
    positions = Eigen::MatrixXd::Random(n_atoms, 3);
    // positions << 0, 0, 0, 0, 0, 1;

    // Make random species.
    for (int i = 0; i < n_atoms; i++) {
      species.push_back(rand() % n_species);
    }

    desc = TwoBody(cutoff, n_species, cutoff_name, cutoff_hyps);
    three_body_desc = ThreeBody(cutoff, n_species, cutoff_name, cutoff_hyps);

    kernel = SquaredExponential(sigma, ls);
  }
};

TEST_F(NBodyTest, TwoBodyTest) {
  dc.push_back(&desc);
  test_struc = Structure(cell, species, positions, cutoff, dc);
  struc_desc = test_struc.descriptors[0];
  cluster_desc.add_all_clusters(struc_desc);

  Eigen::MatrixXd kern_mat = kernel.envs_envs(cluster_desc, cluster_desc,
                                              kernel.kernel_hyperparameters);
  Eigen::MatrixXd envs_struc = kernel.envs_struc(cluster_desc, struc_desc,
                                                 kernel.kernel_hyperparameters);
  Eigen::MatrixXd struc_struc =
      kernel.struc_struc(struc_desc, struc_desc, kernel.kernel_hyperparameters);

  //   std::cout << kern_mat << std::endl;
  //   std::cout << envs_struc << std::endl;
}

TEST_F(NBodyTest, ThreeBodyTest) {
  dc.push_back(&three_body_desc);
  test_struc = Structure(cell, species, positions, cutoff, dc);
  struc_desc = test_struc.descriptors[0];
  cluster_desc.add_all_clusters(struc_desc);

  std::cout << test_struc.descriptors[0].n_types << std::endl;
  std::cout << test_struc.descriptors[0].n_clusters_by_type[0] << std::endl;

  Eigen::MatrixXd kern_mat = kernel.envs_envs(cluster_desc, cluster_desc,
                                              kernel.kernel_hyperparameters);
  Eigen::MatrixXd envs_struc = kernel.envs_struc(cluster_desc, struc_desc,
                                                 kernel.kernel_hyperparameters);
  Eigen::MatrixXd struc_struc =
      kernel.struc_struc(struc_desc, struc_desc, kernel.kernel_hyperparameters);

  //   std::cout << kern_mat << std::endl;
  //   std::cout << envs_struc << std::endl;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_SIDE_STRAIGHT_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_SIDE_STRAIGHT_HPP

#include <cstddef>

#include <boost/math/special_functions/fpclassify.hpp>

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

#include <boost/geometry/strategies/buffer.hpp>
#include <boost/geometry/strategies/side.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

namespace strategy { namespace buffer
{

/*!
\brief Let the buffer use straight sides along segments (the default)
\ingroup strategies
\details This strategy can be used as SideStrategy for the buffer algorithm.
    It is currently the only provided strategy for this purpose

\qbk{
[heading Example]
See the examples for other buffer strategies\, for example
[link geometry.reference.strategies.strategy_buffer_join_round join_round]
[heading See also]
\* [link geometry.reference.algorithms.buffer.buffer_7_with_strategies buffer (with strategies)]
}
 */
class side_straight
{
public :
#ifndef DOXYGEN_SHOULD_SKIP_THIS
    template
    <
        typename Point,
        typename OutputRange,
        typename DistanceStrategy
    >
    static inline result_code apply(
                Point const& input_p1, Point const& input_p2,
                buffer_side_selector side,
                DistanceStrategy const& distance,
                OutputRange& output_range)
    {
        typedef typename coordinate_type<Point>::type coordinate_type;
        typedef typename geometry::select_most_precise
        <
            coordinate_type,
            double
        >::type promoted_type;

        // Generate a block along (left or right of) the segment

        // Simulate a vector d (dx,dy)
        coordinate_type const dx = get<0>(input_p2) - get<0>(input_p1);
        coordinate_type const dy = get<1>(input_p2) - get<1>(input_p1);

        // For normalization [0,1] (=dot product d.d, sqrt)
        promoted_type const length = geometry::math::sqrt(dx * dx + dy * dy);

        if (! geofeatures_boost::math::isfinite(length))
        {
            // In case of coordinates differences of e.g. 1e300, length
            // will overflow and we should not generate output
#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_WARN
            std::cout << "Error in length calculation for points "
                << geometry::wkt(input_p1) << " " << geometry::wkt(input_p2)
                << " length: " << length << std::endl;
#endif
            return result_error_numerical;
        }

        if (geometry::math::equals(length, 0))
        {
            // Coordinates are simplified and therefore most often not equal.
            // But if simplify is skipped, or for lines with two
            // equal points, length is 0 and we cannot generate output.
            return result_no_output;
        }

        promoted_type const d = distance.apply(input_p1, input_p2, side);

        // Generate the normalized perpendicular p, to the left (ccw)
        promoted_type const px = -dy / length;
        promoted_type const py = dx / length;

        if (geometry::math::equals(px, 0)
            && geometry::math::equals(py, 0))
        {
            // This basically should not occur - because of the checks above.
            // There are no unit tests triggering this condition
#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_WARN
            std::cout << "Error in perpendicular calculation for points "
                << geometry::wkt(input_p1) << " " << geometry::wkt(input_p2)
                << " length: " << length
                << " distance: " << d
                << std::endl;
#endif
            return result_no_output;
        }

        output_range.resize(2);

        set<0>(output_range.front(), get<0>(input_p1) + px * d);
        set<1>(output_range.front(), get<1>(input_p1) + py * d);
        set<0>(output_range.back(), get<0>(input_p2) + px * d);
        set<1>(output_range.back(), get<1>(input_p2) + py * d);

        return result_normal;
    }
#endif // DOXYGEN_SHOULD_SKIP_THIS
};

}} // namespace strategy::buffer

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_SIDE_STRAIGHT_HPP


/*
Copyright 2018 Dennis Rohde

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.
*/

#pragma once

#include <cmath>
#include <limits>

#include <boost/chrono/include.hpp>

#include "knn_graph.hpp"
#include "random.hpp"
#include "oracle.hpp"

class Tester_Result{
public:
    bool decision;
    double total_time;
    double query_time;
};

template <typename V = double>
class KNN_Tester {
    typedef typename KNN_Graph<V>::vertices_type vertices_type;

protected:
    bool auto_c1;

public:
    /**
     *
     * tuning parameter c1 for psi
     *
     */
    double c1 = 1;

    /**
     *
     * tuning parameter c2 for |T|
     *
     */
    double c2 = 1;

    KNN_Tester(const bool auto_c1 = true) : auto_c1{auto_c1} {}

    /**
     *
     * Calculates approximate for c1
     * @param Number of dimensions delta
     * @return approximate for c1
     *
     */
    static double c1_approximate(const KNN_Graph<V> &graph) {
        auto delta = graph.dimension();
        return std::pow(2, 0.401 * delta * (1 + 2.85 * delta / std::pow(delta, 1.4)));
    }

    /**
     *
     * Property Testing Algorithm for k-nearest Neighborhood Graphs
     * @param KNN_Graph G, average degree of G, epsilon
     * @return true or false
     *
     */
    virtual Tester_Result test(const KNN_Graph<V> &graph, const double d, const double epsilon = 0.001) {
        if (auto_c1) this->c1 = c1_approximate(graph);
        const auto delta = graph.dimension();
        const auto k = graph.get_k();
        const auto n = graph.number_vertices();
        const auto s = std::ceil(100 * k * sqrt(n) / epsilon * c2);
        const auto t = std::ceil(log(10) * c1 * k * sqrt(n));

        Uniform_Random_Generator<double> urandom_gen;

        const auto S = urandom_gen.get(s);
        const auto T = urandom_gen.get(t);

        std::cout << "|S| = " << s << std::endl;
        std::cout << "|T| = " << t << std::endl;

        bool wrongly_connected_found = false;
        double distn, distw;

        #pragma omp parallel for shared(wrongly_connected_found, distn, distw)
        for (unsigned long long i = 0; i < S.size(); ++i) {
            if (wrongly_connected_found) continue;
            const unsigned long long v = floor(S[i] * n);
            const auto v_value = graph.get_vertex(v);
            if (graph.number_neighbors(v) > 100 * k * d / epsilon) continue;
            const auto &neighbors = graph.get_edges()[v];
            V distN = 0;
            for (const auto &neighbor: neighbors) {
                auto dist = KNN_Graph<V>::euclidean_distance(v_value, graph.get_vertex(neighbor));
                if (dist > distN) {
                    distN = dist;
                }
            }
            for (unsigned long long j = 0; j < T.size(); ++j) {
                if (wrongly_connected_found) {
                    continue;
                }
                unsigned long long w = floor(T[j] * n);
                auto dist = KNN_Graph<V>::euclidean_distance(v_value, graph.get_vertex(w));
                if (v != w and distN - dist > std::numeric_limits<V>::epsilon()) {
                    auto is_neighbor = false;
                    for (const auto &neighbor: neighbors) {
                        if (w == neighbor) {
                            is_neighbor = true;
                            break;
                        }
                    }
                    if (not is_neighbor) {
                        #pragma omp critical
                        {
                            if (not wrongly_connected_found) {
                                wrongly_connected_found = true;
                                distn = distN;
                                distw = dist;
                            }
                        }
                    }
                }
            }
        }
        Tester_Result result;
        if (wrongly_connected_found) {
            std::cout << "Reject!" << std::endl;
            std::cout << distw << " < " << distn << std::endl;
            result.decision = false;
        } else {
            std::cout << "Accept!" << std::endl;
            result.decision = true;
        }
        return result;
    }

    inline auto get_auto_c1() const {
        return auto_c1;
    }

    inline void set_auto_c1(const bool auto_c1) {
        this->auto_c1 = auto_c1;
    }
};

template <typename V = double>
class KNN_Tester_Oracle : public KNN_Tester<V> {
    Query_Oracle<V> Oracle;

public:
    KNN_Tester_Oracle(const Query_Oracle<V> &oracle) : Oracle{std::move(oracle)} {}

    /**
     *
     * Property Testing Algorithm for k-Nearest Neighborhood Graphs - Oracle Version
     * @param KNN_Graph G, average degree of G, epsilon
     * @return
     *
     */
    Tester_Result test(const KNN_Graph<V> &graph, const double d, const double epsilon = 0.001) {
        auto start = boost::chrono::process_real_cpu_clock::now();
        Oracle.reset_timer();
        if (this->auto_c1) this->c1 = this->c1_approximate(graph);
        const auto delta = graph.dimension();
        const auto k = graph.get_k();
        const auto n = graph.number_vertices();
        const auto s = std::ceil(100 * k * sqrt(n) / epsilon * this->c2);
        const auto t = std::ceil(log(10) * this->c1 * k * sqrt(n));

        Uniform_Random_Generator<double> urandom_gen;

        const auto S = urandom_gen.get(s);
        const auto T = urandom_gen.get(t);

        std::cout << "|S| = " << s << std::endl;
        std::cout << "|T| = " << t << std::endl;

        bool wrongly_connected_found = false;
        double distn, distw;

        #pragma omp parallel for shared(wrongly_connected_found, distn, distw)
        for (unsigned long long i = 0; i < S.size(); ++i) {
            if (wrongly_connected_found) continue;
            const unsigned long long v = floor(S[i] * n);
            const auto v_value = graph.get_vertex(v);
            Relation<V> neighbors;
            #pragma omp critical
            {
                neighbors = Oracle.query(v);
            }
            if (neighbors.size() > 100 * k * d / epsilon) continue;
            V distN = 0;
            for (const auto &neighbor: neighbors) {
                auto dist = KNN_Graph<V>::euclidean_distance(v_value, neighbor);
                if (dist > distN) {
                    distN = dist;
                }
            }
            for (unsigned long long j = 0; j < T.size(); ++j) {
                if (wrongly_connected_found) {
                    continue;
                }
                unsigned long long w = floor(T[j] * n);
                auto dist = KNN_Graph<V>::euclidean_distance(v_value, graph.get_vertex(w));
                if (v != w and distN - dist > std::numeric_limits<V>::epsilon()) {
                    auto is_neighbor = false;
                    for (const auto &neighbor: neighbors) {
                        if (graph.get_vertex(w) == neighbor) {
                            is_neighbor = true;
                            break;
                        }
                    }
                    if (not is_neighbor) {
                        #pragma omp critical
                        {
                            if (not wrongly_connected_found) {
                                wrongly_connected_found = true;
                                distn = distN;
                                distw = dist;
                            }
                        }
                    }
                }
            }
        }
        auto stop = boost::chrono::process_real_cpu_clock::now();
        auto total_time = (stop-start).count();
        auto query_time = Oracle.time();

        Tester_Result result;
        result.total_time = total_time / 1000000000.0;
        result.query_time = query_time / 1000000000.0;
        if (wrongly_connected_found) {
            std::cout << "Reject!" << std::endl;
            std::cout << distw << " < " << distn << std::endl;
            result.decision = false;
        } else {
            std::cout << "Accept!" << std::endl;
            result.decision = true;
        }
        return result;
    }
};

template <typename V = double>
class KNN_Improver : public KNN_Tester<V> {
public:
    /**
     *
     * Property Testing Algorithm for k-Nearest Neighborhood Graphs - Graph Restauration
     * @param KNN_Graph G, average degree of G, epsilon
     * @return
     *
     */
    auto improve(KNN_Graph<V> &graph, const double d, const double epsilon = 0.001) {
        auto result = 0ul;
        if (this->auto_c1) this->c1 = this->c1_approximate(graph);
        const auto delta = graph.dimension();
        const auto k = graph.get_k();
        const auto n = graph.number_vertices();
        const auto s = ceil(100 * k * sqrt(n) / epsilon * this->c2);
        const auto t = ceil(log(10) * this->c1 * k * sqrt(n));

        Uniform_Random_Generator<double> urandom_gen;

        const auto S = urandom_gen.get(s);
        const auto T = urandom_gen.get(t);

        std::cout << "|S| = " << s << std::endl;
        std::cout << "|T| = " << t << std::endl;

        #pragma omp parallel for shared(result)
        for (unsigned long long i = 0; i < S.size(); ++i) {
            const unsigned long long v = floor(S[i] * n);
            const auto v_value = graph.get_vertex(v);
            if (graph.number_neighbors(v) > 100 * k * d / epsilon) continue;
            auto &neighbors = graph.get_edges()[v];
            V distN = 0;
            unsigned long long furthest = 0;
            for (const auto &neighbor: neighbors) {
                auto dist = KNN_Graph<V>::euclidean_distance(v_value, graph.get_vertex(neighbor));
                if (dist > distN) {
                    distN = dist;
                    furthest = neighbor;
                }
            }
            for (unsigned long long j = 0; j < T.size(); ++j) {
                unsigned long long w = floor(T[j] * n);
                auto dist = KNN_Graph<V>::euclidean_distance(v_value, graph.get_vertex(w));
                if (v != w and distN - dist > std::numeric_limits<V>::epsilon()) {
                    auto is_neighbor = false;
                    for (const auto &neighbor: neighbors) {
                        if (w == neighbor) {
                            is_neighbor = true;
                            break;
                        }
                    }
                    if (not is_neighbor) {
                        #pragma omp critical
                        {
                            graph.get_edges()[v][furthest] = w;
                            ++result;
                            distN = 0;
                            for (const auto &neighbor: neighbors) {
                                auto distF = KNN_Graph<V>::euclidean_distance(v_value, graph.get_vertex(neighbor));
                                if (distF > distN) {
                                    distN = distF;
                                    furthest = neighbor;
                                }
                            }
                        }
                    }
                }
            }
        }
        return result;
    }
};


#include "catch.hpp"

#include "tntn/Raster.h"
#include "tntn/raster_tools.h"
#include "tntn/SurfacePoints.h"
#include "tntn/Mesh.h"
#include "tntn/Mesh2Raster.h"
#include "tntn/RasterIO.h"

#include <memory>
#include <cstdlib>
#include <boost/filesystem.hpp>

namespace tntn {
namespace unittests {

TEST_CASE("raster default value", "[tntn]")
{
    RasterDouble raster;
    double ndv = raster.get_no_data_value();

    double check = 0;
    detail::NDVDefault<double>::set(check);

    CHECK(ndv == check);
}

TEST_CASE("raster bb", "[tntn]")
{
    RasterDouble raster(798, 212);

    raster.set_pos_x(11.024);
    raster.set_pos_y(-334.242);
    raster.set_cell_size(1.341);

    BBox2D rbb = raster.get_bounding_box();

    double w = (raster.get_width() - 1) * raster.get_cell_size();
    double h = (raster.get_height() - 1) * raster.get_cell_size();

    CHECK(std::abs((rbb.max.x - rbb.min.x) - w) < 0.001);
    CHECK(std::abs((rbb.max.y - rbb.min.y) - h) < 0.001);
}

TEST_CASE("raster coordinates", "[tntn]")
{
    RasterDouble raster(7, 21);

    raster.set_pos_x(11);
    raster.set_pos_y(-334);
    raster.set_cell_size(1.341);

    for(int r = 0; r < raster.get_height(); r++)
    {
        for(int c = 0; c < raster.get_width(); c++)
        {
            double x = raster.col2x(c);
            double y = raster.row2y(r);

            int cn = raster.x2col(x);
            int rn = raster.y2row(y);

            CHECK(cn == c);
            CHECK(rn == r);
        }
    }
}

TEST_CASE("mesh 2 raster", "[tntn]")
{

    std::vector<Triangle> trList;

    Triangle t;

    t[0].x = 0.5;
    t[0].y = 0.5;
    t[0].z = 1;

    t[1].x = 0.5;
    t[1].y = 3.5;
    t[1].z = 1;

    t[2].x = 3.5;
    t[2].y = 3.5;
    t[2].z = 1;

    trList.push_back(t);

    t[0].x = 0.5;
    t[0].y = 0.5;
    t[0].z = -1;

    t[1].x = 3.5;
    t[1].y = 3.5;
    t[1].z = -1;

    t[2].x = 3.5;
    t[2].y = 0.5;
    t[2].z = -1;

    trList.push_back(t);

    Mesh m;
    m.from_triangles(std::move(trList));
    m.generate_decomposed();

    Mesh2Raster m2r;

    RasterDouble raster_4 = m2r.rasterise(m, 4, 4);

    CHECK(raster_4.get_cell_size() == 1);

    RasterDouble raster_10 = m2r.rasterise(m, 10, 10, 4, 4);

    CHECK(raster_10.get_cell_size() == 0.4);
}

TEST_CASE("Raster integer downsampe", "[tntn]")
{
    RasterDouble big(8, 10);
    big.set_all(1);

    RasterDouble small_01 = raster_tools::integer_downsample_mean(big, 2);

    CHECK(small_01.get_height() == big.get_height() / 2);
    CHECK(small_01.get_width() == big.get_width() / 2);

    int ws = small_01.get_width();
    int hs = small_01.get_height();

    for(int rs = 0; rs < hs; rs++)
    {
        for(int cs = 0; cs < ws; cs++)
        {
            CHECK(small_01.value(rs, cs) == 1.0);
        }
    }
}

TEST_CASE("Raster crop", "[tntn]")
{
    struct P
    {
        int r = -10;
        int c = -10;
    };

    Raster<P> raster;
    raster.allocate(5, 6);

    raster.set_cell_size(1);
    raster.set_pos_x(0);
    raster.set_pos_y(0);

    P p;
    p.r = -999;
    p.c = -888;

    raster.set_no_data_value(p);

    int w = raster.get_width();
    int h = raster.get_height();

    for(int r = 0; r < h; r++)
    {
        for(int c = 0; c < w; c++)
        {
            raster.value(r, c).r = r;
            raster.value(r, c).c = c;
        }
    }

    // control - just a copy!
    auto c1 = raster.crop(0, 0, w, h);

    CHECK(c1.get_width() == w);
    CHECK(c1.get_height() == h);
    CHECK(c1.get_no_data_value().r == -999);
    CHECK(c1.get_no_data_value().c == -888);
    CHECK(c1.get_pos_x() == 0);
    CHECK(c1.get_pos_y() == 0);

    for(int r = 0; r < h; r++)
    {
        for(int c = 0; c < w; c++)
        {
            CHECK(c1.value(r, c).r == r);
            CHECK(c1.value(r, c).c == c);
        }
    }

    // crop from top left with width=3 height=4
    // using raster coorinates with origin at top left!
    // (i.e. as data is stored)
    auto c2 = raster.crop(0, 0, 3, 4);

    CHECK(c2.get_width() == 3);
    CHECK(c2.get_height() == 4);
    CHECK(c2.get_no_data_value().r == -999);
    CHECK(c2.get_no_data_value().c == -888);
    CHECK(c2.get_pos_x() == 0);
    CHECK(c2.get_pos_y() == 2);

    for(int r = 0; r < c2.get_height(); r++)
    {
        for(int c = 0; c < c2.get_width(); c++)
        {
            CHECK(c2.value(r, c).r == r);
            CHECK(c2.value(r, c).c == c);
        }
    }

    // crop from bottom left with width=3 height=4
    // using raster coorinates with origin at top left!
    // this should be identical to c2!
    auto c3 = raster.crop_ll(0, 2, 3, 4);

    CHECK(c2.get_height() == c3.get_height());
    CHECK(c2.get_width() == c3.get_width());

    CHECK(c3.get_pos_x() == 0);
    CHECK(c3.get_pos_y() == 2);

    for(int r = 0; r < c2.get_height(); r++)
    {
        for(int c = 0; c < c2.get_width(); c++)
        {
            CHECK(c2.value(r, c).r == c3.value(r, c).r);
            CHECK(c2.value(r, c).c == c3.value(r, c).c);
        }
    }

    // test what happens if crop region is larger than original raster
    // this should return overlapping region:

    Raster<int> raster_int(10, 23);

    raster_int.set_all(-1);
    raster_int.set_pos_x(-14);
    raster_int.set_pos_y(18);
    raster_int.set_cell_size(0.3);
    raster_int.set_no_data_value(-9999);

    for(int r = 0; r < raster_int.get_height(); r++)
    {
        for(int c = 0; c < raster_int.get_width(); c++)
        {
            raster_int.value(r, c) = r * c;
        }
    }

    // crop is large than original in all dimensions:

    Raster<int> cropped_raster;

    raster_int.crop(-12, -5, 100, 80, cropped_raster);

    CHECK(raster_int.get_width() == cropped_raster.get_width());
    CHECK(raster_int.get_height() == cropped_raster.get_height());
    CHECK(raster_int.get_cell_size() == cropped_raster.get_cell_size());
    CHECK(raster_int.get_pos_x() == cropped_raster.get_pos_x());
    CHECK(raster_int.get_pos_y() == cropped_raster.get_pos_y());

    for(int r = 0; r < raster_int.get_height(); r++)
    {
        for(int c = 0; c < raster_int.get_width(); c++)
        {
            CHECK(raster_int.value(r, c) == cropped_raster.value(r, c));
        }
    }

    // crop is overlapping original:
    cropped_raster.clear();

    raster_int.crop(-12, -5, 20, 10, cropped_raster);

    for(int r = 0; r < cropped_raster.get_height(); r++)
    {
        for(int c = 0; c < cropped_raster.get_width(); c++)
        {
            CHECK(cropped_raster.get_no_data_value() != cropped_raster.value(r, c));
            CHECK(raster_int.value(r, c) == cropped_raster.value(r, c));
        }
    }

    cropped_raster.clear();

    int sx = 5;
    int sy = 7;
    raster_int.crop(sx, sy, 300, 500, cropped_raster);

    for(int r = 0; r < cropped_raster.get_height(); r++)
    {
        for(int c = 0; c < cropped_raster.get_width(); c++)
        {
            CHECK(cropped_raster.get_no_data_value() != cropped_raster.value(r, c));
            CHECK(raster_int.value(r + sy, c + sx) == cropped_raster.value(r, c));
        }
    }
}

TEST_CASE("Raster setAll", "[tntn]")
{

    Raster<double> r;
    r.set_all(42.0);

    r.allocate(100, 100);
    r.set_all(23.0);

    CHECK(r.value(50, 50) == 23.0);

    class TestClass
    {
      public:
        TestClass(){};
        TestClass(int vv) : v(vv) {}
        int v = -1;
        double a = 0;
        double b = 0;
    };

    Raster<TestClass> r2;
    TestClass tc(123);
    r2.set_all(tc);

    r2.allocate(10, 1);
    r2.set_all(tc);

    for(int i = 0; i < r2.get_width() * r2.get_height(); i++)
    {
        CHECK(r2.get_ptr()[i].v == 123);
    }

    tc.v = -321;
    r2.allocate(1, 10);
    r2.set_all(tc);

    for(int i = 0; i < r2.get_width() * r2.get_height(); i++)
    {
        CHECK(r2.get_ptr()[i].v == -321);
    }
}

TEST_CASE("move-construct Raster", "[tntn]")
{

    Raster<double> r;
    r.allocate(10, 10);

    Raster<double> r2(std::move(r));

    CHECK(r.get_width() == 0);
    CHECK(r.get_height() == 0);

    CHECK(r2.get_width() == 10);
    CHECK(r2.get_height() == 10);
}

TEST_CASE("move-assign Raster", "[tntn]")
{

    Raster<double> r;
    r.allocate(10, 10);

    Raster<double> r2;
    r2 = std::move(r);

    CHECK(r.get_width() == 0);
    CHECK(r.get_height() == 0);

    CHECK(r2.get_width() == 10);
    CHECK(r2.get_height() == 10);
}

TEST_CASE("Raster::toVertices", "[tntn]")
{

    Raster<double> r;
    r.allocate(10, 10);
    r.set_pos_x(10);
    r.set_pos_y(10);
    r.set_cell_size(1.0);

    for(int y = 0; y < r.get_height(); y++)
    {
        for(int x = 0; x < r.get_width(); x++)
        {
            r.value(y, x) = y * r.get_width() + x;
        }
    }

    SurfacePoints pts;
    pts.reserve(r.get_height() * r.get_width());

    r.to_vertices([&pts](const double x, const double y, const double z) {
        pts.push_back({x, y, z});
    });

    int num_found = 0;
    auto ptsrange = pts.points();
    ptsrange.for_each([&num_found](const Vertex& v) {
        if(v.x == 10.0 && v.y == 10.0)
        {
            //lower-left cell
            num_found++;
            CHECK(v.z == 9 * 10 + 0);
        }
        else if(v.x == 19.0 && v.y == 19.0)
        {
            //upper-right cell
            num_found++;
            CHECK(v.z == 0 * 10 + 9);
        }
    });

    CHECK(num_found == 2);
}

} // namespace unittests
} // namespace tntn


#ifndef DATAFRAME_UPDATER_HPP_
#define DATAFRAME_UPDATER_HPP_

#include <Eigen/Dense>

namespace lidar_slam {

class FrameUpdater {
public:
  FrameUpdater()
      : is_first(true), prev_keypose(Eigen::Isometry3d::Identity()),
        keyframe_delta_trans(0.25), keyframe_delta_angle(0.05), frame_count(0) {
    accum_distance = 0.0;
  }
  ~FrameUpdater() { std::cout << "frame_count:" << frame_count << std::endl; }

  bool update(const Eigen::Isometry3d &pose) {
    // first frame is always registered to the graph
    if (is_first) {
      is_first = false;
      prev_keypose = pose;
      return true;
    }

    // calculate the delta transformation from the previous keyframe
    Eigen::Isometry3d delta = prev_keypose.inverse() * pose;
    double dx = delta.translation().norm();
    Eigen::AngleAxisd axis(delta.rotation());
    double da = axis.angle();

    // std::cout<<"dx:"<<dx<<",da:"<<da<<std::endl;
    accum_distance += dx;
    prev_keypose = pose;
    frame_count++;

    // too close to the previous frame
    if (dx < keyframe_delta_trans && da < keyframe_delta_angle) {
      return false;
    }

    return true;
  }

  double get_accum_distance() const { return accum_distance; }
  long get_unique_id() { return ++frame_id; }
  long get_frame_count() { return frame_count; }

private:
  // parameters
  double keyframe_delta_trans; //
  double keyframe_delta_angle; //

  bool is_first;
  long frame_id;
  double accum_distance;
  long frame_count;
  Eigen::Isometry3d prev_keypose;
};
}

#endif // DATAFRAME_UPDATER_HPP_


//######################################################################
//#   SDF_Fusion Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                or under supervision of Slobodan Ilic
//#######################################################################

#ifndef MARKER_DETECTOR_HPP
#define MARKER_DETECTOR_HPP

#include <opencv2/core/core.hpp>
#include <cstdio>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace cv;
using namespace Eigen;

class  Marker: public vector<Point2f>
{
public:
    //id of  the marker
    int id;
    //size of the markers sides in meters
    float metric_size;
    //matrices of rotation and translation respect to the camera
    cv::Mat Rvec,Tvec;

    Isometry3f pose;

    Marker();
    Marker(const Marker &M);
    Marker(const vector<Point2f> &corners,int _id=-1);
    ~Marker() {}

    bool isValid()const{return id!=-1 && size()==4;}
    void draw(cv::Mat &in, cv::Scalar color, int lineWidth=1,bool writeId=true)const;

    void calculateExtrinsics(float markerSize,cv::Mat  CameraMatrix,cv::Mat Distorsion)throw(cv::Exception);

    cv::Point2f getCenter()const;
    float getPerimeter()const;
    float getArea()const;

    friend bool operator<(const Marker &M1,const Marker&M2){return M1.id<M2.id;}

    friend ostream & operator<<(ostream &str,const Marker &M)
    {
        str<<M.id<<"=";
        for (int i=0;i<4;i++)
            str<<"("<<M[i].x<< ","<<M[i].y<<") ";
        str<<"Txyz=";
        for (int i=0;i<3;i++)
            str<<M.Tvec.ptr<float>(0)[i]<<" ";
        str<<"Rxyz=";
        for (int i=0;i<3;i++)
            str<<M.Rvec.ptr<float>(0)[i]<<" ";

        return str;
    }

};

struct Marker3D:public vector<cv::Point3f> {
    Marker3D() {}
    Marker3D(int _id) {id=_id; }
    Marker3D(const Marker3D&MI): vector<Point3f>(MI){id=MI.id; }
    Marker3D & operator=(const Marker3D&MI){vector<Point3f> ::operator=(MI);id=MI.id;return *this;}
    int id;
};

class  Board
{
public:

    Board();
    Board(string filePath)throw (cv::Exception);
    Board(const Board  &T);
    Board & operator=(const Board  &T);
    void saveToFile(string sfile);
    void readFromFile(string sfile);
    int getIndexOfMarkerId(int id);
    const Marker3D& getMarkerInfo(int id);
    void getIdList(vector<int> &ids,bool append=true)const;

    vector<Marker3D> reference;
    vector<Marker> detected;
    cv::Mat Rvec,Tvec;
    int w,h;
    Isometry3f pose;
    Vector3f plane_point, plane_normal;

};

class SubPixelCorner
{
private:
    int _winSize,_apertureSize,_max_iters;
    cv::TermCriteria _term;
    double eps;
    cv::Mat mask;
public:
    bool enable;
    SubPixelCorner();
    void checkTerm();
    double pointDist(cv::Point2f estimate_corner,cv::Point2f curr_corner);
    void RefineCorner(cv::Mat image,std::vector <cv::Point2f> &corners);
    void generateMask();
};

class MarkerCode {
public:

    MarkerCode(uint n=0);
    MarkerCode(const MarkerCode &MC);

    // Get id of a specific rotation as the number obtaiend from the concatenation of all the bits
    uint getId(uint rot=0) { return _ids[rot]; }

    // Get a bit value in a specific rotation. Refered as a unidimensional string of bits, i.e. pos=y*n+x
    bool get(uint pos, uint rot=0) { return _bits[rot][pos]; }

    //Get the string of bits for a specific rotation
    std::vector<bool> getRotation(uint rot) { return _bits[rot]; }

    void set(unsigned int pos, bool val);
    uint size() {return n()*n(); }
    uint n() {return _n; }
    uint selfDistance(unsigned int &minRot);
    uint selfDistance() {uint minRot;return selfDistance(minRot);}
    uint distance(MarkerCode m, unsigned int &minRot);
    uint distance(MarkerCode m) {uint minRot;return distance(m, minRot);}
    void fromString(std::string s);
    std::string toString();
    cv::Mat getImg(unsigned int pixSize);

private:
    unsigned int _ids[4]; // ids in the four rotations
    vector<bool> _bits[4]; // bit strings in the four rotations
    unsigned int _n; // marker dimension

    uint hammingDistance(vector<bool> m1, vector<bool> m2);

};

class Dictionary : public std::vector<MarkerCode> {
public:

    bool fromFile(std::string filename);
    bool toFile(std::string filename);
    void dumpMarkers();

    uint distance(MarkerCode m, unsigned int &minMarker, unsigned int &minRot);
    uint distance(MarkerCode m) {uint minMarker, minRot;return distance(m,minMarker,minRot);}
    uint minimumDistance();

private:

    // convert to string
    template <class T> static std::string toStr(T num) {
        std::stringstream ss;
        ss << num;
        return ss.str();
    }
};

// Balanced Binary Tree for a marker dictionary
class BalancedBinaryTree {

public:

    void loadDictionary(Dictionary &D);
    bool findId(unsigned int id, unsigned int &orgPos);

private:

    vector< pair<uint,uint> > _orderD; // dictionary sorted by id, first element is the id, second element is the position in original D
    vector< pair<int, int> > _binaryTree; // binary tree itself (as a vector), each element is a node of the tree
    // first element indicate the position in _binaryTree of the lower child
    // second element is the position in _binaryTree of the higher child
    // -1 value indicates no lower or higher child
    int _root; // position in _binaryTree of the root node of the tree

};

/**\brief Main class for marker detection
 *
 */
class  MarkerDetector
{
    //Represent a candidate to be a maker
    class MarkerCandidate : public Marker{
    public:
        MarkerCandidate(){}
        MarkerCandidate(const Marker &M): Marker(M){}
        MarkerCandidate(const  MarkerCandidate &M): Marker(M){
            contour=M.contour;
            idx=M.idx;
        }
        MarkerCandidate & operator=(const  MarkerCandidate &M){
            (*(Marker*)this)=(*(Marker*)&M);
            contour=M.contour;
            idx=M.idx;
            return *this;
        }

        vector<cv::Point> contour;//all the points of its contour
        int idx;//index position in the global contour list
    };
public:

    MarkerDetector();
    MarkerDetector(Mat camera, Mat distortion);
    ~MarkerDetector();

	int getNumDetected() {return num_detected;}

    void init(Matrix3f camera);

    float detect(const Mat &input,float markerSizeMeters) throw (cv::Exception);

    bool createDictionary(string filename, uint dictSize, uint n);
    bool createBoard(string boardfile, int w, int h);

    bool loadDictionary(string filename);
    bool loadBoard(string filename);

    enum ThresholdMethods {FIXED_THRES,ADPT_THRES,CANNY};

    void setThresholdMethod(ThresholdMethods m) {_thresMethod=m;}
    ThresholdMethods getThresholdMethod()const {return _thresMethod;}

    /**
     * Set the parameters of the threshold method
     * We are currently using the Adptive threshold
     *   @param param1: blockSize of the pixel neighborhood that is used to calculate a threshold value for the pixel
     *   @param param2: The constant subtracted from the mean or weighted mean
     */
    void setThresholdParams(double param1,double param2) {_thresParam1=param1;_thresParam2=param2;}

    const cv::Mat & getThresholdedImage() {return thres;}

    enum CornerRefinementMethod {NONE,HARRIS,SUBPIX,LINES};

    void setCornerRefinementMethod(CornerRefinementMethod method) {_cornerMethod=method;}

    CornerRefinementMethod getCornerRefinementMethod()const {return _cornerMethod;}

    /**Specifies the min and max sizes of the markers as a fraction of the image size. By size we mean the maximum
     * of cols and rows.
     * @param min size of the contour to consider a possible marker as valid (0,1]
     * @param max size of the contour to consider a possible marker as valid [0,1)
     *
     */
    void setMinMaxSize(float mini=0.03,float maxi=0.5){_minSize=max(0.0f,mini);_maxSize=min(0.0f,maxi);}
    void getMinMaxSize(float &min,float &max){min=_minSize;max=_maxSize;}

    // Enables/Disables erosion process that is REQUIRED for chessboard like boards.
    void enableErosion(bool enable){_doErosion=enable;}

    void setDesiredSpeed(int val);
    int getDesiredSpeed()const {return _speed;}

    // Specifies size for the canonical marker image. Bigger is slower. Minimum 10, Default 56
    void setWarpSize(int val) {_markerWarpSize = max(10,val);}
    int getWarpSize()const {return _markerWarpSize;}

    int identifyMarker(const cv::Mat &in,int &nRotations);
    void pyrDown(unsigned int level){pyrdown_level=level;}

    //Returns a list candidates to be markers (rectangles), for which no valid id was found after calling detectRectangles
    const vector<vector<Point2f> > &getCandidates() {return _candidates;}

    void draw3dAxis(cv::Mat &Image,Marker &m);
    void draw3dCube(cv::Mat &Image,Marker &m);
    void drawBoardAxis(cv::Mat &Image);
    void drawBoardCube(cv::Mat &Image);

    void dumpBoardImage(string filename, uint pixelpermarker);

    cv::Mat camera, distortion;
    vector<Marker> detectedMarkers;
    Dictionary D;
    Board board;

    void set_repj_err_thres(float Repj_err_thres){_repj_err_thres=Repj_err_thres;}
    float get_repj_err_thres  ( )const {return _repj_err_thres;}

private:

	float num_detected; // Mira 21.Oct.

    //Given the input image with markers, creates an output image with it in the canonical position
    bool warp(cv::Mat &in,cv::Mat &out,cv::Size size, vector<Point2f> points)throw (cv::Exception);

    // Refine MarkerCandidate Corner using LINES method
    void refineCandidateLines(MarkerCandidate &candidate);

    //Thesholds the passed image with the specified method.
    void thresHold(int method,const cv::Mat &grey,cv::Mat &thresImg,double param1=-1,double param2=-1);

    // This function returns in candidates all the rectangles found in a thresolded image
    void detectRectangles(const cv::Mat &thresImg,vector<vector<Point2f> > & candidates);

    void detectMarkers(const Mat &input,float markerSizeMeters) throw (cv::Exception);
    void detectRectangles(const cv::Mat &thresImg,vector<MarkerCandidate> & candidates);
    //Current threshold method
    ThresholdMethods _thresMethod;
    //Threshold parameters
    double _thresParam1,_thresParam2;
    //Current corner method
    CornerRefinementMethod _cornerMethod;
    //minimum and maximum size of a contour lenght
    float _minSize,_maxSize;

    BalancedBinaryTree _binaryTree;
    unsigned int _n,_ncellsBorder,_correctionDistance;
    int _swidth; // cell size in the canonical image

    float _repj_err_thres;

    int _speed;
    int _markerWarpSize;
    bool _doErosion;
    float _borderDistThres; //border around image limits in which corners are not allowed to be detected.
    vector<std::vector<cv::Point2f> > _candidates;     // Rectangles that have no valid id
    int pyrdown_level;

    cv::Mat grey,thres,thres2,reduced;

    bool isInto(cv::Mat &contour,std::vector<cv::Point2f> &b);
    int perimeter(std::vector<cv::Point2f> &a);

    // auxiliar functions to perform LINES refinement
    void interpolate2Dline( const vector< cv::Point2f > &inPoints, cv::Point3f &outLine);
    cv::Point2f getCrossPoint(const cv::Point3f& line1, const cv::Point3f& line2);
    void distortPoints(vector<cv::Point2f> in, vector<cv::Point2f> &out, const cv::Mat &camMatrix, const cv::Mat &distCoeff);

    void draw(cv::Mat out,const std::vector<Marker> &markers );

    //int threads = 1;
    //int curr_thread = 0;
	int threads;
	int curr_thread;
    template<typename T> void joinVectors(vector<vector<T> >  &vv,vector<T> &v,bool clearv=false){
        if (clearv) v.clear();
        for(size_t i=0;i<vv.size();i++)
            for(size_t j=0;j<vv[i].size();j++) v.push_back(vv[i][j]);
    }
};

#endif


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file xor_bids_test.cpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2014-2-25
 */

#include "test_utils/auctions.hpp"

#include "paal/auctions/auction_components.hpp"
#include "paal/auctions/xor_bids.hpp"
#include "paal/data_structures/fraction.hpp"
#include "paal/utils/functors.hpp"

#include <boost/mem_fn.hpp>
#include <boost/none.hpp>
#include <boost/test/unit_test.hpp>

#include <functional>
#include <iterator>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

BOOST_AUTO_TEST_CASE(testXorBidsAuction)
{
   using Bidder = std::string;
   using Bidders = std::unordered_set<Bidder>;
   using Item = std::string;
   using Items = std::unordered_set<Item>;
   using Value = double;
   using Bid = std::pair<Items, Value>;
   using Bids = std::vector<Bid>;
   namespace pu = paal::utils;
   namespace pa = paal::auctions;

   //input data
   Bidders bidders {"John", "Bob", "Steve"};
   std::unordered_map<Bidder, Bids> bids {
      {"John", {
         {Items{}, 0},
         {{"apple", "knife"}, 1.7},
      }},
      {"Bob", {
         {{"lemon", "orange", "sugar", "knife"}, 2.7},
         {{"apple", "lemon", "knife"}, 1.8},
         {{"apple", "ball", "orange", "sugar", "lemon", "knife"}, 4},
      }},
      {"Steve", {
         {{"lemon"}, 4},
         {{"apple"}, 4.6},
         {{"ball"}, 3.1},
      }},
   };
   auto get_bids = [&](const Bidder& bidder) -> const Bids& { return bids.at(bidder); };
   auto get_value = boost::mem_fn(&Bid::second);
   auto get_items = boost::mem_fn(&Bid::first);
   auto get_copies_num = [](const Item& item) { return item == "knife" ? 1 : 3; };
   Items items;
   pa::extract_items_from_xor_bids(
      bidders, get_bids, get_items, std::inserter(items, items.begin())
   );

   Items want_items {"apple", "ball", "knife", "lemon", "orange", "sugar"};

   // value query auction
   {
      auto auction = pa::make_xor_bids_to_value_query_auction(
         bidders, items, get_bids, get_value, get_items, get_copies_num
      );
      // TODO this can be refactored even more into lambda
      // check_bidders_and_items() defined for all auction
      // types when C++ polymorphic lambdas arrive
      check_auction_bidders_and_items(auction, bidders, want_items);
      BOOST_CHECK_EQUAL(auction.call<pa::get_copies_num>("apple"), 3);
      BOOST_CHECK_EQUAL(auction.call<pa::get_copies_num>("knife"), 1);
      auto const tests = {
         std::make_tuple("John", Items{"apple", "ball"}, 0.),
         std::make_tuple("John", Items{"apple", "knife"}, 1.7),
         std::make_tuple("John", Items{"knife", "orange", "lemon"}, 0.),
         std::make_tuple("Bob", Items{}, 0.),
         std::make_tuple("Bob", Items{"apple", "lemon", "orange", "sugar", "knife"}, 2.7),
         std::make_tuple("Bob", Items{"apple", "lemon", "sugar", "knife", "ball"}, 1.8),
         std::make_tuple("Bob", Items{"apple", "ball", "orange", "sugar", "lemon", "knife"}, 4.0),
         std::make_tuple("Steve", Items{"apple", "lemon", "ball"}, 4.6),
         std::make_tuple("Steve", Items{"lemon", "ball"}, 4.0)
      };
      for (auto t: tests)
         BOOST_CHECK_EQUAL(
            auction.call<pa::value_query>(std::get<0>(t), std::get<1>(t)), std::get<2>(t)
         );
   }

   auto map_to_func = [](const std::unordered_map<Item, Value> &m)
   {
      return [&](const Item& item) { return m.count(item) > 0 ? m.at(item) : 1; };
   };

   // demand query auction
   {
      auto auction = pa::make_xor_bids_to_demand_query_auction(
         bidders, items, get_bids, get_value, get_items, get_copies_num
      );
      check_auction_bidders_and_items(auction, bidders, want_items);
      BOOST_CHECK_EQUAL(auction.call<pa::get_copies_num>("sugar"), 3);
      BOOST_CHECK_EQUAL(auction.call<pa::get_copies_num>("knife"), 1);

      auto const empty = std::make_pair(Items{}, 0);
      // TODO refactor these checks once C++14 polymorphic lambdas arrive
      BOOST_CHECK(check_demand_query<Item>(auction, "John", pu::return_one_functor(), empty));
      BOOST_CHECK(check_demand_query<Item>(
         auction, "John", pu::make_dynamic_return_constant_functor(0.8),
         std::make_pair(Items{"apple", "knife"}, 0.1)
      ));
      BOOST_CHECK(check_demand_query<Item>(auction, "Bob", pu::return_one_functor(), empty));
      BOOST_CHECK(check_demand_query<Item>(
         auction, "Bob", map_to_func({{"apple", 0.1}, {"lemon", 0.1}, {"knife", 0.1}, {"orange", 0.1}}),
         std::make_pair(Items{"apple", "ball", "orange", "sugar", "lemon", "knife"}, 1.6)
      ));
      BOOST_CHECK(check_demand_query<Item>(
         auction, "Bob", map_to_func({{"apple", 0.3}, {"lemon", 0.3}, {"knife", 0.3}, {"orange", 0.3}}),
         std::make_pair(Items{"apple", "lemon", "knife"}, 0.9)
      ));
      BOOST_CHECK(check_demand_query<Item>(auction, "Steve", pu::return_one_functor(),
         std::make_pair(Items{"apple"}, 3.6)));
      BOOST_CHECK(check_demand_query<Item>(
         auction, "Steve", pu::make_dynamic_return_constant_functor(5.0), empty
      ));
      BOOST_CHECK(check_demand_query<Item>(
         auction, "Steve", map_to_func({{"lemon", 2}, {"apple", 3}}), std::make_pair(Items{"ball"}, 2.1)
      ));
   }

   // gamma oracle auction
   {
      using Frac = paal::data_structures::fraction<Value, Value>;

      auto auction = pa::make_xor_bids_to_gamma_oracle_auction(
         bidders, items, get_bids, get_value, get_items, get_copies_num
      );
      check_auction_bidders_and_items(auction, bidders, want_items);
      BOOST_CHECK_EQUAL(auction.call<pa::get_copies_num>("apple"), 3);
      BOOST_CHECK_EQUAL(auction.call<pa::get_copies_num>("knife"), 1);
      BOOST_CHECK_EQUAL(auction.get<pa::gamma>(), 1);

      // TODO refactor these checks once C++14 polymorphic lambdas arrive
      BOOST_CHECK(check_gamma_oracle<Items>(
         auction, "John", pu::return_one_functor(), 1.7, boost::none
      ));
      BOOST_CHECK(check_gamma_oracle<Items>(
         auction, "John", pu::return_one_functor(), 1.6,
         std::make_pair(Items{"apple", "knife"}, Frac(2, 0.1))
      ));
      BOOST_CHECK(check_gamma_oracle<Items>(
         auction, "Bob", pu::return_one_functor(), 4, boost::none
      ));
      BOOST_CHECK(check_gamma_oracle<Items>(
         auction, "Bob", map_to_func({{"sugar", 10}, {"orange", 5}}), 1,
         std::make_pair(Items{"apple", "lemon", "knife"}, Frac(3, 0.8))
      ));
      BOOST_CHECK(check_gamma_oracle<Items>(
         auction, "Bob", map_to_func({{"sugar", 10}, {"orange", 5}}), 2,
         std::make_pair(Items{"apple", "ball", "orange", "sugar", "lemon", "knife"}, Frac(19, 2))
      ));
      BOOST_CHECK(check_gamma_oracle<Items>(
         auction, "Bob", map_to_func({{"ball", 7}}), 2,
         std::make_pair(Items{"lemon", "orange", "sugar", "knife"}, Frac(4, 0.7))
      ));
      BOOST_CHECK(check_gamma_oracle<Items>(
         auction, "Steve", pu::return_one_functor(), 1,
         std::make_pair(Items{"apple"}, Frac(1, 3.6))
      ));
      BOOST_CHECK(check_gamma_oracle<Items>(
         auction, "Steve", map_to_func({{"apple", 2}, {"lemon", 1.5}}), 1,
         std::make_pair(Items{"ball"}, Frac(1, 2.1))
      ));
   }
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/ext/std/array.hpp>
#include <boost/hana/ext/std/tuple.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/range.hpp>

#include <array>
#include <tuple>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_ASSERT(
        to<List>(std::make_tuple(1, '2', 3.3)) == list(1, '2', 3.3)
    );

    BOOST_HANA_CONSTANT_ASSERT(
        to<List>(range(int_<1>, int_<4>)) == list(int_<1>, int_<2>, int_<3>)
    );

    BOOST_HANA_CONSTEXPR_ASSERT(
        to<List>(std::array<int, 3>{{1, 2, 3}}) == list(1, 2, 3)
    );
    //! [main]
}