#include <iostream>
//
#include <polyvec/api.hpp>
#include <polyvec/curve-tracer/bezier_merging.hpp>
#include <polyvec/curve-tracer/curve_bezier.hpp>
#include <polyvec/core/log.hpp>
#include <polyvec/misc.hpp>
#include <polyvec/utils/num.hpp>
#include <polyvec/utils/string.hpp>
#include <polyvec/io/vtk_curve_writer.hpp>
#include <polyvec/geometry/winding_number.hpp>
//
#include <Eigen/Core>
#include <Eigen/Geometry>

// These are only required by the second test, i.e., _potrace_merge()
#include <dev/pdf.hpp>
#include <polyvec/image-segment/image_segment.hpp>
#include <polyvec/io/image.hpp>
#include <polyvec/utils/potrace.hpp>

NAMESPACE_BEGIN()

::polyvec::BezierCurve build_bezier(const Eigen::Matrix<double, 2, 4> ctrl) {
  ::polyvec::BezierCurve bz;
  bz.set_control_points(ctrl);
  return bz;
};

void draw_control_polygon(const std::vector<Eigen::Matrix2Xd>& curves) {
  using namespace polyvec;
  for (int i = 0; i < (int)curves.size(); ++i) {
    if (curves[i].cols() == 4) {
      draw::line(curves[i].col(0), curves[i].col(1),
                 Style::outline(colors::talking_orange, .1));
      draw::line(curves[i].col(1), curves[i].col(2),
                 Style::outline(colors::talking_orange, .1));
      draw::line(curves[i].col(2), curves[i].col(3),
                 Style::outline(colors::talking_orange, .1));
    } else {
      // draw::line(curves[i].col(0), curves[i].col(1),
      //            Style::outline(colors::forest_green, .1));
    }
  }
}

void draw_curves(const std::vector<Eigen::Matrix2Xd>& curves) {
  using namespace polyvec;
  for (int i = 0; i < (int)curves.size(); ++i) {
    if (curves[i].cols() == 4) {
      Eigen::Vector3d color =
          i % 2 == 0 ? colors::talking_orange : colors::enemys_blood;
      auto bz = build_bezier(curves[i]);
      auto tess = bz.get_tesselation2();
      for (int j = 0; j < (int)tess.cols() - 1; ++j)
        draw::line(tess.col(j), tess.col(j + 1),
                   Style::outline(color, 7));
    } else {
      Eigen::Vector3d color = i % 2 == 0 ? colors::forest_green : colors::green;
      draw::line(curves[i].col(0), curves[i].col(1), Style::outline(color, 7));
    }
  }
}

void draw_curves_fill(const std::vector<Eigen::Matrix2Xd>& curves) {
  using namespace polyvec;
  vec<real2> polygon;
  for (int i = 0; i < (int)curves.size(); ++i) {
    if (curves[i].cols() == 4) {
      auto bz = build_bezier(curves[i]);
      auto tess = bz.get_tesselation2();
      for (int j = 0; j < (int)tess.cols() - 1; ++j)
        polygon.push_back(tess.col(j));
    } else {
      for (int j = 0; j < (int)curves[i].cols() - 1; ++j)
           polygon.push_back(curves[i].col(j));
    }
  }
  draw::polygon(polygon, Style::fill(colors::dark_gray));
}

void _tiny_merges() {
  using Mat24 = Eigen::Matrix<double, 2, 4>;
  using Mat22 = Eigen::Matrix<double, 2, 2>;

  std::vector<Eigen::Matrix2Xd> curves;
  std::vector<Eigen::Matrix2Xd> curves_merged;
  std::vector<std::vector<int>> out2in;
  bool is_circular;
  double per_curve_cost;

  int dump_counter = 0;
  auto write_results = [&]() {
    // Write the unmerged
    polyvec::VtkCurveWriter writer;

    writer.add_point(curves.back().col(curves.back().cols() - 1));

    for (const Eigen::Matrix2Xd& bzc : curves) {
      if (bzc.cols() == 4)
        writer.add_polyline(build_bezier(bzc).get_tesselation2());
      if (bzc.cols() == 2) writer.add_polyline(bzc);
      for (int i = 0; i < (int)bzc.cols(); ++i) {
        writer.add_point(bzc.col(i));
      }
    }

    writer.dump(polyfit::StringUtils::fmt("test_dump/merge_curves_%04d_%d.vtk",
                                          0, dump_counter));
    writer.clear();

    // Write the merged

    writer.add_point(curves.back().col(curves.back().cols() - 1));

    for (const Eigen::Matrix2Xd& bzc : curves_merged) {
      if (bzc.cols() == 4)
        writer.add_polyline(build_bezier(bzc).get_tesselation2());
      if (bzc.cols() == 2) writer.add_polyline(bzc);
      for (int i = 0; i < (int)bzc.cols(); ++i) {
        writer.add_point(bzc.col(i));
      }
    }

    writer.dump(polyfit::StringUtils::fmt("test_dump/merge_curves_%04d_%d.vtk",
                                          1, dump_counter));
    writer.clear();

    ++dump_counter;
  };

  auto attempt_merge = [&]() {
    polyfit::BezierMerging::MergeRecursivelyOptions options;
    options.distance_tolerance = 1.;
    options.alpha_max = 0.99; // allow a bigger alpha here so that the merge succeed
    options.per_bezier_const =per_curve_cost;
    options.allow_merging_lines = true; // let this work for more testing.

    polyfit::BezierMerging::merge_recursively(
        curves, is_circular, options,  curves_merged, out2in);
    printf("ATTEMP %d \n ", dump_counter);
    for (int i = 0; i < (int)curves_merged.size(); ++i) {
      printf("Curve %d | merged ", i);
      for (int j = 0; j < (int)out2in[i].size(); ++j)
        printf("%d, ", out2in[i][j]);
      printf(" \n ");
    }

    write_results();
  };

  //
  // First try a single bezier
  //
  is_circular = false;
  curves.clear();
  curves.push_back(Mat24());
  curves.back().col(0) << -1, 0;
  curves.back().col(1) << -0.25, 0.75;
  curves.back().col(2) << 0.7, 0.3;
  curves.back().col(3) << 1, 0;
  attempt_merge();

  //
  // Now  single bezier with one line before failes
  //
  is_circular = false;
  per_curve_cost = 0.2;
  curves.clear();
  //
  curves.push_back(Mat22());
  curves.front().col(0) << -2, -1;
  curves.front().col(1) << -1, 0;
  //
  curves.push_back(Mat24());
  curves.back().col(0) << -1, 0;
  curves.back().col(1) << -0.25, 0.75;
  curves.back().col(2) << 0.7, 0.3;
  curves.back().col(3) << 1, 0;
  attempt_merge();

  //
  // Reduce the cost and try again.
  //
  is_circular = false;
  per_curve_cost = 0.10;
  attempt_merge();

  //
  //  Now  single bezier with one line before and one after
  //
  is_circular = false;
  per_curve_cost = 0.2;
  curves.clear();
  //
  curves.push_back(Mat22());
  curves.back().col(0) << -1.5, -0.5;
  curves.back().col(1) << -1, 0;
  //
  curves.push_back(Mat24());
  curves.back().col(0) << -1, 0;
  curves.back().col(1) << -0.25, 0.75;
  curves.back().col(2) << 0.7, 0.3;
  curves.back().col(3) << 1, 0;
  //
  curves.push_back(Mat22());
  curves.back().col(0) << 1, 0;
  curves.back().col(1) << 1.5, -0.5;
  //
  attempt_merge();

  //
  // Reduce the cost and try again.
  //
  is_circular = false;
  per_curve_cost = 0.025;
  attempt_merge();

  //
  // Same as before just transform
  //
  is_circular = false;
  per_curve_cost = 0.2;
  curves.clear();
  curves.push_back(Mat22());
  curves.back().col(0) << -1.25, -0.25;
  curves.back().col(1) << -1, 0;
  //
  curves.push_back(Mat24());
  curves.back().col(0) << -1, 0;
  curves.back().col(1) << -0.25, 0.75;
  curves.back().col(2) << 0.7, 0.3;
  curves.back().col(3) << 1, 0;
  //
  curves.push_back(Mat22());
  curves.back().col(0) << 1, 0;
  curves.back().col(1) << 1.1, -0.1;
  {
    Eigen::Matrix2d A;
    A << -3, 1, 2, -4;
    Eigen::Vector2d b;
    b << -1, 4;
    for (Eigen::Matrix2Xd& bz : curves) {
      bz = ((A * bz).colwise() + b).eval();
    }
  }
  attempt_merge();

  //
  // Reduce the cost and try again.
  //
  is_circular = false;
  per_curve_cost = 0.025;
  attempt_merge();

  //
  // Now add two beziers
  //
  is_circular = false;
  per_curve_cost = 0.2;
  curves.clear();
  //
  curves.push_back(Mat22());
  curves.back().col(0) << -1.25, -0.25;
  curves.back().col(1) << -1, 0;
  //
  curves.push_back(Mat24());
  curves.back().col(0) << -1, 0;  //
  curves.back().col(1) << -0.6, 0.4;
  curves.back().col(2) << -0.2, 0.6;  //
  curves.back().col(3) << 0, 0.6;     //
  curves.push_back(Mat24());
  curves.back().col(0) << 0, 0.6;    //
  curves.back().col(1) << 0.3, 0.6;  //
  curves.back().col(2) << 0.65, 0.35;
  curves.back().col(3) << 1, 0;  //
  //
  curves.push_back(Mat22());
  curves.back().col(0) << 1, 0;
  curves.back().col(1) << 1.1, -0.1;

  {
    Eigen::Matrix2d A;
    A << -3, 1, 2, -4;
    Eigen::Vector2d b;
    b << -1, 4;

    for (Eigen::Matrix2Xd& bz : curves) {
      bz = ((A * bz).colwise() + b).eval();
    }
  }
  attempt_merge();

  //
  // Reduce cost and run again
  //
  is_circular = false;
  per_curve_cost = 0.025;
  attempt_merge();
}

void _potrace_output_merges(const std::string input_address,
                            const std::vector<double>& percurve_penalty) {
  using namespace polyfit;
  static int dump_id = 0;

  // Read the image
  IO::Image I;
  if (!IO::read_image(input_address.c_str(), I)) {
    printf("image not found %s", input_address.c_str());
    return;
  }

  // Get the boundary
  std::vector<mat2x> boundaries;
  std::vector<vec4> colors;
  ImageSegment::expand_and_cleanup(I);
  ImageSegment::extract_closed_regions(I, boundaries, colors);
  const mat2x& R =
      boundaries[ImageSegment::find_binary_color_region(boundaries)];

  // Run the potrace pipeline to get some curves
  std::vector<Eigen::Matrix2Xd> curves = PotraceUtil::run_pipeline(
      R, false /* does not really matter */, false /*absolutely don't merge */);

  std::unique_ptr<::polyvec::DevicePDF> pdf;

  //
  // Draw the image by random colors
  //
  pdf.reset(new ::polyvec::DevicePDF(
      ::polyvec::misc::sfmt("test_dump/merge_curves_potrace_%04d.pdf", dump_id).c_str(),
      (int)(2 + percurve_penalty.size()), 2));

  draw_curves_fill({R});
  pdf->draw(0, 0);

  draw_curves(curves);
  pdf->draw(1, 0);

  draw_curves_fill(curves);
  pdf->draw(1, 1);

  for (int j = 0; j < (int)percurve_penalty.size(); ++j) {
    std::vector<Eigen::Matrix2Xd> curves_merged;
    std::vector<std::vector<int>> out2in;

    polyfit::BezierMerging::MergeRecursivelyOptions options;
    options.distance_tolerance = 0.3;
    options.alpha_max = 0.8;
    options.per_bezier_const =percurve_penalty[j];
    options.allow_merging_lines = true; // let this work for more testing.

    polyfit::BezierMerging::merge_recursively(
        curves, true, options, curves_merged, out2in);

    draw_curves(curves_merged);
    pdf->draw(j+2, 0);

    draw_curves_fill(curves_merged);
    pdf->draw(j+2, 1);
  }

  ++dump_id;
}

