#ifndef WALKING_PATTERN_GENERATOR_H
#define WALKING_PATTERN_GENERATOR_H

#include <Eigen/Dense>
#include <cmath>
#include <string>
#include <avatar_locomanipulation/data_types/footstep.hpp>
#include <avatar_locomanipulation/data_types/trajectory_SE3.hpp>

#include <avatar_locomanipulation/helpers/hermite_curve_vec.hpp>
#include <avatar_locomanipulation/helpers/hermite_quaternion_curve.hpp>
#include <iostream>

class WalkingPatternGenerator{
public:
  WalkingPatternGenerator();
  ~WalkingPatternGenerator();

  static int const SWING_VRP_TYPE;
  static int const DOUBLE_SUPPORT_TRANSFER_VRP_TYPE;

  static int const STATE_INITIAL_TRANSFER;
  static int const STATE_SWING;
  static int const STATE_DOUBLE_SUPPORT;
  static int const STATE_FINAL_TRANSFER;

  std::vector<Footstep> footstep_list;

  std::vector<Eigen::Vector3d> rvrp_list; // List of virtual repelant points.
  std::vector<Eigen::Vector3d> dcm_ini_list; // List of initial DCM states
  std::vector<Eigen::Vector3d> dcm_eos_list; // List of end-of-step DCM states

  std::vector<int> rvrp_type_list; // List of type of virtual repelant point

  Eigen::Vector3d start_stance_rvrp;
  Eigen::Vector3d start_stance_dcm_ini;
  Eigen::Vector3d start_stance_dcm_eos;

  // DCM parameters:
  double gravity = 9.81;
  double z_vrp = 0.95; //1.0; // desired VRP height / CoM height
  double b = std::sqrt(z_vrp/gravity); // time constant of DCM dynamics

  double t_ds = 0.45; //0.9; // time in double support
  double t_ss = 1.0;//1.2; // time in single support
  double t_settle = -b*log(0.001); // settling time at the end of the full walking trajectory
  double t_transfer = -b*log(1.0 - 0.01); // settling time at the end of the full walking trajectory

  double swing_height = 0.1; //0.05; // swing height in meters.

  void setCoMHeight(double z_vrp_in); // Sets the desired CoM Height
  void setDoubleSupportTime(double t_ds_in); // Sets the desired double support time
  void setSingleSupportSwingTime(double t_ss_in); // Sets the desired single support swing time
  void setSettlingPercentage(double percentage_convergence); // Percentage to settle. Default 0.999
  void setSwingHeight(double swing_height_in); // Sets the swing height of the robot. Default 0.1m

  // DCM trajectory calculation
  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        initial_footstance  - a footstep object describing the stance leg.
  // populates this object's footstep_list, rvrp_list, dcm_ini_list, dcm_eos_list
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & initial_footstance, bool clear_list=false);

  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        initial_footstance  - a footstep object describing the stance leg.
  //        initial_rvrp        - an initial virtual repelant point (eg: average of the stance feet's rvrp).
  // populates this object's footstep_list, rvrp_list, dcm_ini_list, dcm_eos_list
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & initial_footstance, const Eigen::Vector3d & initial_rvrp);

  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        left_footstance        - a footstep object describing the left stance feet
  //        right_footstance       - a footstep object describing the right stance feet
  // populates this object's footstep_list, rvrp_list, dcm_ini_list, dcm_eos_list.
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & left_footstance, const Footstep & right_footstance);

  // input: input_footstep_list - a list of footsteps to take not including the current stance configuration.
  //        left_footstance        - a footstep object describing the left stance feet
  //        right_footstance       - a footstep object describing the right stance feet
  //        initial_com            - the initial location of the com
  // populates this object's footstep_list, rvrp_list, dcm_ini_list, dcm_eos_list.
  void initialize_footsteps_rvrp(const std::vector<Footstep> & input_footstep_list, const Footstep & left_footstance, const Footstep & right_footstance, const Eigen::Vector3d & initial_com);

   // Outputs the average r_vrp location given two footstances
  void get_average_rvrp(const Footstep & footstance_1, const Footstep & footstance_2, Eigen::Vector3d & average_rvrp);

    // computes all the dcm states. Computation properly populates the dcm_ini_list and dcm_eos_list
  void computeDCM_states();

  // Initialize trajectory clock
  void initialize_internal_clocks();

  // Given a delta_t compute the next desired DCM
  Eigen::Vector3d get_next_desired_DCM(const double & dt);

  // Given the current
  Eigen::Vector3d get_desired_DCM(const int & step_index, const double & t);
  Eigen::Vector3d get_com_vel(const Eigen::Vector3d & current_com, const int & step_index, const double & t);

  double get_total_trajectory_time();
  void initialize_trajectory_discretization(const int & N_samples);

  void construct_trajectories(const std::vector<Footstep> & input_footstep_list,
                              const Footstep & initial_left_footstance,
                              const Footstep & initial_right_footstance,
                              const Eigen::Vector3d & initial_com,
                              const Eigen::Quaterniond initial_pelvis_ori);

  // trajectory objects
  Footstep mid_foot_;
  TrajSE3         traj_SE3_tmp;;
  TrajSE3         traj_SE3_left_foot;
  TrajSE3         traj_SE3_right_foot;
  TrajOrientation traj_ori_pelvis;
  TrajEuclidean   traj_pos_com;
  TrajEuclidean   traj_dcm_pos;

  std::vector<int> state_list;
  std::vector<int> bin_size_list;
  std::vector<int> stance_list;
  std::vector<Footstep> stance_location_list;
  std::vector<Footstep> swing_landing_location_list;

  void compute_trajectory_lists();
  void compute_pelvis_orientation_trajectory(const Eigen::Quaterniond & init_pelvis_ori,
                                             const Footstep & initial_left_footstance,
                                             const Footstep & initial_right_footstance);

  void compute_foot_trajectories(const Footstep & initial_left_footstance,
                                 const Footstep & initial_right_footstance);
  void compute_com_dcm_trajectory(const Eigen::Vector3d & initial_com);

  void setOrientationTrajectory(const int & starting_index, const int & N_bins, HermiteQuaternionCurve & curve, TrajOrientation & traj_ori);
  void setSwingFootTrajectory(const Footstep & init_location, const Footstep & landing_location, const int & starting_index, const int & N_bins, TrajSE3 & swing_foot);
  void setConstantSE3(const int & starting_index, const int & N_bins, TrajSE3 & traj, const Eigen::Vector3d & pos, const Eigen::Quaterniond & quat);

private:
  // input: r_vrp_d_i - the desired virtual repelant point for the i-th step.
  //        t_step    - the time interval to use for backwards integration
  //        dcm_eos_i - the DCM state at the end of the i-th step.
  // computes the step i's initial DCM state and the end-of-step i-1's dcm state.
  // The computation is stored in the dcm_ini_list and dcm_eos_list.
  Eigen::Vector3d computeDCM_ini_i(const Eigen::Vector3d & r_vrp_d_i, const double & t_step, const Eigen::Vector3d & dcm_eos_i);

  // Get the t_step for step i.
  double get_t_step(const int & step_i);

  double internal_timer;
  double internal_t_step;
  double internal_step_timer;
  int internal_step_i;

  int N_size;
  double internal_dt;

};

#endif


//
// Copyright (c) 2016-2019 CNRS INRIA
//

#ifndef __pinocchio_math_fwd_hpp__
#define __pinocchio_math_fwd_hpp__

#include "pinocchio/fwd.hpp"
#include <math.h>
#include <boost/math/constants/constants.hpp>

namespace pinocchio
{

  template <typename T>
  struct is_floating_point : boost::is_floating_point<T>
  {
  };

  ///
  /// \brief Returns the value of PI according to the template parameters Scalar
  ///
  /// \tparam Scalar The scalar type of the return pi value
  ///
  template<typename Scalar>
  const Scalar PI()
  { return boost::math::constants::pi<Scalar>(); }

  /// \brief Foward declaration of TaylorSeriesExpansion.
  template<typename Scalar> struct TaylorSeriesExpansion;

