#include <memory>

#include <Eigen/Core>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include "./imu_integrator_test_fixture.cc"  // NOLINT

using namespace imu_integrator;  // NOLINT

TEST_F(PosegraphErrorTerms, ImuIntegratorZeroReadingsZeroBiases) {
  gravity_magnitude_ = 0.0;
  constructIntegrator();

  delta_time_seconds_ = 1.0;
  debiased_imu_readings_.setZero();
  current_state_.setZero();
  current_state_.block<kStateOrientationBlockSize, 1>(
      kStateOrientationOffset, 0) = Eigen::Vector4d(0, 0, 0, 1);

  integrate();

  EXPECT_NEAR_EIGEN(current_state_, next_state_, 1e-15);
  EXPECT_ZERO_EIGEN(next_cov_, 1e-15);
}

TEST_F(PosegraphErrorTerms, ImuIntegratorJustNonzeroGravity) {
  gravity_magnitude_ = 1.0;
  constructIntegrator();

  delta_time_seconds_ = 1.0;
  debiased_imu_readings_.setZero();
  current_state_.setZero();
  current_state_.block<kStateOrientationBlockSize, 1>(
      kStateOrientationOffset, 0) = Eigen::Vector4d(0, 0, 0, 1);

  integrate();

  double expected_translation =
      -gravity_magnitude_ * pow(delta_time_seconds_, 2.0) / 2;
  double expected_final_velocity = -gravity_magnitude_ * delta_time_seconds_;

  EXPECT_NEAR_EIGEN(
      getOrientationFromState(next_state_), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_ZERO_EIGEN(getGyroBiasFromState(next_state_), 1e-15);
  EXPECT_NEAR_EIGEN(
      getVelocityFromState(next_state_),
      Eigen::Vector3d(0, 0, expected_final_velocity), 1e-15);
  EXPECT_ZERO_EIGEN(getAccelBiasFromState(next_state_), 1e-15);
  EXPECT_NEAR_EIGEN(
      getPositionFromState(next_state_),
      Eigen::Vector3d(0, 0, expected_translation), 1e-15);
  EXPECT_ZERO_EIGEN(next_cov_, 1e-15);
}

TEST_F(PosegraphErrorTerms, ImuIntegratorConstantForwardAcceleration) {
  gravity_magnitude_ = 9.81;
  constructIntegrator();

  double linear_acceleration = 0.3;

  delta_time_seconds_ = 1.0;
  debiased_imu_readings_.setZero();
  debiased_imu_readings_.block<3, 1>(kAccelReadingOffset, 0) =
      Eigen::Vector3d(linear_acceleration, 0, gravity_magnitude_);
  debiased_imu_readings_.block<3, 1>(
      kImuReadingSize + kAccelReadingOffset, 0) =
      Eigen::Vector3d(linear_acceleration, 0, gravity_magnitude_);
  current_state_.setZero();
  current_state_.block<kStateOrientationBlockSize, 1>(
      kStateOrientationOffset, 0) = Eigen::Vector4d(0, 0, 0, 1);

  integrate();

  double expected_translation =
      linear_acceleration * pow(delta_time_seconds_, 2.0) / 2;
  double expected_final_velocity = linear_acceleration * delta_time_seconds_;

  EXPECT_NEAR_EIGEN(
      getOrientationFromState(next_state_), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_ZERO_EIGEN(getGyroBiasFromState(next_state_), 1e-15);
  EXPECT_NEAR_EIGEN(
      getVelocityFromState(next_state_),
      Eigen::Vector3d(expected_final_velocity, 0, 0), 1e-15);
  EXPECT_ZERO_EIGEN(getAccelBiasFromState(next_state_), 1e-15);
  EXPECT_NEAR_EIGEN(
      getPositionFromState(next_state_),
      Eigen::Vector3d(expected_translation, 0, 0), 1e-15);
  EXPECT_ZERO_EIGEN(next_cov_, 1e-15);
}

TEST_F(PosegraphErrorTerms, ImuIntegratorConstForwardAccelInitialVelocity) {
  gravity_magnitude_ = 9.81;
  constructIntegrator();

  double linear_acceleration = 0.3;
  double linear_velocity = 2.5;

  delta_time_seconds_ = 1.8;
  debiased_imu_readings_.setZero();
  debiased_imu_readings_.block<3, 1>(kAccelReadingOffset, 0) =
      Eigen::Vector3d(linear_acceleration, 0, gravity_magnitude_);
  debiased_imu_readings_.block<3, 1>(
      kImuReadingSize + kAccelReadingOffset, 0) =
      Eigen::Vector3d(linear_acceleration, 0, gravity_magnitude_);
  current_state_.setZero();
  current_state_.block<kStateOrientationBlockSize, 1>(
      kStateOrientationOffset, 0) = Eigen::Vector4d(0, 0, 0, 1);
  current_state_.block<kVelocityBlockSize, 1>(
      kStateVelocityOffset, 0) =
      Eigen::Vector3d(linear_velocity, 0, 0);

  integrate();

  double expected_translation =
      linear_velocity * delta_time_seconds_ +
      linear_acceleration * pow(delta_time_seconds_, 2.0) / 2;
  double expected_final_velocity =
      linear_velocity + linear_acceleration * delta_time_seconds_;

  EXPECT_NEAR_EIGEN(
      getOrientationFromState(next_state_), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_ZERO_EIGEN(getGyroBiasFromState(next_state_), 1e-15);
  EXPECT_NEAR_EIGEN(
      getVelocityFromState(next_state_),
      Eigen::Vector3d(expected_final_velocity, 0, 0), 1e-15);
  EXPECT_ZERO_EIGEN(getAccelBiasFromState(next_state_), 1e-15);
  EXPECT_NEAR_EIGEN(
      getPositionFromState(next_state_),
      Eigen::Vector3d(expected_translation, 0, 0), 1e-15);
  EXPECT_ZERO_EIGEN(next_cov_, 1e-15);
}

TEST_F(PosegraphErrorTerms, ImuIntegratorConstAccelAndVelocityWhenRotated) {
  gravity_magnitude_ = 9.81;
  constructIntegrator();

  double linear_acceleration = 0.3;
  double linear_velocity = 2.5;

  delta_time_seconds_ = 1.2;
  debiased_imu_readings_.setZero();
  debiased_imu_readings_.block<3, 1>(kAccelReadingOffset, 0) =
      Eigen::Vector3d(linear_acceleration, 0, gravity_magnitude_);
  debiased_imu_readings_.block<3, 1>(
      kImuReadingSize + kAccelReadingOffset, 0) =
      Eigen::Vector3d(linear_acceleration, 0, gravity_magnitude_);
  current_state_.setZero();

  // note that state contains B_q_G quaternion (it simplifies Jacobians)
  current_state_.block<kStateOrientationBlockSize, 1>(
      kStateOrientationOffset, 0) =
      Eigen::Vector4d(0, 0, sqrt(2) / 2, sqrt(2) / 2);
  current_state_.block<kVelocityBlockSize, 1>(
      kStateVelocityOffset, 0) =
      Eigen::Vector3d(0, linear_velocity, 0);

  integrate();

  double expected_translation =
      linear_velocity * delta_time_seconds_ +
      linear_acceleration * pow(delta_time_seconds_, 2.0) / 2;
  double expected_final_velocity =
      linear_velocity + linear_acceleration * delta_time_seconds_;

  EXPECT_NEAR_EIGEN(
      getOrientationFromState(next_state_),
      Eigen::Vector4d(0, 0, sqrt(2) / 2, sqrt(2) / 2), 1e-15);
  EXPECT_ZERO_EIGEN(getGyroBiasFromState(next_state_), 1e-15);
  EXPECT_NEAR_EIGEN(
      getVelocityFromState(next_state_),
      Eigen::Vector3d(0, expected_final_velocity, 0), 1e-15);
  EXPECT_ZERO_EIGEN(getAccelBiasFromState(next_state_), 1e-15);
  EXPECT_NEAR_EIGEN(
      getPositionFromState(next_state_),
      Eigen::Vector3d(0, expected_translation, 0), 1e-15);
  EXPECT_ZERO_EIGEN(next_cov_, 1e-15);
}

TEST_F(PosegraphErrorTerms, ImuIntegratorConstantAngularVelocity) {
  gravity_magnitude_ = 9.81;
  constructIntegrator();

  delta_time_seconds_ = 1.0;
  debiased_imu_readings_.setZero();

  double angular_velocity = M_PI / 180;

  debiased_imu_readings_.block<3, 1>(kAccelReadingOffset, 0) =
      Eigen::Vector3d(0, 0, gravity_magnitude_);
  debiased_imu_readings_.block<3, 1>(
      kImuReadingSize + kAccelReadingOffset, 0) =
      Eigen::Vector3d(0, 0, gravity_magnitude_);

  debiased_imu_readings_.block<3, 1>(kGyroReadingOffset, 0) =
      Eigen::Vector3d(0, 0, angular_velocity);
  debiased_imu_readings_.block<3, 1>(
      kImuReadingSize + kGyroReadingOffset, 0) =
      Eigen::Vector3d(0, 0, angular_velocity);

  current_state_.setZero();

  // note that state contains B_q_G quaternion (it simplifies Jacobians)
  current_state_.block<kStateOrientationBlockSize, 1>(
      kStateOrientationOffset, 0) = Eigen::Vector4d(0, 0, 0, 1);

  integrate();

  // calculate quaternion
  Eigen::Quaterniond final_attitude;
  final_attitude.setIdentity();
  final_attitude.w() = cos(angular_velocity * delta_time_seconds_ / 2);
  final_attitude.z() = sin(angular_velocity * delta_time_seconds_ / 2);

  EXPECT_NEAR_EIGEN(
      getOrientationFromState(next_state_), final_attitude.coeffs(), 1e-10);
  EXPECT_ZERO_EIGEN(getGyroBiasFromState(next_state_), 1e-15);
  EXPECT_NEAR_EIGEN(
      getVelocityFromState(next_state_), Eigen::Vector3d(0, 0, 0), 1e-15);
  EXPECT_ZERO_EIGEN(getAccelBiasFromState(next_state_), 1e-15);
  EXPECT_NEAR_EIGEN(
      getPositionFromState(next_state_), Eigen::Vector3d(0, 0, 0), 1e-15);
  EXPECT_ZERO_EIGEN(next_cov_, 1e-15);
}

TEST_F(PosegraphErrorTerms, ImuIntegratorAngularVelocitySlope) {
  delta_time_seconds_ = 1.0;
  gravity_magnitude_ = 0.0;
  constructIntegrator();

  debiased_imu_readings_.setZero();

  double angular_velocity = M_PI / 180;
  debiased_imu_readings_.setZero();
  debiased_imu_readings_.block<3, 1>(kGyroReadingOffset, 0) =
      Eigen::Vector3d(0, 0, angular_velocity);
  debiased_imu_readings_.block<3, 1>(
      kImuReadingSize + kGyroReadingOffset, 0) =
      Eigen::Vector3d(0, 0, 0);

  current_state_.setZero();
  // note that state contains B_q_G quaternion (it simplifies Jacobians)
  current_state_.block<kStateOrientationBlockSize, 1>(
      kStateOrientationOffset, 0) = Eigen::Vector4d(0, 0, 0, 1);

  integrate();

  Eigen::Quaterniond final_attitude;
  final_attitude.setIdentity();
  // final angle will be equal to an area under a triangular curve
  final_attitude.w() = cos(angular_velocity * delta_time_seconds_ / 4);
  final_attitude.z() = sin(angular_velocity * delta_time_seconds_ / 4);

  EXPECT_NEAR_EIGEN(
      getOrientationFromState(next_state_), final_attitude.coeffs(), 1e-8);
}

TEST_F(PosegraphErrorTerms, ImuIntegratorLinearAccelerationSlope) {
  delta_time_seconds_ = 3.0;
  gravity_magnitude_ = 9.81;
  constructIntegrator();

  double initial_acceleration = 0.5;
  debiased_imu_readings_.setZero();
  debiased_imu_readings_.block<3, 1>(kAccelReadingOffset, 0) =
      Eigen::Vector3d(initial_acceleration, 0, gravity_magnitude_);
  debiased_imu_readings_.block<3, 1>(
      kImuReadingSize + kAccelReadingOffset, 0) =
      Eigen::Vector3d(0, 0, gravity_magnitude_);

  current_state_.setZero();
  // note that state contains B_q_G quaternion (it simplifies Jacobians)
  current_state_.block<kStateOrientationBlockSize, 1>(
      kStateOrientationOffset, 0) = Eigen::Vector4d(0, 0, 0, 1);

  integrate();

  EXPECT_NEAR_EIGEN(
      getVelocityFromState(next_state_),
      Eigen::Vector3d(initial_acceleration * delta_time_seconds_ / 2, 0, 0),
      1e-15);
  EXPECT_NEAR_EIGEN(
      getPositionFromState(next_state_),
      Eigen::Vector3d(
          initial_acceleration * pow(delta_time_seconds_, 2.0) / 3, 0, 0),
      1e-15);
}

MAPLAB_UNITTEST_ENTRYPOINT


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/preprocessor/arithmetic/dec.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>
#include <boost/preprocessor/control/expr_iif.hpp>
#include <boost/preprocessor/list/adt.hpp>
#include <boost/preprocessor/repetition/for.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/tuple/to_list.hpp>
#include <limits>
#include <pup.h>

#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Options/Options.hpp"
#include "Parallel/CharmPupable.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/EquationOfState.hpp"  // IWYU pragma: keep
#include "Utilities/TMPL.hpp"

/// \cond
class DataVector;
/// \endcond

// IWYU pragma: no_forward_declare Tensor

namespace EquationsOfState {
/*!
 * \ingroup EquationsOfStateGroup
 * \brief Equation of state for a polytropic fluid
 *
 * A polytropic equation of state \f$p=K\rho^{\Gamma}\f$ where \f$K\f$ is the
 * polytropic constant and \f$\Gamma\f$ is the polytropic exponent. The
 * polytropic exponent is related to the polytropic index \f$N_p\f$ by
 * \f$N_p=1/(\Gamma-1)\f$.
 */
template <bool IsRelativistic>
class PolytropicFluid : public EquationOfState<IsRelativistic, 1> {
 public:
  static constexpr size_t thermodynamic_dim = 1;
  static constexpr bool is_relativistic = IsRelativistic;

  struct PolytropicConstant {
    using type = double;
    static constexpr Options::String help = {"Polytropic constant K"};
    static double lower_bound() noexcept { return 0.0; }
  };

  struct PolytropicExponent {
    using type = double;
    static constexpr Options::String help = {"Polytropic exponent Gamma"};
    static double lower_bound() noexcept { return 1.0; }
  };

  static constexpr Options::String help = {
      "A polytropic fluid equation of state.\n"
      "The pressure is related to the rest mass density by p = K rho ^ Gamma, "
      "where p is the pressure, rho is the rest mass density, K is the "
      "polytropic constant, and Gamma is the polytropic exponent. The "
      "polytropic index N is defined as Gamma = 1 + 1 / N."};

  using options = tmpl::list<PolytropicConstant, PolytropicExponent>;

  PolytropicFluid() = default;
  PolytropicFluid(const PolytropicFluid&) = default;
  PolytropicFluid& operator=(const PolytropicFluid&) = default;
  PolytropicFluid(PolytropicFluid&&) = default;
  PolytropicFluid& operator=(PolytropicFluid&&) = default;
  ~PolytropicFluid() override = default;

  PolytropicFluid(double polytropic_constant,
                  double polytropic_exponent) noexcept;

  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBERS(PolytropicFluid, 1)

  WRAPPED_PUPable_decl_base_template(  // NOLINT
      SINGLE_ARG(EquationOfState<IsRelativistic, 1>), PolytropicFluid);

 private:
  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBER_IMPLS(1)

  double polytropic_constant_ = std::numeric_limits<double>::signaling_NaN();
  double polytropic_exponent_ = std::numeric_limits<double>::signaling_NaN();
};

/// \cond
template <bool IsRelativistic>
PUP::able::PUP_ID EquationsOfState::PolytropicFluid<IsRelativistic>::my_PUP_ID =
    0;
/// \endcond
}  // namespace EquationsOfState


/*
*   Greedy Search
*   by R. Falque
*   29/11/2018
*/

#ifndef GREEDY_SEARCH
#define GREEDY_SEARCH

#include <Eigen/Core>
#include <vector>

typedef int vertex_t;
typedef double weight_t;

struct neighbor {
    vertex_t target;
    weight_t weight;
    neighbor(vertex_t arg_target, weight_t arg_weight)
        : target(arg_target), weight(arg_weight) { }
};

typedef std::vector<std::vector<neighbor> > adjacency_list_t;

class greedy_search
{
public:
	greedy_search(Eigen::MatrixXd V,
					Eigen::MatrixXi F,
					std::vector<int> indexes_of_sub_V_in_V);
	~greedy_search(){
	}

	std::vector< int > return_k_closest_points(int source, int k);

private:
	int k_;
	int graph_size_;
	adjacency_list_t adjacency_list_;

	Eigen::MatrixXd V_;
	Eigen::MatrixXd sub_V_;
	Eigen::MatrixXi F_;

	std::vector<int> indexes_of_sub_V_in_V_;
};

#endif


#include <crab/config.h>

#ifdef HAVE_LDD
#include "../common.hpp"
#include <crab/domains/ldd/ldd.hpp>
#include <boost/optional.hpp>

using namespace std;
using namespace ikos;
using namespace crab::cfg_impl;
using namespace crab::domain_impl;
using namespace crab::domains::ldd;

typedef linear_constraint<z_number, varname_t> linear_constraint_t;
typedef linear_expression<z_number, varname_t> linear_expression_t;
typedef interval <z_number> interval_t;

LddManager* create_ldd_man (size_t num_vars) {
  DdManager* cudd = Cudd_Init (0, 0, CUDD_UNIQUE_SLOTS, 127, 0);
  theory_t * theory = tvpi_create_boxz_theory (num_vars);
  LddManager* ldd = Ldd_Init (cudd, theory);

  const bool dvo = true;
  if(dvo) Cudd_AutodynEnable (cudd, CUDD_REORDER_GROUP_SIFT);
  return ldd;
}

void destroy_ldd_man (LddManager* ldd) {
  DdManager *cudd = NULL;
  theory_t *theory = NULL;
  if (ldd) {
    cudd = Ldd_GetCudd (ldd);
    theory = Ldd_GetTheory (ldd);
    Ldd_Quit(ldd);
  }

  if (theory) tvpi_destroy_theory(theory);
  if (cudd) Cudd_Quit(cudd);
}

void dump (LddNodePtr val) {
  LddManager *ldd = getLddManager (val);
  DdManager *cudd = Ldd_GetCudd (ldd);
  FILE *fp = Cudd_ReadStdout(cudd);
  Cudd_SetStdout(cudd,stderr);
  if (val.get () == Ldd_GetTrue (ldd)) cout << "true\n";
  else if (val.get () == Ldd_GetFalse (ldd)) cout << "false\n";
  else Ldd_PrintMinterm(ldd,val.get ());
  Cudd_SetStdout(cudd,fp);
}

LddNodePtr top (LddManager *ldd)
{
  return lddPtr (ldd, Ldd_GetTrue (ldd));
}

LddNodePtr bot (LddManager *ldd)
{
  return lddPtr (ldd, Ldd_GetFalse (ldd));
}

bool isBot (LddManager * ldd, LddNodePtr v)
{
  return &*v == Ldd_GetFalse (ldd);
}

bool isTop (LddManager * ldd, LddNodePtr v)
{
  return &*v == Ldd_GetTrue (ldd);
}

LddNodePtr approx (LddManager *ldd, LddNodePtr v)
{
  return lddPtr (ldd, Ldd_TermMinmaxApprox(ldd, &*v));
}

LddNodePtr meet (LddManager *ldd, LddNodePtr n1, LddNodePtr n2)
{
  return lddPtr (ldd, Ldd_And (ldd, &*n1, &*n2));
}

LddNodePtr join (LddManager *ldd, LddNodePtr n1, LddNodePtr n2)
{
  return approx (ldd, lddPtr (ldd, Ldd_Or (ldd, &*n1, &*n2)));
}

bool isLeq (LddManager *ldd, LddNodePtr n1, LddNodePtr n2)
{
  return Ldd_TermLeq(ldd, &(*n1), &(*n2));
}

LddNodePtr widen (LddManager *ldd, LddNodePtr n1, LddNodePtr n2)
{
  return lddPtr (ldd, Ldd_IntervalWiden (ldd, &*n1, &*n2));
}

int getVarId (varname_t v) {
  return v.index ();
}

/** return term for variable v, neg for negation of variable */
linterm_t termForVal(LddManager* ldd, varname_t v, bool neg = false)
{
  int varId = getVarId (v);
  int sgn = neg ? -1 : 1;
  linterm_t term = Ldd_GetTheory (ldd)->create_linterm_sparse_si (&varId, &sgn, 1);
  return term;
}

/** v := k, where k is a constant */
LddNodePtr assign (LddManager *ldd, LddNodePtr n, varname_t v, z_number k) {
  if (isBot (ldd, n)) return n;

  mpq_class kk ((mpz_class) k);
  linterm_t t = termForVal (ldd, v);
  constant_t kkk = (constant_t) tvpi_create_cst (kk.get_mpq_t ());
  LddNodePtr newVal =
      lddPtr(ldd, Ldd_TermReplace(ldd, &(*n), t, NULL, NULL, kkk, kkk));
  Ldd_GetTheory (ldd)->destroy_cst(kkk);

  assert (!isBot (ldd, newVal));
  return newVal;
}

/** v := k, where k is an interval */
LddNodePtr assign (LddManager* ldd, LddNodePtr n, varname_t v, interval_t ival) {

  if (isBot (ldd, n)) return n;

  constant_t kmin = NULL, kmax = NULL;

  if (boost::optional <z_number> l = ival.lb ().number ()) {
    mpq_class val ((mpz_class) (*l));
    kmin = (constant_t)tvpi_create_cst (val.get_mpq_t ());
  }

  if (boost::optional <z_number> u = ival.ub ().number ()) {
    mpq_class val ((mpz_class) (*u));
    kmax = (constant_t)tvpi_create_cst (val.get_mpq_t ());
  }

  linterm_t t = termForVal(ldd, v);
  LddNodePtr newVal =
      lddPtr(ldd, Ldd_TermReplace(ldd, &(*n), t, NULL, NULL, kmin, kmax));

  if (kmin) Ldd_GetTheory (ldd)->destroy_cst(kmin);
  if (kmax) Ldd_GetTheory (ldd)->destroy_cst(kmax);

  assert (!isBot (ldd, newVal));
  return newVal;
}

/** v := a * u + k, where a, k are constants and u variable */
LddNodePtr apply (LddManager* ldd,
                  const LddNodePtr n,
                  varname_t v, varname_t u,
                  z_number a, z_number k)
{
  if (isTop (ldd, n) || isBot (ldd, n)) return n;

  linterm_t t = termForVal (ldd, v);
  linterm_t r = termForVal (ldd, u);

  mpq_class aa ((mpz_class) a);
  mpq_class kk ((mpz_class) k);

  constant_t aaa = (constant_t) tvpi_create_cst (aa.get_mpq_t ());
  constant_t kkk = (constant_t) tvpi_create_cst (kk.get_mpq_t ());
  LddNodePtr newVal =
      lddPtr(ldd, Ldd_TermReplace(ldd, &(*n), t, r, aaa, kkk, kkk));
  Ldd_GetTheory (ldd)->destroy_cst(aaa);
  Ldd_GetTheory (ldd)->destroy_cst(kkk);

  assert (!isBot (ldd, newVal));
  return newVal;
}

/** v := u */
LddNodePtr assign (LddManager* ldd, const LddNodePtr n,
                   varname_t v, varname_t u)
{
  return apply (ldd, n, v, u, 1, 0);
}

int main (int argc, char** argv )
{
  SET_TEST_OPTIONS(argc,argv)

  LddManager* ldd = create_ldd_man (500);

  variable_factory_t vfac;
  varname_t x = vfac["x"];
  varname_t y = vfac["y"];

  LddNodePtr s1  = top (ldd);
  LddNodePtr s11 = assign (ldd, s1, x, 5);
  LddNodePtr s111 = assign (ldd, s11, y, 8);

  LddNodePtr s2  = top (ldd);
  LddNodePtr s22 = assign (ldd, s2, x, 10);
  LddNodePtr s222 = assign (ldd, s22, y, 12);

  cout << "Join \n";
  cout.flush ();
  dump (s111);
  dump (s222);
  cout << "Result \n";
  cout.flush ();
  LddNodePtr s3 = join (ldd, s111, s222);
  dump (s3);

  // FIXME: seg fault here
  // destroy_ldd_man (ldd);
  return 0;
}
#endif


#pragma once

#include <deal.II/base/quadrature.h>
#include <deal.II/base/types.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/mapping_q.h>
#include <deal.II/grid/grid_tools.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include "grid/grid_tools.hpp"

namespace boltzmann {

template <int DIM>
class GridTransfer
{
 private:
  typedef dealii::types::global_dof_index index_t;
  typedef Eigen::SparseMatrix<double> sparse_matrix_t;
  typedef dealii::DoFHandler<DIM> dh_t;

 public:
  const sparse_matrix_t& get_transfer_matrix() { return T_; }
  void init(const dh_t& dh, const dh_t& dh_src);

 private:
  //  dealii::DoFHandler<DIM> dofhandler_;
  sparse_matrix_t T_;
};

template <int DIM>
void
GridTransfer<DIM>::init(const dh_t& dh, const dh_t& dh_src)
{
  auto v2d = vertex_to_dof_index(dh);
  auto v2d_src = vertex_to_dof_index(dh_src);
  unsigned int N = dh.n_dofs();
  unsigned int n = dh_src.n_dofs();
  T_.resize(N, n);
  const auto& vertices = dh.get_triangulation().get_vertices();
  const auto& vertices_source = dh_src.get_triangulation().get_vertices();
  std::set<index_t> todo_vertices;
  for (index_t i = 0; i < N; ++i) todo_vertices.insert(i);

  const double tol = 1e-9;
  // identifiy same vertices
  for (unsigned int j = 0; j < vertices_source.size(); ++j) {
    for (unsigned int i = 0; i < vertices.size(); ++i) {
      if (vertices_source[j].distance(vertices[i]) < tol) {
        T_.insert(v2d[i], v2d_src[j]) = 1;
        todo_vertices.erase(i);
      }
    }
  }

  typedef dealii::FEValues<DIM> fe_values_t;
  const auto& fe = dh_src.get_fe();

  const int dofs_per_cell = fe.dofs_per_cell;
  dealii::MappingQ1<DIM, DIM> map;
  std::vector<index_t> local_dof_indices(dofs_per_cell);

  // go through all remaining vertices and find
  // their parent cells to find the interpolation coefficients
  for (index_t i : todo_vertices) {
    const auto& p = vertices[i];
    auto cell = dealii::GridTools::find_active_cell_around_point(dh_src, p);
    cell->get_dof_indices(local_dof_indices);
    auto q = map.transform_real_to_unit_cell(cell, p);

    dealii::Quadrature<DIM> quad(q);
    dealii::UpdateFlags update_flags = (dealii::update_values | dealii::update_quadrature_points);
    fe_values_t fval(map, fe, quad, update_flags);

    fval.reinit(cell);
    for (int ix = 0; ix < dofs_per_cell; ++ix) {
      double v = fval.shape_value(ix, 0);
      if (std::abs(v) > 1e-10) T_.insert(v2d[i], local_dof_indices[ix]) = v;
    }
  }
}

}  // end namespace boltzmann


// Copyright (c) 2020 [Your Name]. All rights reserved.

#include "my_app.h"

#include <cinder/app/App.h>
#include <cinder/Font.h>
#include <cinder/Text.h>
#include <cinder/Vector.h>
#include <cinder/gl/draw.h>
#include <cinder/gl/gl.h>
#include <Eigen/Dense>
#include "Eigen/Core"
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/core/eigen.hpp"
#include <iostream>
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "mylibrary/model.h"

#include "opencv2/photo/photo.hpp"
#include "CinderImGui.h"

#include <algorithm>
#include <chrono>
#include <cmath>
#include <string>
#include "randomizedSVD.h"

namespace myapp {

    using namespace cv;

    using cinder::Color;
    using cinder::ColorA;
    using cinder::Rectf;
    using cinder::TextBox;
    using cinder::app::KeyEvent;
    using std::string;
    const char kNormalFont[] = "Arial";
    void PrintText(const string &text, const Color &color, const cinder::ivec2 &size,
                   const cinder::vec2 &loc);
//    float slide = 0.f;
//    string file;
//    double compression;
//    bool display_image = false;
//    bool compress_image = false;
//    bool rewrite_image = false;
//    bool make_new_file = false;
//    bool use_compression = false;
//    bool exit_image_app = false;
//    bool exit_compression_app = false;
//    bool enter_compress = false;
    MyApp::MyApp() {

        ImGui::initialize();

    }

    void MyApp::setup() {
      //  draw();
    }

    void MyApp::update() {

        cinder::gl::clear( Color( 0, 0, 0 ) );

        if (ImGui::Button("Grayscale Image Sharpening")) {
            display_image = !display_image;
        } else if (ImGui::Button("Compress Image")) {
            compress_image = !compress_image;
        }

        if (display_image) {

            ui::ScopedWindow window("Grayscale Image Sharpening", ImGuiWindowFlags_None);
            ui::Text("Select How Sharp You Want Your Image");
            ui::SliderFloat("slide", &slide, 0.f, 2.f);

            string file_name = "";
            if(ui::InputText("Enter your image", &file_name)) {

               if (file_name.size() != 0) {
                   file = file_name;
               }

            }

            if (file.size() != 0) {

                if (!mylibrary::IsValidFile(file)) {
                    ui::Text("Invalid File");
                } else {
                    mylibrary::SharpenImage(file, slide, rewrite_image, make_new_file);
                }

            }
            if (exit_image_app) {

                display_image = false;
                exit_image_app = false;

            }

            rewrite_image = ui::Button("Rewrite File");
            make_new_file = ui::Button("Make New File");
            exit_image_app = ui::Button("Exit");

        } else if (compress_image) {

            ui::ScopedWindow window("Compress Image", ImGuiWindowFlags_None);
            ui::Text("Choose How Compressed You Want Your Image (as a double between 0 and 1)");

            ui::InputDouble("Enter Compression", &compression);

            ui::InputText("Enter Your Image", &file);

            if (enter_compress || rewrite_image || make_new_file) {
                if (!mylibrary::IsValidFile(file)) {
                    invalid_file = true;
                    //ui::Text("Invalid File");
                } else if (!mylibrary::CompressImage(file, compression, rewrite_image, make_new_file)) {
                    invalid_compression = true;
                    // ui::Text("Invalid Compression Value");
                } else {
                    invalid_file = false;
                    invalid_compression = false;
                }

            }

            if (invalid_file) {
                ui::Text("Invalid File");
            } else if (invalid_compression) {
                ui::Text("Invalid Compression Value");
            }

            if (exit_compression_app) {
                compress_image = false;
                exit_compression_app = false;
            }

            enter_compress = ui::Button("Enter");
            rewrite_image = ui::Button("Rewrite File");
            make_new_file = ui::Button("Make New File");
            exit_compression_app = ui::Button("Exit");

        }

    }

    void MyApp::draw() {
        DrawMenu();

      //  int slid = 0;
     //   ui::SliderInt("slidee", &slid, 0, 100);

    }

    void MyApp::DrawMenu() {
        //const string app_name = "Image Altering App";
        const Color color = Color::white();
        const cinder::ivec2 size = {500, 50};
        const cinder::vec2 center = getWindowCenter();

        PrintText("Image Altering App", color, size, {center.x, 100});

    }

    void MyApp::keyDown(KeyEvent event) {}

    //template <typename C>
    void PrintText(const string &text, const Color &color, const cinder::ivec2 &size,
                   const cinder::vec2 &loc) {
        cinder::gl::color(color);

        auto box = TextBox()
                .alignment(TextBox::CENTER)
                .font(cinder::Font(kNormalFont, 30))
                .size(size)
                .color(color)
                .backgroundColor(ColorA(0, 0, 0, 0))
                .text(text);

        const auto box_size = box.getSize();
        const cinder::vec2 locp = {loc.x - box_size.x / 2, loc.y - box_size.y / 2};
        const auto surface = box.render();
        const auto texture = cinder::gl::Texture::create(surface);
        cinder::gl::draw(texture, locp);
    }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// 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)
//
// Example showing Boost.Geometry combined with Boost.Graph, calculating shortest routes
// input: two WKT's, provided in subfolder data
// output: text, + an SVG, displayable in e.g. Firefox)

// Second variant, with bundled properties

#include <iostream>
#include <fstream>
#include <iomanip>
#include <limits>

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

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/io/wkt/read.hpp>

// For output:
#include <boost/geometry/io/svg/svg_mapper.hpp>

// For distance-calculations over the Earth:
//#include <boost/geometry/extensions/gis/geographic/strategies/andoyer.hpp>

// Read an ASCII file containing WKT's, fill a vector of tuples
// The tuples consist of at least <0> a geometry and <1> an identifying string
template <typename Geometry, typename Tuple, typename Box>
void read_wkt(std::string const& filename, std::vector<Tuple>& tuples, Box& box)
{
    std::ifstream cpp_file(filename.c_str());
    if (cpp_file.is_open())
    {
        while (! cpp_file.eof() )
        {
            std::string line;
            std::getline(cpp_file, line);
            Geometry geometry;
            boost::trim(line);
            if (! line.empty() && ! boost::starts_with(line, "#"))
            {
                std::string name;

                // Split at ';', if any
                std::string::size_type pos = line.find(";");
                if (pos != std::string::npos)
                {
                    name = line.substr(pos + 1);
                    line.erase(pos);

                    boost::trim(line);
                    boost::trim(name);
                }

                Geometry geometry;
                boost::geometry::read_wkt(line, geometry);

                Tuple tuple(geometry, name);

                tuples.push_back(tuple);
                boost::geometry::expand(box, boost::geometry::return_envelope<Box>(geometry));
            }
        }
    }
}

// To calculate distance, declare and construct a strategy with average earth radius
boost::geometry::strategy::distance::haversine<double> const haversine(6372795.0);

// Define properties for vertex
template <typename Point>
struct bg_vertex_property
{
    bg_vertex_property()
    {
        boost::geometry::assign_zero(location);
    }
    bg_vertex_property(Point const& loc)
        : location(loc)
    {
    }

    Point location;
};

// Define properties for edge
template <typename Linestring>
struct bg_edge_property
{
    bg_edge_property(Linestring const& line)
        : length(boost::geometry::length(line, haversine))
        , m_line(line)
    {
    }

    inline Linestring const& line() const
    {
        return m_line;
    }

    double length;
private :
    Linestring m_line;
};

// Utility function to add a vertex to a graph. It might exist already. Then do not insert,
// but return vertex descriptor back. It might not exist. Then add it (and return).
// To efficiently handle this, a std::map is used.
template <typename M, typename K, typename G>
inline typename boost::graph_traits<G>::vertex_descriptor find_or_insert(M& map, K const& key, G& graph)
{
    typename M::const_iterator it = map.find(key);
    if (it == map.end())
    {
        // Add a vertex to the graph
        typename boost::graph_traits<G>::vertex_descriptor new_vertex
            = boost::add_vertex(bg_vertex_property<typename M::key_type>(key), graph);

        // Add to the map, using POINT as key
        map[key] = new_vertex;
        return new_vertex;
    }
    return it->second;
}

template
<
    typename Graph,
    typename RoadTupleVector,
    typename CityTupleVector
>
void add_roads_and_connect_cities(Graph& graph,
            RoadTupleVector const& roads,
            CityTupleVector& cities)
{
    typedef typename boost::range_value<RoadTupleVector>::type road_type;
    typedef typename boost::tuples::element<0, road_type>::type line_type;
    typedef typename boost::geometry::point_type<line_type>::type point_type;

    typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_type;

    // Define a map to be used during graph filling
    // Maps from point to vertex-id's
    typedef std::map<point_type, vertex_type, boost::geometry::less<point_type> > map_type;
    map_type map;

    // Fill the graph
    BOOST_FOREACH(road_type const& road, roads)
    {
        line_type const& line = road.template get<0>();
        // Find or add begin/end point of these line
        vertex_type from = find_or_insert(map, line.front(), graph);
        vertex_type to = find_or_insert(map, line.back(), graph);
        boost::add_edge(from, to, bg_edge_property<line_type>(line), graph);
    }

    // Find nearest graph vertex for each city, using the map
    typedef typename boost::range_value<CityTupleVector>::type city_type;
    BOOST_FOREACH(city_type& city, cities)
    {
        double min_distance = 1e300;
        for(typename map_type::const_iterator it = map.begin(); it != map.end(); ++it)
        {
            double dist = boost::geometry::distance(it->first, city.template get<0>());
            if (dist < min_distance)
            {
                min_distance = dist;
                // Set the vertex
                city.template get<2>() = it->second;
            }
        }
    }
}

template <typename Graph, typename Route>
inline void add_edge_to_route(Graph const& graph,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex1,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex2,
            Route& route)
{
    std::pair
        <
            typename boost::graph_traits<Graph>::edge_descriptor,
            bool
        > opt_edge = boost::edge(vertex1, vertex2, graph);
    if (opt_edge.second)
    {
        // Get properties of edge and of vertex
        bg_edge_property<Route> const& edge_prop = graph[opt_edge.first];
        bg_vertex_property
            <
                typename boost::geometry::point_type<Route>::type
            > const& vertex_prop = graph[vertex2];

        // Depending on how edge connects to vertex, copy it forward or backward
        if (boost::geometry::equals(edge_prop.line().front(), vertex_prop.location))
        {
            std::copy(edge_prop.line().begin(), edge_prop.line().end(),
                std::back_inserter(route));
        }
        else
        {
            std::reverse_copy(edge_prop.line().begin(), edge_prop.line().end(),
                std::back_inserter(route));
        }
    }
}

template <typename Graph, typename Route>
inline void build_route(Graph const& graph,
            std::vector<typename boost::graph_traits<Graph>::vertex_descriptor> const& predecessors,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex1,
            typename boost::graph_traits<Graph>::vertex_descriptor vertex2,
            Route& route)
{
    typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_type;
    vertex_type pred = predecessors[vertex2];

    add_edge_to_route(graph, vertex2, pred, route);
    while (pred != vertex1)
    {
        add_edge_to_route(graph, predecessors[pred], pred, route);
        pred = predecessors[pred];
    }
}

int main()
{
    // Define a point in the Geographic coordinate system (currently spherical-equatorial)
    // (geographic calculations are in an extension; for sample it makes no difference)
    typedef boost::geometry::model::point
        <
            double, 2, boost::geometry::cs::spherical_equatorial<boost::geometry::degree>
        > point_type;

    typedef boost::geometry::model::linestring<point_type> line_type;

    // Define the graph, lateron containing the road network
    // With this, specify bundled properties for vertex and edge,
    // as in http://www.boost.org/doc/libs/1_43_0/libs/graph/doc/bundles.html
    typedef boost::adjacency_list
        <
            boost::vecS, boost::vecS, boost::undirectedS
            , bg_vertex_property<point_type> // bundled
            , bg_edge_property<line_type>
        > graph_type;

    typedef boost::graph_traits<graph_type>::vertex_descriptor vertex_type;

    // Init a bounding box, lateron used to define SVG map
    boost::geometry::model::box<point_type> box;
    boost::geometry::assign_inverse(box);

    graph_type graph;

    // Read the cities
    typedef boost::tuple<point_type, std::string, vertex_type> city_type;
    std::vector<city_type> cities;
    read_wkt<point_type>("data/cities.wkt", cities, box);

    // Read the road network
    typedef boost::tuple<line_type, std::string> road_type;
    std::vector<road_type> roads;
    read_wkt<line_type>("data/roads.wkt", roads, box);

    // Add roads and connect cities
    add_roads_and_connect_cities(graph, roads, cities);

    double const km = 1000.0;
    std::cout << "distances, all in KM" << std::endl
        << std::fixed << std::setprecision(0);

    // Main functionality: calculate shortest routes from/to all cities

    // For the first one, the complete route is stored as a linestring
    bool first = true;
    line_type route;

    int const n = boost::num_vertices(graph);
    BOOST_FOREACH(city_type const& city1, cities)
    {
        std::vector<vertex_type> predecessors(n);
        std::vector<double> costs(n);

        // Call Dijkstra (without named-parameter to be compatible with all VC)
        boost::dijkstra_shortest_paths(graph, city1.get<2>(),
                &predecessors[0], &costs[0],
                boost::get(&bg_edge_property<line_type>::length, graph),
                boost::get(boost::vertex_index, graph),
                std::less<double>(), std::plus<double>(),
                (std::numeric_limits<double>::max)(), double(),
                boost::dijkstra_visitor<boost::null_visitor>());

        BOOST_FOREACH(city_type const& city2, cities)
        {
            if (! boost::equals(city1.get<1>(), city2.get<1>()))
            {
                double distance = costs[city2.get<2>()] / km;
                double acof = boost::geometry::distance(city1.get<0>(), city2.get<0>(), haversine) / km;

                std::cout
                    << std::setiosflags (std::ios_base::left) << std::setw(15)
                        << city1.get<1>() << " - "
                    << std::setiosflags (std::ios_base::left) << std::setw(15)
                        << city2.get<1>()
                    << " -> through the air: " << std::setw(4) << acof
                    << " , over the road: " << std::setw(4) << distance
                    << std::endl;

                if (first)
                {
                    build_route(graph, predecessors,
                            city1.get<2>(), city2.get<2>(),
                            route);
                    first = false;
                }
            }
        }
    }

#if defined(HAVE_SVG)
    // Create the SVG
    std::ofstream stream("routes.svg");
    boost::geometry::svg_mapper<point_type> mapper(stream, 600, 600);

    // Map roads
    BOOST_FOREACH(road_type const& road, roads)
    {
        mapper.add(road.get<0>());
    }

    BOOST_FOREACH(road_type const& road, roads)
    {
        mapper.map(road.get<0>(),
                "stroke:rgb(128,128,128);stroke-width:1");
    }

    mapper.map(route,
            "stroke:rgb(0, 255, 0);stroke-width:6;opacity:0.5");

    // Map cities
    BOOST_FOREACH(city_type const& city, cities)
    {
        mapper.map(city.get<0>(),
                "fill:rgb(255,255,0);stroke:rgb(0,0,0);stroke-width:1");
        mapper.text(city.get<0>(), city.get<1>(),
                "fill:rgb(0,0,0);font-family:Arial;font-size:10px", 5, 5);
    }
#endif

    return 0;
}


#pragma once
#include "utils/math_utils.h"
#include "libs/sources_handling/source_data.h"
#include "libs/sources_handling/input_data.h"
#include <boost/range/algorithm/copy.hpp>
#include <boost/iterator/function_input_iterator.hpp>
#include <boost/range/algorithm/remove_if.hpp>
#include <vector>
#include <iostream>

using missing_input = std::vector<double>;
using data_set = missing_input;

template<typename It> inline
missing_input slice_data(const data_set& from, It pivot, std::size_t step)
{
    if (step == 1 && *pivot != 0u && *pivot < from.size()-2u) {
        auto sb = step;
        if (*(pivot-1) + 1 == *pivot) {
            ++sb;
        }
        auto sf = step;
        if (*(pivot+1) - 1 == *pivot) {
            ++sf;
        }
        return missing_input{from[*pivot - sb], from[*pivot + sf]};
    }
    auto st  = std::min(step, *pivot);
    auto ws = std::min(step, from.size()-*pivot) * 2 + 1;
    auto at = std::next(std::begin(from), (*pivot - st));
    auto l = std::next(at, ws);
    return missing_input(at, l);
}

template<typename It> inline
math::fx_list_type xi_values(It pivot, std::size_t step)
{
    if (step == 1 && *pivot != 0u) {
        auto sb = step;
        auto sf = step;
        if (*(pivot-1) + 1 == *pivot) {
            ++sb;
        }
        if (*(pivot+1) - 1 == *pivot) {
            ++sf;
        }
        math::fx_list_type::value_type p = static_cast<math::fx_list_type::value_type>(*pivot);
        return math::fx_list_type{p-sb, p+sf};
    } else {
        return math::fx_list_type{};
    }
}

struct reverse_generator
{
    using result_type = input_data;

    reverse_generator(const double* s, const double* t) :
                start(s), end(t), index(0)
    {
    }

    result_type operator () ()
    {
        if (start != end) {
            return result_type{*start++, index++};
        } else {
            return result_type{0, 0};
        }
    }

    const double* start = nullptr;
    const double* end = nullptr;
    unsigned int index = 0;
};

inline source_data
from_double_array(const double* from, const double* to,
        const std::string& name)
{
    reverse_generator gen{from, to};
    return source_data{name,
                    boost::make_function_input_iterator(gen, 0u),
                    boost::make_function_input_iterator(gen, (unsigned int)(to-from))
            };

}

template<typename It> inline source_data
filter_out(source_data input, It invaid_start, It invalid_end)
{
    auto i = boost::remove_if(input, [invaid_start, invalid_end](auto entry) {
                                return std::binary_search(invaid_start, invalid_end, entry.row);
                        }
            );
    return source_data{input.id(), std::begin(input), i};
}


//=======================================================================
// Copyright (c) Aaron Windsor 2007
//
// 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)
//=======================================================================

#ifndef __CHROBAK_PAYNE_DRAWING_HPP__
#define __CHROBAK_PAYNE_DRAWING_HPP__

#include <vector>
#include <list>
#include <boost/config.hpp>
#include <boost/utility.hpp>  //for next and prior
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

namespace boost
{

  namespace graph { namespace detail
  {

    template<typename Graph,
             typename VertexToVertexMap,
             typename VertexTo1DCoordMap>
    void accumulate_offsets(typename graph_traits<Graph>::vertex_descriptor v,
                            std::size_t offset,
                            const Graph& g,
                            VertexTo1DCoordMap x,
                            VertexTo1DCoordMap delta_x,
                            VertexToVertexMap left,
                            VertexToVertexMap right)
    {
      typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
      // Suggestion of explicit stack from Aaron Windsor to avoid system stack
      // overflows.
      typedef std::pair<vertex_descriptor, std::size_t> stack_entry;
      std::stack<stack_entry> st;
      st.push(stack_entry(v, offset));
      while (!st.empty()) {
        vertex_descriptor v = st.top().first;
        std::size_t offset = st.top().second;
        st.pop();
        if (v != graph_traits<Graph>::null_vertex()) {
          x[v] += delta_x[v] + offset;
          st.push(stack_entry(left[v], x[v]));
          st.push(stack_entry(right[v], x[v]));
        }
      }
    }

  } /*namespace detail*/ } /*namespace graph*/

  template<typename Graph,
           typename PlanarEmbedding,
           typename ForwardIterator,
           typename GridPositionMap,
           typename VertexIndexMap>
  void chrobak_payne_straight_line_drawing(const Graph& g,
                                           PlanarEmbedding embedding,
                                           ForwardIterator ordering_begin,
                                           ForwardIterator ordering_end,
                                           GridPositionMap drawing,
                                           VertexIndexMap vm
                                           )
  {

    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::edge_descriptor edge_t;
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
    typedef typename PlanarEmbedding::value_type::const_iterator
      edge_permutation_iterator_t;
    typedef typename graph_traits<Graph>::vertices_size_type v_size_t;
    typedef std::vector<vertex_t> vertex_vector_t;
    typedef std::vector<v_size_t> vsize_vector_t;
    typedef std::vector<bool> bool_vector_t;
    typedef boost::iterator_property_map
      <typename vertex_vector_t::iterator, VertexIndexMap>
      vertex_to_vertex_map_t;
    typedef boost::iterator_property_map
      <typename vsize_vector_t::iterator, VertexIndexMap>
      vertex_to_vsize_map_t;
    typedef boost::iterator_property_map
      <typename bool_vector_t::iterator, VertexIndexMap>
      vertex_to_bool_map_t;

    vertex_vector_t left_vector(num_vertices(g),
                                graph_traits<Graph>::null_vertex()
                                );
    vertex_vector_t right_vector(num_vertices(g),
                                 graph_traits<Graph>::null_vertex()
                                 );
    vsize_vector_t seen_as_right_vector(num_vertices(g), 0);
    vsize_vector_t seen_vector(num_vertices(g), 0);
    vsize_vector_t delta_x_vector(num_vertices(g),0);
    vsize_vector_t y_vector(num_vertices(g));
    vsize_vector_t x_vector(num_vertices(g),0);
    bool_vector_t installed_vector(num_vertices(g),false);

    vertex_to_vertex_map_t left(left_vector.begin(), vm);
    vertex_to_vertex_map_t right(right_vector.begin(), vm);
    vertex_to_vsize_map_t seen_as_right(seen_as_right_vector.begin(), vm);
    vertex_to_vsize_map_t seen(seen_vector.begin(), vm);
    vertex_to_vsize_map_t delta_x(delta_x_vector.begin(), vm);
    vertex_to_vsize_map_t y(y_vector.begin(), vm);
    vertex_to_vsize_map_t x(x_vector.begin(), vm);
    vertex_to_bool_map_t installed(installed_vector.begin(), vm);

    v_size_t timestamp = 1;
    vertex_vector_t installed_neighbors;

    ForwardIterator itr = ordering_begin;
    vertex_t v1 = *itr; ++itr;
    vertex_t v2 = *itr; ++itr;
    vertex_t v3 = *itr; ++itr;

    delta_x[v2] = 1;
    delta_x[v3] = 1;

    y[v1] = 0;
    y[v2] = 0;
    y[v3] = 1;

    right[v1] = v3;
    right[v3] = v2;

    installed[v1] = installed[v2] = installed[v3] = true;

    for(ForwardIterator itr_end = ordering_end; itr != itr_end; ++itr)
      {
        vertex_t v = *itr;

        // First, find the leftmost and rightmost neighbor of v on the outer
        // cycle of the embedding.
        // Note: since we're moving clockwise through the edges adjacent to v,
        // we're actually moving from right to left among v's neighbors on the
        // outer face (since v will be installed above them all) looking for
        // the leftmost and rightmost installed neigbhors

        vertex_t leftmost = graph_traits<Graph>::null_vertex();
        vertex_t rightmost = graph_traits<Graph>::null_vertex();

        installed_neighbors.clear();

        vertex_t prev_vertex = graph_traits<Graph>::null_vertex();
        edge_permutation_iterator_t pi, pi_end;
        pi_end = embedding[v].end();
        for(pi = embedding[v].begin(); pi != pi_end; ++pi)
          {
            vertex_t curr_vertex = source(*pi,g) == v ?
              target(*pi,g) : source(*pi,g);

            // Skip any self-loops or parallel edges
            if (curr_vertex == v || curr_vertex == prev_vertex)
                continue;

            if (installed[curr_vertex])
              {
                seen[curr_vertex] = timestamp;

                if (right[curr_vertex] != graph_traits<Graph>::null_vertex())
                  {
                    seen_as_right[right[curr_vertex]] = timestamp;
                  }
                installed_neighbors.push_back(curr_vertex);
              }

            prev_vertex = curr_vertex;
          }

        typename vertex_vector_t::iterator vi, vi_end;
        vi_end = installed_neighbors.end();
        for(vi = installed_neighbors.begin(); vi != vi_end; ++vi)
          {
            if (right[*vi] == graph_traits<Graph>::null_vertex() ||
                seen[right[*vi]] != timestamp
                )
              rightmost = *vi;
            if (seen_as_right[*vi] != timestamp)
              leftmost = *vi;
          }

        ++timestamp;

        //stretch gaps
        ++delta_x[right[leftmost]];
        ++delta_x[rightmost];

        //adjust offsets
        std::size_t delta_p_q = 0;
        vertex_t stopping_vertex = right[rightmost];
        for(vertex_t temp = right[leftmost]; temp != stopping_vertex;
            temp = right[temp]
            )
          {
            delta_p_q += delta_x[temp];
          }

        delta_x[v] = ((y[rightmost] + delta_p_q) - y[leftmost])/2;
        y[v] = y[leftmost] + delta_x[v];
        delta_x[rightmost] = delta_p_q - delta_x[v];

        bool leftmost_and_rightmost_adjacent = right[leftmost] == rightmost;
        if (!leftmost_and_rightmost_adjacent)
          delta_x[right[leftmost]] -= delta_x[v];

        //install v
        if (!leftmost_and_rightmost_adjacent)
          {
            left[v] = right[leftmost];
            vertex_t next_to_rightmost;
            for(vertex_t temp = leftmost; temp != rightmost;
                temp = right[temp]
                )
              {
                next_to_rightmost = temp;
              }

            right[next_to_rightmost] = graph_traits<Graph>::null_vertex();
          }
        else
          {
            left[v] = graph_traits<Graph>::null_vertex();
          }

        right[leftmost] = v;
        right[v] = rightmost;
        installed[v] = true;

      }

    graph::detail::accumulate_offsets
      (*ordering_begin,0,g,x,delta_x,left,right);

    vertex_iterator_t vi, vi_end;
    for(boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
      {
        vertex_t v(*vi);
        drawing[v].x = x[v];
        drawing[v].y = y[v];
      }

  }

  template<typename Graph,
           typename PlanarEmbedding,
           typename ForwardIterator,
           typename GridPositionMap>
  inline void chrobak_payne_straight_line_drawing(const Graph& g,
                                                  PlanarEmbedding embedding,
                                                  ForwardIterator ord_begin,
                                                  ForwardIterator ord_end,
                                                  GridPositionMap drawing
                                                  )
  {
    chrobak_payne_straight_line_drawing(g,
                                        embedding,
                                        ord_begin,
                                        ord_end,
                                        drawing,
                                        get(vertex_index,g)
                                        );
  }

} // namespace boost

#endif //__CHROBAK_PAYNE_DRAWING_HPP__


#include <iostream>
#include <math.h>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

#include "MotionModels.h"

using std::cout;
using std::endl;

using namespace boost::numeric::odeint;

namespace syllo {

     void write_cout( const double &x , const double t )
     {
	  cout << t << '\t' << x << endl;
     }

     void write_lorenz( const state_3d_type &x , const double t)
     {
	  cout << t << '\t' << x[0] << '\t' << x[1] << '\t' << x[2] << endl;
     }

     ///void test()
     ///{
     ///	  state_type x;
     ///	  runge_kutta4< state_type > stepper;
     ///	  harm_osc osc(0.15);
     ///	  integrate_const( stepper ,  osc, x , 0.0 , 10.0 , 0.01 );
     ///	  const double dt = 0.01;
     ///	  for( double t = 0.0 ; t < 10.0 ; t += dt ) {
     ///	       stepper.do_step(  osc, x , t , dt );
     ///	  }
     ///}

     ///void lorenz_trajectory(std::vector<>)
     ///{
     ///}

     //void integrate()
     //{
     //	  integrate( syllo::lorenz , x , 0.0 , 25.0 , 0.1 , syllo::write_lorenz );
     //}

     void cart_model(const state_3d_type &x , state_3d_type &dxdt , double t)
     {
	  /// 0 : x-position
	  /// 1 : y-position
	  /// 2 : theta

	  double u = 2;
	  double u_theta = SYLLO_PI/20;
	  //double u_theta = 0;
	  double L = 3;
	  dxdt[0] = u*cos(x[2]);
	  dxdt[1] = u*sin(x[2]);
	  dxdt[2] = u/L*tan(u_theta);
     }

     struct streaming_observer
     {
	  std::ostream &m_out;
	  streaming_observer( std::ostream &out ) : m_out( out ) {}

	  void operator()( const state_3d_type &x , double t ) const
	       {
		    m_out << "Step - t:" << t;
		    for( size_t i=0 ; i < x.size() ; ++i )
			 m_out << "\tx[" << i << "]:" << x[i];
		    m_out << "\n";
	       }
     };

     void cart_trajectory(double t0, double dt, double tend,
			  std::vector<double> &xPos,
			  std::vector<double> &yPos,
			  std::vector<double> &heading,
			  std::vector<cv::Point2f> &cart_pos_truth)
     {
	  //state_3d_type x = {0,0,SYLLO_PI/4};
	  state_3d_type x = {0,0,0};
	  //typedef controlled_runge_kutta< runge_kutta_cash_karp54< state_3d_type > > stepper_type;
	  //integrate_adaptive( stepper_type() , cart_model , x , t0 , tend , dt , streaming_observer( cout ) );
	  //integrate_const( runge_kutta4< state_3d_type >() , cart_model , x , t0 , tend , dt , streaming_observer( cout) );

	  //integrate_const( runge_kutta4< state_3d_type >() , lorenz , x , t0 , tend , dt , streaming_observer( cout) );

	  ///state_3d_type x = {0,0,0};
     	  runge_kutta4< state_3d_type > stepper;
	  /////integrate_const( stepper ,  cart_model, x , t0 , tend , dt );
	  ///

	  //typedef controlled_runge_kutta< state_3d_type > controlled_stepper_type;
	  //controlled_stepper_type controlled_stepper;

	  for( double t = t0 ; t < tend ; t += dt ) {
	       xPos.push_back(x[0]);
	       yPos.push_back(x[1]);
	       heading.push_back(x[2]*180/SYLLO_PI);
	       cart_pos_truth.push_back(cv::Point2f(x[0], x[1]));
	       //cart_pos_truth.push_back(cv::Point2f(x[0], x[2]));
     	       //stepper.do_step(cart_model, x , t , dt );

	       //cout << "t:" << t << "\t" << x[0] << "\t" << x[1] << "\t" << x[2] << endl;

	       //stepper.do_step(lorenz, x , t , dt );
	       stepper.do_step(cart_model, x , t , dt );
	       //controlled_stepper.do_step(cart_model, x , t , dt );
	  }
     }

     void lorenz( const state_3d_type &x , state_3d_type &dxdt , double t )
     {
	  const double sigma = 10.0;
	  const double R = 28.0;
	  const double b = 8.0 / 3.0;

	  dxdt[0] = sigma * ( x[1] - x[0] );
	  dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
	  dxdt[2] = -b * x[2] + x[0] * x[1];
     }

     void simple_rhs( const double x , double &dxdt , const double t )
     {
	  dxdt = 3.0/(2.0*t*t) + x/(2.0*t);
     }

     void addMotionNoise(std::vector<cv::Point2f> &input,
			 std::vector<cv::Point2f> &output)
     {
	  boost::mt19937 rng;
	  boost::normal_distribution<> nd(0.0, 1.0);
	  boost::variate_generator<boost::mt19937&,
				   boost::normal_distribution<> > var_nor(rng, nd);

	  std::vector<cv::Point2f>::iterator it;
	  for (it = input.begin() ; it != input.end() ; it++) {
	            //double value = var_nor() + (*it);
	       cv::Point2f newPoint;
	       newPoint.x = it->x + var_nor();
	       newPoint.y = it->y + var_nor();
	       output.push_back(newPoint);
	  }
     }

}


#include <vector>

#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "MODI.hpp"

namespace ublas = boost::numeric::ublas;
using namespace std;

namespace t_simplex
{
	/* DECLARATION OF GLOBALS */
	double ** _tsC = NULL;				// Cost matrix
	double _tsMaxW;						// Maximum of all weights

										// MODI Method
	double transportSimplex(
		const unsigned &update_xij_mode,
		ublas::matrix<double> &custom_cij,
		TsFlow *flowTable,
		unsigned int *flowSize,
		TsSignature *facility,
		TsSignature *customer,
		const double &sum_dj,
		const double &sum_bi,
		const vector <double > &dj)
	{
		int m = facility->n, n = customer->n; // Matrix Sizes
		int i = 0, j = 0;
		double totalCost = 0.0, diff = 0.0;
		int *P1 = NULL, *P2 = NULL;

		TsBasic *basics = NULL;					///Array of basic variables.
		bool **isBasic = NULL;						//Flag matrix. isBasic[i][j] is true there is flow between source i and sink j
		TsBasic **facBasics = NULL;					//Array of pointers to the first basic variable in each row
		TsBasic **cusBasics = NULL;					//Array of pointers to the first basic variable in each column
		double *demand = facility->weights;						//Array of sink demands
		double *capacity = NULL;					//Array of source supplies
		bool flag = false;

		// Equalize source and sink weights.
		diff = sum_bi - sum_dj;
		if (diff > 0.0) n++;
		_tsMaxW = sum_bi > sum_dj ? sum_bi : sum_dj;

		basics = new TsBasic[m + n];
		isBasic = new bool*[m];
		for (i = 0; i < m; ++i)
			isBasic[i] = NULL;
		for (i = 0; i < m; ++i)
		{
			isBasic[i] = new bool[n];
			for (j = 0; j < n; ++j)
				isBasic[i][j] = 0;
		}
		facBasics = new TsBasic*[m];
		for (i = 0; i < m; ++i)
			facBasics[i] = NULL;
		cusBasics = new TsBasic*[n];
		for (i = 0; i < n; ++i)
			cusBasics[i] = NULL;

		// Compute the cost matrix
		_tsC = new double*[m];
		for (i = 0; i < m; ++i)
			_tsC[i] = NULL;

		for (i = 0, P1 = facility->features; i < m; ++i, P1++)
		{
			_tsC[i] = new double[n];
			for (j = 0, P2 = customer->features; j < n; ++j, P2++)
			{
				if (i == facility->n || j == customer->n)
					_tsC[i][j] = 0;
				else
					_tsC[i][j] = custom_cij(i, j);
			}
		}

		capacity = new double[m];					//init the source array
		for (i = 0; i < facility->n; ++i)
			capacity[i] = facility->weights[i];

		demand = new double[n];					//init the sink array
		for (i = 0; i < customer->n; ++i)
			demand[i] = customer->weights[i];

		if (n != customer->n)
			demand[customer->n] = diff;

		// Find the initail basic feasible solution. Use either _initRussel or _initVogel
		_initVogel(capacity, demand, basics, facBasics, cusBasics, isBasic, m, n);

		// Enter the main pivot loop
		totalCost = _pivot(basics, facBasics, cusBasics, isBasic, m, n, dj);

		// Fill the Flow data structure
		TsBasic * basicPtr = NULL;
		TsFlow * flowPtr = flowTable;
		unsigned int counter = 0;

		if (flowTable != NULL) {
			for (i = 0; i < m + n; ++i) {
				basicPtr = basics + i;

				if (basicPtr != NULL)
				{
					if (basicPtr->i >= 0 && basicPtr->i < facility->n
						&& basicPtr->j >= 0 && basicPtr->j < customer->n
						&& isBasic[basicPtr->i][basicPtr->j]
						&& basicPtr->val != 0.0
						&& counter < m + n - 1)
					{
						flowPtr->to = basicPtr->j;
						flowPtr->from = basicPtr->i;
						flowPtr->amount = basicPtr->val;
						flowPtr++;
						counter++;
					}
				}
			}
		}
		if (flowSize != NULL)
		{
			*flowSize = (int)(flowPtr - flowTable);
		}

		for (i = 0; i < m; i++)
		{
			delete[] isBasic[i];
			isBasic[i] = NULL;
		}
		delete[] isBasic;
		isBasic = NULL;

		for (i = 0; i < m; i++)
		{
			delete[] _tsC[i];
			_tsC[i] = NULL;
		}
		delete[] _tsC;
		_tsC = NULL;

		delete[] facBasics;
		facBasics = NULL;
		delete[] cusBasics;
		cusBasics = NULL;
		delete[] basics;
		basics = NULL;

		return totalCost;
	}

	/*
	Main pivot loop.
	Pivots until the system is optimal and return the optimal transportation cost.
	*/
	double _pivot(
		TsBasic * basics,
		TsBasic ** facBasics,
		TsBasic ** cusBasics,
		bool ** isBasic,
		int &m,
		int &n,
		const vector <double > &dj)
	{
		double * ui = NULL;
		double * vj = NULL;
		TsStone * stonePath = NULL;

		TsStone * spitra = NULL, *spitrb = NULL, *leaving = NULL;
		TsBasic * XP = NULL;
		TsBasic * basicsEnd = basics + m + n;
		TsBasic * entering = basicsEnd - 1;
		TsBasic dummyBasic;
		dummyBasic.i = -1;
		dummyBasic.j = 0;

		unsigned int i = 0, j = 0, lowI = 0, lowJ = 0, numPivots = 0;
		double objectiveValue = TSINFINITY, oldObjectiveValue = 0.0;
		double lowVal = 0.0;

		ui = new double[m];
		vj = new double[n];
		stonePath = new TsStone[m + n];

		while (1)
		{
			oldObjectiveValue = objectiveValue;
			objectiveValue = 0.0;

			for (XP = basics; XP != basicsEnd; XP++)
			{
				if (XP != entering)
				{
					//cout << endl << "i = " << XP->i << " j = " << XP->j << endl;
					if (XP->i >= 0 && XP->j >= 0 && XP->i < m && XP->j < dj.size()
						&& _tsC[XP->i][XP->j] != 0.0 && dj[XP->j] != 0.0)
					{
						objectiveValue += _tsC[XP->i][XP->j] * (XP->val / dj[XP->j]);
					}
				}
			}

			// Compute ui, vj
			stonePath[0].node = &dummyBasic;
			stonePath[0].prev = NULL;
			spitrb = _BFS(stonePath, facBasics, cusBasics, true);

			spitra = stonePath;
			vj[spitra->node->j] = 0;
			for (spitra++; spitra != spitrb; spitra++) {
				if (spitra->node->i == spitra->prev->node->i) {
					//node is in same row as parent
					vj[spitra->node->j] = _tsC[spitra->node->i][spitra->node->j] - ui[spitra->node->i];
				}
				else if (spitra->node->j == spitra->prev->node->j) {
					ui[spitra->node->i] = _tsC[spitra->node->i][spitra->node->j] - vj[spitra->node->j];
				}
			}

			// find Theta
			lowVal = 0.0;
			for (i = 0; i < m; ++i)
				for (j = 0; j < n; ++j)
					if (!isBasic[i][j] && _tsC[i][j] - ui[i] - vj[j] < lowVal)
					{
						lowVal = _tsC[i][j] - ui[i] - vj[j];
						lowI = i;
						lowJ = j;
					}

			if (lowVal >=  0.0 || (oldObjectiveValue - objectiveValue) < TSPIVOTLIMIT)
			{
				delete[] ui;
				delete[] vj;
				delete[] stonePath;
				//std::cout << "Pivots: " << numPivots << "\t";
				return objectiveValue;
			}

			// Add the entering variable to stone path
			entering->i = lowI;
			entering->j = lowJ;
			isBasic[lowI][lowJ] = 1;
			entering->val = 0;
			entering->nextFac = facBasics[lowI];
			if (facBasics[lowI] != NULL) facBasics[lowI]->prevFac = entering;
			entering->nextCus = cusBasics[lowJ];
			if (cusBasics[lowJ] != NULL) cusBasics[lowJ]->prevCus = entering;
			facBasics[lowI] = entering;
			entering->prevFac = facBasics[lowI];
			cusBasics[lowJ] = entering;
			entering->prevCus = cusBasics[lowJ];
			stonePath[0].node = entering;
			stonePath[0].prev = NULL;

			// Use breadth-first search to find a loop of basics.
			spitra = spitrb = _BFS(stonePath, facBasics, cusBasics);
			lowVal = TSINFINITY;
			bool add = false;

			// Find the lowest flow along the loop (leaving variable)
			do
			{
				if (!add && spitrb->node->val < lowVal)
				{
					leaving = spitrb;
					lowVal = spitrb->node->val;
				}
				add = !add;
			} while (spitrb = spitrb->prev);

			add = false;
			spitrb = spitra;

			// Alternately increase and decrease flow along the loop
			do
			{
				if (add)
					spitrb->node->val += lowVal;
				else
					spitrb->node->val -= lowVal;
				add = !add;
			} while (spitrb = spitrb->prev);

			i = leaving->node->i;
			j = leaving->node->j;
			isBasic[i][j] = 0;

			if (facBasics[i] == leaving->node)
			{
				facBasics[i] = leaving->node->nextFac;
				facBasics[i]->prevFac = NULL;
			}
			else
			{
				leaving->node->prevFac->nextFac = leaving->node->nextFac;
				if (leaving->node->nextFac != NULL)
					leaving->node->nextFac->prevFac = leaving->node->prevFac;
			}

			if (cusBasics[j] == leaving->node)
			{
				cusBasics[j] = leaving->node->nextCus;
				cusBasics[j]->prevCus = NULL;
			}
			else
			{
				leaving->node->prevCus->nextCus = leaving->node->nextCus;
				if (leaving->node->nextCus != NULL)
					leaving->node->nextCus->prevCus = leaving->node->prevCus;
			}

			entering = leaving->node;
			numPivots++;
		}
	}

	TsStone * _BFS(
		TsStone * stoneTree,
		TsBasic ** facBasics,
		TsBasic ** cusBasics,
		bool complete)
	{
		bool column = true;
		int jumpoffset = 0;
		TsBasic * bitr;
		TsStone * sitra = &stoneTree[0], *sitrb = &stoneTree[1];
		do {
			if (column)
			{
				for (bitr = cusBasics[sitra->node->j]; bitr != NULL; bitr = bitr->nextCus)
				{
					if (bitr != sitra->node)
					{
						sitrb->node = bitr;
						sitrb->prev = sitra;
						sitrb++;
					}
				}
			}
			else
			{
				for (bitr = facBasics[sitra->node->i]; bitr != NULL; bitr = bitr->nextFac)
				{
					if (bitr != sitra->node)
					{
						sitrb->node = bitr;
						sitrb->prev = sitra;
						sitrb++;
					}
				}
			}

			sitra++;
			if (sitra == sitrb) //no cycle found and no cycles in tree
				return sitra;

			if (sitra->node->i == sitra->prev->node->i)
				column = true;
			else
				column = false;

			// cycle found
			if (!complete && sitra->node->i == stoneTree[0].node->i
				&& sitra->node->j != stoneTree[0].node->j  && column == false)
				return sitra;
		} while (1);
	}

	/**********************
	Vogel's initialization method
	**********************/
	void _initVogel(
		double *S,
		double *D,
		TsBasic * basicsEnd,
		TsBasic ** facBasics,
		TsBasic ** cusBasics,
		bool ** isBasic,
		int m,
		int n)
	{
		int i, j;
		TsVogPen *srcPens = NULL;
		TsVogPen *snkPens = NULL;
		TsVogPen *pitra, *pitrb;  //iterators
		TsVogPen *maxPen;
		TsVogPen srcPenHead, snkPenHead;
		bool maxIsSrc;
		double lowVal;

		srcPens = new TsVogPen[m];
		snkPens = new TsVogPen[n];

		srcPenHead.next = pitra = srcPens;
		for (i = 0; i < m; i++)
		{
			pitra->i = i;
			pitra->next = pitra + 1;
			pitra->prev = pitra - 1;
			pitra->one = pitra->two = 0;
			pitra->oneCost = pitra->twoCost = TSINFINITY;
			pitra++;
		}
		(--pitra)->next = NULL;
		srcPens[0].prev = &srcPenHead;

		snkPenHead.next = pitra = snkPens;
		for (i = 0; i < n; i++)
		{
			pitra->i = i;
			pitra->next = pitra + 1;
			pitra->prev = pitra - 1;
			pitra->one = pitra->two = 0;
			pitra->oneCost = pitra->twoCost = TSINFINITY;
			pitra++;
		}
		(--pitra)->next = NULL;
		snkPens[0].prev = &snkPenHead;

		for (pitra = srcPenHead.next, i = 0; pitra != NULL; pitra = pitra->next, i++)
		{
			for (pitrb = snkPenHead.next, j = 0; pitrb != NULL; pitrb = pitrb->next, j++)
			{
				//initialize Source Penalties;
				addPenalty(pitra, _tsC[i][j], j);
				addPenalty(pitrb, _tsC[i][j], i);
			}
		}

		while (srcPenHead.next != NULL && snkPenHead.next != NULL)
		{
			maxIsSrc = true;
			for (maxPen = pitra = srcPenHead.next; pitra != NULL; pitra = pitra->next)
				if ((pitra->twoCost - pitra->oneCost) > (maxPen->twoCost - maxPen->oneCost))
					maxPen = pitra;

			for (pitra = snkPenHead.next; pitra != NULL; pitra = pitra->next)
				if ((pitra->twoCost - pitra->oneCost) > (maxPen->twoCost - maxPen->oneCost))
				{
					maxPen = pitra;
					maxIsSrc = false;
				}

			if (maxIsSrc)
			{
				i = maxPen->i;
				j = maxPen->one;
			}
			else
			{
				j = maxPen->i;
				i = maxPen->one;
			}

			if (D[j] - S[i] > _tsMaxW * TSEPSILON || (srcPenHead.next->next != NULL && fabs(S[i] - D[j]) < _tsMaxW * TSEPSILON))
			{
				//delete source
				lowVal = S[i];
				maxPen = srcPens + i;
				maxPen->prev->next = maxPen->next;
				if (maxPen->next != NULL)
					maxPen->next->prev = maxPen->prev;

				for (pitra = snkPenHead.next; pitra != NULL; pitra = pitra->next) {
					if (pitra->one == i || pitra->two == i) {
						pitra->oneCost = TSINFINITY;
						pitra->twoCost = TSINFINITY;
						for (pitrb = srcPenHead.next; pitrb != NULL; pitrb = pitrb->next)
							addPenalty(pitra, _tsC[pitrb->i][pitra->i], pitrb->i);
					}
				}
			}
			else
			{
				//delete sink
				lowVal = D[j];
				maxPen = snkPens + j;
				maxPen->prev->next = maxPen->next;
				if (maxPen->next != NULL)
					maxPen->next->prev = maxPen->prev;

				for (pitra = srcPenHead.next; pitra != NULL; pitra = pitra->next)
				{
					if (pitra->one == j || pitra->two == j) {
						pitra->oneCost = TSINFINITY;
						pitra->twoCost = TSINFINITY;
						for (pitrb = snkPenHead.next; pitrb != NULL; pitrb = pitrb->next)
							addPenalty(pitra, _tsC[pitra->i][pitrb->i], pitrb->i);
					}
				}
			}

			S[i] -= lowVal;
			D[j] -= lowVal;

			isBasic[i][j] = 1;
			basicsEnd->val = lowVal;
			basicsEnd->i = i;
			basicsEnd->j = j;

			basicsEnd->nextCus = cusBasics[j];
			if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
			basicsEnd->nextFac = facBasics[i];
			if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

			facBasics[i] = basicsEnd;
			basicsEnd->prevCus = NULL;
			cusBasics[j] = basicsEnd;
			basicsEnd->prevFac = NULL;

			basicsEnd++;

		}
		delete[] srcPens;
		delete[] snkPens;
	}
}


/*
    Copyright (C) 2008 Wei Dong <wdong@princeton.edu>. All Rights Reserved.

    This file is part of LSHKIT.

    LSHKIT is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    LSHKIT is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with LSHKIT.  If not, see <http://www.gnu.org/licenses/>.
*/

/**
  * \file sketch-run.cpp
  * \brief Example of sketch construction with Gaussian LSH.
  *
  * This program uses sketch filtering to accelerate K-NN search.
  * The idea is to first search against a dataset of sketches and keep the
  * top C*K points as candidates.  The candidates are than ranked using
  * the raw feature vectors.  The sketch database can be viewed as a index.
  *
  * In this program, we use 2-stable LSH based sketch.  Each sketch is a bit-vector
  * of M bits, and each bit is produced by a independent hash function from the
  * family DeltaLSB<GaussianLsh>.
  *
  * The program reconstruct the sketches by default.  You can give the
  * --index option to make the program save the sketches.  The next
  * time you run the program with the same --index option, the program
  * will try to load the previously saved sketches.  When a saved sketch database is
  * used, you need to make sure that the dataset and other parameters match
  * the previous run.  However, the benchmark file, Q and K can be different.
\verbatim
Allowed options:
  -h [ --help ]          produce help message.
  -W [ -- ] arg (=1)
  -M [ -- ] arg (=1)     skech size / byte
  -C [ -- ] arg (=10)    # candidates = C x K
  -Q [ -- ] arg (=100)   # queries to use
  -K [ -- ] arg (=50)    K-NNs retrieved
  -D [ --data ] arg      data file
  -B [ --benchmark ] arg benchmark file
  --index arg            sketch file
  --asym                 Asymmetric distance estimation
\endverbatim
  */

#include <boost/program_options.hpp>
#include <boost/progress.hpp>
#include <boost/format.hpp>
#include <boost/timer.hpp>
#include <lshkit.h>

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

int main (int argc, char *argv[])
{
    string data_file;
    string index_file;
    string benchmark;

    float W, R;
    unsigned M;
    unsigned Q, K, C;
    bool do_benchmark = true;
    bool use_index = false; // load the index from a file
    bool do_asym = false;

    boost::timer timer;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message.")
        (",W", po::value(&W)->default_value(1.0), "")
        (",M", po::value(&M)->default_value(1), "skech size / byte")
        (",C", po::value(&C)->default_value(10), "# candidates = C x K")
        (",Q", po::value(&Q)->default_value(100), "# queries to use")
        (",K", po::value(&K)->default_value(50), "K-NNs retrieved")
        (",R", po::value(&R)->default_value(numeric_limits<float>::max()), "R-NN distnace range")
        ("data,D", po::value(&data_file), "data file")
        ("benchmark,B", po::value(&benchmark), "benchmark file")
        ("index", po::value(&index_file), "sketch file")
        ("asym", "Asymmetric distance estimation")
        ;

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

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

    if ((Q == 0) || (vm.count("benchmark") == 0)) {
        do_benchmark = false;
    }

    if (vm.count("asym") == 1) {
        do_asym = true;
    }

    if (vm.count("index") == 1) {
        use_index = true;
    }

    cout << "LOADING DATA..." << endl;
    timer.restart();
    FloatMatrix data(data_file);
    cout << boost::format("LOAD TIME: %1%s.") % timer.elapsed() << endl;

    typedef Sketch<DeltaLSB<GaussianLsh> > MySketch;

    MySketch sketcher;
    Matrix<unsigned char> index;

    bool index_loaded = false;

    if (use_index) {
        ifstream is(index_file.c_str(), ios_base::binary);
        if (is) {
            is.exceptions(ios_base::eofbit | ios_base::failbit | ios_base::badbit);
            cout << "LOADING INDEX..." << endl;
            timer.restart();
            sketcher.load(is);
            index.load(is);
            BOOST_VERIFY(is);
            BOOST_VERIFY(M <= sketcher.getChunks());
            cout << boost::format("LOAD TIME: %1%s.") % timer.elapsed() << endl;
            index_loaded = true;
        }
    }

    if (!index_loaded) {

        // We define a short name for the MPLSH index.
        MySketch::Parameter param;

        // Setup the parameters.  Note that L is not provided here.
        param.W = W;
        param.dim = data.getDim();
        DefaultRng rng;

        sketcher.reset(M, param, rng);
        // The accessor.

        // Initialize the index structure.  Note L is passed here.
        cout << "CONSTRUCTING INDEX..." << endl;

        index.reset(M, data.getSize());

        // CONSTRUCT SKETCHES FOR DATABASE
        timer.restart();
        {
            boost::progress_display progress(data.getSize());
            for (unsigned i = 0; i < data.getSize(); ++i)
            {
                sketcher.apply(data[i], index[i]);
                ++progress;
            }
        }
        cout << boost::format("CONSTRUCTION TIME: %1%s.") % timer.elapsed() << endl;

        if (use_index) {
            timer.restart();
            cout << "SAVING INDEX..." << endl;
            {
                ofstream os(index_file.c_str(), ios_base::binary);
                os.exceptions(ios_base::eofbit | ios_base::failbit | ios_base::badbit);
                sketcher.save(os);
                index.save(os);
            }
            cout << boost::format("SAVING TIME: %1%s") % timer.elapsed() << endl;
        }
    }

    BOOST_VERIFY(index.getSize() == data.getSize());

    if (do_benchmark) {

        Benchmark<> bench;
        cout << "LOADING BENCHMARK..." << endl;
        bench.load(benchmark);
        bench.resize(Q, K);
        cout << "DONE." << endl;

        for (unsigned i = 0; i < Q; ++i)
        {
            for (unsigned j = 0; j < K; ++j)
            {
                assert(bench.getAnswer(i)[j].key < data.getSize());
            }
        }

        cout << "RUNNING QUERIES..." << endl;

        Stat recall;
        Topk<unsigned> candidate;
        Topk<unsigned> topk;
        unsigned char *query_sketch = new unsigned char[sketcher.getChunks()];
        boost::progress_display progress(Q);
        metric::l2<float> l2(data.getDim());

        timer.restart();
        if (do_asym)
        {
            float *asym = new float[sketcher.getBits()];
            WeightedHammingHelper<> helper(M);
            for (unsigned i = 0; i < Q; ++i)
            {
                // Query for one point.
                unsigned query = bench.getQuery(i);
                candidate.reset(C * K, R);
                topk.reset(K, R);

                sketcher.apply(data[query], query_sketch, asym);
                helper.update(query_sketch, asym);
                // Scan the sketches for candidates.
                for (unsigned j = 0; j < index.getSize(); ++j) {
                    if (j == query) continue;
                    Topk<unsigned>::Element e;
                    e.key = j;
                    e.dist = helper.distTo(index[j]);
                    candidate << e;
                }
                // Rank with raw feature.
                for (unsigned j = 0; j < candidate.size(); ++j) {
                    Topk<unsigned>::Element e;
                    e.key = candidate[j].key;
                    e.dist = l2(data[query], data[e.key]);
                    topk << e;
                }
                recall << bench.getAnswer(i).recall(topk);
                ++progress;
            }
            delete []asym;
        }
        else
        // symmetric sketch
        {
            metric::hamming<unsigned char> hamming(M);
            for (unsigned i = 0; i < Q; ++i)
            {
                // Query for one point.
                unsigned query = bench.getQuery(i);
                candidate.reset(C * K, R);
                topk.reset(K, R);

                sketcher.apply(data[query], query_sketch);
                for (unsigned j = 0; j < index.getSize(); ++j) {
                    if (j == query) continue;
                    Topk<unsigned>::Element e;
                    e.key = j;
                    e.dist = hamming(index[j], query_sketch);
                    candidate << e;
                }
                for (unsigned j = 0; j < candidate.size(); ++j) {
                    Topk<unsigned>::Element e;
                    e.key = candidate[j].key;
                    e.dist = l2(data[query], data[e.key]);
                    topk << e;
                }
                recall << bench.getAnswer(i).recall(topk);
                ++progress;
            }
        }

        delete []query_sketch;
        cout << boost::format("QUERY TIME: %1%s.") % timer.elapsed() << endl;
        cout << "[RECALL] " << recall.getAvg() << " +/- " << recall.getStd() << endl;

    }

    return 0;
}


// Copyright (C) 2008-2018 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0 (see accompanying
// file LICENSE_1_0.txt or a copy at http://www.boost.org/LICENSE_1_0.txt).
// See: http://www.boost.org/doc/libs/release/libs/contract/doc/html/index.html

#include <limits>
#include <cassert>

//[introduction
#include <boost/contract.hpp>

void inc(int& x) {
    boost::contract::old_ptr<int> old_x = BOOST_CONTRACT_OLDOF(x); // Old value.
    boost::contract::check c = boost::contract::function()
        .precondition([&] {
            BOOST_CONTRACT_ASSERT(x < std::numeric_limits<int>::max()); // Line 17.
        })
        .postcondition([&] {
            BOOST_CONTRACT_ASSERT(x == *old_x + 1); // Line 20.
        })
    ;

    ++x; // Function body.
}
//]

int main() {
    int x = 10;
    inc(x);
    assert(x == 11);
    return 0;
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [rec]
#include <boost/simd/arithmetic.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft = bs::pack <float, 8>;
using pack_it = bs::pack <std::int16_t, 8>;

int main()
{
  pack_ft pf = {-2.0f,            2.0f,              0.0f,             -0.0f,
                bs::Inf<float>(), bs::Minf<float>(), bs::Nan<float>(),  1.5f  };
  pack_it pi = {-3, -2, -1, 0, 1, 2, 3, 4};

  std::cout
    << "---- simd" << '\n'
    << "<- pf =                           " << pf << '\n'
    << "-> bs::rec(pf) =                  " << bs::rec(pf) << '\n'
    << "<- pi =                           " << pi << '\n'
    << "-> bs::rec(pi) =                  " << bs::rec(pi) << '\n';

  float xf = 5.0f;
  std::int16_t xi =  0,  yi = 2;

   std::cout
     << "---- scalar"  << '\n'
     << "<- xf =                           " << xf << '\n'
     << "-> bs::rec(xf) =                  " << bs::rec(xf) << '\n'
     << "<- xi =                           " << xi << '\n'
     << "-> bs::rec(xi) =                  " << bs::rec(xi) << '\n'
     << "<- yi =                           " << yi << '\n'
     << "-> bs::rec(yi) =                  " << bs::rec(yi) << '\n';
   return 0;
}
//! [rec]


#define BOOST_TEST_MODULE "test_read_com_pulling_force_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/input/read_external_interaction.hpp>

BOOST_AUTO_TEST_CASE(read_com_pulling_force_interaction)
{
    mjolnir::LoggerManager::set_default_logger("test_read_com_pulling_force_interaction.log");

    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction = "CoMPullingForce"
            parameters  = [
                {indices = [0,1,2],    force = [ 1.0, 1.0, 1.0]},
                {indices = " [0, 3) ", force = [ 2.0, 2.0, 2.0]},
                {indices = ["[0, 3) ", "[4, 6]"], force = [ 3.0, 3.0, 3.0]},
                {indices = [0,1,2],    force = 0.0144, direction = [ 1.0, 2.0, 3.0]},
            ]
            )"_toml;

        const auto base = mjolnir::read_external_interaction<traits_type>(v);
        BOOST_TEST(static_cast<bool>(base));

        const auto derv = dynamic_cast<mjolnir::CoMPullingForceInteraction<traits_type>*>(base.get());
        BOOST_TEST(static_cast<bool>(derv));

        const auto& interaction = *derv;

        const auto expected1 = std::vector<std::size_t>{0, 1, 2};
        const auto expected2 = std::vector<std::size_t>{0, 1, 2};
        const auto expected3 = std::vector<std::size_t>{0, 1, 2, 4, 5, 6};

        const auto& actual1 = interaction.parameters().at(0).first;
        const auto& actual2 = interaction.parameters().at(1).first;
        const auto& actual3 = interaction.parameters().at(2).first;

        BOOST_TEST_REQUIRE(actual1.size() == expected1.size());
        BOOST_TEST_REQUIRE(actual2.size() == expected2.size());
        BOOST_TEST_REQUIRE(actual3.size() == expected3.size());

        BOOST_TEST_REQUIRE(std::equal(actual1.begin(), actual1.end(), expected1.begin()));
        BOOST_TEST_REQUIRE(std::equal(actual2.begin(), actual2.end(), expected2.begin()));
        BOOST_TEST_REQUIRE(std::equal(actual3.begin(), actual3.end(), expected3.begin()));

        BOOST_TEST(mjolnir::math::X(interaction.parameters().at(0).second) == 1.0, boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Y(interaction.parameters().at(0).second) == 1.0, boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Z(interaction.parameters().at(0).second) == 1.0, boost::test_tools::tolerance(1e-8));

        BOOST_TEST(mjolnir::math::X(interaction.parameters().at(1).second) == 2.0, boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Y(interaction.parameters().at(1).second) == 2.0, boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Z(interaction.parameters().at(1).second) == 2.0, boost::test_tools::tolerance(1e-8));

        BOOST_TEST(mjolnir::math::X(interaction.parameters().at(2).second) == 3.0, boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Y(interaction.parameters().at(2).second) == 3.0, boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Z(interaction.parameters().at(2).second) == 3.0, boost::test_tools::tolerance(1e-8));

        BOOST_TEST(mjolnir::math::X(interaction.parameters().at(3).second) == 1.0 * 0.0144 / std::sqrt(1.0 + 4.0 + 9.0), boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Y(interaction.parameters().at(3).second) == 2.0 * 0.0144 / std::sqrt(1.0 + 4.0 + 9.0), boost::test_tools::tolerance(1e-8));
        BOOST_TEST(mjolnir::math::Z(interaction.parameters().at(3).second) == 3.0 * 0.0144 / std::sqrt(1.0 + 4.0 + 9.0), boost::test_tools::tolerance(1e-8));
    }
}