void _polyfit_output_merges(const std::string input_address,
                            const std::vector<double>& percurve_penalty) {
  using namespace polyfit;
  static int dump_id = 0;

  std::unique_ptr<::polyvec::DevicePDF> pdf;

  //
  // Draw the image by random colors
  //
  pdf.reset(new ::polyvec::DevicePDF(
      ::polyvec::misc::sfmt("test_dump/merge_curves_polyfit_%04d.pdf", dump_id).c_str(),
      (int)(1 + percurve_penalty.size()), 2));

  FILE *fl = fopen(input_address.c_str(),"r");
  std::vector<Eigen::Matrix2Xd> curves = BezierMerging::read_curve_sequence(fl);
  fclose(fl);

  draw_curves(curves);
  pdf->draw(0, 0);

  draw_curves_fill(curves);
  pdf->draw(0, 1);

  for (int j = 0; j < (int)percurve_penalty.size(); ++j) {
    std::vector<Eigen::Matrix2Xd> curves_merged;
    std::vector<std::vector<int>> out2in;

    polyfit::BezierMerging::MergeRecursivelyOptions options;
    options.distance_tolerance = 0.3;
    options.alpha_max = 0.8;
    options.per_bezier_const =percurve_penalty[j];
    options.allow_merging_lines = true; // let this work for more testing.

    polyfit::BezierMerging::merge_recursively(
        curves, true, options, curves_merged, out2in);

    draw_curves(curves_merged);
    pdf->draw(j+1, 0);

    draw_curves_fill(curves_merged);
    pdf->draw(j+1, 1);
  }

  ++dump_id;
}

NAMESPACE_END()

NAMESPACE_BEGIN(polyvectest)
NAMESPACE_BEGIN(BezierMerging)

int merge_curves(int, char**) {
  // ::_polyfit_output_merges(POLYVEC_TEST_PATH "/bezier_merging/merge_failure_01.txt", {3});
  // return EXIT_FAILURE;

  // MINY TESTS
  ::_tiny_merges();

  // POLYFIT FAILURES
  ::_polyfit_output_merges(POLYVEC_TEST_PATH "/bezier_merging/merge_failure_01.txt", {3});

  // POTRACE
  #define PREFIX "/home/hooshi/code/raster2vector-2019/polyvec_data/binary-perfect"
  std::vector<double> params_to_test = {1, 2, 3 , 4};
  // ::_potrace_output_merges(PREFIX "/castle/32.png",params_to_test);
  ::_potrace_output_merges(PREFIX "/castle/64.png", params_to_test);
  // ::_potrace_output_merges(PREFIX "/apple/32.png", params_to_test);
  ::_potrace_output_merges(PREFIX "/apple/64.png", params_to_test);
  // ::_potrace_output_merges(PREFIX "/plane/32.png", params_to_test);
  ::_potrace_output_merges(PREFIX "/plane/64.png", params_to_test);
  ::_potrace_output_merges(PREFIX "/tabletennis/32.png", params_to_test);
  ::_potrace_output_merges(PREFIX "/tabletennis/64.png", params_to_test);
  #undef PREFIX

  return EXIT_FAILURE;
}

NAMESPACE_END(BezierMerging)
NAMESPACE_END(polyvectest)


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2004 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * SLAM : Simultaneous Locatization and Mapping
 *  Kalman filter representing representation of SLAM
 */

		// Bayes++ Bayesian filtering schemes
#include "BayesFilter/bayesFlt.hpp"
		// Bayes++ SLAM
#include "SLAM.hpp"
#include "kalmanSLAM.hpp"
#include <iostream>
#include <boost/numeric/ublas/io.hpp>

namespace SLAM_filter
{

template <class Base>
inline void zero(FM::ublas::matrix_range<Base> A)
// Zero a matrix_range
{	// Note A cannot be a reference
	typedef typename Base::value_type Base_value_type;
	FM::noalias(A) = FM::ublas::scalar_matrix<Base_value_type>(A.size1(),A.size2(), Base_value_type());
}

Kalman_SLAM::Kalman_SLAM( Kalman_filter_generator& filter_generator ) :
	SLAM(),
	fgenerator(filter_generator),
	loc(0), full(0)
{
	nL = 0;
	nM = 0;
}

Kalman_SLAM::~Kalman_SLAM()
{
	fgenerator.dispose (loc);
	fgenerator.dispose (full);
}

void Kalman_SLAM::init_kalman (const FM::Vec& x, const FM::SymMatrix& X)
{
	// TODO maintain map states
	nL = x.size();
	nM = 0;
	if (loc) fgenerator.dispose (loc);
	if (full) fgenerator.dispose (full);
		// generate a location filter for prediction
	loc = fgenerator.generate(nL);
		// generate full filter
	full = fgenerator.generate(nL);
		// initialise location states
	full->x.sub_range(0,nL) = x;
	full->X.sub_matrix(0,nL,0,nL) = X;
	full->init();
}

void Kalman_SLAM::predict( BF::Linrz_predict_model& lpred )
{
		// extract location part of full
	loc->x = full->x.sub_range(0,nL);
	loc->X = full->X.sub_matrix(0,nL,0,nL);
		// predict location, independent of map
	loc->init();
	loc->predict (lpred);
	loc->update();
		// return location to full
	full->x.sub_range(0,nL) = loc->x;
	full->X.sub_matrix(0,nL,0,nL) = loc->X;
	full->init();
}

void Kalman_SLAM::observe( unsigned feature, const Feature_observe& fom, const FM::Vec& z )
{
	// Assume features added sequentially
	if (feature >= nM) {
		error (BF::Logic_exception("Observe non existing feature"));
		return;
	}
	// TODO Implement nonlinear form
	// Create a augmented sparse observe model for full states
	BF::Linear_uncorrelated_observe_model fullm(full->x.size(), 1);
	fullm.Hx.clear();
	fullm.Hx.sub_matrix(0,nL, 0,nL) = fom.Hx.sub_matrix(0,nL, 0,nL);
	fullm.Hx(0,nL+feature) = fom.Hx(0,nL);
	fullm.Zv = fom.Zv;
	full->observe(fullm, z);
}

void Kalman_SLAM::observe_new( unsigned feature, const Feature_observe_inverse& fom, const FM::Vec& z )
// fom: must have a the special form required for SLAM::obeserve_new
{
		// size consistency, single state feature
	if (fom.Hx.size1() != 1)
		error (BF::Logic_exception("observation and model size inconsistent"));

		// make new filter with additional (uninitialized) feature state
	if (feature >= nM)
	{
		nM = feature+1;
		Kalman_filter_generator::Filter_type* nf = fgenerator.generate(nL+nM);
		FM::noalias(nf->x.sub_range(0,full->x.size())) = full->x;
		FM::noalias(nf->X.sub_matrix(0,full->x.size(),0,full->x.size())) = full->X;

		fgenerator.dispose(full);
		full = nf;
	}
		// build augmented location and observation
	FM::Vec sz(nL+z.size());
	sz.sub_range(0,nL) = full->x.sub_range(0,nL);
	sz.sub_range(nL,nL+z.size() )= z;

	// TODO use named references rather then explict Ha Hb
	FM::Matrix Ha (fom.Hx.sub_matrix(0,1, 0,nL) );
	FM::Matrix Hb (fom.Hx.sub_matrix(0,1, nL,nL+z.size()) );
	FM::Matrix tempHa (1,nL);
	FM::Matrix tempHb (1,sz.size());

		// feature covariance with existing location and features
        // X+ = [0 Ha] X [0 Ha]' + Hb Z Hb'
        // - zero existing feature covariance
	zero( full->X.sub_matrix(0,full->X.size1(), nL+feature,nL+feature+1) );
	full->X.sub_matrix(nL+feature,nL+feature+1,0,nL+nM) = FM::prod(Ha,full->X.sub_matrix(0,nL, 0,nL+nM) );
		// feature state and variance
	full->x[nL+feature] = fom.h(sz)[0];
	full->X(nL+feature,nL+feature) = ( FM::prod_SPD(Ha,full->X.sub_matrix(0,nL, 0,nL),tempHa) +
													  FM::prod_SPD(Hb,fom.Zv,tempHb)
													 ) (0,0);

	full->init ();
}

void Kalman_SLAM::observe_new( unsigned feature, const FM::Float& t, const FM::Float& T )
{
		// Make space in scheme for feature, requires the scheme can deal with resized state
	if (feature >= nM)
	{
		Kalman_filter_generator::Filter_type* nf = fgenerator.generate(nL+feature+1);
		FM::noalias(nf->x.sub_range(0,full->x.size())) = full->x;
		FM::noalias(nf->X.sub_matrix(0,full->x.size(),0,full->x.size())) = full->X;
		zero( nf->X.sub_matrix(0,nf->X.size1(), nL+nM,nf->X.size2()) );

		nf->x[nL+feature] = t;
		nf->X(nL+feature,nL+feature) = T;
		nf->init ();
		fgenerator.dispose(full);
		full = nf;
		nM = feature+1;
	}
	else
	{
		full->x[nL+feature] = t;
		full->X(nL+feature,nL+feature) = T;
		full->init ();
	}
}

void Kalman_SLAM::forget( unsigned feature, bool must_exist )
{
	full->x[nL+feature] = 0.;
			// ISSUE uBLAS has problems accessing the lower symmetry via a sub_matrix proxy, there two two parts seperately
	zero( full->X.sub_matrix(0,nL+feature, nL+feature,nL+feature+1) );
	zero( full->X.sub_matrix(nL+feature,nL+feature+1, nL+feature,full->X.size1()) );
	full->init();
}

void Kalman_SLAM::decorrelate( Bayesian_filter::Bayes_base::Float d )
// Reduce correlation by scaling cross-correlation terms
{
	std::size_t i,j;
	const std::size_t n = full->X.size1();
	for (i = 1; i < n; ++i)
	{
		FM::SymMatrix::Row Xi(full->X,i);
		for (j = 0; j < i; ++j)
		{
			Xi[j] *= d;
		}
		for (j = i+1; j < n; ++j)
		{
			Xi[j] *= d;
		}
	}
	full->init();
}

}//namespace SLAM