  namespace math
  {

#define PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(name) \
    template<typename Scalar> \
    Scalar name(const Scalar & value) \
    { using std::name; return name(value); }

#define PINOCCHIO_OVERLOAD_MATH_BINARY_OPERATOR(name) \
    namespace internal \
    { \
      template<typename T1, typename T2> \
      struct return_type_##name \
      { \
        typedef T1 type; \
      }; \
      template<typename T1, typename T2> \
      struct call_##name \
      { \
        static inline typename return_type_##name<T1,T2>::type \
        run(const T1 & a, const T2 & b) \
        { using std::name; return name(a,b); } \
      }; \
    } \
    template<typename T1, typename T2> \
    inline typename internal::return_type_##name<T1,T2>::type name(const T1 & a, const T2 & b) \
    { return internal::call_##name<T1,T2>::run(a,b); }

    PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(fabs)
    PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(sqrt)
    PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(atan)
    PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(acos)
    PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(asin)
    PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(cos)
    PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(sin)

    PINOCCHIO_OVERLOAD_MATH_BINARY_OPERATOR(pow)
    PINOCCHIO_OVERLOAD_MATH_BINARY_OPERATOR(min)
    PINOCCHIO_OVERLOAD_MATH_BINARY_OPERATOR(max)
    PINOCCHIO_OVERLOAD_MATH_BINARY_OPERATOR(atan2)
  }
}

#endif //#ifndef __pinocchio_math_fwd_hpp__


/* test_student_t_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/student_t_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::student_t_distribution<>
#define BOOST_RANDOM_ARG1 n
#define BOOST_RANDOM_ARG1_DEFAULT 1.0
#define BOOST_RANDOM_ARG1_VALUE 7.5

#define BOOST_RANDOM_DIST0_MIN -(std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MIN -(std::numeric_limits<double>::infinity)()
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<double>::infinity)()

#define BOOST_RANDOM_TEST1_PARAMS

#define BOOST_RANDOM_TEST2_PARAMS (100.0)

#include "test_distribution.ipp"


///////////////////////////////////////////////////////////////////////////////
// weighted_sum_kahan.hpp
//
//  Copyright 2011 Simon West. 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 BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_SUM_KAHAN_HPP_EAN_11_05_2011
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_SUM_KAHAN_HPP_EAN_11_05_2011

#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/weight.hpp>
#include <boost/accumulators/framework/accumulators/external_accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/numeric/conversion/cast.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
#if _MSC_VER > 1400
# pragma float_control(push)
# pragma float_control(precise, on)
#endif

    ///////////////////////////////////////////////////////////////////////////////
    // weighted_sum_kahan_impl
    template<typename Sample, typename Weight, typename Tag>
    struct weighted_sum_kahan_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;

        // for boost::result_of
        typedef weighted_sample result_type;

        template<typename Args>
        weighted_sum_kahan_impl(Args const &args)
          : weighted_sum_(
                args[parameter::keyword<Tag>::get() | Sample()] * numeric::one<Weight>::value),
                compensation(boost::numeric_cast<weighted_sample>(0.0))
        {
        }

        template<typename Args>
        void
#if BOOST_ACCUMULATORS_GCC_VERSION > 40305
        __attribute__((optimize("no-associative-math")))
#endif
        operator ()(Args const &args)
        {
            const weighted_sample myTmp1 = args[parameter::keyword<Tag>::get()] * args[weight] - this->compensation;
            const weighted_sample myTmp2 = this->weighted_sum_ + myTmp1;
            this->compensation = (myTmp2 - this->weighted_sum_) - myTmp1;
            this->weighted_sum_ = myTmp2;

        }

        result_type result(dont_care) const
        {
            return this->weighted_sum_;
        }

    private:
        weighted_sample weighted_sum_;
        weighted_sample compensation;
    };

#if _MSC_VER > 1400
# pragma float_control(pop)
#endif

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_sum_kahan
// tag::weighted_sum_of_variates_kahan
//
namespace tag
{
    struct weighted_sum_kahan
      : depends_on<>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::weighted_sum_kahan_impl<mpl::_1, mpl::_2, tag::sample> impl;
    };

    template<typename VariateType, typename VariateTag>
    struct weighted_sum_of_variates_kahan
      : depends_on<>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::weighted_sum_kahan_impl<VariateType, mpl::_2, VariateTag> impl;
    };

}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_sum_kahan
// extract::weighted_sum_of_variates_kahan
//
namespace extract
{
    extractor<tag::weighted_sum_kahan> const weighted_sum_kahan = {};
    extractor<tag::abstract_weighted_sum_of_variates> const weighted_sum_of_variates_kahan = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_sum_kahan)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_sum_of_variates_kahan)
}

using extract::weighted_sum_kahan;
using extract::weighted_sum_of_variates_kahan;

// weighted_sum(kahan) -> weighted_sum_kahan
template<>
struct as_feature<tag::weighted_sum(kahan)>
{
    typedef tag::weighted_sum_kahan type;
};

template<typename VariateType, typename VariateTag>
struct feature_of<tag::weighted_sum_of_variates_kahan<VariateType, VariateTag> >
  : feature_of<tag::abstract_weighted_sum_of_variates>
{
};

}} // namespace boost::accumulators

#endif


#include "cloud_math.h"
#include <algorithm>

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Eigen>
#include <Eigen/Sparse>

#define LOG1(message)

namespace Cloud
{

//----( random numbers )------------------------------------------------------

void randomize (Vector<float> x, float sigma)
{
  const size_t x_size = x.size;
  float * restrict x_data = x.data;

  for (size_t i = 0; i < x_size; ++i) {
    x_data[i] = sigma * random_std();
  }
}

size_t random_index (const Vector<float> & likes)
{
  float total = sum(likes);
  ASSERT_LT(0, total);

  while (true) {
    float t = random_unif(0, total);

    for (int i = 0, I = likes.size; i < I; ++i) {

      t -= likes[i];
      if (t < 0) return i;
    }
  }
}

size_t random_index (const VectorXf & likes)
{
  float total = likes.sum();
  ASSERT_LT(0, total);

  while (true) {
    float t = random_unif(0, total);

    for (int i = 0, I = likes.size(); i < I; ++i) {

      t -= likes(i);
      if (t < 0) return i;
    }
  }
}

int random_index (const VectorSf & likes)
{
  float total = likes.sum();
  ASSERT_LT(0, total);

  while (true) {
    float t = random_unif(0, total);

    for (VectorSf::InnerIterator iter(likes); iter; ++iter) {

      t -= iter.value();
      if (t < 0) return iter.index();
    }
  }
}

void generate_noise (Vector<int8_t> & noise, float sigma)
{
  const float quantization_correction = 1 / 6.0f;
  sigma = sqrtf(sqr(sigma) + quantization_correction);

  for (size_t i = 0, I = noise.size; i < I; ++i) {
    noise[i] = roundi(sigma * random_std());
  }
}

//----( normalization )-------------------------------------------------------

void normalize_l1 (VectorXf & x, float tot) { x *= tot / x.sum(); }
void normalize_l1 (VectorSf & x, float tot) { x *= tot / x.sum(); }
void normalize_l1 (MatrixXf & x, float tot) { x *= tot / x.sum(); }
void normalize_l1 (MatrixSf & x, float tot) { x *= tot / x.sum(); }

void normalize_rows_l1 (size_t I, size_t J, Vector<float> & A)
{
  for (size_t i = 0; i < I; ++i) {

    Vector<float> row = A.block(J, i);

    row /= sum(row);
  }
}

void normalize_columns_l1 (MatrixXf & A)
{
  const int I = A.rows();
  const int J = A.cols();

  for (int j = 0; j < J; ++j) {

    Vector<float> col(I, & A.coeffRef(0,j));

    col /= sum(col);
  }
}

void normalize_columns_l1 (MatrixXd & A)
{
  const int I = A.rows();
  const int J = A.cols();

  for (int j = 0; j < J; ++j) {

    Vector<double> col(I, & A.coeffRef(0,j));

    col /= sum(col);
  }
}

void normalize_columns_l1 (MatrixSf & A)
{
  // WARNING this assumes column-major ordering

  for (int i = 0; i < A.outerSize(); ++i) {
    A.col(i) *= 1.0f / A.col(i).sum();
  }
}

//----( sparse tools )--------------------------------------------------------

double density (const VectorXf & x)
{
  const size_t I = x.size();

  double sum_x1 = 0;
  double sum_x2 = 0;

  const float * restrict x_ = x.data();

  for (size_t i = 0; i < I; ++i) {
    double xi = x_[i];

    sum_x1 += max(-xi,xi);
    sum_x2 += xi * xi;
  }

  return sqr(sum_x1) / sum_x2 / I;
}

float density (const VectorSf & x)
{
  float sum_x1 = 0;
  float sum_x2 = 0;

  for (VectorSf::InnerIterator iter(x); iter; ++iter) {
    float xi = iter.value();

    sum_x1 += max(-xi,xi);
    sum_x2 += xi * xi;
  }

  return sqr(sum_x1) / sum_x2 / x.size();
}

double density (const MatrixXf & x)
{
  const size_t I = x.rows() * x.cols();

  double sum_x1 = 0;
  double sum_x2 = 0;

  const float * restrict x_ = x.data();

  for (size_t i = 0; i < I; ++i) {
    double xi = x_[i];

    sum_x1 += max(-xi,xi);
    sum_x2 += xi * xi;
  }

  return sqr(sum_x1) / sum_x2 / I;
}

double density (const MatrixSf & x)
{
  double sum_x1 = 0;
  double sum_x2 = 0;

  for (int i = 0; i < x.outerSize(); ++i) {
    for (MatrixSf::InnerIterator iter(x,i); iter; ++iter) {
      double xi = iter.value();

      sum_x1 += max(-xi,xi);
      sum_x2 += xi * xi;
    }
  }

  return sqr(sum_x1) / sum_x2 / x.size();
}

namespace {
struct SparseVectorEntry
{
  float value;
  int index;

  SparseVectorEntry () {}
  SparseVectorEntry (float v, int i) : value(v), index(i) {}

  bool operator< (const SparseVectorEntry & other) const
  {
    return value > other.value;
  }
};
struct SparseMatrixEntry
{
  float value;
  int row;
  int col;

  SparseMatrixEntry () {}
  SparseMatrixEntry (float v, int i, int j) : value(v), row(i), col(j) {}

  bool operator< (const SparseMatrixEntry & other) const
  {
    return value > other.value;
  }
};
} // anonymous namespace

float sparsify_size (VectorSf & sparse, int size)
{
  ASSERT_LT(0, size);
  ASSERT_LT(size, sparse.nonZeros());

  static std::vector<SparseVectorEntry> entries;

  for (VectorSf::InnerIterator iter(sparse); iter; ++iter) {
    entries.push_back(SparseVectorEntry(iter.value(), iter.index()));
  }

  std::nth_element(entries.begin(), entries.begin() + size, entries.end());

  VectorSf sparser(sparse.size());
  sparser.reserve(size);

  for (int e = 0; e < size; ++e) {
    const SparseVectorEntry & entry = entries[e];
    sparser.insert(entry.index) = entry.value;
  }

  sparser.finalize();
  entries.clear();

  std::swap(sparse, sparser);

  return sparser.sum() - sparse.sum();
}

float sparsify_size (MatrixSf & sparse, int size)
{
  ASSERT_LT(0, size);
  ASSERT_LT(size, sparse.nonZeros());

  LOG("sparsifying " << sparse.rows() << " x " << sparse.cols()
      << " matrix from " << sparse.nonZeros() << " to " << size << " entries");

  static std::vector<SparseMatrixEntry> entries;

  for (int i = 0; i < sparse.outerSize(); ++i) {
    for (MatrixSf::InnerIterator iter(sparse,i); iter; ++iter) {
      entries.push_back(
          SparseMatrixEntry(iter.value(), iter.row(), iter.col()));
    }
  }

  std::nth_element(entries.begin(), entries.begin() + size, entries.end());

  MatrixSf sparser(sparse.rows(), sparse.cols());
  sparser.reserve(size);

  for (int e = 0; e < size; ++e) {
    const SparseMatrixEntry & entry = entries[e];
    sparser.insert(entry.row, entry.col) = entry.value;
  }

  sparser.finalize();
  entries.clear();

  std::swap(sparse, sparser);

  float old_sum = sparser.sum();
  float new_sum = sparse.sum();
  float loss = (old_sum - new_sum) / old_sum;
  float density = float(size) / sparser.nonZeros();
  LOG("sparsifying to density " << density
      << " loses " << (loss*100) << "% of mass");

  return old_sum - new_sum;
}

float sparsify_absolute (
    const VectorXf & dense,
    VectorSf & sparse,
    float thresh)
{
  ASSERT_LE(0, thresh);

  const int I = dense.size();

  sparse.resize(I);

  float loss = 0;

  for (int i = 0; i < I; ++i) {

    const float dense_i = dense(i);
    const float abs_i = fabsf(dense_i);

    if (abs_i > thresh) sparse.insert(i) = dense_i;
    else loss += abs_i;
  }

  sparse.finalize();

  return loss;
}

float sparsify_absolute (
    const VectorSf & sparse,
    VectorSf & sparser,
    float thresh)
{
  ASSERT_LE(0, thresh);

  sparser.resize(sparse.size());

  float loss = 0;

  for (VectorSf::InnerIterator iter(sparse); iter; ++iter) {

    const float value_i = iter.value();
    const float abs_i = fabsf(value_i);

    if (abs_i > thresh) sparser.insert(iter.index()) = value_i;
    else loss += abs_i;
  }

  sparser.finalize();

  return loss;
}

float sparsify_absolute (
    const VectorXf & dense,
    VectorSf & sparse,
    const Vector<float> & thresh)
{
  ASSERT1_LE(0, min(thresh));

  const int I = dense.size();

  sparse.resize(I);

  float loss = 0;

  for (int i = 0; i < I; ++i) {

    const float dense_i = dense(i);
    const float abs_i = fabsf(dense_i);

    if (abs_i > thresh[i]) sparse.insert(i) = dense_i;
    else loss += abs_i;
  }

  sparse.finalize();

  return loss;
}

void sparsify_absolute (const MatrixXf & dense, MatrixSf & sparse, float thresh)
{
  ASSERT_LE(0, thresh);

  LOG("sparsifying " << dense.rows() << " x " << dense.cols()
      << " matrix to threshold " << thresh);

  const int I = dense.rows();
  const int J = dense.cols();

  sparse.resize(I,J);

  double sum_dense = 0;
  double sum_sparse = 0;

  for (int j = 0; j < J; ++j) {
    for (int i = 0; i < I; ++i) {

      const float dense_ij = dense(i,j);
      const float abs_ij = fabsf(dense_ij);
      sum_dense += abs_ij;

      if (abs_ij > thresh) {

        sparse.insert(i,j) = dense_ij;
        sum_sparse += abs_ij;
      }
    }
  }

  sparse.finalize();

  float density = sparse.nonZeros() / float(I * J);
  float loss = (sum_dense - sum_sparse) / sum_dense;
  LOG("sparsifying to density " << density
      << " loses " << (100 * loss) << "% of mass");
}

void sparsify_soft_relative_to_row_col_max (
    const MatrixXf & dense,
    MatrixSf & sparse,
    float relthresh,
    bool ignore_diagonal)
{
  ASSERT_LT(0, relthresh);

  LOG("sparsifying " << dense.rows() << " x " << dense.cols()
      << " positive matrix to relative threshold " << relthresh);

  VectorXf row_max;
  VectorXf col_max;

  if (ignore_diagonal) {

    VectorXf diag = dense.diagonal();
    const_cast<MatrixXf &>(dense).diagonal().setZero();

    row_max = dense.rowwise().maxCoeff();
    col_max = dense.colwise().maxCoeff();

    const_cast<MatrixXf &>(dense).diagonal() = diag;

  } else {

    row_max = dense.rowwise().maxCoeff();
    col_max = dense.colwise().maxCoeff();

  }

  const int I = dense.rows();
  const int J = dense.cols();

  sparse.resize(I,J);

  double sum_dense = 0;
  double sum_sparse = 0;

  for (int j = 0; j < J; ++j) {
    for (int i = 0; i < I; ++i) {

      const float dense_ij = dense(i,j);
      sum_dense += dense_ij;

      const float thresh = relthresh * min(row_max(i), col_max(j));
      if (dense_ij > thresh) {

        sparse.insert(i,j) = dense_ij;
        sum_sparse += dense_ij;
      }
    }
  }

  sparse.finalize();

  float density = sparse.nonZeros() / float(I * J);
  float loss = (sum_dense - sum_sparse) / sum_dense;
  LOG("sparsifying to density " << density
      << " loses " << (100 * loss) << "% of mass");
}

float max_entries_heuristic (MatrixXf & dense)
{
  int N = dense.rows() + dense.cols();
  return N * log2f(N);
}

void sparsify_hard_relative_to_row_col_max (
    const MatrixXf & dense,
    MatrixSf & sparse,
    float relthresh,
    int max_entries,
    bool ignore_diagonal)
{
  do {
    sparsify_soft_relative_to_row_col_max(
        dense,
        sparse,
        relthresh,
        ignore_diagonal);
    relthresh *= 1.5f;
  } while (sparse.nonZeros() > max_entries);
}

//----( joint probabilities )-------------------------------------------------

double likelihood_entropy (const VectorXf & likes)
{
  double sum_l = 0;
  double sum_l_log_l = 0;

  for (size_t i = 0, I = likes.size(); i < I; ++i) {

    float li = likes[i];
    if (li > 0) {

      sum_l += li;
      sum_l_log_l += li * log(li);
    }
  }

  return log(sum_l) - sum_l_log_l / sum_l;
}

double likelihood_entropy (const VectorSf & likes)
{
  double sum_l = 0;
  double sum_l_log_l = 0;

  for (VectorSf::InnerIterator iter(likes); iter; ++iter) {

    float li = iter.value();
    if (li > 0) {

      sum_l += li;
      sum_l_log_l += li * log(li);
    }
  }

  return log(sum_l) - sum_l_log_l / sum_l;
}

double likelihood_entropy_rate (const MatrixSf & joint_likes)
{
  double sum_L = 0;
  double sum_L_H = 0;

  for (int i = 0; i < joint_likes.outerSize(); ++i) {

    double sum_l = 0;
    double sum_l_log_l = 0;

    for (MatrixSf::InnerIterator iter(joint_likes,i); iter; ++iter) {

      float li = iter.value();
      if (li > 0) {

        sum_l += li;
        sum_l_log_l += li * log(li);
      }
    }

    sum_L += sum_l;
    sum_L_H += sum_l * log(sum_l) - sum_l_log_l;
  }

  return sum_L_H / sum_L;
}

double likelihood_mutual_info (const MatrixSf & joint_likes)
{
  double sum_lij = 0;
  double sum_lij_log_lij = 0;

  double sum_li = 0;
  double sum_li_log_li = 0;

  Vector<double> likes_j(joint_likes.innerSize());
  likes_j.set(0);

  for (int i = 0; i < joint_likes.outerSize(); ++i) {

    double li = 0;

    for (MatrixSf::InnerIterator iter(joint_likes,i); iter; ++iter) {

      float lij = iter.value();
      if (lij > 0) {

        sum_lij += lij;
        sum_lij_log_lij += lij * log(lij);

        li += lij;
        likes_j[iter.index()] += lij;
      }
    }

    if (li > 0) {
      sum_li += li;
      sum_li_log_li += li * log(li);
    }
  }

  double sum_lj = 0;
  double sum_lj_log_lj = 0;

  for (size_t j = 0, J = likes_j.size; j < J; ++j) {

    double lj = likes_j[j];
    if (lj > 0) {

      sum_lj += lj;
      sum_lj_log_lj += lj * log(lj);
    }
  }

  double hij = log(sum_lij) - sum_lij_log_lij / sum_lij;
  double hi = log(sum_li) - sum_li_log_li / sum_li;
  double hj = log(sum_lj) - sum_lj_log_lj / sum_lj;

  return hi + hj - hij;
}

void constrain_marginals_bp (
    MatrixXf & joint,
    const VectorXf & prior_dom,
    const VectorXf & prior_cod,
    VectorXf & temp_dom,
    VectorXf & temp_cod,
    float tol,
    size_t max_steps,
    bool logging)
{
  // Enforce simultaineous constraints on a joint PMF
  //
  //   /\x. sum y. J(y,x) = p(x)
  //   /\y. sum x. J(y,x) = q(y)

  ASSERT_EQ(prior_dom.size(), joint.cols());
  ASSERT_EQ(prior_cod.size(), joint.rows());
  ASSERT_LT(0, prior_dom.minCoeff());
  ASSERT_LT(0, prior_cod.minCoeff());

  if (logging) LOG("  constraining marginals via full BP");

  const size_t X = joint.cols();
  const size_t Y = joint.rows();

  const Vector<float> p = as_vector(prior_dom);
  const Vector<float> q = as_vector(prior_cod);

  Vector<float> J = as_vector(joint);
  Vector<float> sum_y_J = as_vector(temp_dom);
  Vector<float> sum_x_J = as_vector(temp_cod);

  float stepsize = 0;
  size_t steps = 0;
  while (steps < max_steps) {
    ++steps;
    if (logging) cout << "   step " << steps << "/" << max_steps << flush;

    stepsize = 0;

    // constrain sum y. J(y,x) = 1 first,
    // in case joint is initalized with conditional

    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      sum_y_J[x] = sum(J_x);
    }
    ASSERT_LT(0, min(sum_y_J)); // XXX error here
    imax(stepsize, sqrtf(max_dist_squared(sum_y_J, p)));
    idiv_store_rhs(p, sum_y_J);
    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      J_x *= sum_y_J[x];
    }

    sum_x_J.zero();
    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      sum_x_J += J_x;
    }
    ASSERT_LT(0, min(sum_x_J));
    imax(stepsize, sqrtf(max_dist_squared(sum_x_J, q)));
    idiv_store_rhs(q, sum_x_J);
    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      J_x *= sum_x_J;
    }

    if (logging) LOG(", stepsize = " << stepsize);
    if (stepsize < tol) break;
  }
}

//----( distances )-----------------------------------------------------------

float squared_distance (const Point & x, const Point & y)
{
  ASSERT_EQ(x.size, y.size);

  typedef int Accum;

  const uint8_t * restrict x_data = x.data;
  const uint8_t * restrict y_data = y.data;

  Accum result = 0;
  for (size_t i = 0, I = x.size; i < I; ++i) {
    result += sqr(Accum(x_data[i]) - Accum(y_data[i]));
  }

  return result;
}

} // namespace Cloud


#ifndef HEADER_SHAPEMODELTRI
#define HEADER_SHAPEMODELTRI

#include <string>
#include <string>
#include <iostream>
#include <armadillo>
#include <set>
#include <map>
#include <limits>
#include <cassert>

#include <ShapeUQLib/ShapeModel.hpp>
#include <ShapeUQLib/Facet.hpp>

/**
Declaration of the ShapeModelTri class. Specialized
implementation storing an explicit facet/vertex shape model
*/
template <class PointType>
class ShapeModelTri : public ShapeModel<PointType> {

public:

	/**
	Constructor
	@param frame_graph Pointer to the graph storing
	reference frame relationships
	@param frame_graph Pointer to the reference frame graph
	*/
	ShapeModelTri(std::string ref_frame_name,
		FrameGraph * frame_graph) : ShapeModel<PointType>(ref_frame_name,frame_graph){};

	ShapeModelTri(){};

	ShapeModelTri(const std::vector<std::vector<int> > & vertices,
		const std::vector<int> & super_elements,
		const std::vector<PointType> & control_points);

	/**
	Determines whether the provided point lies inside or outside the shape model.
	The shape model must have a closed surface for this method to be trusted
	@param point coordinates of the point to be tested expressed in the shape model frame
	@param tol numerical tolerance ,i.e value under which the lagrangian of the "surface field"
		below which the point is considered outside
	@return true if point is contained inside the shape, false otherwise
	*/
	bool contains(double * point, double tol = 1e-6) ;

	/**
	Checks that the normals were consistently oriented. If not,
	the ordering of the vertices in the provided shape model file is incorrect
	@param tol numerical tolerance (if consistent: norm(Sum(oriented_surface_area)) / average_facet_surface_area << tol)
	*/
	void check_normals_consistency(double tol = 1e-3) const;

	/**
	Saves the shape model in the form of an .obj file
	@param path Location of the saved file
	@param X translation component to apply
	@param M rotational component to apply
	*/
	void save(std::string path,
		const arma::vec & X = arma::zeros<arma::vec>(3),
		const arma::mat & M = arma::eye<arma::mat>(3,3)) const;

	/**
	Samples N points over each facet of the shape model
	@param N number of samples per facet
	@param points reference to matrix holding points coordinates
	@param normals reference to matrix holding normals coordinates
	*/
	void random_sampling(unsigned int N,arma::mat & points, arma::mat & normals) const;

	virtual unsigned int get_NElements() const;

	/**
	Updates the values of the center of mass, volume, surface area
	*/
	virtual void update_mass_properties();

	/**
	Update all the facets of the shape model
	*/
	void update_facets() ;

	void add_element(Facet & el);
	void set_elements(std::vector<Facet> elements);

	virtual void clear();

	/**
	Updates the specified facets of the shape model. Ensures consistency between the vertex coordinates
	and the facet surface area, normals and centers.
	@param facets Facets to be updated
	@param compute_dyad true if the facet dyad needs to be computed/updated
	*/
	void update_facets(std::set<Facet *> & facets);

	/**
	Computes the surface area of the shape model
	*/
	virtual void compute_surface_area();
	/**
	Computes the volume of the shape model
	*/
	virtual void compute_volume();
	/**
	Computes the center of mass of the shape model
	*/
	virtual void compute_center_of_mass();
	/**
	Computes the inertia tensor of the shape model
	*/
	virtual void compute_inertia();

	Facet & get_element(int e);

	virtual const std::vector<int> & get_element_control_points(int e) const;
	virtual arma::vec::fixed<3> get_point_normal_coordinates(unsigned int i) const;

protected:

	std::vector<Facet> elements;

};

#endif


#include "LITD_VirtualCar.h"
#include "LITD_VirtualPoint.h"
#include "math_utilities.h"
#include <Eigen/Dense>

#include <stdio.h>
#include <unistd.h>
#include <iostream>

LITD_VirtualCar::LITD_VirtualCar()
{
    carPosition.x = 0.0;
    carPosition.y = -0.2;
    carPosition.h = 0.0;
    carSpeed = 0.0;

    stanleyGain = 1.5;
    vKp = 0.1;
    vKi =0.5;

    vpOld.x = carPosition.x;
    vpOld.y = carPosition.y;
    vIntegrate = 0.0;
}

double LITD_VirtualCar::getActSpeed(LITD_VirtualPoint vp, double dtime){
    double distance = (carPosition.getVector2d() - vpOld.getVector2d()).norm();
    std::cout << "Distance : " << distance << std::endl;
    vpOld = carPosition;
    double actSpeed = distance / dtime;
    return actSpeed;
}

void LITD_VirtualCar::speedRegulator(LITD_VirtualPoint vp, double dtime){
    double actSpeed = getActSpeed(vp, dtime);
    double diff = vp.speed - actSpeed;
    vIntegrate += diff;

    carSpeed = vKp * diff + vKi * dtime * vIntegrate;

    std::cout << "-----------------------" << std::endl;
    std::cout << "Vp speed : " << vp.speed << std::endl;
    std::cout << "Act Speed : " << actSpeed << std::endl;
    std::cout << "Speed_diff : " << diff << std::endl;
    std::cout << "Integrate val: " << vIntegrate << std::endl;
    std::cout << "carspeed: " << carSpeed  << std::endl;
    std::cout << "-----------------------" << std::endl;
}

//Params: position of otpimum point, delta time step
//returns the new position of the car after dtime
LITD_VirtualPoint LITD_VirtualCar::updateStep(LITD_VirtualPoint vp, double dtime)
{

    double rad2degree = 180.0 / M_PI;
    //vector between car and virtualpoint
    Vector2d diff = vp.getVector2d() - carPosition.getVector2d();

    //calc sign to steer in direction of road
    int sign = 1;
    double diff_heading_abs = wrapTo2Pi(atan2(diff(1), diff(0)));
    if(wrapTo2Pi(diff_heading_abs - wrapTo2Pi(vp.h))> 4.712 ){
        sign = -1;
    }
    //calc new direction

    //calc normal distance of tangent to car (e)
    e = (vp.getVector2d() - carPosition.getVector2d()).norm() * sign;

    //calc angle between car heading and point tangent
    theta_c =  wrapTo2Pi(vp.h) - wrapTo2Pi(carPosition.h);

    //calc steering-angle with stanley-approach
    carSteeringAngle = theta_c + atan2(stanleyGain*e, carSpeed);

    //Debug Messages
    std::cout << "-----------------------" << std::endl;
    std::cout << "point heading : " << vp.h << "(" << rad2degree * vp.h << "°)" << std::endl;
    std::cout << "car heading: " << carPosition.h << "(" << rad2degree * carPosition.h << "°)" << std::endl;
    std::cout << "diff heading: " << diff_heading_abs << "(" << rad2degree * diff_heading_abs << "°)" << std::endl;
    std::cout << "e: " << e << std::endl;
    std::cout << "Theta_C: " << theta_c << "(" << rad2degree * theta_c << "°)" << std::endl;

    for (int i=0; i<SIM_STEPS; i++){

        std::cout << "h: " << carPosition.h << std::endl;
        carPosition.h += tan(carSteeringAngle)/CAR_AXIS_DIST * carSpeed * dtime/SIM_STEPS;
        carPosition.x += cos(carPosition.h) * carSpeed * dtime/SIM_STEPS;
        carPosition.y += sin(carPosition.h) * carSpeed * dtime/SIM_STEPS;
    }

    std::cout << "new car pos x: " << carPosition.x << std::endl;
    std::cout << "new car pos y: " << carPosition.y << std::endl;
    std::cout << "-----------------------" << std::endl;

 speedRegulator(vp, dtime);
    //calc next point of front axis
    LITD_VirtualPoint newFrontAxisPoint;
    //newFrontAxisPoint.x = carPosition.x+ cos(theta_c) * carSpeed  * dtime;
    //newFrontAxisPoint.y = carPosition.y + sin(theta_c) * carSpeed * dtime;
    //newFrontAxisPoint.h = tan(carSteeringAngle) / (CAR_AXIS_DIST);

    //carPosition = newFrontAxisPoint;

}


#include "system/CameraModel.h"
#include "system/DsoSystem.h"
#include "util/types.h"
#include <Eigen/Core>
#include <gtest/gtest.h>

using namespace fishdso;

TEST(CameraModelTest, CoreanCameraReprojection) {
  // some real-world data
  double scale = 604.0;
  Vec2 center(1.58492, 1.07424);
  int unmapPolyDeg = 5;
  VecX unmapPolyCoeffs(unmapPolyDeg, 1);
  unmapPolyCoeffs << 1.14169, -0.203229, -0.362134, 0.351011, -0.147191;
  int width = 1920, height = 1208;
  CameraModel cam(width, height, scale, center, unmapPolyCoeffs);

  std::srand(42);
  const int testnum = 2000;
  double sqErr = 0.0;

  for (int i = 0; i < testnum; ++i) {
    Vec2 pnt(double(rand() % width), double(rand() % height));
    Vec3 ray = cam.unmap(pnt.data());
    ray *= 10.5;

    Vec2 pntBack = cam.map(ray.data());
    sqErr += (pnt - pntBack).squaredNorm();
  }

  double rmse = std::sqrt(sqErr / testnum); // rmse in pixels
  std::cout << "reprojection rmse = " << rmse << std::endl;
  EXPECT_LT(rmse, 0.1);
}

TEST(CameraModelTest, SolelyPolynomial) {
  // here camera is initialised so that its mapping only inverses the given
  // polynomial
  double scale = 1;
  Vec2 center(0.0, 0.0);
  int unmapPolyDeg = 2;
  VecX unmapPolyCoefs(unmapPolyDeg, 1);
  unmapPolyCoefs << 1.0, -1.0;
  int width = 2, height = 0;
  CameraModel cam(width, height, scale, center, unmapPolyCoefs);

  std::srand(43);
  const int testnum = 2000;
  double sqErr = 0.0;
  for (int i = 0; i < testnum; ++i) {
    double x = double(std::rand()) / RAND_MAX;
    Vec3 pnt(x, 0, 1 - x * x);
    Vec2 projected = cam.map(pnt.data());
    sqErr += (projected - Vec2(x, 0)).squaredNorm();
  }
  double rmse = std::sqrt(sqErr / testnum);
  std::cout << "rmse = " << rmse << std::endl;
  EXPECT_LT(rmse, 0.0005);
}

TEST(CameraModelTest, CamerasPyramid) {
  double scale = 604.0;
  Vec2 center(1.58447, 1.07353);
  int unmapPolyDeg = 7;
  int pyrLevels = 6;
  VecX unmapPolyCoeffs(unmapPolyDeg, 1);
  unmapPolyCoeffs << 1.14544, -0.146714, -0.967996, 2.13329, -2.42001, 1.33018,
      -0.292722;
  int width = 1920, height = 1208;
  CameraModel cam(width, height, scale, center, unmapPolyCoeffs);
  StdVector<CameraModel> camPyr = cam.camPyr(pyrLevels);

  std::mt19937 mt;
  std::uniform_real_distribution<> xs(0, width - 1);
  std::uniform_real_distribution<> ys(0, height - 1);

  const int testCount = 1000;
  for (int lvl = 0; lvl < pyrLevels; ++lvl)
    for (int it = 0; it < testCount; ++it) {
      double x = xs(mt), y = ys(mt);
      Vec2 pnt(x, y);
      Vec2 pntScaled(x / (1 << lvl), y / (1 << lvl));
      Vec3 unmapOrig = cam.unmap(pnt).normalized();
      Vec3 unmapPyr = camPyr[lvl].unmap(pntScaled).normalized();
      double cos = unmapOrig.dot(unmapPyr);
      if (cos < -1)
        cos = -1;
      if (cos > 1)
        cos = 1;
      double angle = (180.0 / M_PI) * std::acos(cos);
      EXPECT_LT(angle, 0.01);
    }
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


extern "C" {
    #include <vlfeat/fisher.h>
    #include <vlfeat/gmm.h>
}

#include <Eigen/Core>
#include <glog/logging.h>
#include <memory>
#include <vector>

#include "fisher_vector_extractor.h"

namespace GraphSfM {
namespace feature {
Eigen::MatrixXf ConvertVectorOfFeaturesToMatrix(const std::vector<Eigen::VectorXf>& features)
{
    Eigen::MatrixXf feature_table(features[0].size(), features.size());
    for (int i = 0; i < feature_table.cols(); i++) {
        feature_table.col(i) = features[i];
    }
    return feature_table;
}

FisherVectorExtractor::FisherVectorExtractor(const Options& options)
    : gmm_(new GaussianMixtureModel(options.num_gmm_clusters)),
      training_feature_sampler_(options.max_num_features_for_training)
{

}

FisherVectorExtractor::~FisherVectorExtractor()
{

}

void FisherVectorExtractor::AddFeaturesForTraining(const std::vector<Eigen::VectorXf>& features)
{
    for (const Eigen::VectorXf& feature : features) {
      CHECK(!feature.hasNaN()) << "Feature: " << feature.transpose();
      training_feature_sampler_.AddElementToSampler(feature);
    }
}

bool FisherVectorExtractor::Train()
{
    // Get the features randomly sampled for training.
    const auto& sampled_features = training_feature_sampler_.GetAllSamples();
    CHECK_GT(sampled_features.size(), 0);
    LOG(INFO) << "Training GMM for Fisher Vector extractin with "
              << sampled_features.size() << " features sampled from "
              << training_feature_sampler_.NumElementsAdded()
              << " total features.";
    // Train the GMM using the training feaures.
    const Eigen::MatrixXf feature_table = ConvertVectorOfFeaturesToMatrix(sampled_features);
    return gmm_->Compute(feature_table);
}

Eigen::VectorXf
FisherVectorExtractor::ExtractGlobalDescriptor(const std::vector<Eigen::VectorXf>& features)
{
    // Ensure there are input features and they are not zero dimensions.
    CHECK_GT(features.size(), 0);
    CHECK_GT(features[0].size(), 0);
    // Convert the features into a continuous memory block. The matrix is of size
    // D x N where D is the number of descrip
    const Eigen::MatrixXf feature_table = ConvertVectorOfFeaturesToMatrix(features);
    // Compute the fisher vector encoding.
    Eigen::VectorXf fisher_vector(2 * feature_table.rows() * gmm_->num_clusters());
    vl_fisher_encode(fisher_vector.data(),
                     VL_TYPE_FLOAT,
                     gmm_->GetMeans(),
                     feature_table.rows(),
                     gmm_->num_clusters(),
                     gmm_->GetCovariances(),
                     gmm_->GetPriors(),
                     feature_table.data(),
                     feature_table.cols(),
                     VL_FISHER_FLAG_IMPROVED);
    DCHECK(std::isfinite(fisher_vector.sum()));
    return fisher_vector;
}

void FisherVectorExtractor::ExportGaussianMixtureModel(std::string filename)
{
    size_t dimension = gmm_->GetDimension();
    size_t num_clusters = gmm_->GetNumClusters();
    float const *means = (float const *)gmm_->GetMeans();
    float const *covariances = (float const *)gmm_->GetCovariances();
    float const *priors = (float const *)gmm_->GetPriors();

    std::ofstream out(filename);
    if (!out.is_open()) {
        LOG(ERROR) << filename << " cannot be created!";
        return;
    }
    out << dimension << " " << num_clusters << std::endl;

    for (int i = 0; i < dimension * num_clusters; i++) {
        out << means[i] << " ";
    }
    out << std::endl;

    for (int i = 0; i < dimension * num_clusters; i++) {
        out << covariances[i] << " ";
    }
    out << std::endl;

    for (int i = 0;  i < num_clusters; i++) {
        out << priors[i] << " ";
    }
    out << std::endl;
    out.close();
}

void FisherVectorExtractor::ImportGaussianMixtureModel(std::string filename)
{
    std::ifstream in(filename);
    if (!in.is_open()) {
        LOG(ERROR) << filename << " cannot be created!";
    }

    size_t dimension, num_clusters;
    float *means = new float [dimension * num_clusters];
    float *covariances = new float [dimension * num_clusters];
    float *priors = new float [num_clusters];

    in >> dimension >> num_clusters;

    for (int i = 0; i < dimension * num_clusters; i++) {
        in >> means[i];
    }
    for (int i = 0; i < dimension * num_clusters; i++) {
        in >> covariances[i];
    }
    for (int i = 0; i < num_clusters; i++) {
        in >> priors[i];
    }
    in.close();

    // gmm_.reset(vl_gmm_new(VL_TYPE_FLOAT, dimension, num_clusters));
    if (!gmm_->Init(dimension, num_clusters)) {
        delete [] means;
        delete [] covariances;
        delete [] priors;
        LOG(ERROR) << "Gaussian Mixture Model import failed!";
        return;
    }
    gmm_->SetMeans(means);
    gmm_->SetCovariances(covariances);
    gmm_->SetPriors(priors);

    delete [] means;
    delete [] covariances;
    delete [] priors;
}

}  // namespace feature
}  // namespace GraphSfM


/**
 * @author Mike Bogochow
 * @version 2.5.0, Dec 8, 2015
 *
 * @file main.cpp
 *
 * pBidder main
 */

#include "defs.h"
#include "Path.h"
#include "SpanningTree.h"
#include "Graph.h"

// boost
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
//#include <boost/lexical_cast.hpp>
#include <boost/graph/graph_utility.hpp>

#include <iostream>
#include <algorithm> // std::remove
#include <limits>

//#define _a 0
//#define _b 1
//#define _c 2
//#define _d 3
//#define _e 4
//#define _f 5
//#define _g 6
//#define _h 7
//#define _i 8

const size_t __num_nodes = 9;
const size_t __num_edges = 36;
Edge __edges[__num_edges] = {
    Edge(_a, _b), Edge(_a, _c), Edge(_a, _d), Edge(_a, _e), Edge(_a, _f), Edge(_a, _g), Edge(_a, _h), Edge(_a, _i),
    Edge(_b, _c), Edge(_b, _d), Edge(_b, _e), Edge(_b, _f), Edge(_b, _g), Edge(_b, _h), Edge(_b, _i),
    Edge(_c, _d), Edge(_c, _e), Edge(_c, _f), Edge(_c, _g), Edge(_c, _h), Edge(_c, _i),
    Edge(_d, _e), Edge(_d, _f), Edge(_d, _g), Edge(_d, _h), Edge(_d, _i),
    Edge(_e, _f), Edge(_e, _g), Edge(_e, _h), Edge(_e, _i),
    Edge(_f, _g), Edge(_f, _h), Edge(_f, _i),
    Edge(_g, _h), Edge(_g, _i),
    Edge(_h, _i)
};
int __weights[__num_edges] = {
    4,       8,       11,      8,      7,        4,       2,      9,
    14,      10,      2,       1,      6,        7,       4,
    8,       11,      8,       7,      4,        2,
    9,       14,      10,      2,      1,
    6,       7,       4,       8,
    11,      8,       7,
    4,       2,
    9 };

/**
 * Main
 */
int main(int argc, char **argv)
{
  Graph *g = new Graph(__edges, __num_edges, __weights, __num_nodes);
//g->print();
  // TODO add my location to graph

  Graph *sub;
  std::vector<Vertex> allocated;
  std::vector<Vertex> unallocated;
  unallocated.reserve(__num_nodes);

  int rtc = 0; // my cost
  for (Vertex i = 0; i < __num_nodes; i++)
    unallocated.push_back(i);

  // each round of bidding
  for (Vertex i = 0; i < __num_nodes; i++)
  {
    std::pair<Vertex, mbogo_weight_t> currentBid = std::make_pair(
        std::numeric_limits<Vertex>::max(),
        std::numeric_limits<mbogo_weight_t>::max());

//std::cerr << "~~~ Round " << i+1 << std::endl;
    // each unallocated target
    for (std::vector<Vertex>::iterator t = unallocated.begin();
        t != unallocated.end(); t++)
    {
      Path *path;
      SpanningTree *tree;
      mbogo_weight_t cost;
      mbogo_weight_t bid;
      std::vector<Vertex> possibleAllocation;

      possibleAllocation = allocated;
//std::cerr << "Adding " << *t << " for possible allocation" << std::endl;
      possibleAllocation.push_back(*t);

      // Catch simple cases for efficiency
      if (possibleAllocation.size() == 1)
      {
        cost = bid = 0;
      }
      else if (possibleAllocation.size() == 2)
      {
        path = Path::fromPair(
            std::make_pair(possibleAllocation.front(),
                possibleAllocation.back()));
        cost = path->getTotalCost(g->getGraph());
        bid = cost - rtc;

        delete path;
      }
      else
      {
        sub = g->getSubgraph(possibleAllocation);
//sub->print();

        tree = SpanningTree::fromGraph(sub->getGraph());
//std::cout << "Spanning Tree:" << std::endl;
//tree->print();

        path = Path::fromTree(tree);

        cost = path->getTotalCost(g->getGraph());//= get_path_cost(g, path); // TODO
        bid = cost - rtc;

        delete path;
        delete tree;
        delete sub;
      }

//std::cerr << "bid[" << i << "_" << *t << "]=" << bid << " (cost-rtc)=(" << cost << "-" << rtc << ")" << std::endl;

      if (currentBid.second > bid && bid >= 0)
        currentBid = std::make_pair(*t, bid);

//std::cerr << "cur_bid=" << currentBid.first << ":" << currentBid.second << std::endl;
    }

    // submit current bid
    // get results

    // if win
    //   allocated.push_back(winning target);
    //   unallocated.erase(std::remove(vec.begin(), vec.end(), winning target), vec.end());
    //   update rtc

    // TODO change
    int winner = unallocated.front();
    allocated.push_back(winner);
    unallocated.erase(std::remove(unallocated.begin(), unallocated.end(), winner), unallocated.end());
    rtc += currentBid.second;
  }

  // Do final calculation with allocated nodes
  sub = g->getSubgraph(allocated);
  SpanningTree *tree = SpanningTree::fromGraph(sub->getGraph());

  // Output spanning tree
  std::cout << "Spanning Tree:" << std::endl;
  tree->print();

  // Get the path from the tree
  Path *path = Path::fromTree(tree);
  mbogo_weight_t pathCost = path->getTotalCost(g->getGraph());//get_path_cost(g, path);

  // Output the path
  std::cout << "Path:" << std::endl;
  path->print();
  std::cerr << "cost: " << pathCost << std::endl;

  delete path;
  delete tree;
  delete g;
  delete sub;

  return EXIT_SUCCESS;
}


/**
 * Sample host program to generate a sequence
 * using jump and parallel generation.
 *
 * 1. make initial state from a seed
 * 2. calculate initial position for each work group.
 *    (This step is time comsuming)
 * 3. Loop:
 *   3.1 generate sub-sequences parallel
 *   3.2 jump for next loop
 */

#include "opencl_tools.hpp"
#include <cstddef>
#include <cfloat>
#include <ctime>
#include <NTL/GF2X.h>
#include <NTL/ZZ.h>

typedef uint32_t uint;
#include "mtgp32-calc-poly.hpp"
#include "mtgp-calc-jump.hpp"
#include "mtgp32-fast-jump.h"
#include "mtgp32-sample-common.h"
#include "parse_opt.h"

using namespace std;
using namespace cl;
using namespace NTL;

/* ================== */
/* OpenCL information */
/* ================== */
std::vector<cl::Platform> platforms;
std::vector<cl::Device> devices;
cl::Context context;
std::string programBuffer;
cl::Program program;
cl::Program::Sources source;
cl::CommandQueue queue;
std::string errorMessage;

/* ========================= */
/* Sample global variables
/* ========================= */
/**
 * max size of jump table
 * 2^(2*MAX_JUMP_TABLE-1) work groups will be supported
 * currently max 2048 work groups are supported
 */
#define MAX_JUMP_TABLE 6
static mtgp32_fast_t mtgp32;
static bool thread_max = false;
/* small size for check */
static const int jump_step = MTGP32_LS * 10;
static ZZ jump;
static uint32_t jump_poly[MTGP32_N];
static uint32_t jump_initial[MTGP32_N * MAX_JUMP_TABLE];

/* =========================
   declaration
   ========================= */
static int test(int argc, char * argv[]);
static void make_jump_table(int group_num);
static void initialize_by_seed(options& opt,
			       Buffer& status_buffer,
			       int group,
			       uint32_t seed);
static void initialize_by_array(options& opt,
				Buffer& status_buffer,
				int group,
				uint32_t seed_array[],
				int seed_size);
static void status_jump(Buffer& status_buffer, int group);
static void generate_uint32(int group_num,
			    Buffer& status_buffer,
			    int data_size);
static void generate_single12(int group_num,
			      Buffer& status_buffer,
			      int data_size);
static void generate_single01(int group_num,
			      Buffer& status_buffer,
			      int data_size);
static int init_check_data(mtgp32_fast_t * mtgp32,
			   uint32_t seed);
static int init_check_data_array(mtgp32_fast_t * mtgp32,
				 uint32_t seed_array[],
				 int size);
static void free_check_data(mtgp32_fast_t * mtgp32);
static void check_data(uint32_t * h_data, int num_data);
static void check_single12(float * h_data, int num_data);
static void check_single01(float * h_data, int num_data);

/* =========================
   mtgp32 sample code
   ========================= */
/**
 * main
 * catch errors
 *@param argc number of arguments
 *@param argv array of arguments
 *@return 0 normal, -1 error
 */
int main(int argc, char * argv[]) {
    try {
	return test(argc, argv);
    } catch (cl::Error e) {
	cerr << "Error Code:" << e.err() << endl;
	cerr << errorMessage << endl;
	cerr << e.what() << endl;
    }
}

/**
 * sample main
 *@param argc number of arguments
 *@param argv array of arguments
 *@return 0 normal, -1 error
 */
static int test(int argc, char * argv[]) {
#if defined(DEBUG)
    cout << "test start" << endl;
#endif
    options opt;
    if (!parse_opt(opt, argc, argv)) {
	return -1;
    }
    // OpenCL setup
#if defined(DEBUG)
    cout << "openCL setup start" << endl;
#endif
    platforms = getPlatforms();
    devices = getDevices();
    context = getContext();
#if defined(APPLE) || defined(__MACOSX) || defined(__APPLE__)
    source = getSource("mtgp32-jump.cli");
#else
    source = getSource("mtgp32-jump.cl");
#endif
    program = getProgram();
    queue = getCommandQueue();
#if defined(DEBUG)
    cout << "openCL setup end" << endl;
#endif

    int max_group_size = getMaxGroupSize();
    if (opt.group_num > max_group_size) {
	cout << "group_num greater than max value("
	     << max_group_size << ")"
	     << endl;
	return -1;
    }
    int max_size = getMaxWorkItemSize(0);
    if (MTGP32_TN > max_size) {
	cout << "workitem size is greater than max value("
	     << dec << max_size << ")"
	     << "current:" << dec << MTGP32_N << endl;
	return -1;
    }
    if (MTGP32_N > max_size) {
	thread_max = true;
    }
    int local_mem_size = getLocalMemSize();
    if (local_mem_size < sizeof(uint32_t) * MTGP32_N * 2) {
	cout << "local memory size is smaller than min value("
	     << dec << sizeof(uint32_t) * MTGP32_N * 2
	     << ") current:"
	     << dec << local_mem_size << endl;
	return -1;
    }
    Buffer status_buffer(context,
			 CL_MEM_READ_WRITE,
			 sizeof(uint32_t) * MTGP32_N * opt.group_num);

    make_jump_table(opt.group_num);
    int data_count = opt.data_count;
    int data_unit = jump_step * opt.group_num;

    // initialize by seed
    // generate uint32_t
    init_check_data(&mtgp32, 1234);
    initialize_by_seed(opt, status_buffer, opt.group_num, 1234);
    while (data_count > 0) {
	generate_uint32(opt.group_num, status_buffer, data_unit);
	status_jump(status_buffer, opt.group_num);
	data_count -= data_unit;
    }
    free_check_data(&mtgp32);

    // initialize by array
    // generate single float
    uint32_t seed_array[5] = {1, 2, 3, 4, 5};
    init_check_data_array(&mtgp32, seed_array, 5);
    initialize_by_array(opt, status_buffer, opt.group_num,
			seed_array, 5);
    data_count = opt.data_count;
    while (data_count > 0) {
	generate_single12(opt.group_num, status_buffer, data_unit);
	status_jump(status_buffer, opt.group_num);
	data_count -= data_unit;
	if (data_count > 0) {
	    generate_single01(opt.group_num, status_buffer, data_unit);
	    status_jump(status_buffer, opt.group_num);
	    data_count -= data_unit;
	}
    }
    free_check_data(&mtgp32);
    return 0;
}

/**
 * prepare jump polynomial.
 * this step may be pre-computed in practical use.
 * @param group_num number of work groups
 */
static void make_jump_table(int group_num)
{
#if defined(DEBUG)
    cout << "make_jump_table start" << endl;
#endif
    mtgp32_fast_t dummy;
    int rc = mtgp32_init(&dummy, &mtgp32_params_fast_11213[0], 1);
    if (rc) {
	cerr << "init error" << endl;
	throw cl::Error(rc, "mtgp32 init error");
    }
    GF2X poly;
    clock_t start = clock();
    calc_characteristic(poly, &dummy);
    clock_t end = clock();
    double time = (double)(end - start) / CLOCKS_PER_SEC * 1000.0;
    cout << "calc_characteristic: " << dec << time << "ms" << endl;
    ZZ step;
    step = jump_step;
    start = clock();
    for (int i = 0; i < MAX_JUMP_TABLE; i++) {
	calc_jump(&jump_initial[i * MTGP32_N],
		  MTGP32_N,
		  step,
		  poly);
	step *= 4;
    }
    step = jump_step;
    step *= group_num - 1;
    calc_jump(jump_poly, MTGP32_N, step, poly);
    end = clock();
    time = (double)(end - start) / CLOCKS_PER_SEC * 1000.0;
    cout << "make jump table: " << dec << time << "ms" << endl;
#if defined(DEBUG)
    cout << "step:" << dec << step << endl;
    cout << "jump_poly[0]:" << hex << jump_poly[0] << endl;
    cout << "jump_poly[1]:" << hex << jump_poly[1] << endl;
    cout << "jump_initial[0]:" << hex << jump_initial[0 * MTGP32_N] << endl;
    cout << "jump_initial[1]:" << hex << jump_initial[1 * MTGP32_N] << endl;
    cout << "jump_initial[2]:" << hex << jump_initial[2 * MTGP32_N] << endl;
#endif
#if defined(DEBUG)
    cout << "make_jump_table end" << endl;
#endif
}

/**
 * initialize mtgp status in device global memory
 * using seed and fixed jump.
 * jump step is fixed to 3^162.
 *@param opt command line option
 *@param status_buffer mtgp status in device global memory
 *@param group number of group
 *@param seed seed for initialization
 */
static void initialize_by_seed(options& opt,
			       Buffer& status_buffer,
			       int group,
			       uint32_t seed)
{
#if defined(DEBUG)
    cout << "initialize_by_seed start" << endl;
#endif
    // jump table
    Buffer jump_table_buffer(context,
			     CL_MEM_READ_WRITE,
			     MTGP32_N * MAX_JUMP_TABLE * sizeof(uint32_t));
    queue.enqueueWriteBuffer(jump_table_buffer,
			     CL_TRUE,
			     0,
			     MTGP32_N * MAX_JUMP_TABLE * sizeof(uint32_t),
			     jump_initial);

    Kernel init_kernel(program, "mtgp32_jump_seed_kernel");
#if defined(DEBUG)
    cout << "arg0 start" << endl;
#endif
    init_kernel.setArg(0, status_buffer);
    init_kernel.setArg(1, seed);
    init_kernel.setArg(2, jump_table_buffer);
#if defined(DEBUG)
    cout << "arg2 end" << endl;
#endif
    int local_item = MTGP32_N;
    if (thread_max) {
	local_item = MTGP32_TN;
    }
    NDRange global(group * local_item);
    NDRange local(local_item);
    Event event;
#if defined(DEBUG)
    cout << "global:" << dec << group * local_item << endl;
    cout << "group:" << dec << group << endl;
    cout << "local:" << dec << local_item << endl;
#endif
    queue.enqueueNDRangeKernel(init_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &event);
    double time = get_time(event);
    cout << "initializing time = " << time * 1000 << "ms" << endl;
#if 0
    uint status[group * MTGP32_N];
    queue.enqueueReadBuffer(status_buffer,
			    CL_TRUE,
			    0,
			    sizeof(uint32_t) * MTGP32_N * group,
			    status);
#if defined(DEBUG)
    cout << "status[0]:" << hex << status[0] << endl;
    cout << "status[MTGP32_N - 1]:" << hex << status[MTGP32_N - 1] << endl;
    cout << "status[MTGP32_N]:" << hex << status[MTGP32_N] << endl;
    cout << "status[MTGP32_N + 1]:" << hex << status[MTGP32_N + 1] << endl;
#endif
    check_status(status, group);
#endif
#if defined(DEBUG)
    cout << "initialize_by_seed end" << endl;
#endif
}

/**
 * initialize mtgp status in device global memory
 * using an array of seeds and jump.
 *@param opt command line option
 *@param status_buffer mtgp status in device global memory
 *@param group number of group
 *@param seed_array seeds for initialization
 *@param seed_size size of seed_array
 */
static void initialize_by_array(options& opt,
				Buffer& status_buffer,
				int group,
				uint32_t seed_array[],
				int seed_size)
{
#if defined(DEBUG)
    cout << "initialize_by_array start" << endl;
#endif
    // jump table
    Buffer jump_table_buffer(context,
			     CL_MEM_READ_WRITE,
			     MTGP32_N * MAX_JUMP_TABLE * sizeof(uint32_t));
    queue.enqueueWriteBuffer(jump_table_buffer,
			     CL_TRUE,
			     0,
			     MTGP32_N * MAX_JUMP_TABLE * sizeof(uint32_t),
			     jump_initial);

    Buffer seed_array_buffer(context,
			     CL_MEM_READ_WRITE,
			     seed_size * sizeof(uint32_t));
    queue.enqueueWriteBuffer(seed_array_buffer,
			     CL_TRUE,
			     0,
			     seed_size * sizeof(uint32_t),
			     seed_array);
    Kernel init_kernel(program, "mtgp32_jump_array_kernel");
    init_kernel.setArg(0, status_buffer);
    init_kernel.setArg(1, seed_array_buffer);
    init_kernel.setArg(2, seed_size);
    init_kernel.setArg(3, jump_table_buffer);
    int local_item = MTGP32_N;
    if (thread_max) {
	local_item = MTGP32_TN;
    }
    NDRange global(group * local_item);
    NDRange local(local_item);
    Event event;
    queue.enqueueNDRangeKernel(init_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &event);
    double time = get_time(event);
#if 0
    uint status[group * MTGP32_N];
    queue.enqueueReadBuffer(status_buffer,
			    CL_TRUE,
			    0,
			    sizeof(uint32_t) * MTGP32_N * group,
			    status);
    check_status(status, group);
#endif
    cout << "initializing time = " << time * 1000 << "ms" << endl;
#if defined(DEBUG)
    cout << "initialize_by_array end" << endl;
#endif
}

/**
 * jump mtgp status in device global memory
 *@param status_buffer mtgp status in device global memory
 *@param group number of group
 */
static void status_jump(Buffer& status_buffer, int group)
{
#if defined(DEBUG)
    cout << "jump start" << endl;
#endif
    // jump table
    Buffer jump_table_buffer(context,
			     CL_MEM_READ_WRITE,
			     MTGP32_N * sizeof(uint32_t));
    queue.enqueueWriteBuffer(jump_table_buffer,
			     CL_TRUE,
			     0,
			     MTGP32_N * sizeof(uint32_t),
			     jump_poly);

    Kernel init_kernel(program, "mtgp32_jump_kernel");
    init_kernel.setArg(0, status_buffer);
    init_kernel.setArg(1, jump_table_buffer);
    int local_item = MTGP32_N;
    if (thread_max) {
	local_item = MTGP32_TN;
    }
    NDRange global(group * local_item);
    NDRange local(local_item);
    Event event;
#if defined(DEBUG)
    cout << "global:" << dec << group * local_item << endl;
    cout << "group:" << dec << group << endl;
    cout << "local:" << dec << local_item << endl;
#endif
    queue.enqueueNDRangeKernel(init_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &event);
    double time = get_time(event);
    cout << "jump time = " << time * 1000 << "ms" << endl;
#if defined(DEBUG)
    cout << "jump end" << endl;
#endif
}

/**
 * generate 32 bit unsigned random numbers in device global memory
 *@param group_num number of groups for execution
 *@param status_buffer mtgp status in device global memory
 *@param data_size number of data to generate
 */
static void generate_uint32(int group_num,
			    Buffer& status_buffer,
			    int data_size)
{
#if defined(DEBUG)
    cout << "generate_uint32 start" << endl;
#endif
    int item_num = MTGP32_TN * group_num;
    int min_size = MTGP32_LS * group_num;
    if (data_size % min_size != 0) {
	data_size = (data_size / min_size + 1) * min_size;
    }
    Kernel uint_kernel(program, "mtgp32_uint32_kernel");
    Buffer output_buffer(context,
			 CL_MEM_READ_WRITE,
			 data_size * sizeof(uint32_t));
    uint_kernel.setArg(0, status_buffer);
    uint_kernel.setArg(1, output_buffer);
    uint_kernel.setArg(2, data_size / group_num);
    NDRange global(item_num);
    NDRange local(MTGP32_TN);
    Event generate_event;
#if defined(DEBUG)
    cout << "generate_uint32 enque kernel start" << endl;
#endif
    queue.enqueueNDRangeKernel(uint_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &generate_event);
#if defined(DEBUG)
    cout << "generate_uint32 enque kernel end" << endl;
#endif
    uint32_t * output = new uint32_t[data_size];
#if defined(DEBUG)
    cout << "generate_uint32 event wait start" << endl;
#endif
    generate_event.wait();
#if defined(DEBUG)
    cout << "generate_uint32 event wait end" << endl;
#endif
#if defined(DEBUG)
    cout << "generate_uint32 readbuffer start" << endl;
#endif
    queue.enqueueReadBuffer(output_buffer,
			    CL_TRUE,
			    0,
			    data_size * sizeof(uint32_t),
			    &output[0]);
#if defined(DEBUG)
    cout << "generate_uint32 readbuffer end" << endl;
#endif
    check_data(output, data_size);
    print_uint32(&output[0], data_size, item_num);
    double time = get_time(generate_event);
    cout << "generate time:" << time * 1000 << "ms" << endl;
    delete[] output;
#if defined(DEBUG)
    cout << "generate_uint32 end" << endl;
#endif
}

/**
 * generate single precision floating point numbers in the range [1, 2)
 * in device global memory
 *@param group_num number of groups for execution
 *@param status_buffer mtgp status in device global memory
 *@param data_size number of data to generate
 */
static void generate_single12(int group_num,
			      Buffer& status_buffer,
			      int data_size)
{
    int item_num = MTGP32_TN * group_num;
    int min_size = MTGP32_LS * group_num;
    if (data_size % min_size != 0) {
	data_size = (data_size / min_size + 1) * min_size;
    }
    Kernel single_kernel(program, "mtgp32_single12_kernel");
    Buffer output_buffer(context,
			 CL_MEM_READ_WRITE,
			 data_size * sizeof(float));
    single_kernel.setArg(0, status_buffer);
    single_kernel.setArg(1, output_buffer);
    single_kernel.setArg(2, data_size / group_num);
    NDRange global(item_num);
    NDRange local(MTGP32_TN);
    Event generate_event;
    queue.enqueueNDRangeKernel(single_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &generate_event);
    float * output = new float[data_size];
    generate_event.wait();
    queue.enqueueReadBuffer(output_buffer,
			    CL_TRUE,
			    0,
			    data_size * sizeof(float),
			    &output[0]);
    check_single12(output, data_size);
    print_float(output, data_size, item_num);
    double time = get_time(generate_event);
    delete[] output;
    cout << "generate time:" << time * 1000 << "ms" << endl;
}

/**
 * generate single precision floating point numbers in the range [0, 1)
 * in device global memory
 *@param group_num number of groups for execution
 *@param status_buffer mtgp status in device global memory
 *@param data_size number of data to generate
 */
static void generate_single01(int group_num,
			      Buffer& status_buffer,
			      int data_size)
{
    int item_num = MTGP32_TN * group_num;
    int min_size = MTGP32_LS * group_num;
    if (data_size % min_size != 0) {
	data_size = (data_size / min_size + 1) * min_size;
    }
    Kernel single_kernel(program, "mtgp32_single01_kernel");
    Buffer output_buffer(context,
			 CL_MEM_READ_WRITE,
			 data_size * sizeof(float));
    single_kernel.setArg(0, status_buffer);
    single_kernel.setArg(1, output_buffer);
    single_kernel.setArg(2, data_size / group_num);
    NDRange global(item_num);
    NDRange local(MTGP32_TN);
    Event generate_event;
    queue.enqueueNDRangeKernel(single_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &generate_event);
    float * output = new float[data_size];
    generate_event.wait();
    queue.enqueueReadBuffer(output_buffer,
			    CL_TRUE,
			    0,
			    data_size * sizeof(float),
			    &output[0]);
    check_single01(output, data_size);
    print_float(output, data_size, item_num);
    double time = get_time(generate_event);
    delete[] output;
    cout << "generate time:" << time * 1000 << "ms" << endl;
}

/* ==============
 * check programs
 * ==============*/
static int init_check_data(mtgp32_fast_t * mtgp32,
			   uint32_t seed)
{
#if defined(DEBUG)
    cout << "init_check_data start" << endl;
#endif
    int rc = mtgp32_init(mtgp32,
			 &mtgp32_params_fast_11213[0],
			 seed);
    if (rc) {
	return rc;
    }
#if defined(DEBUG)
    cout << "init_check_data end" << endl;
#endif
    return 0;
}

static int init_check_data_array(mtgp32_fast_t * mtgp32,
				 uint32_t seed_array[],
				 int size)
{
#if defined(DEBUG)
    cout << "init_check_data_array start" << endl;
#endif
    int rc = mtgp32_init_by_array(mtgp32,
				  &mtgp32_params_fast_11213[0],
				  seed_array,
				  size);
    if (rc) {
	return rc;
    }
#if defined(DEBUG)
    cout << "init_check_data_array end" << endl;
#endif
    return 0;
}

static void free_check_data(mtgp32_fast_t * mtgp32)
{
#if defined(DEBUG)
    cout << "free_check_data start" << endl;
#endif
    mtgp32_free(mtgp32);
#if defined(DEBUG)
    cout << "free_check_data end" << endl;
#endif
}

static void check_data(uint32_t * h_data, int num_data)
{
#if defined(DEBUG)
    cout << "check_data start" << endl;
#endif
    bool error = false;
    bool disp_flg = true;
    int count = 0;
    for (int j = 0; j < num_data; j++) {
	uint32_t r = mtgp32_genrand_uint32(&mtgp32);
	if ((h_data[j] != r) && disp_flg) {
	    cout << "mismatch"
		 << " j = " << dec << j
		 << " data = " << hex << h_data[j]
		 << " r = " << hex << r << endl;
	    cout << "check_data check N.G!" << endl;
	    count++;
	    error = true;
	}
	if (count > 10) {
	    disp_flg = false;
	}
    }
    if (!error) {
	cout << "check_data check O.K!" << endl;
    } else {
	throw cl::Error(-1, "mtgp32 check_data error!");
    }
#if defined(DEBUG)
    cout << "check_data end" << endl;
#endif
}

static void check_single12(float * h_data, int num_data)
{
#if defined(DEBUG)
    cout << "check_single start" << endl;
#endif
    bool error = false;
    bool disp_flg = true;
    int count = 0;
    for (int j = 0; j < num_data; j++) {
	float r =  mtgp32_genrand_close1_open2(&mtgp32);
	if (!(-FLT_EPSILON <= h_data[j] - r &&
	     h_data[j] - r <= FLT_EPSILON)
	    && disp_flg) {
	    cout << "mismatch"
		 << " j = " << dec << j
		 << " data = " << dec << h_data[j]
		 << " r = " << dec << r << endl;
	    cout << "check_data check N.G!" << endl;
	    count++;
	    error = true;
	}
	if (count > 10) {
	    disp_flg = false;
	}
    }
    if (!error) {
	cout << "check_single check O.K!" << endl;
    } else {
	throw cl::Error(-1, "mtgp32 check_data error!");
    }
#if defined(DEBUG)
    cout << "check_single end" << endl;
#endif
}

static void check_single01(float * h_data, int num_data)
{
#if defined(DEBUG)
    cout << "check_single start" << endl;
#endif
    bool error = false;
    bool disp_flg = true;
    int count = 0;
    for (int j = 0; j < num_data; j++) {
	float r =  mtgp32_genrand_close_open(&mtgp32);
	if (!(-FLT_EPSILON <= h_data[j] - r &&
	     h_data[j] - r <= FLT_EPSILON)
	    && disp_flg) {
	    cout << "mismatch"
		 << " j = " << dec << j
		 << " data = " << dec << h_data[j]
		 << " r = " << dec << r << endl;
	    cout << "check_data check N.G!" << endl;
	    count++;
	    error = true;
	}
	if (count > 10) {
	    disp_flg = false;
	}
    }
    if (!error) {
	cout << "check_single check O.K!" << endl;
    } else {
	throw cl::Error(-1, "mtgp32 check_data error!");
    }
#if defined(DEBUG)
    cout << "check_single end" << endl;
#endif
}


/**
 * Copyright 2020, Massachusetts Institute of Technology,
 * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Jingnan Shi, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 */

#include "teaser/matcher.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <flann/flann.hpp>

#include "teaser/geometry.h"

namespace teaser {

std::vector<std::pair<int, int>> Matcher::calculateCorrespondences(
    teaser::PointCloud& source_points, teaser::PointCloud& target_points,
    teaser::FPFHCloud& source_features, teaser::FPFHCloud& target_features, bool use_absolute_scale,
    bool use_crosscheck, bool use_tuple_test, float tuple_scale) {

  Feature cloud_features;
  pointcloud_.push_back(source_points);
  pointcloud_.push_back(target_points);

  // It compute the global_scale_ required to set correctly the search radius
  normalizePoints(use_absolute_scale);

  for (auto& f : source_features) {
    Eigen::VectorXf fpfh(33);
    for (int i = 0; i < 33; i++)
      fpfh(i) = f.histogram[i];
    cloud_features.push_back(fpfh);
  }
  features_.push_back(cloud_features);

  cloud_features.clear();
  for (auto& f : target_features) {
    Eigen::VectorXf fpfh(33);
    for (int i = 0; i < 33; i++)
      fpfh(i) = f.histogram[i];
    cloud_features.push_back(fpfh);
  }
  features_.push_back(cloud_features);

  advancedMatching(use_crosscheck, use_tuple_test, tuple_scale);

  return corres_;
}

void Matcher::normalizePoints(bool use_absolute_scale) {
  int num = 2;
  float scale = 0;

  means_.clear();

  for (int i = 0; i < num; ++i) {
    float max_scale = 0;

    // compute mean
    Eigen::Vector3f mean;
    mean.setZero();

    int npti = pointcloud_[i].size();
    for (int ii = 0; ii < npti; ++ii) {
      Eigen::Vector3f p(pointcloud_[i][ii].x, pointcloud_[i][ii].y, pointcloud_[i][ii].z);
      mean = mean + p;
    }
    mean = mean / npti;
    means_.push_back(mean);

    for (int ii = 0; ii < npti; ++ii) {
      pointcloud_[i][ii].x -= mean(0);
      pointcloud_[i][ii].y -= mean(1);
      pointcloud_[i][ii].z -= mean(2);
    }

    // compute scale
    for (int ii = 0; ii < npti; ++ii) {
      Eigen::Vector3f p(pointcloud_[i][ii].x, pointcloud_[i][ii].y, pointcloud_[i][ii].z);
      float temp = p.norm(); // because we extract mean in the previous stage.
      if (temp > max_scale) {
        max_scale = temp;
      }
    }

    if (max_scale > scale) {
      scale = max_scale;
    }
  }

  // mean of the scale variation
  if (use_absolute_scale) {
    global_scale_ = 1.0f;
  } else {
    global_scale_ = scale; // second choice: we keep the maximum scale.
  }

  if (global_scale_ != 1.0f) {
    for (int i = 0; i < num; ++i) {
      int npti = pointcloud_[i].size();
      for (int ii = 0; ii < npti; ++ii) {
        pointcloud_[i][ii].x /= global_scale_;
        pointcloud_[i][ii].y /= global_scale_;
        pointcloud_[i][ii].z /= global_scale_;
      }
    }
  }
}
void Matcher::advancedMatching(bool use_crosscheck, bool use_tuple_test, float tuple_scale) {

  int fi = 0; // source idx
  int fj = 1; // destination idx

  bool swapped = false;

  if (pointcloud_[fj].size() > pointcloud_[fi].size()) {
    int temp = fi;
    fi = fj;
    fj = temp;
    swapped = true;
  }

  int nPti = pointcloud_[fi].size();
  int nPtj = pointcloud_[fj].size();

  ///////////////////////////
  /// Build FLANNTREE
  ///////////////////////////
  KDTree feature_tree_i(flann::KDTreeSingleIndexParams(15));
  buildKDTree(features_[fi], &feature_tree_i);

  KDTree feature_tree_j(flann::KDTreeSingleIndexParams(15));
  buildKDTree(features_[fj], &feature_tree_j);

  std::vector<int> corres_K, corres_K2;
  std::vector<float> dis;
  std::vector<int> ind;

  std::vector<std::pair<int, int>> corres;
  std::vector<std::pair<int, int>> corres_cross;
  std::vector<std::pair<int, int>> corres_ij;
  std::vector<std::pair<int, int>> corres_ji;

  ///////////////////////////
  /// INITIAL MATCHING
  ///////////////////////////
  std::vector<int> i_to_j(nPti, -1);
  for (int j = 0; j < nPtj; j++) {
    searchKDTree(&feature_tree_i, features_[fj][j], corres_K, dis, 1);
    int i = corres_K[0];
    if (i_to_j[i] == -1) {
      searchKDTree(&feature_tree_j, features_[fi][i], corres_K, dis, 1);
      int ij = corres_K[0];
      i_to_j[i] = ij;
    }
    corres_ji.push_back(std::pair<int, int>(i, j));
  }

  for (int i = 0; i < nPti; i++) {
    if (i_to_j[i] != -1)
      corres_ij.push_back(std::pair<int, int>(i, i_to_j[i]));
  }

  int ncorres_ij = corres_ij.size();
  int ncorres_ji = corres_ji.size();

  // corres = corres_ij + corres_ji;
  for (int i = 0; i < ncorres_ij; ++i)
    corres.push_back(std::pair<int, int>(corres_ij[i].first, corres_ij[i].second));
  for (int j = 0; j < ncorres_ji; ++j)
    corres.push_back(std::pair<int, int>(corres_ji[j].first, corres_ji[j].second));

  ///////////////////////////
  /// CROSS CHECK
  /// input : corres_ij, corres_ji
  /// output : corres
  ///////////////////////////
  if (use_crosscheck) {
    std::cout << "CROSS CHECK" << std::endl;
    // build data structure for cross check
    corres.clear();
    corres_cross.clear();
    std::vector<std::vector<int>> Mi(nPti);
    std::vector<std::vector<int>> Mj(nPtj);

    int ci, cj;
    for (int i = 0; i < ncorres_ij; ++i) {
      ci = corres_ij[i].first;
      cj = corres_ij[i].second;
      Mi[ci].push_back(cj);
    }
    for (int j = 0; j < ncorres_ji; ++j) {
      ci = corres_ji[j].first;
      cj = corres_ji[j].second;
      Mj[cj].push_back(ci);
    }

    // cross check
    for (int i = 0; i < nPti; ++i) {
      for (int ii = 0; ii < Mi[i].size(); ++ii) {
        int j = Mi[i][ii];
        for (int jj = 0; jj < Mj[j].size(); ++jj) {
          if (Mj[j][jj] == i) {
            corres.push_back(std::pair<int, int>(i, j));
            corres_cross.push_back(std::pair<int, int>(i, j));
          }
        }
      }
    }
  } else {
    std::cout << "Skipping Cross Check." << std::endl;
  }

  ///////////////////////////
  /// TUPLE CONSTRAINT
  /// input : corres
  /// output : corres
  ///////////////////////////
  if (use_tuple_test && tuple_scale != 0) {
    std::cout << "TUPLE CONSTRAINT" << std::endl;
    srand(time(NULL));
    int rand0, rand1, rand2;
    int idi0, idi1, idi2;
    int idj0, idj1, idj2;
    float scale = tuple_scale;
    int ncorr = corres.size();
    int number_of_trial = ncorr * 100;
    std::vector<std::pair<int, int>> corres_tuple;

    for (int i = 0; i < number_of_trial; i++) {
      rand0 = rand() % ncorr;
      rand1 = rand() % ncorr;
      rand2 = rand() % ncorr;

      idi0 = corres[rand0].first;
      idj0 = corres[rand0].second;
      idi1 = corres[rand1].first;
      idj1 = corres[rand1].second;
      idi2 = corres[rand2].first;
      idj2 = corres[rand2].second;

      // collect 3 points from i-th fragment
      Eigen::Vector3f pti0 = {pointcloud_[fi][idi0].x, pointcloud_[fi][idi0].y,
                              pointcloud_[fi][idi0].z};
      Eigen::Vector3f pti1 = {pointcloud_[fi][idi1].x, pointcloud_[fi][idi1].y,
                              pointcloud_[fi][idi1].z};
      Eigen::Vector3f pti2 = {pointcloud_[fi][idi2].x, pointcloud_[fi][idi2].y,
                              pointcloud_[fi][idi2].z};

      float li0 = (pti0 - pti1).norm();
      float li1 = (pti1 - pti2).norm();
      float li2 = (pti2 - pti0).norm();

      // collect 3 points from j-th fragment
      Eigen::Vector3f ptj0 = {pointcloud_[fj][idj0].x, pointcloud_[fj][idj0].y,
                              pointcloud_[fj][idj0].z};
      Eigen::Vector3f ptj1 = {pointcloud_[fj][idj1].x, pointcloud_[fj][idj1].y,
                              pointcloud_[fj][idj1].z};
      Eigen::Vector3f ptj2 = {pointcloud_[fj][idj2].x, pointcloud_[fj][idj2].y,
                              pointcloud_[fj][idj2].z};

      float lj0 = (ptj0 - ptj1).norm();
      float lj1 = (ptj1 - ptj2).norm();
      float lj2 = (ptj2 - ptj0).norm();

      if ((li0 * scale < lj0) && (lj0 < li0 / scale) && (li1 * scale < lj1) &&
          (lj1 < li1 / scale) && (li2 * scale < lj2) && (lj2 < li2 / scale)) {
        corres_tuple.push_back(std::pair<int, int>(idi0, idj0));
        corres_tuple.push_back(std::pair<int, int>(idi1, idj1));
        corres_tuple.push_back(std::pair<int, int>(idi2, idj2));
      }
    }
    corres.clear();

    for (size_t i = 0; i < corres_tuple.size(); ++i)
      corres.push_back(std::pair<int, int>(corres_tuple[i].first, corres_tuple[i].second));
  } else {
    std::cout << "Skipping Tuple Constraint." << std::endl;
  }

  if (swapped) {
    std::vector<std::pair<int, int>> temp;
    for (size_t i = 0; i < corres.size(); i++)
      temp.push_back(std::pair<int, int>(corres[i].second, corres[i].first));
    corres.clear();
    corres = temp;
  }
  corres_ = corres;

  ///////////////////////////
  /// ERASE DUPLICATES
  /// input : corres_
  /// output : corres_
  ///////////////////////////
  std::sort(corres_.begin(), corres_.end());
  corres_.erase(std::unique(corres_.begin(), corres_.end()), corres_.end());
}

template <typename T> void Matcher::buildKDTree(const std::vector<T>& data, Matcher::KDTree* tree) {
  int rows, dim;
  rows = (int)data.size();
  dim = (int)data[0].size();
  std::vector<float> dataset(rows * dim);
  flann::Matrix<float> dataset_mat(&dataset[0], rows, dim);
  for (int i = 0; i < rows; i++)
    for (int j = 0; j < dim; j++)
      dataset[i * dim + j] = data[i][j];
  KDTree temp_tree(dataset_mat, flann::KDTreeSingleIndexParams(15));
  temp_tree.buildIndex();
  *tree = temp_tree;
}

template <typename T>
void Matcher::searchKDTree(Matcher::KDTree* tree, const T& input, std::vector<int>& indices,
                           std::vector<float>& dists, int nn) {
  int rows_t = 1;
  int dim = input.size();

  std::vector<float> query;
  query.resize(rows_t * dim);
  for (int i = 0; i < dim; i++)
    query[i] = input(i);
  flann::Matrix<float> query_mat(&query[0], rows_t, dim);

  indices.resize(rows_t * nn);
  dists.resize(rows_t * nn);
  flann::Matrix<int> indices_mat(&indices[0], rows_t, nn);
  flann::Matrix<float> dists_mat(&dists[0], rows_t, nn);

  tree->knnSearch(query_mat, indices_mat, dists_mat, nn, flann::SearchParams(128));
}

} // namespace teaser


#include <chrono>
#include <fstream>
#include <iostream>
#include <random>
#include <utility>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <common_robotics_utilities/simple_hierarchical_clustering.hpp>
#include <common_robotics_utilities/simple_kmeans_clustering.hpp>

using PointVector = common_robotics_utilities::math::VectorVector4d;
using IndexClusteringResult
    = common_robotics_utilities::simple_hierarchical_clustering
        ::IndexClusteringResult;
using PointClusteringResult
    = common_robotics_utilities::simple_hierarchical_clustering
        ::ClusteringResult<Eigen::Vector4d, PointVector>;

void SaveClustering(
    const PointClusteringResult& clusters,
    const std::string& filename)
{
  // Use the cluster index to set z-coordinates for easy visualization
  std::ofstream clustering_output_file(filename, std::ios_base::out);
  if (!clustering_output_file.is_open())
  {
    throw std::invalid_argument(
        "Log file " + filename + " must be write-openable");
  }
  std::cout << "Saving clustering to " << filename << std::endl;
  for (size_t idx = 0; idx < clusters.Clusters().size(); idx++)
  {
    const PointVector& cluster = clusters.Clusters().at(idx);
    const double cluster_num = 1.0 + static_cast<double>(idx);
    for (const Eigen::Vector4d& point : cluster)
    {
      clustering_output_file << point(0) << "," << point(1) << ","
                             << cluster_num << std::endl;
    }
  }
  clustering_output_file.close();
}

void SaveIndexClustering(
    const PointVector& raw_points,
    const IndexClusteringResult& index_clusters,
    const std::string& filename)
{
  SaveClustering(
      common_robotics_utilities::simple_hierarchical_clustering
          ::MakeElementClusteringFromIndexClustering<
              Eigen::Vector4d, PointVector>(raw_points, index_clusters),
      filename);
}

int main(int argc, char** argv)
{
  const size_t num_points
      = (argc >= 2) ? static_cast<size_t>(atoi(argv[1])) : 1000;
  const double cluster_threshold
      = (argc >= 3) ? atof(argv[2]) : 0.1;
  if (cluster_threshold < 0.0)
  {
    throw std::invalid_argument("cluster_threshold < 0.0");
  }
  std::cout << "Generating " << num_points << " points..." << std::endl;
  const int64_t seed = 42;
  std::mt19937_64 prng(seed);
  std::uniform_real_distribution<double> dist(0.0, 10.0);
  PointVector random_points(num_points);
  for (size_t idx = 0; idx < num_points; idx++)
  {
    const double x = dist(prng);
    const double y = dist(prng);
    random_points.at(idx) = Eigen::Vector4d(x, y, 0.0, 0.0);
  }
  std::function<double(const Eigen::Vector4d&, const Eigen::Vector4d&)>
      distance_fn = [] (const Eigen::Vector4d& v1, const Eigen::Vector4d& v2)
  {
    return (v1 - v2).norm();
  };
  const std::vector<bool> use_parallel_options = {false, true};
  for (const bool use_parallel : use_parallel_options)
  {
    // Cluster using index-cluster methods
    // Single-link clustering
    std::cout << "Single-link hierarchical index clustering " << num_points
              << " points..." << std::endl;
    const auto slhic_start = std::chrono::steady_clock::now();
    const auto single_link_index_clusters
        = common_robotics_utilities::simple_hierarchical_clustering
            ::IndexCluster(
                random_points, distance_fn, 1.0,
                common_robotics_utilities::simple_hierarchical_clustering
                    ::ClusterStrategy::SINGLE_LINK, use_parallel);
    const auto slhic_end = std::chrono::steady_clock::now();
    const double slhic_elapsed =
        std::chrono::duration<double>(slhic_end - slhic_start).count();
    std::cout << "...took " << slhic_elapsed << " seconds" << std::endl;
    // Save
    SaveIndexClustering(
        random_points, single_link_index_clusters, (use_parallel)
            ? "/tmp/test_parallel_single_link_index_clustering.csv"
            : "/tmp/test_serial_single_link_index_clustering.csv");
    // Complete-link clustering
    std::cout << "Complete-link hierarchical index clustering " << num_points
              << " points..." << std::endl;
    const auto clhic_start = std::chrono::steady_clock::now();
    const auto complete_link_index_clusters
        = common_robotics_utilities::simple_hierarchical_clustering
            ::IndexCluster(
                random_points, distance_fn, 1.0,
                common_robotics_utilities::simple_hierarchical_clustering
                    ::ClusterStrategy::COMPLETE_LINK, use_parallel);
    const auto clhic_end = std::chrono::steady_clock::now();
    const double clhic_elapsed =
        std::chrono::duration<double>(clhic_end - clhic_start).count();
    std::cout << "...took " << clhic_elapsed << " seconds" << std::endl;
    // Save
    SaveIndexClustering(
        random_points, complete_link_index_clusters, (use_parallel)
            ? "/tmp/test_parallel_complete_link_index_clustering.csv"
            : "/tmp/test_serial_complete_link_index_clustering.csv");
    // Cluster using value-cluster methods
    // Single-link clustering
    std::cout << "Single-link hierarchical clustering " << num_points
              << " points..." << std::endl;
    const auto slhc_start = std::chrono::steady_clock::now();
    const auto single_link_clusters
        = common_robotics_utilities::simple_hierarchical_clustering::Cluster(
            random_points, distance_fn, 1.0,
            common_robotics_utilities::simple_hierarchical_clustering
                ::ClusterStrategy::SINGLE_LINK, use_parallel);
    const auto slhc_end = std::chrono::steady_clock::now();
    const double slhc_elapsed =
        std::chrono::duration<double>(slhc_end - slhc_start).count();
    std::cout << "...took " << slhc_elapsed << " seconds" << std::endl;
    // Save
    SaveClustering(single_link_clusters, (use_parallel)
        ? "/tmp/test_parallel_single_link_clustering.csv"
        : "/tmp/test_serial_single_link_clustering.csv");
    // Complete-link clustering
    std::cout << "Complete-link hierarchical clustering " << num_points
              << " points..." << std::endl;
    const auto clhc_start = std::chrono::steady_clock::now();
    const auto complete_link_clusters
        = common_robotics_utilities::simple_hierarchical_clustering::Cluster(
            random_points, distance_fn, 1.0,
            common_robotics_utilities::simple_hierarchical_clustering
                ::ClusterStrategy::COMPLETE_LINK, use_parallel);
    const auto clhc_end = std::chrono::steady_clock::now();
    const double clhc_elapsed =
        std::chrono::duration<double>(clhc_end - clhc_start).count();
    std::cout << "...took " << clhc_elapsed << " seconds" << std::endl;
    // Save
    SaveClustering(complete_link_clusters, (use_parallel)
        ? "/tmp/test_parallel_complete_link_clustering.csv"
        : "/tmp/test_serial_complete_link_clustering.csv");
    // Cluster using K-means
    const int32_t num_clusters = 50;
    std::cout << "K-means clustering " << num_points << " points into "
              << num_clusters << " clusters..." << std::endl;
    std::function<Eigen::Vector4d(const PointVector&)>
        average_fn = [] (const PointVector& cluster)
    {
      return common_robotics_utilities::math::AverageEigenVector4d(cluster);
    };
    const auto kmeans_start = std::chrono::steady_clock::now();
    const std::vector<int32_t> kmeans_labels
        = common_robotics_utilities::simple_kmeans_clustering::Cluster(
            random_points, distance_fn, average_fn, num_clusters, 42, true,
            use_parallel);
    const auto kmeans_end = std::chrono::steady_clock::now();
    const double kmeans_elapsed =
        std::chrono::duration<double>(kmeans_end - kmeans_start).count();
    std::cout << "...took " << kmeans_elapsed << " seconds" << std::endl;
    // Get value clusters from the K-means labels
    std::vector<PointVector> kmeans_clusters(num_clusters);
    for (size_t idx = 0; idx < kmeans_labels.size(); idx++)
    {
      const size_t cluster_num = static_cast<size_t>(kmeans_labels.at(idx));
      kmeans_clusters.at(cluster_num).push_back(random_points.at(idx));
    }
    // Save
    SaveClustering(complete_link_clusters, (use_parallel)
        ? "/tmp/test_parallel_kmeans_clustering.csv"
        : "/tmp/test_serial_kmeans_clustering.csv");
  }
  std::cout << "Done saving, you can plot as a 3d scatterplot to see clustering"
            << std::endl;
  return 0;
}


/*! \file perlinNoise.hxx
 *  \brief breastPhantom Perlin Noise header file
 *  \author Christian G. Graff
 *  \version 1.0
 *  \date 2018
 *
 *  \copyright To the extent possible under law, the author(s) have
 *  dedicated all copyright and related and neighboring rights to this
 *  software to the public domain worldwide. This software is
 *  distributed without any warranty.  You should have received a copy
 *  of the CC0 Public Domain Dedication along with this software.
 *  If not, see <http://creativecommons.org/publicdomain/zero/1.0/>.
 *
 */

#ifndef PERLINNOISE_HXX_
#define PERLINNOISE_HXX_

#ifndef __BOOST__
#define __BOOST__
#include <boost/random.hpp>
#include <boost/math/distributions/beta.hpp>
#include <boost/program_options.hpp>
#endif

class perlinNoise{

private:
  double frequency;
  double lacunarity;
  double persistence;
  int numOctaves;
  int32_t seed;
  int32_t xNoiseGen,yNoiseGen,zNoiseGen,seedNoiseGen,shiftNoiseGen;
  double makeInt32Range(double x);
  double pInterp(double x);
  double linInterp(double lbound, double rbound, double x);
  double coherentNoise(double x, double y, double z, int32_t mySeed);
  double gradientNoise(double x, double y, double z,
		       int32_t ia, int32_t ib, int32_t ic, int32_t mySeed);

public:
  double getNoise(double* r);
  void setSeed(int32_t inSeed);
  perlinNoise(boost::program_options::variables_map vm, int32_t inSeed, const char* type);
  perlinNoise(boost::program_options::variables_map vm, int32_t inSeed, double freq, double lac, double pers, int oct);
  perlinNoise(boost::program_options::variables_map vm, const char* type);
};

#endif /* PERLINNOISE_HXX_ */


#include <iostream>
#include <fstream>
#include <vector>
#include <string>

#include <vigra/impex.hxx>
#include <vigra/multi_array.hxx>

#include <opencv/cv.hpp>

#include <boost/program_options.hpp>

#include "sift.hpp"
#include "interestpoint.hpp"

namespace po = boost::program_options;

/*
 * Main Function takes a greyvalue image as input
 */
int main(int argc, char** argv) {
    std::string img_file;
    f32_t sigma, k;
    u16_t octaves, dogsPerEpoch;
    bool subpixel;
    bool result;

    po::options_description desc("Options");

    desc.add_options()
        ("help", "Print help messages")
        ("img,i", po::value<std::string>(&img_file), "The image on which sift will be executed")
        ("sigma,s", po::value<f32_t>(&sigma)->default_value(1.6), "The sigma value of the Gaussian calculations")
        ("k,k", po::value<f32_t>(&k)->default_value(std::sqrt(2)), "The constant which is calculated on sigma for the DoGs")
        ("octaves,o", po::value<u16_t>(&octaves)->default_value(4), "How many octaves should be calculated")
        ("dogsPerEpoch,d", po::value<u16_t>(&dogsPerEpoch)->default_value(3), "How many DoGs should be created per epoch")
        ("subpixel,p", po::value<bool>(&subpixel)->default_value(false), "Starts with the doubled size of initial image")
        ("result,r", po::value<bool>(&result)->default_value(false), "Print the resulting InterestPoints in a file")
        ;
    po::positional_options_description p;
    p.add("img", 1);
    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
        po::notify(vm);

        if (vm.count("help")) {
            std::cout << desc << "\n";
            return 1;
        }

        vigra::ImageImportInfo info(img_file.c_str());
        vigra::MultiArray<2, f32_t> img(vigra::Shape2(info.shape()));
        vigra::importImage(info, img);

        sift::Sift sift(dogsPerEpoch, octaves, sigma, k, subpixel);
        std::vector<sift::InterestPoint> interestPoints = sift.calculate(img);

        auto image = cv::imread(img_file.c_str(), CV_LOAD_IMAGE_COLOR);
        u16_t subpixel_divisor = sift.subpixel ? 2 : 1;
        for (const sift::InterestPoint& p : interestPoints) {
            u16_t x = (p.loc.x * std::pow(2, p.octave)) / subpixel_divisor;
            u16_t y = (p.loc.y * std::pow(2, p.octave)) / subpixel_divisor;
            cv::RotatedRect r(cv::Point2f(x, y),
                    cv::Size(p.scale * 10, p.scale * 10),
                    p.orientation);

            cv::Point2f points[4];
            r.points( points );
            cv::line(image, points[0], points[1], cv::Scalar(255, 0, 0));
            cv::line(image, points[0], points[3], cv::Scalar(255, 0, 0));
            cv::line(image, points[2], points[3], cv::Scalar(255, 0, 0));
            cv::line(image, points[1], points[2], cv::Scalar(255, 0, 0));
        }

        cv::imwrite(img_file + "_orientation.png", image);

        if (result) {
            std::ofstream out("interstpoints.txt");
            out << "Location\tscale\torientation\tdescriptors\n";
            for (const sift::InterestPoint& p : interestPoints) {
               out << "[" << p.loc.x << ", " << p.loc.y <<  "]\t" << p.scale << "\t" << p.orientation << "\t" << "[";
               for (f32_t d : p.descriptors) {
                   out << d << ", ";
               }
               out << "]\n";
            }
            out.close();
        }
    } catch (std::exception& ex) {
        std::cerr << ex.what() << std::endl;
    }

    return 0;
}


#include <sparse.h>

#include <sparse_fill.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(SPARSE);

BOOST_AUTO_TEST_CASE(product_block_test)
{
  // scalar case
  typedef sparse::Block<1,1,float> block_type1;

  block_type1 s1(5), s2(2), s3(0);

  sparse::prod(s1,s2,s3);
  BOOST_CHECK(s3[0] == 5*2);

  // block case
  sparse::Block<4,3,float> a;
  sparse::Block<3,3,float> b;
  sparse::Block<4,3,float> c(sparse::zero_block< sparse::Block<4,3,float> >() );
  sparse::fill(a, 1);
  sparse::fill(b, 1);
  sparse::prod(a,b,c);

  BOOST_CHECK( c[0] ==  30 );
  BOOST_CHECK( c[1] ==  36 );
  BOOST_CHECK( c[2] ==  42 );
  BOOST_CHECK( c[3] ==  66 );
  BOOST_CHECK( c[4] ==  81 );
  BOOST_CHECK( c[5] ==  96 );
  BOOST_CHECK( c[6] == 102 );
  BOOST_CHECK( c[7] == 126 );
  BOOST_CHECK( c[8] == 150 );
  BOOST_CHECK( c[9] == 138 );
  BOOST_CHECK( c[10] == 171 );
  BOOST_CHECK( c[11] == 204 );

}

BOOST_AUTO_TEST_SUITE_END();


//! \file examples/Arrangement_on_surface_2/bgl_dual_adapter.cpp
// Adapting the dual of an arrangement to a BGL graph.

#include <CGAL/config.h>

#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/visitors.hpp>

#include <CGAL/Arr_extended_dcel.h>
#include <CGAL/graph_traits_dual_arrangement_2.h>
#include <CGAL/Arr_face_index_map.h>

#include "Extended_face_property_map.h"
#include "arr_exact_construction_segments.h"
#include "arr_print.h"

typedef CGAL::Arr_face_extended_dcel<Traits, unsigned int> Dcel;
typedef CGAL::Arrangement_2<Traits, Dcel>                  Ex_arrangement;
typedef CGAL::Dual<Ex_arrangement>                         Dual_arrangement;
typedef CGAL::Arr_face_index_map<Ex_arrangement>           Face_index_map;
typedef Extended_face_property_map<Ex_arrangement,unsigned int>
                                                           Face_property_map;

int main() {
  // Construct an arrangement of seven intersecting line segments.
  Point p1(1, 1), p2(1, 4), p3(2, 2), p4(3, 7), p5(4, 4), p6(7, 1), p7(9, 3);
  Ex_arrangement  arr;
  insert(arr, Segment(p1, p6));
  insert(arr, Segment(p1, p4));  insert(arr, Segment(p2, p6));
  insert(arr, Segment(p3, p7));  insert(arr, Segment(p3, p5));
  insert(arr, Segment(p6, p7));  insert(arr, Segment(p4, p7));

  // Create a mapping of the arrangement faces to indices.
  Face_index_map index_map(arr);

  // Perform breadth-first search from the unbounded face, using the event
  // visitor to associate each arrangement face with its discover time.
  int time = -1;
  boost::breadth_first_search(Dual_arrangement(arr), arr.unbounded_face(),
                              boost::vertex_index_map(index_map).visitor
                              (boost::make_bfs_visitor
                               (stamp_times(Face_property_map(), time,
                                            boost::on_discover_vertex()))));

  // Print the discover time of each arrangement face.
  for (auto fit = arr.faces_begin(); fit != arr.faces_end(); ++fit) {
    std::cout << "Discover time " << fit->data() << " for ";
    if (fit != arr.unbounded_face()) {
      std::cout << "face ";
      print_ccb<Ex_arrangement>(fit->outer_ccb());
    }
    else std::cout << "the unbounded face.\n";
  }
  return 0;
}