/**
 * @file init_rules_test.cpp
 * @author Marcus Edel
 *
 * Tests for the various weight initialize methods.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/init_rules/kathirvalavakumar_subavathi_init.hpp>
#include <mlpack/methods/ann/init_rules/nguyen_widrow_init.hpp>
#include <mlpack/methods/ann/init_rules/oivs_init.hpp>
#include <mlpack/methods/ann/init_rules/orthogonal_init.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>
#include <mlpack/methods/ann/init_rules/zero_init.hpp>

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

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(InitRulesTest);

// Test the RandomInitialization class with a constant value.
BOOST_AUTO_TEST_CASE(ConstantInitTest)
{
  arma::mat weights;
  RandomInitialization constantInit(1, 1);
  constantInit.Initialize(weights, 100, 100);

  bool b = arma::all(arma::vectorise(weights) == 1);
  BOOST_REQUIRE_EQUAL(b, 1);
}

// Test the OrthogonalInitialization class.
BOOST_AUTO_TEST_CASE(OrthogonalInitTest)
{
  arma::mat weights;
  OrthogonalInitialization orthogonalInit;
  orthogonalInit.Initialize(weights, 100, 200);

  arma::mat orthogonalWeights = arma::eye<arma::mat>(100, 100);
  weights *= weights.t();

  for (size_t i = 0; i < weights.n_rows; i++)
    for (size_t j = 0; j < weights.n_cols; j++)
      BOOST_REQUIRE_SMALL(weights.at(i, j) - orthogonalWeights.at(i, j), 1e-3);

  orthogonalInit.Initialize(weights, 200, 100);
  weights = weights.t() * weights;

  for (size_t i = 0; i < weights.n_rows; i++)
    for (size_t j = 0; j < weights.n_cols; j++)
      BOOST_REQUIRE_SMALL(weights.at(i, j) - orthogonalWeights.at(i, j), 1e-3);
}

// Test the OrthogonalInitialization class with a non default gain.
BOOST_AUTO_TEST_CASE(OrthogonalInitGainTest)
{
  arma::mat weights;

  const double gain = 2;
  OrthogonalInitialization orthogonalInit(gain);
  orthogonalInit.Initialize(weights, 100, 200);

  arma::mat orthogonalWeights = arma::eye<arma::mat>(100, 100);
  orthogonalWeights *= (gain * gain);
  weights *= weights.t();

  for (size_t i = 0; i < weights.n_rows; i++)
    for (size_t j = 0; j < weights.n_cols; j++)
      BOOST_REQUIRE_SMALL(weights.at(i, j) - orthogonalWeights.at(i, j), 1e-3);
}

// Test the ZeroInitialization class. If you think about it, it's kind of
// ridiculous to test the zero init rule. But at least we make sure it
// builds without any problems.
BOOST_AUTO_TEST_CASE(ZeroInitTest)
{
  arma::mat weights;
  ZeroInitialization zeroInit;
  zeroInit.Initialize(weights, 100, 100);

  bool b = arma::all(arma::vectorise(weights) == 0);
  BOOST_REQUIRE_EQUAL(b, 1);
}

// Test the KathirvalavakumarSubavathiInitialization class.
BOOST_AUTO_TEST_CASE(KathirvalavakumarSubavathiInitTest)
{
  arma::mat data = arma::randu<arma::mat>(100, 1);

  arma::mat weights;
  KathirvalavakumarSubavathiInitialization kathirvalavakumarSubavathiInit(
      data, 1.5);
  kathirvalavakumarSubavathiInit.Initialize(weights, 100, 100);

  BOOST_REQUIRE_EQUAL(1, 1);
}

// Test the NguyenWidrowInitialization class.
BOOST_AUTO_TEST_CASE(NguyenWidrowInitTest)
{
  arma::mat weights;
  NguyenWidrowInitialization nguyenWidrowInit;
  nguyenWidrowInit.Initialize(weights, 100, 100);

  BOOST_REQUIRE_EQUAL(1, 1);
}

// Test the OivsInitialization class.
BOOST_AUTO_TEST_CASE(OivsInitTest)
{
  arma::mat weights;
  OivsInitialization<> oivsInit;
  oivsInit.Initialize(weights, 100, 100);

  BOOST_REQUIRE_EQUAL(1, 1);
}

BOOST_AUTO_TEST_SUITE_END();


/**
 * Fast map matching.
 *
 * Definition of geometry
 *
 * @author: Can Yang
 * @version: 2017.11.11
 */

#ifndef FMM_GEOMTYPES_HPP
#define FMM_GEOMTYPES_HPP

#include <ogrsf_frmts.h> // C++ API for GDAL
#include <boost/geometry.hpp>
#include <string>
#include <sstream>

namespace FMM {
/**
 * Core data types
 */
namespace CORE{

/**
 *  Point class
 */
typedef boost::geometry::model::point<double, 2,
    boost::geometry::cs::cartesian> Point; // Point for rtree box
/**
 *  Linestring geometry class
 *
 *  This class wraps a boost linestring geometry.
 */
class LineString {
public:
  /**
   * This is the boost geometry linestring class, stored inside the
   * LineString class.
   */
  typedef boost::geometry::model::linestring<Point> linestring_t;
  /**
   * Get the x coordinate of i-th point in the line
   * @param i point index
   * @return x coordinate
   */
  inline double get_x(int i) const{
    return boost::geometry::get<0>(line.at(i));
  };
  /**
   * Get the y coordinate of i-th point in the line
   * @param i point index starting from 0 to N-1, where N is the
   * number of points in the line
   * @return y coordinate
   */
  inline double get_y(int i) const{
    return boost::geometry::get<1>(line.at(i));
  };
  /**
   * Set x coordinate of i-th point in the line
   * @param i point index
   * @param v the value to update the old coordinate
   */
  inline void set_x(int i, double v){
    boost::geometry::set<0>(line.at(i),v);
  };
  /**
   * Set y coordinate of i-th point in the line
   * @param i point index
   * @param v the value to update the old coordinate
   */
  inline void set_y(int i, double v){
    boost::geometry::set<1>(line.at(i),v);
  };
  /**
   * Add a point to the end of the current line
   * @param x x coordinate of the point to add
   * @param y y coordinate of the point to add
   */
  inline void add_point(double x,double y){
    boost::geometry::append(line, Point(x,y));
  };
  /**
   * Add a point to the end of the current line
   * @param point the point to be added
   */
  inline void add_point(const Point& point){
    boost::geometry::append(line, point);
  };
  /**
   * Get the i-th point in the line
   * @param  i point index starting from 0 to N-1
   * @return The i-th point of the line.
   *
   * Note that the point is a copy of the original point.
   * Manipulating the returned point will not change the
   * original line.
   */
  inline Point get_point(int i) const{
    return Point(boost::geometry::get<0>(
      line.at(i)),boost::geometry::get<1>(line.at(i)));
  };
  /**
   * Get a constance reference of the i-th point in the line
   * @param  i point index
   * @return  A constant reference to the ith point of line, which
   * avoids create a new point.
   */
  inline const Point &at(int i) const{
    return line.at(i);
  }
  /**
   * Get the number of points in a line
   * @return the point number
   */
  inline int get_num_points() const{
    return boost::geometry::num_points(line);
  };
  /**
   * Check if the line is empty or not
   * @return true if the line is empty, otherwise false
   */
  inline bool is_empty(){
    return boost::geometry::num_points(line)==0;
  };
  /**
   * Remove all points in the current line.
   */
  inline void clear(){
    boost::geometry::clear(line);
  };
  /**
   * Get the length of the line
   * @return the length value
   */
  inline double get_length() const {
    return boost::geometry::length(line);
  };
  /**
   * Export a string containing WKT representation of the line.
   * @return The WKT of the line
   *
   * Example: LINESTRING (30 10, 10 30, 40 40)
   */
  inline std::string export_wkt() const{
    std::ostringstream ss;
    ss << boost::geometry::wkt(line);
    return ss.str();
  };
  /**
   * Export a string containing GeoJSON representation of the line.
   * @return The GeoJSON of the line
   */
  inline std::string export_json() const{
    std::ostringstream ss;
    int N = get_num_points();
    if (N>0){
      ss << "{\"type\":\"LineString\",\"coordinates\": [";
      for (int i=0;i<N;++i){
        ss << "[" << get_x(i) << "," << get_y(i) <<"]"
           << (i==N-1 ? "": ",");
      }
      ss << "]}";
    }
    return ss.str();
  };
  /**
   * Get a const reference to the inner boost geometry linestring
   * @return const reference to the inner boost geometry linestring
   */
  inline const linestring_t &get_geometry_const() const{
    return line;
  };
  /**
   * Get a reference to the inner boost geometry linestring
   * @return a reference to the inner boost geometry linestring
   */
  linestring_t &get_geometry(){
    return line;
  };

  /**
   * Compare if two linestring are the same.
   *
   * It the two lines overlap with each other within a threshold of 1e-6,
   * they are considered equal. This is used in the test class.
   *
   * @param rhs the linestring
   * @return true if the two lines are equal.
   */
  inline bool operator==(const LineString& rhs) const {
    int N = get_num_points();
    if (rhs.get_num_points()!=N)
      return false;
    bool result = true;
    for (int i=0;i<N;++i){
      if (boost::geometry::distance(get_point(i),rhs.get_point(i))>1e-6)
        result = false;
    }
    return result;
  };
  /**
   * Overwrite the operator of << of linestring
   * @param os an input stream
   * @param rhs a linestring object
   * @return the wkt representation of the line will be written to the stream.
   */
  friend std::ostream& operator<<(std::ostream& os, const LineString& rhs);
private:
  linestring_t line;
}; // LineString

std::ostream& operator<<(std::ostream& os,const FMM::CORE::LineString& rhs);

/**
 * Convert a OGRLineString to a linestring
 * @param line a pointer to OGRLineString
 * @return a linestring
 */
LineString ogr2linestring(const OGRLineString *line);

/**
 * Convert a OGRMultiLineString to a linestring.
 *
 * If the multilinestring contains multiple lines, only
 * the first linestring will be converted and returned as
 * a result.
 *
 * This function is used in reading data from shapefile.
 *
 * @param mline a pointer to the OGRMultiLineString
 * @return a linestring.
 *
 */
LineString ogr2linestring(const OGRMultiLineString *mline);

/**
 * Convert a wkt into a linestring
 * @param  wkt A wkt representation of a line
 * @return  a linestring
 */
LineString wkt2linestring(const std::string &wkt);

/**
 * Convert a linestring into a OGRLineString
 * @param  line input line
 * @return  A OGRLineString, the caller is responsible for
 * freeing the memory.
 */
OGRLineString *linestring2ogr(const LineString &line);

/**
 * Convert a point into a OGRPoint
 * @param  p input point
 * @return  A OGRPoint, the caller is responsible for
 * freeing the memory.
 */
OGRPoint *point2ogr(const Point &p);

}; // CORE

}; // FMM

#endif // FMM_GEOMTYPES_HPP


#include <Eigen/Dense>
#include <iostream>

using namespace std;
using namespace Eigen;

typedef Matrix<float, 1, Dynamic> MatrixType;
typedef Map<MatrixType> MapType;
typedef Map<const MatrixType> MapTypeConst; // a read-only map
const int n_dims = 5;

void main() {
	{
		int arr[8];
		for (int i = 0; i < 8; i++) {
			arr[i] = i;
		}

		cout << "Column-major: \n" << Map<Matrix<int, 2, 4>>(arr) << endl;
		cout << "Row-major: \n" << Map<Matrix<int, 2, 4, RowMajor>>(arr) << endl;
		cout << "Row-major using stride: \n" << Map<Matrix<int, 2, 4>, Unaligned, Stride<1, 4>>(arr) << endl;
	}

	{
		MatrixType m1(n_dims), m2(n_dims);
		m1.setRandom();
		m2.setRandom();

		float * p = &m2(0); // get the addres storing the data for m2
		MapType m2map(p, m2.size()); //m2map shares data with m2
		MapTypeConst m2mapconst(p, m2.size()); // a read-only accessor for m2

		cout << "m1: \n" << m1 << endl;
		cout << "m2: \n" << m2 << endl;
		cout << "Squared euclidean distance: \n" << (m1 - m2).squaredNorm() << endl;
		cout << "Squared euclidean distance, using map: \n" << (m1 - m2map).squaredNorm() << endl;
		m2map(3) = 7; // this will change m2, since they share the same array
		cout << "Updated m2: \n" << m2 << endl;
		cout << "m2 coefficient 2, constant accessor: \n" << m2mapconst(2) << endl;

	}

	{
		// changing the mapped array
		int data[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9 };
		Map<RowVectorXi> v(data, 4);
		cout << "The mapped vector v is : " << v << endl;
		new(&v) Map<RowVectorXi>(data + 4, 5);
		cout << "Now v is : " << v << endl;
	}

	{
		// declare a Map obj without first knowing the mapped array's location in memory
		//int n_matrices = 4;
		//Map<Matrix3f> A(NULL);
		//VectorXf b(n_matrices);
		//for (int i = 0; i < n_dims; i++) {
		//	new (&A) Map<Matrix3f>(get_matrix_pointer(i));
		//	b(i) = A.trace();
		//}
	}

	system("pause");
}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_SF_CBRT_HPP
#define BOOST_MATH_SF_CBRT_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/roots.hpp>
#include <boost/math/special_functions/math_fwd.hpp>

namespace boost{ namespace math{

namespace detail
{

   template <class T>
   struct cbrt_functor
   {
       cbrt_functor(T const& target) : a(target){}
       std::tr1::tuple<T, T, T> operator()(T const& z)
       {
         T sqr = z * z;
         return std::tr1::make_tuple(sqr * z - a, 3 * sqr, 6 * z);
       }
   private:
       T a;
   };

template <class T, class Policy>
T cbrt_imp(T z, const Policy&)
{
   BOOST_MATH_STD_USING
   int i_exp, sign(1);
   if(z < 0)
   {
      z = -z;
      sign = -sign;
   }
   if(z == 0)
      return 0;

   frexp(z, &i_exp);
   T min = static_cast<T>(ldexp(0.5, i_exp/3));
   T max = static_cast<T>(ldexp(2.0, i_exp/3));
   T guess = static_cast<T>(ldexp(1.0, i_exp/3));
   int digits = (policies::digits<T, Policy>()) / 2;
   return sign * tools::halley_iterate(detail::cbrt_functor<T>(z), guess, min, max, digits);
}

} // namespace detail

template <class T, class Policy>
inline typename tools::promote_args<T>::type cbrt(T z, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   return detail::cbrt_imp(result_type(z), pol);
}

template <class T>
inline typename tools::promote_args<T>::type cbrt(T z)
{
   return cbrt(z, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SF_CBRT_HPP


// Copyright Louis Dionne 2016
// Distributed under the Boost Software License, Version 1.0.

#include <boost/mpl/equal.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/vector_c.hpp>
namespace mpl = boost::mpl;

// sample(dimensions)
// base dimensions                     M  L  T  I  Θ  J  N
using mass        = mpl::vector_c<int, 1, 0, 0, 0, 0, 0, 0>;
using length      = mpl::vector_c<int, 0, 1, 0, 0, 0, 0, 0>;
using time        = mpl::vector_c<int, 0, 0, 1, 0, 0, 0, 0>;
using charge      = mpl::vector_c<int, 0, 0, 0, 1, 0, 0, 0>;
using temperature = mpl::vector_c<int, 0, 0, 0, 0, 1, 0, 0>;
using intensity   = mpl::vector_c<int, 0, 0, 0, 0, 0, 1, 0>;
using amount      = mpl::vector_c<int, 0, 0, 0, 0, 0, 0, 1>;

// composite dimensions
using velocity     = mpl::vector_c<int, 0, 1, -1, 0, 0, 0, 0>; // L/T
using acceleration = mpl::vector_c<int, 0, 1, -2, 0, 0, 0, 0>; // L/T²
using force        = mpl::vector_c<int, 1, 1, -2, 0, 0, 0, 0>; // ML/T²
// end-sample

// sample(dimensions-compare)
template <typename A, typename B>
constexpr bool compatible = mpl::equal<A, B>::value;
// end-sample

// sample(quantity)
template <typename Dimensions>
struct quantity {
  double value_;

  explicit quantity(double v) : value_(v) { }

  template <typename OtherDimensions>
  explicit quantity(quantity<OtherDimensions> other)
    : value_(other.value_)
  {
    static_assert(compatible<Dimensions, OtherDimensions>,
      "Constructing quantities with incompatible dimensions!");
  }

  explicit operator double() const { return value_; }
};
// end-sample

// sample(dimensions-compose)
template <typename D1, typename D2>
auto operator*(quantity<D1> a, quantity<D2> b) {
  using D = typename mpl::transform<D1, D2, mpl::plus<>>::type;
  return quantity<D>{static_cast<double>(a) * static_cast<double>(b)};
}

template <typename D1, typename D2>
auto operator/(quantity<D1> a, quantity<D2> b) {
  using D = typename mpl::transform<D1, D2, mpl::minus<>>::type;
  return quantity<D>{static_cast<double>(a) / static_cast<double>(b)};
}

template <typename D1, typename D2>
auto operator+(quantity<D1> a, quantity<D2> b) {
  static_assert(compatible<D1, D2>,
    "Adding quantities with incompatible dimensions!");
  return quantity<D1>{static_cast<double>(a) + static_cast<double>(b)};
}

// etc..
// end-sample

#if 0
// sample(usage)
quantity<mass>         m{10.3};
quantity<length>       d{3.6};
quantity<time>         t{2.4};
quantity<velocity>     v{d / t};
quantity<acceleration> a{...};
quantity<force>        f{m * v}; // Compiler error!
quantity<force>        f{m * a}; // Works as expected
// end-sample
#endif

int main() {
  quantity<mass>         m{10.3};
  quantity<length>       d{3.6};
  quantity<time>         t{2.4};
  quantity<velocity>     v{d / t};
  quantity<acceleration> a{3.9};
  quantity<force>        f{m * a};

  quantity<force> f2 = f + f;
}


#include "advent.hpp"

#include <Eigen/Core>
#include <concepts>
#include <fstream>
#include <iostream>
#include <queue>
#include <string>
#include <vector>

using Eigen::Array;

using std::ifstream;
using std::priority_queue;
using std::string;
using std::vector;

template<typename T, typename U, typename Cmp = std::less<U>>
requires std::equality_comparable<U>
struct priority_item {
    T data;
    U priority;

    friend auto operator<(priority_item const& lhs, priority_item const& rhs) -> bool {
        return Cmp{}(lhs.priority, rhs.priority);
    }

    friend auto operator==(priority_item const& lhs, priority_item const& rhs) -> bool {
        return lhs.priority == rhs.priority;
    }
};

struct point {
    i64 x;
    i64 y;
    auto operator<=>(const point&) const -> bool = default;
};

auto day15(int argc, char** argv) -> int
{
    // read input
    if (argc < 2) {
        fmt::print("Error: no input.");
        return 1;
    }

    ifstream infile(argv[1]); // NOLINT
    string line;
    std::getline(infile, line);

    const i64 ncol = line.size();
    const i64 nrow = std::count(std::istreambuf_iterator<char>(infile), std::istreambuf_iterator<char>(), '\n') + 1;
    infile.seekg(0); // rewind the input stream
    Array<i64, -1, -1> map(nrow, ncol);

    i64 row{0};
    while(std::getline(infile, line)) {
        std::string str(line);
        std::transform(str.begin(), str.end(), map.row(row).begin(), [](auto c) { return static_cast<i64>(c - '0'); });
        ++row;
    }

    // define a simple A* search
    using item = priority_item<point, i64, std::greater<>>;
    decltype(map) costs = map;

    auto astar = [&](auto start, auto end, auto&& cost) {
        priority_queue<item> q;
        q.push({start, cost(start)});

        costs.fill(-1);
        costs(start.x, start.y) = cost(start);

        while (!q.empty()) {
            auto [p, c] = q.top(); q.pop();
            if (p == end) {
                break;
            }
            auto [x, y] = p;
            for (auto i = std::max(0L, x-1); i <= std::min(end.x, x+1); ++i) {
                for (auto j = std::max(0L, y-1); j <= std::min(end.y, y+1); ++j) {
                    if ((i == x && j == y) || (i != x && j != y)) {
                        continue;
                    }
                    point next{i, j};
                    auto new_cost = c + cost(next);
                    if (costs(i, j) == -1 || new_cost < costs(i, j)) {
                        costs(i, j) = new_cost;
                        q.push({next, new_cost});
                    }
                }
            }
        }
    };

    // part 1
    auto cost_p1 = [&](point const& p) { return map(p.x, p.y); };
    const point s1{0, 0};
    const point s2{map.rows()-1, map.cols()-1};
    astar(s1, s2, cost_p1);
    fmt::print("part 1: {}\n", costs(s2.x, s2.y) - costs(s1.x, s1.y));

    // part 2
    constexpr i64 times_larger{5};
    constexpr i64 wrap{9};

    auto cost_p2 = [&](point const& p) {
        auto d = p.x / map.rows() + p.y / map.cols();
        auto x = p.x % map.rows();
        auto y = p.y % map.cols();
        auto c = map(x, y) + d;
        return c > wrap ? c - wrap : c;
    };
    costs.resize(map.rows() * times_larger, map.cols() * times_larger);
    const point s3{costs.rows()-1, costs.cols()-1};
    astar(s1, s3, cost_p2);
    fmt::print("part 2: {}\n", costs(s3.x, s3.y) - costs(s1.x, s1.y));

    return 0;
}


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_MANTISSA_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MANTISSA_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns the signed mantissa of the input argument.

    @par Header <boost/simd/function/mantissa.hpp>

    @par Note
    The @ref exponent @c e and signed @ref mantissa @c m of a floating
    point entry @c x are related by
    \f$x = m\times 2^e\f$, with |m| \f$\in[1, 2[\f$. (Except for zero,
    for which \f$m = 0\f$).

    @see frexp, pow, exponent

    @par Example:

      @snippet mantissa.cpp mantissa

    @par Possible output:

      @snippet mantissa.txt mantissa

  **/
  Value mantissa(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/mantissa.hpp>
#include <boost/simd/function/simd/mantissa.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_MAXMAG_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_MAXMAG_HPP_INCLUDED
#include <boost/simd/ieee/functions/maxmag.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::maxmag_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return select(gt(boost::simd::abs(a0), boost::simd::abs(a1)), a0, a1);
    }
  };
} } }

#endif


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <Eigen/Dense>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkDoubleArray.h>
#include <vtkSmartPointer.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned,K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

int main(){
    clock_t t1;
    t1 = clock();
    for(auto i=0; i < 10000; ++i){
        auto reader = vtkSmartPointer<vtkPolyDataReader>::New();
        reader->SetFileName("Bad.vtk");
        reader->Update();
        auto poly = reader->GetOutput();
        auto N = poly->GetNumberOfPoints();
        auto pts = (double*) poly->GetPoints()->GetData()->GetVoidPointer(0);
        Map3Xd points(pts,3,N);

        // Project points to unit sphere
        points.colwise().normalize();

        // Reset the center of the sphere to origin by translating
        Vector3d center = points.rowwise().mean();
        points = points.colwise() - center;

        // Rotate all points so that the point in 0th column is along z-axis
        Vector3d c = points.col(0);
        double_t cos_t = c(2);
        double_t sin_t = std::sqrt( 1 - cos_t*cos_t );
        Vector3d axis;
        axis << c(1), -c(0), 0.;
        Matrix3d rotMat, axis_cross, outer;
        axis_cross << 0. , -axis(2), axis(1),
                        axis(2), 0., -axis(0),
                        -axis(1), axis(0), 0.;

        outer.noalias() = axis*axis.transpose();

        rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross + (1-cos_t)*outer;
        Matrix3Xd rPts(3,N);
        rPts = rotMat*points; // The points on a sphere rotated

        // Calculate the stereographic projections
        Vector3d p0;
        Map3Xd l0( &(rPts(0,1)), 3, N-1 );
        Matrix3Xd l(3,N-1), proj(3,N-1);
        p0 << 0,0,-1;
        c = rPts.col(0);
        l = (l0.colwise() - c).colwise().normalized();
        for( auto j=0; j < N-1; ++j ){
            proj.col(j) = ((p0(2) - l0(2,j))/l(2,j))*l.col(j) + l0.col(j);
        }
        // Insert the projected points in a CGAL vertex_with_info vector
        std::vector< std::pair< Point, unsigned> > verts;
        for( auto j=0; j < N-1; ++j ){
            verts.push_back(std::make_pair(Point(proj(0,j),proj(1,j)),j+1));
        }

        Delaunay dt( verts.begin(), verts.end() );

        // Write the finite faces of the triangulation to a VTK file
        vtkNew<vtkCellArray> triangles;
        for( auto ffi = dt.finite_faces_begin(); ffi != dt.finite_faces_end(); ++ffi){
            triangles->InsertNextCell(3);
            for(auto j=2; j >= 0; --j)
                triangles->InsertCellPoint(ffi->vertex(j)->info());
        }

        // Iterate over infinite faces
        Face_circulator fc = dt.incident_faces(dt.infinite_vertex()), done(fc);
        if (fc != 0) {
            do{
                triangles->InsertNextCell(3);
                for(auto j=2; j >= 0; --j){
                    auto vh = fc->vertex(j);
                    auto id = dt.is_infinite(vh)? 0 : vh->info();
                    triangles->InsertCellPoint(id);
                }
            }while(++fc != done);
        }
        poly->SetPolys(triangles);

        // Write to VTK file
        vtkNew<vtkPolyDataWriter> writer;
        writer->SetFileName("StereoMesh.vtk");
        writer->SetInputData(poly);
        writer->Write();
    }
    float diff((float)clock() - (float)t1);
    std::cout << "Time elapsed : " << diff / CLOCKS_PER_SEC
              << " seconds" << std::endl;
    return 0;
}


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

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

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_PYTHAGORAS_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_PYTHAGORAS_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>

#include <boost/geometry/core/access.hpp>

#include <boost/geometry/strategies/distance.hpp>

#include <boost/geometry/util/calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <size_t I, typename T>
struct compute_pythagoras
{
    template <typename Point1, typename Point2>
    static inline T apply(Point1 const& p1, Point2 const& p2)
    {
        T const c1 = boost::numeric_cast<T>(get<I-1>(p1));
        T const c2 = boost::numeric_cast<T>(get<I-1>(p2));
        T const d = c1 - c2;
        return d * d + compute_pythagoras<I-1, T>::apply(p1, p2);
    }
};

template <typename T>
struct compute_pythagoras<0, T>
{
    template <typename Point1, typename Point2>
    static inline T apply(Point1 const&, Point2 const&)
    {
        return boost::numeric_cast<T>(0);
    }
};

}
#endif // DOXYGEN_NO_DETAIL

namespace comparable
{

/*!
\brief Strategy to calculate comparable distance between two points
\ingroup strategies
\tparam Point1 \tparam_first_point
\tparam Point2 \tparam_second_point
\tparam CalculationType \tparam_calculation
*/
template <typename CalculationType = void>
class pythagoras
{
public :

    template <typename Point1, typename Point2>
    struct calculation_type
        : util::calculation_type::geometric::binary
          <
              Point1,
              Point2,
              CalculationType
          >
    {};

    template <typename Point1, typename Point2>
    static inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& p1, Point2 const& p2)
    {
        BOOST_CONCEPT_ASSERT( (concept::ConstPoint<Point1>) );
        BOOST_CONCEPT_ASSERT( (concept::ConstPoint<Point2>) );

        // Calculate distance using Pythagoras
        // (Leave comment above for Doxygen)

        assert_dimension_equal<Point1, Point2>();

        return detail::compute_pythagoras
            <
                dimension<Point1>::value,
                typename calculation_type<Point1, Point2>::type
            >::apply(p1, p2);
    }
};

} // namespace comparable

/*!
\brief Strategy to calculate the distance between two points
\ingroup strategies
\tparam CalculationType \tparam_calculation

\qbk{
[heading Notes]
[note Can be used for points with two\, three or more dimensions]
[heading See also]
[link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
}

*/
template
<
    typename CalculationType = void
>
class pythagoras
{
public :

    template <typename P1, typename P2>
    struct calculation_type
        : util::calculation_type::geometric::binary
          <
              P1,
              P2,
              CalculationType,
              double,
              double // promote integer to double
          >
    {};

    /*!
    \brief applies the distance calculation using pythagoras
    \return the calculated distance (including taking the square root)
    \param p1 first point
    \param p2 second point
    */
    template <typename P1, typename P2>
    static inline typename calculation_type<P1, P2>::type
    apply(P1 const& p1, P2 const& p2)
    {
        // The cast is necessary for MSVC which considers sqrt __int64 as an ambiguous call
        return std::sqrt
            (
                 boost::numeric_cast<typename calculation_type<P1, P2>::type>
                    (
                        comparable::pythagoras<CalculationType>::apply(p1, p2)
                    )
            );
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType>
struct tag<pythagoras<CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename CalculationType, typename P1, typename P2>
struct return_type<distance::pythagoras<CalculationType>, P1, P2>
    : pythagoras<CalculationType>::template calculation_type<P1, P2>
{};

template <typename CalculationType>
struct comparable_type<pythagoras<CalculationType> >
{
    typedef comparable::pythagoras<CalculationType> type;
};

template <typename CalculationType>
struct get_comparable<pythagoras<CalculationType> >
{
    typedef comparable::pythagoras<CalculationType> comparable_type;
public :
    static inline comparable_type apply(pythagoras<CalculationType> const& )
    {
        return comparable_type();
    }
};

template <typename CalculationType, typename Point1, typename Point2>
struct result_from_distance<pythagoras<CalculationType>, Point1, Point2>
{
private :
    typedef typename return_type<pythagoras<CalculationType>, Point1, Point2>::type return_type;
public :
    template <typename T>
    static inline return_type apply(pythagoras<CalculationType> const& , T const& value)
    {
        return return_type(value);
    }
};

// Specializations for comparable::pythagoras
template <typename CalculationType>
struct tag<comparable::pythagoras<CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename CalculationType, typename P1, typename P2>
struct return_type<comparable::pythagoras<CalculationType>, P1, P2>
    : comparable::pythagoras<CalculationType>::template calculation_type<P1, P2>
{};

template <typename CalculationType>
struct comparable_type<comparable::pythagoras<CalculationType> >
{
    typedef comparable::pythagoras<CalculationType> type;
};

template <typename CalculationType>
struct get_comparable<comparable::pythagoras<CalculationType> >
{
    typedef comparable::pythagoras<CalculationType> comparable_type;
public :
    static inline comparable_type apply(comparable::pythagoras<CalculationType> const& )
    {
        return comparable_type();
    }
};

template <typename CalculationType, typename Point1, typename Point2>
struct result_from_distance<comparable::pythagoras<CalculationType>, Point1, Point2>
{
private :
    typedef typename return_type<comparable::pythagoras<CalculationType>, Point1, Point2>::type return_type;
public :
    template <typename T>
    static inline return_type apply(comparable::pythagoras<CalculationType> const& , T const& value)
    {
        return_type const v = value;
        return v * v;
    }
};

template <typename Point1, typename Point2>
struct default_strategy<point_tag, Point1, Point2, cartesian_tag, cartesian_tag, void>
{
    typedef pythagoras<> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_PYTHAGORAS_HPP


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

typedef double scalar;

// Dynamic Eigen typedefs
typedef Eigen::Matrix<scalar, -1, 1> VectorX;
typedef Eigen::Matrix<scalar, 1, -1> RowVectorX;
typedef Eigen::Matrix<scalar, -1, -1> MatrixX;

// 3D Eigen typedefs
typedef Eigen::Matrix<scalar, 3, 1> Vector3;
typedef Eigen::Matrix<scalar, 1, 3> RowVector3;
typedef Eigen::Matrix<scalar, 3, 3> Matrix3;

// Vectorfield and Scalarfield typedefs
#ifdef USE_CUDA
    #include "managed_allocator.hpp"
    typedef std::vector<Vector3, managed_allocator<Vector3>> vectorfield;
    typedef std::vector<scalar, managed_allocator<scalar>> scalarfield;
#else
    typedef std::vector<Vector3> vectorfield;
    typedef std::vector<scalar> scalarfield;
#endif


// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <gtest/gtest.h>

#include <boost/exception/to_string.hpp>

#include <array>

#include "SurgSim/Framework/Timer.h"
#include "SurgSim/DataStructures/Grid.h"

namespace SurgSim
{
namespace DataStructures
{

/// This class test the grid timings for a given concentration of elements per cell and a given number of element per
/// dimension. These two information are embedded in GTest WithParamInterface which takes a
/// tuple<double = concentrationPerCell, size_t = numElementsPerDimension>
class Grid3DPerformanceTests : public ::testing::Test,
							   public ::testing::WithParamInterface<std::tuple<double, size_t>>
{
public:
	typedef Eigen::Matrix<size_t, 3, 1> Vector3ui;

	virtual void SetUp()
	{
		m_h = 0.1;
		m_bounds.min().setConstant(-pow(2, 10) / 2.0);
		m_bounds.max().setConstant(pow(2, 10) / 2.0);
		m_grid = std::make_shared<Grid<size_t, 3>>(Eigen::Matrix<double, 3, 1>::Constant(m_h), m_bounds);
	}

	void addElementsUniformDistribution(const Vector3ui& numElementsPerAxis, double concentrationPerAxis)
	{
		double coef = m_h / static_cast<double>(concentrationPerAxis);
		size_t elementId = 0;

		for (size_t x = 0; x < numElementsPerAxis[0]; x++)
		{
			for (size_t y = 0; y < numElementsPerAxis[1]; y++)
			{
				for (size_t z = 0; z < numElementsPerAxis[2]; z++)
				{
					SurgSim::Math::Vector3d point(x * coef, y * coef, z * coef);
					m_grid->addElement(elementId, point);
					elementId++;
				}
			}
		}
	}

	double performTimingTest(double concentrationPerCell, size_t numElementPerDimension)
	{
		SurgSim::Framework::Timer timer;
		Vector3ui numElementsPerAxis = Vector3ui::Constant(numElementPerDimension);
		double concentrationPerAxis = pow(concentrationPerCell, 1.0 / 3.0);

		timer.start();

		// Clear the grid from all previously added elements and clear the neighbor's list
		m_grid->reset();
		// Add all elements in the grid, triggering a dirty flag for the neighbor's list
		addElementsUniformDistribution(numElementsPerAxis, concentrationPerAxis);
		// Request any neighbor's list to force all neighbor's lists recalculation
		m_grid->getNeighbors(0);

		timer.endFrame();

		return timer.getCumulativeTime();
	}

protected:
	/// Grid size (cells are cubic in this test)
	double m_h;

	/// Grid boundary
	Eigen::AlignedBox<double, 3> m_bounds;

	/// Grid
	std::shared_ptr<Grid<size_t, 3>> m_grid;
};

TEST_P(Grid3DPerformanceTests, Grid3DTest)
{
	double concentrationPerCell;
	size_t numElementsPerDimension;
	std::tie(concentrationPerCell, numElementsPerDimension) = GetParam();
	size_t numElements = numElementsPerDimension * numElementsPerDimension * numElementsPerDimension;
	RecordProperty("ElementsPerCell", boost::to_string(concentrationPerCell));
	RecordProperty("NumberOfElements", boost::to_string(numElements));
	RecordProperty("Duration", boost::to_string(performTimingTest(concentrationPerCell, numElementsPerDimension)));
}

INSTANTIATE_TEST_CASE_P(
	Grid3D,
	Grid3DPerformanceTests,
	::testing::Combine(
		// Concentration per cell is fine between 1 and 3^3, then coarser
		::testing::Values(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0, 16.0, 17.0,
						  18.0, 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, 25.0, 26.0, 27.0,
						  4 * 4 * 4, 5 * 5 * 5, 6 * 6 * 6, 7 * 7 * 7, 8 * 8 * 8, 9 * 9 * 9, 10 * 10 * 10),
		// Number of elements per dimension
		::testing::Values(50, 60, 60, 70, 80, 90, 100, 110, 120)));

} // namespace DataStructures
} // namespace SurgSim


#pragma once

#ifdef POLYSOLVE_WITH_AMGCL

////////////////////////////////////////////////////////////////////////////////
#include <polysolve/LinearSolver.hpp>

#include <Eigen/Core>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/make_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/coarsening/smoothed_aggregation.hpp>
#include <amgcl/coarsening/plain_aggregates.hpp>
#include <amgcl/coarsening/aggregation.hpp>
#include <amgcl/coarsening/ruge_stuben.hpp>
#include <amgcl/relaxation/spai0.hpp>
#include <amgcl/relaxation/gauss_seidel.hpp>
#include <amgcl/solver/cg.hpp>
#include <amgcl/solver/bicgstab.hpp>
#include <amgcl/solver/gmres.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/profiler.hpp>
#include <amgcl/io/mm.hpp>
#include <amgcl/relaxation/chebyshev.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/preconditioner/runtime.hpp>
#include <amgcl/value_type/static_matrix.hpp>
// #include <amgcl/backend/vexcl.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/reorder.hpp>
#include <amgcl/adapter/eigen.hpp>
#include <amgcl/profiler.hpp>
#include <memory>
#include <type_traits>

////////////////////////////////////////////////////////////////////////////////
//
// WARNING:
// The matrix is assumed to be in row-major format, since AMGCL assumes that the
// outer index is for row. If the matrix is symmetric, you are fine, because CSR
// and CSC are the same. If the matrix is not symmetric and you pass in a
// column-major matrix, the solver will actually solve A^T x = b.
//

namespace polysolve
{
    class LinearSolverAMGCL : public LinearSolver
    {

    public:
        LinearSolverAMGCL();
        ~LinearSolverAMGCL();

    private:
        POLYSOLVE_DELETE_MOVE_COPY(LinearSolverAMGCL)

    public:
        //////////////////////
        // Public interface //
        //////////////////////

        // Set solver parameters
        virtual void setParameters(const json &params) override;

        // Retrieve information
        virtual void getInfo(json &params) const override;

        // Analyze sparsity pattern
        virtual void analyzePattern(const StiffnessMatrix &A, const int precond_num) override { precond_num_ = precond_num; }

        // Factorize system matrix
        virtual void factorize(const StiffnessMatrix &A) override;

        // Solve the linear system Ax = b
        virtual void solve(const Ref<const VectorXd> b, Ref<VectorXd> x) override;

        // Name of the solver type (for debugging purposes)
        virtual std::string name() const override { return "AMGCL"; }

    private:
        using Backend = amgcl::backend::builtin<double>;
        using Solver = amgcl::make_solver<
            amgcl::runtime::preconditioner<Backend>,
            amgcl::runtime::solver::wrapper<Backend>>;
        std::unique_ptr<Solver> solver_;
        json params_;
        typename Backend::params backend_params_;
        int precond_num_;

        // Output info
        size_t iterations_;
        double residual_error_;
    };
} // namespace polysolve

#endif


/*
 [auto_generated]
 libs/numeric/odeint/test/implicit_euler.cpp

 [begin_description]
 This file tests the implicit Euler stepper.
 [end_description]

 Copyright 2010-2011 Mario Mulansky
 Copyright 2010-2012 Karsten Ahnert

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

// disable checked iterator warning for msvc
#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_implicit_euler

#include <boost/test/unit_test.hpp>

#include <utility>
#include <iostream>

#include <boost/numeric/odeint/stepper/implicit_euler.hpp>
//#include <boost/numeric/odeint/util/ublas_resize.hpp>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

typedef double value_type;
typedef boost::numeric::ublas::vector< value_type > state_type;
typedef boost::numeric::ublas::matrix< value_type > matrix_type;

/* use functors, because functions don't work with msvc 10, I guess this is a bug */
struct sys
{
    void operator()( const state_type &x , state_type &dxdt , const value_type t ) const
    {
        dxdt( 0 ) = x( 0 ) + 2 * x( 1 );
        dxdt( 1 ) = x( 1 );
    }
};

struct jacobi
{
    void operator()( const state_type &x , matrix_type &jacobi , const value_type t ) const
    {
        jacobi( 0 , 0 ) = 1;
        jacobi( 0 , 1 ) = 2;
        jacobi( 1 , 0 ) = 0;
        jacobi( 1 , 1 ) = 1;
    }
};

BOOST_AUTO_TEST_SUITE( implicit_euler_test )

BOOST_AUTO_TEST_CASE( test_euler )
{
    implicit_euler< value_type > stepper;
    state_type x( 2 );
    x(0) = 0.0; x(1) = 1.0;

    value_type eps = 1E-12;

    /* make_pair doesn't work with function pointers on msvc 10 */
    stepper.do_step( std::make_pair( sys() , jacobi() ) , x , 0.0 , 0.1 );

    using std::abs;

    // compare with analytic solution of above system
    BOOST_CHECK_MESSAGE( abs( x(0) - 20.0/81.0 ) < eps , x(0) - 20.0/81.0 );
    BOOST_CHECK_MESSAGE( abs( x(1) - 10.0/9.0 ) < eps , x(0) - 10.0/9.0 );

}

BOOST_AUTO_TEST_SUITE_END()


/*
 [auto_generated]
 libs/numeric/odeint/test/rosenbrock4.cpp

 [begin_description]
 This file tests the Rosenbrock 4 stepper and its controller and dense output stepper.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

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

// disable checked iterator warning for msvc
#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_rosenbrock4

#include <utility>
#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include <boost/numeric/odeint/stepper/rosenbrock4.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4_controller.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4_dense_output.hpp>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

typedef boost::multiprecision::cpp_dec_float_50 value_type;
typedef boost::numeric::ublas::vector< value_type > state_type;
typedef boost::numeric::ublas::matrix< value_type > matrix_type;

struct sys
{
    void operator()( const state_type &x , state_type &dxdt , const value_type &t ) const
    {
        dxdt( 0 ) = x( 0 ) + 2 * x( 1 );
        dxdt( 1 ) = x( 1 );
    }
};

struct jacobi
{
    void operator()( const state_type &x , matrix_type &jacobi , const value_type &t , state_type &dfdt ) const
    {
        jacobi( 0 , 0 ) = 1;
        jacobi( 0 , 1 ) = 2;
        jacobi( 1 , 0 ) = 0;
        jacobi( 1 , 1 ) = 1;
        dfdt( 0 ) = 0.0;
        dfdt( 1 ) = 0.0;
    }
};

BOOST_AUTO_TEST_SUITE( rosenbrock4_test )

BOOST_AUTO_TEST_CASE( test_rosenbrock4_stepper )
{
    typedef rosenbrock4< value_type > stepper_type;
    stepper_type stepper;

    typedef stepper_type::state_type state_type;
    typedef stepper_type::value_type stepper_value_type;
    typedef stepper_type::deriv_type deriv_type;
    typedef stepper_type::time_type time_type;

    state_type x( 2 ) , xerr( 2 );
    x(0) = 0.0; x(1) = 1.0;

    stepper.do_step( std::make_pair( sys() , jacobi() ) , x ,
                     static_cast<value_type>(0.0) , static_cast<value_type>(0.1) , xerr );

    stepper.do_step( std::make_pair( sys() , jacobi() ) , x ,
                     static_cast<value_type>(0.0) , static_cast<value_type>(0.1) );

//    using std::abs;
//    value_type eps = 1E-12;
//
//    // compare with analytic solution of above system
//    BOOST_CHECK_SMALL( abs( x(0) - 20.0/81.0 ) , eps );
//    BOOST_CHECK_SMALL( abs( x(1) - 10.0/9.0 ) , eps );

}

BOOST_AUTO_TEST_CASE( test_rosenbrock4_controller )
{
    typedef rosenbrock4_controller< rosenbrock4< value_type > > stepper_type;
    stepper_type stepper;

    typedef stepper_type::state_type state_type;
    typedef stepper_type::value_type stepper_value_type;
    typedef stepper_type::deriv_type deriv_type;
    typedef stepper_type::time_type time_type;

    state_type x( 2 );
    x( 0 ) = 0.0 ; x(1) = 1.0;

    value_type t = 0.0 , dt = 0.01;
    stepper.try_step( std::make_pair( sys() , jacobi() ) , x , t , dt );
}

BOOST_AUTO_TEST_CASE( test_rosenbrock4_dense_output )
{
    typedef rosenbrock4_dense_output< rosenbrock4_controller< rosenbrock4< value_type > > > stepper_type;
    typedef rosenbrock4_controller< rosenbrock4< value_type > > controlled_stepper_type;
    controlled_stepper_type  c_stepper;
    stepper_type stepper( c_stepper );

    typedef stepper_type::state_type state_type;
    typedef stepper_type::value_type stepper_value_type;
    typedef stepper_type::deriv_type deriv_type;
    typedef stepper_type::time_type time_type;
    state_type x( 2 );
    x( 0 ) = 0.0 ; x(1) = 1.0;
    stepper.initialize( x , 0.0 , 0.1 );
    std::pair< value_type , value_type > tr = stepper.do_step( std::make_pair( sys() , jacobi() ) );
    stepper.calc_state( 0.5 * ( tr.first + tr.second ) , x );
}

BOOST_AUTO_TEST_CASE( test_rosenbrock4_copy_dense_output )
{
    typedef rosenbrock4_controller< rosenbrock4< value_type > > controlled_stepper_type;
    typedef rosenbrock4_dense_output< controlled_stepper_type > stepper_type;

    controlled_stepper_type  c_stepper;
    stepper_type stepper( c_stepper );
    stepper_type stepper2( stepper );
}

BOOST_AUTO_TEST_SUITE_END()


#include <Core/Animation/Pose/PoseOperation.hpp>
#include <Eigen/Geometry>

namespace Ra {
namespace Core {
namespace Animation {

bool compatible( const Pose& p0, const Pose& p1 ) {
    return ( p0.size() == p1.size() );
}

Pose relativePose( const Pose& modelPose, const RestPose& restPose )  {
    CORE_ASSERT( compatible( modelPose, restPose ), " Poses with different size " );
    Pose T( restPose.size() );
    #pragma omp parallel for
    for( int i = 0; i < int(T.size()); ++i ) {
        T[i] = modelPose[i] * restPose[i].inverse( Eigen::Affine );
    }
    return T;
}

Pose applyTransformation(const Pose& pose, const AlignedStdVector<Transform> &transform ) {
    Pose T( std::min( pose.size(), transform.size() ) );
    #pragma omp parallel for
    for( int i = 0; i < int(T.size()); ++i ) {
        T[i] = transform[i] * pose[i];
    }
    return T;
}

Pose applyTransformation( const Pose& pose, const Transform& transform ) {
    Pose T( pose.size() );
    #pragma omp parallel for
    for( int i = 0; i < int(T.size()); ++i ) {
        T[i] = transform * pose[i];
    }
    return T;
}

bool areEqual( const Pose &p0, const Pose &p1 ) {
    CORE_ASSERT( compatible( p0, p1 ), " Poses with different size " );
    const uint n = p0.size();
    for( uint i = 0; i < n; ++i ) {
        if( !p0[i].isApprox( p1[i]) ) {
            return false;
        }
    }
    return true;
}

Pose interpolatePoses(const Pose& a, const Pose& b, const Scalar t ) {
    CORE_ASSERT( ( a.size() == b.size() ), "Poses are wrong");
    CORE_ASSERT( ( ( t >= 0.0 ) && ( t <= 1.0 ) ), "T is wrong");

    const uint size = a.size();
    Pose interpolatedPose( size );

#pragma omp parallel for
    for ( int i = 0; i < int(size); ++i ) {
        // interpolate between the transforms
        Ra::Core::Transform aTransform = a[i];
        Ra::Core::Transform bTransform = b[i];

        Ra::Core::Quaternion aRot = Ra::Core::Quaternion( aTransform.rotation() );
        Ra::Core::Quaternion bRot = Ra::Core::Quaternion( bTransform.rotation() );
        Ra::Core::Quaternion interpRot = aRot.slerp(t, bRot);

        Ra::Core::Vector3 interpTranslation = ( 1.0 - t ) * aTransform.translation() + t * bTransform.translation();

        Ra::Core::Transform interpolatedTransform;
        interpolatedTransform.linear() = interpRot.toRotationMatrix();
        interpolatedTransform.translation() = interpTranslation;

        interpolatedPose[i] = interpolatedTransform;
    }

    return interpolatedPose;
}

void interpolateTransforms( const Ra::Core::Transform& a, const Ra::Core::Transform& b, const Scalar t, Ra::Core::Transform& interpolated ) {
    Ra::Core::Quaternion aRot = Ra::Core::Quaternion( a.rotation() );
    Ra::Core::Quaternion bRot = Ra::Core::Quaternion( b.rotation() );
    Ra::Core::Quaternion interpRot = aRot.slerp( t, bRot );

    Ra::Core::Vector3 interpTranslation = ( 1.0 - t) * a.translation() + t * b.translation();

    interpolated.linear() = interpRot.toRotationMatrix();
    interpolated.translation() = interpTranslation;
}

} // namespace Animation
} // namespace Core
} // namespace Ra


#pragma once

#include <memory>
#include <Eigen/Dense>
#include <boost/math/distributions/normal.hpp>

namespace calotypes
{

/*! \brief Base interface for functions that operate on pairs of data. It must be
 * that kernel(a,b) = kernel(b,a) */
template <class Data>
class KernelFunction
{
public:

	typedef std::shared_ptr<KernelFunction> Ptr;
	typedef Data DataType;

	KernelFunction() {}

	/*! \brief Calculates k(a,b) = k(a - b) */
	virtual double Evaluate( const Data& a, const Data& b ) const
	{
		return Evaluate( Difference( a, b ) );
	}

	/*! \brief Calculates a - b */
	virtual double Difference( const Data& a, const Data& b ) const = 0;

	/*! \brief Calculates k(x) */
	virtual double Evaluate( double x ) const { return x; }

};

/*! \brief Pushes a kernel through a Gaussian PDF. Uses boost's implementation. */
template <class Data>
class GaussianKernelAdaptor
: public KernelFunction<Data>
{
public:

	typedef std::shared_ptr<GaussianKernelAdaptor> Ptr;

	/*! \brief Construct a Gaussian kernel with specified standard deviation. */
	GaussianKernelAdaptor( typename KernelFunction<Data>::Ptr& k, double s )
	: kernel( k ), normal( 0, s ) {}

	virtual double Difference( const Data& a, const Data& b ) const
	{
		return kernel->Difference( a, b );
	}

	virtual double Evaluate( double x ) const
	{
		return boost::math::pdf( normal, x );
	}

protected:

	typename KernelFunction<Data>::Ptr kernel;
	boost::math::normal_distribution<double> normal;
};

/*! \brief Computes the positive-definite kernel (Gram) matrix for a given kernel.
 * Assumes kernel(a,a) = 0 and kernel(a,b) = kernel(b,a). */
template <class Data>
void ComputeGramMatrix( const KernelFunction<Data>& kernel, const std::vector<Data>& data,
						Eigen::MatrixXf& K )
{
	unsigned int N = data.size();
	K = Eigen::MatrixXf( N, N );

	for( unsigned int i = 0; i < N; i++ )
	{
		K(i,i) = kernel.Evaluate( data[i], data[i] );
	}
	for( unsigned int i = 0; i < N-1; i++ )
	{
		for( unsigned int j = i+1; j < N; j++ )
		{
			K(i,j) = kernel.Evaluate( data[i], data[j] );
			K(j,i) = K(i,j);
		}
	}
}

} // end namespace calotypes


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <iterator>
#include <vector>
#include <algorithm>
#include <utility>
#include <boost/graph/edge_list.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/utility.hpp>
#include <boost/graph/graph_utility.hpp>

/*

  This example demonstrates the usage of the
  connected_components_on_edgelist algorithm. This differs from the
  connect_components algorithm in that the graph object
  only needs to provide access to the "list" of edges (via the
  edges() function).

  The example graphs come from "Introduction to
  Algorithms", Cormen, Leiserson, and Rivest p. 87 (though we number
  the vertices from zero instead of one).

  Sample output:

  An undirected graph (edge list):
  (0,1) (1,4) (4,0) (2,5)
  Total number of components: 3
  Vertex 0 is in the component who's representative is 1
  Vertex 1 is in the component who's representative is 1
  Vertex 2 is in the component who's representative is 5
  Vertex 3 is in the component who's representative is 3
  Vertex 4 is in the component who's representative is 1
  Vertex 5 is in the component who's representative is 5

  component 0 contains: 4 1 0
  component 1 contains: 3
  component 2 contains: 5 2

 */

using namespace std;
using boost::tie;

int main(int, char*[])
{
    using namespace boost;
    typedef int Index; // ID of a Vertex
    typedef pair< Index, Index > Edge;
    const int N = 6;
    const int E = 4;
    Edge edgelist[] = { Edge(0, 1), Edge(1, 4), Edge(4, 0), Edge(2, 5) };

    edge_list< Edge*, Edge, ptrdiff_t, std::random_access_iterator_tag > g(
        edgelist, edgelist + E);
    cout << "An undirected graph (edge list):" << endl;
    print_edges(g, identity_property_map());
    cout << endl;

    disjoint_sets_with_storage<> ds(N);
    incremental_components(g, ds);

    component_index< int > components(
        &ds.parents()[0], &ds.parents()[0] + ds.parents().size());

    cout << "Total number of components: " << components.size() << endl;
    for (int k = 0; k != N; ++k)
        cout << "Vertex " << k
             << " is in the component who's representative is "
             << ds.find_set(k) << endl;
    cout << endl;

    for (std::size_t i = 0; i < components.size(); ++i)
    {
        cout << "component " << i << " contains: ";
        component_index< int >::component_iterator j = components[i].first,
                                                   jend = components[i].second;
        for (; j != jend; ++j)
            cout << *j << " ";
        cout << endl;
    }

    return 0;
}


#include <iostream>
#include <sstream>
#include <vector>
#include <string>
#include <cmath>
#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/variance.hpp>

using namespace std;

#include "cDataPoint.h"
#include "KMeans.h"

void KMeans::Add( const vector<double>& p )
{
    if( myLocations.size() )
    {
        if( myLocations[0]->myDim != p.size() )
        {
            throw std::runtime_error("Inconsistent data attribute count");
        }
    }
    myLocations.push_back( dp_t( new cDataPoint( p ) ) );
}

    void KMeans::Add( cDataPoint& p )
    {
        Add( p.d );
    }

double KMeans::TotalDistanceToCluster()
{
    double total = 0;
    for( int kc = 0; kc < myClusterCount; kc++ )
    {
        for( int kl = 0; kl < (int)myLocations.size(); kl++ )
        {
            if( myAssigns[kl] == kc )
            {
                // add distance from location to cluster center it is assigned to
                total += myClusters[kc].dist( myLocations[ kl ] );
            }
        }
    }
    return total;
}

std::string KMeans::text()
{
    stringstream ss;
    int kc = 0;
    for( auto& c : myClusters )
    {
        ss << "Cluster " << kc << " : ";
        for( int k = 0; k < (int)myLocations.size(); k++ )
            if( myAssigns[k] == kc )
                ss << myLocations[k]->text() << ", ";
        ss << "\n" << ClusterStats( kc ) << "\n";
        kc++;
    }
    return ss.str();
}

void KMeans::Assign()
{
    myAssigns.clear();
    for( auto& c : myClusters )
        c.clear();

    for( auto& cl : myLocations )
    {
        //cout << "assigning " << cl.Text() << "\n";
        double m = 1000000000000000000000.0;
        int closest_cluster;
        for( int si = 0; si < myClusterCount; si++ )
        {
            //cout << "test cluster " << myClusters[ si ].text() << "\n";
            double td = myClusters[ si ].dist( cl );
            if( td < m )
            {
                m = td;
                closest_cluster = si;
            }
        }
        myAssigns.push_back( closest_cluster );
        myClusters[ closest_cluster ].add( cl );
    }
}

void KMeans::MoveClustersToMean()
{
    for( int ks = 0; ks < myClusterCount; ks++ )
    {
        cDataPoint A( myLocations[0]->myDim );
        int count = 0;
        for( int kl = 0; kl < (int)myLocations.size(); kl++ )
        {
            if( ks == myAssigns[ kl ] )
            {
                A = A + *myLocations[ kl ].get();
                count++;
            }
        }
        //cout << "cluster " << ks << " has "<< count << " " << A.Text() << "\n";
        if( count )
            myClusters[ ks ].move( A / count );
    }
}

void KMeans::Init(
    int clusterCount,
    bool frandom )
{
    myClusterCount = clusterCount;

    if( myClusterCount > (int)myLocations.size() )
        throw std::runtime_error("KMeans::ClusterCount less locations than clusters");

    if( frandom )
        ClusterLocationInitRandom();
    else
        ClusterLocationInitIndex();
}
void KMeans::ClusterLocationInitIndex()
{
    myClusters.clear();
    for( int k = 0; k < myClusterCount; k++ )
    {
        int ic = k * (double)myLocations.size()/myClusterCount;
        //cout << ic <<" "<< myLocations[ic].Text() << ", ";
        myClusters.push_back( cCluster( *myLocations[ic].get() ) );
    }
}
void KMeans::ClusterLocationInitRandom()
{
    /* initialize random seed: */
    srand (time(NULL));
    myClusters.clear();
    for( int k = 0; k < myClusterCount; k++ )
    {
        myClusters.push_back( *myLocations[ rand() % myLocations.size() ].get() );
    }
}

void KMeans::Iter( int max )
{
    double dt = 0;

    for( int kiter=0; kiter < max; kiter++ )
    {
        // assign locations to nearest cluster
        Assign();

        // move clusters to mean location of locations assigned
        MoveClustersToMean();

        double d = TotalDistanceToCluster();
        if( fabs( dt - d ) < .000001 )
            break;
    }
}

std::string KMeans::ClusterStats( int cluster )
{
    std::stringstream ss;
    ss << "Cluster " << cluster << " means ";
    if( 0 > cluster || cluster > (int)myClusters.size())
        return ss.str();
    ss << myClusters[ cluster ].text();

    typedef boost::accumulators::accumulator_set<double, boost::accumulators::stats<
    boost::accumulators::tag::min,
          boost::accumulators::tag::max,
          boost::accumulators::tag::mean,
          boost::accumulators::tag::variance,
          boost::accumulators::tag::count> >
          Accumulator_t;
    vector< Accumulator_t > vac( myLocations[0]->myDim );

    for( int k = 0; k < (int)myLocations.size(); k++ )
    {
        if( cluster == myAssigns[ k ] )
        {
            for( int kd = 0; kd < (int)myLocations[0]->myDim; kd++ )
            {
                vac[ kd ]( myLocations[k]->d[kd] );
            }
        }
    }

    ss << " mins: ";
    for( int kd = 0; kd < (int)myLocations[0]->myDim; kd++ )
    {
        ss << boost::accumulators::min( vac[kd] ) << ", ";
    }
    ss << " maxs: ";
    for( int kd = 0; kd < (int)myLocations[0]->myDim; kd++ )
    {
        ss << boost::accumulators::max( vac[kd] ) << ", ";
    }
    ss << " sds: ";
    for( int kd = 0; kd < (int)myLocations[0]->myDim; kd++ )
    {
        ss << sqrt(boost::accumulators::variance( vac[kd] )) << ", ";
    }
    ss << "\n";

    return ss.str();
}

std::vector< cCluster >&
KMeans::clusters()
{
    return myClusters;
}

double cCluster::dist( dp_t o )
{
    return cDataPoint::dist( myCenter, *o.get() );
}
std::string cCluster::text() const
{
    return myCenter.text();
}
void cCluster::move( const cDataPoint& r )
{
    myCenter = r;
}
cCluster::cCluster( const cDataPoint& r )
{
    myCenter = r;
}
void cCluster::clear()
{
    myPoints.clear();
}
void cCluster::add( dp_t p )
{
    myPoints.push_back( p );
}
std::vector< dp_t >& cCluster::points()
{
    return myPoints;
}
cDataPoint& cCluster::center()
{
    return myCenter;
}


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

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_COMPARE_SPHERICAL_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_COMPARE_SPHERICAL_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/strategies/compare.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace compare
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename Units>
struct shift
{
};

template <>
struct shift<degree>
{
    static inline double full() { return 360.0; }
    static inline double half() { return 180.0; }
};

template <>
struct shift<radian>
{
    static inline double full() { return 2.0 * boost::math::constants::pi<double>(); }
    static inline double half() { return boost::math::constants::pi<double>(); }
};

} // namespace detail
#endif

/*!
\brief Compare (in one direction) strategy for spherical coordinates
\ingroup strategies
\tparam Point point-type
\tparam Dimension dimension
*/
template <typename CoordinateType, typename Units, typename Compare>
struct circular_comparator
{
    static inline CoordinateType put_in_range(CoordinateType const& c,
            double min_border, double max_border)
    {
        CoordinateType value = c;
        while (value < min_border)
        {
            value += detail::shift<Units>::full();
        }
        while (value > max_border)
        {
            value -= detail::shift<Units>::full();
        }
        return value;
    }

    inline bool operator()(CoordinateType const& c1, CoordinateType const& c2)  const
    {
        Compare compare;

        // Check situation that one of them is e.g. std::numeric_limits.
        static const double full = detail::shift<Units>::full();
        double mx = 10.0 * full;
        if (c1 < -mx || c1 > mx || c2 < -mx || c2 > mx)
        {
            // do normal comparison, using circular is not useful
            return compare(c1, c2);
        }

        static const double half = full / 2.0;
        CoordinateType v1 = put_in_range(c1, -half, half);
        CoordinateType v2 = put_in_range(c2, -half, half);

        // Two coordinates on a circle are
        // at max <= half a circle away from each other.
        // So if it is more, shift origin.
        CoordinateType diff = geometry::math::abs(v1 - v2);
        if (diff > half)
        {
            v1 = put_in_range(v1, 0, full);
            v2 = put_in_range(v2, 0, full);
        }

        return compare(v1, v2);
    }
};

}} // namespace strategy::compare

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

// Specialize for the longitude (dim 0)
template
<
    typename Point,
    template<typename> class CoordinateSystem,
    typename Units
>
struct strategy_compare<spherical_polar_tag, 1, Point, CoordinateSystem<Units>, 0>
{
    typedef typename coordinate_type<Point>::type coordinate_type;
    typedef strategy::compare::circular_comparator
        <
            coordinate_type,
            Units,
            std::less<coordinate_type>
        > type;
};

template
<
    typename Point,
    template<typename> class CoordinateSystem,
    typename Units
>
struct strategy_compare<spherical_polar_tag, -1, Point, CoordinateSystem<Units>, 0>
{
    typedef typename coordinate_type<Point>::type coordinate_type;
    typedef strategy::compare::circular_comparator
        <
            coordinate_type,
            Units,
            std::greater<coordinate_type>
        > type;
};

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_COMPARE_SPHERICAL_HPP


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <math.h>
#include <Eigen/Dense>
#include <float.h>
#include "fcm.h"
#include "record.h"

extern vector<traj_base> traj;
extern vector<centers_fcm_base> centers_fcm;

FCM::FCM(double m, double epsilon) {
  m_epsilon = epsilon;
  m_m = m;
  m_membership = nullptr;
  m_data = nullptr;
  m_cluster_center = nullptr;
  m_num_clusters = 0;
  m_num_dimensions = 0;
}

FCM::~FCM() {
  if (m_data != nullptr) {
    delete m_data;
    m_data = nullptr;
  }

  if (m_membership != nullptr) {
    delete m_membership;
    m_membership = nullptr;
  }

  if (m_cluster_center != nullptr) {
    delete m_cluster_center;
    m_cluster_center = nullptr;
  }
}

double FCM::update_membership() {

  long k, i;
  float new_uik;
  double max_diff = 0.0, diff;

  if (m_data == nullptr || m_data->rows() == 0) {
    throw std::logic_error("ERROR: data should not be empty when updating the membership");
  }

  if (m_membership == nullptr || m_membership->rows() == 0 || m_membership->rows() != m_data->rows()) {
    this->init_membership();
  }
  if (m_num_clusters == 0) {
    throw std::logic_error("ERROR: the number of clusters should be set");
  }

  for (i = 0; i < m_num_clusters; i++) {
    for (k = 0; k < m_data->rows(); k++) {
      //cout << "point: " << k << " and cluster" << i <<endl;
      //cout << "\nwill ask for the new new_uik"<< endl;
      new_uik = this->compute_membership_point(i, k);
      diff = new_uik - (*m_membership)(k, i); // We need the membership inversed which is more natural for us
      if (diff > max_diff) {
        max_diff = diff;
      }
      (*m_membership)(k, i) = new_uik;
    }
  }
  return max_diff;
}

void FCM::compute_centers() {
  long i, j, k;
  double numerator, denominator;
  MatrixXf t;
  t.resize(m_data->rows(), m_num_clusters);
  if (m_data == nullptr || m_data->rows() == 0) {
    throw std::logic_error("ERROR: number of rows is zero");
    return;
  }
  for (i = 0; i < m_data->rows(); i++) { // compute (u^m) for each cluster for each point
    for (j = 0; j < m_num_clusters; j++) {
      t(i, j) = float(pow((*m_membership)(i, j), m_m));
    }
  }
  for (j = 0; j < m_num_clusters; j++) { // loop for each cluster
    for (k = 0; k < m_num_dimensions; k++) { // for each dimension
      numerator = 0.0;
      denominator = 0.0;
      for (i = 0; i < m_data->rows(); i++) {
        numerator += t(i, j) * (*m_data)(i, k);
        denominator += t(i, j);
      }
      (*m_cluster_center)(j, k) = float(numerator / denominator);
    }
  }
}

double FCM::get_dist(long i, long k) {
  /*
   * distance which is denoted in the paper as d
   * k is the data point
   * i is the cluster center point
  */
  //cout<<"get_dist: point: "<<k<<" and cluster "<<i<<endl;
  long j;
  double sqsum = 0.0;
  if (m_num_clusters == 0) {
    throw std::logic_error("ERROR: number of clusters should not be zero\n");
  }
  if (m_num_dimensions == 0) {
    throw std::logic_error("ERROR: number of dimensions should not be zero\n");
  }
  for (j = 0; j < m_num_dimensions; j++) {
    sqsum += pow(((*m_data)(k, j) - (*m_cluster_center)(i, j)), 2);
  }
  return sqrt(sqsum);
}

float FCM::compute_membership_point(long i, long k) {
  /*
   * i the cluster
   * k is the data point
  */
  //cout << __func__ <<"  num of cluster: "<<m_num_clusters<<endl;
  long j;
  double t, seg = 0.0;
  double exp = 2 / (m_m - 1);
  double dik, djk;
  if (m_num_clusters == 0) {
    throw std::logic_error("ERROR: number of clusters should not be zero\n");
  }
  for (j = 0; j < m_num_clusters; j++) {
    //std::cout << i << "  " << j << "  " << k << std::endl;
    dik = this->get_dist(i, k);
    djk = this->get_dist(j, k);
    //std::cout << dik << "  " << djk << std::endl;
    if (djk == 0) {
      djk = DBL_MIN;
    }
    t = dik / djk;
    t = pow(t, exp);
    //cout << "cluster: " << i << "data: " << k << " - " << "t: "<<t<<endl;
    seg += t;
  }
  //std::cin.get();

  //cout << "seg: "<<seg << " u: "<<(1.0/seg)<<endl;
  return float(1.0 / seg);
}

void FCM::set_data(MatrixXf *data) {
  if (m_data != nullptr) {
    delete m_data;
  }
  if (data->rows() == 0) {
    throw std::logic_error("ERROR: seting empty data");
  }
  m_data = data;
  m_num_dimensions = m_data->cols();
}

void FCM::set_membership(MatrixXf *membership) {
  if (m_data == 0) {
    throw std::logic_error("ERROR: the data should present before setting up the membership");
  }
  if (m_num_clusters == 0) {
    if (membership->cols() == 0) {
      throw std::logic_error("ERROR: the number of clusters is 0 and the membership matrix is empty");
    }
    else {
      this->set_num_clusters(membership->cols());
    }
  }
  if (m_membership != nullptr) {
    delete m_membership;
  }
  m_membership = membership;
  if (m_membership->rows() == 0) {
    m_membership->resize(m_data->rows(), m_num_clusters);
  }
}

void FCM::init_membership() {
  long i, j;
  double mem;
  if (m_num_clusters == 0) {
    throw std::logic_error("ERROR: the number of clusters is 0");
  }
  if (m_data == nullptr) {
    throw std::logic_error("ERROR: the data should present before setting up the membership");
  }
  if (m_membership != nullptr) {
    delete m_membership;
  }
  m_membership = new MatrixXf;
  m_membership->resize(m_data->rows(), m_num_clusters);
  mem = 1.0 / m_num_clusters;
  for (j = 0; j < m_num_clusters; j++) {
    for (i = 0; i < m_data->rows(); i++) {
      (*m_membership)(i, j) = float(mem);
    }
  }
}

void FCM::set_num_clusters(long num_clusters) {
  m_num_clusters = num_clusters;
  if (m_cluster_center) {
    delete m_cluster_center;
  }
  m_cluster_center = new MatrixXf;
  m_cluster_center->resize(m_num_clusters, m_num_dimensions);
}

MatrixXf * FCM::get_data() {
  return m_data;
}

MatrixXf * FCM::get_membership() {
  return m_membership;
}

MatrixXf * FCM::get_cluster_center() {
  return m_cluster_center;
}

// DUNN INDEX //
double d_eu_distance(vector<double> v1, vector<double> v2) {
  double d_eu = 0.0;
  for (int n = 0; n < v1.size(); ++n)
    d_eu += pow((v1[n] - v2[n]), 2);

  if (d_eu > 0) return sqrt(d_eu);
  else return 0.0;
}
//-------------------------------------------------------------------
double intra_distance(int i) {
  double max_dist = 0.0;
  for (int n = 0; n < traj.size(); ++n)
    if (traj[n].means_class == i)
      for (int m = 0; m < traj.size(); ++m)
        if (traj[m].means_class == i)
        {
          vector<double> v1 = { traj[n].average_speed, traj[n].v_max, traj[n].v_min, traj[n].sinuosity };
          vector<double> v2 = { traj[m].average_speed, traj[m].v_max, traj[m].v_min, traj[m].sinuosity };
          double dist = d_eu_distance(v1, v2);
          if (max_dist < dist)
            max_dist = dist;
        }

  return max_dist;
}
//-------------------------------------------------------------------
double inter_distance(int n, int m) {
  double min_dist = 1e6;
  for (auto &t1: traj)
    if (t1.means_class == n)
      for (auto &t2:traj)
        if (t2.means_class == m)
        {
          vector<double> v1 = { t1.average_speed, t1.v_max, t1.v_min, t1.sinuosity };
          vector<double> v2 = { t2.average_speed, t2.v_max, t2.v_min, t2.sinuosity };
          double dist = d_eu_distance(v1, v2);
          if (min_dist > dist)
            min_dist = dist;
        }

  return min_dist;
}
//-------------------------------------------------------------------
double measure_dunn_index() {
  double num=  1e6;
  double den = 0.0;

  for (auto &c : centers_fcm) {
    double intra_dist_ci = intra_distance(c.idx);
    if (intra_dist_ci > den)
      den = intra_dist_ci;
  }
  std::cout << "den measured" << std::endl;

  for (int n=0; n<centers_fcm.size()-1; ++n)
    for (int m = n + 1; m < centers_fcm.size(); ++m) {
      double inter_n_m = inter_distance(n, m);
      if (inter_n_m < num)
        num = inter_n_m;
    }

  std::cout << "num measured " << std::endl;
  return (num/den);
}
//-------------------------------------------------------------------