// Copyright (c) 2009-2016 Craig Henderson
// https://github.com/cdmh/mapreduce

#include <boost/config.hpp>
#if defined(BOOST_MSVC)
#   pragma warning(disable: 4127)

// turn off checked iterators to avoid performance hit
#   if !defined(__SGI_STL_PORT)  &&  !defined(_DEBUG)
#       define _SECURE_SCL 0
#       define _HAS_ITERATOR_DEBUGGING 0
#   endif
#endif

#include "../Thirdparty/mapreduce/include/mapreduce.hpp"
#include "utils.cpp"
#include<chrono>
using namespace std;
using namespace std::chrono;

double* I;
// vector<vector<pair<int, double> > > H_Compact;/
map<int, vector<pair<int, double> > > H_Map;
vector<int> activeIndices;
vector<int> dangling_nodes;
double alpha = 0.85;
int activeNodeSize;

template<typename MapTask>
class datasource : mapreduce::detail::noncopyable{
public:
    datasource(unsigned size_) : sequence_(0), size(size_){
    }

    bool const setup_key(typename MapTask::key_type &key){
    	int tempIndex = sequence_++;
        key = activeIndices[tempIndex];
        // return ((H_Compact[key].size() != 0) && (key < size));
        // return key < size;
        // return H_Map.find(key) != H_Map.end();
        return tempIndex < activeNodeSize;
    }

    bool const get_data(typename MapTask::key_type const &key, typename MapTask::value_type &value){
    	// value.clear();
    	for(int j=0;j<H_Map[key].size();j++){
			value.push_back(H_Map[key][j]);
		}
		// value = H_Map[key];

		// value.assign()
        return true;
    }

private:
    int sequence_;
    int size;
};

struct map_task : public mapreduce::map_task<int, vector<pair<int, double> > >{
    template<typename Runtime>
    void operator()(Runtime &runtime, key_type const &key, value_type const &value) const
    {
    	// if(value.size() != 0){
		typename Runtime::reduce_task_type::key_type const emit_key = key;
    	runtime.emit_intermediate(emit_key, value);
    	// }
    }
};

struct reduce_task : public mapreduce::reduce_task<int, vector<pair<int, double> > >{
    template<typename Runtime, typename It>
    void operator()(Runtime &runtime, key_type const &key, It it, It ite) const
    {
		double sum = 0;
		vector<pair<int, double> > results;

		for(unsigned i=0;i<(*it).size();i++){
			sum += ((*it)[i].second)*(I[(*it)[i].first]);
		}
		sum *= alpha;
		if(sum != 0){
			results.push_back(make_pair(0, sum));
			runtime.emit(key, results);
		}
		return;
    }
};

typedef mapreduce::job<map_task, reduce_task, mapreduce::null_combiner, datasource<map_task> > job;

int main(int argc, char *argv[]){
    mapreduce::specification spec;

    if(argc < 2){
    	cerr << "Invalid Arguments!" << endl;
    	return 0;
    }

    string filename = "../test/" + (string)argv[1] + ".txt";

    if (argc > 2){
        spec.map_tasks = max(1, atoi(argv[1]));
    }

    if(argc > 3){
        spec.reduce_tasks = atoi(argv[2]);
    }

    else{
        spec.reduce_tasks = max(1U, thread::hardware_concurrency());
    }

    // Datasource maybe from a file!
    int size = getMatrixSize(filename);
    int n = size;
    cout << "Matrix Size: " << size << endl;

    // H_Compact.resize(n);
    // readCompactMatrixFile(filename, n, dangling_nodes, H_Compact);
    readMapMatrixFile(filename, n, dangling_nodes, H_Map, activeIndices);
    // printMap(H_Map);

    activeNodeSize = activeIndices.size();
    cout <<"\nPageRank analysis MapReduce..." << endl;
    auto start = high_resolution_clock::now();

    double DI[n] = {0};

    for(int i=0;i<dangling_nodes.size();i++){
    	DI[dangling_nodes[i]] = 1.0/n;
	}

	double GI[n] = {0};
	I = (double*)malloc((size)*sizeof(double));
	memset(I, 0, n*sizeof(double));
	I[0] = 1;

	bool converged = false;
	int index = 0;
	// return 0;
	while(index < 100){
		double randomDanglingSum = 0;

		for(int i=0;i<n;i++){
			randomDanglingSum += alpha*DI[i]*I[i] + ((1.0-alpha)/n)*I[i];
		}

		memset(GI, 0, n*sizeof(double));

		job::datasource_type datasource(size);
		job job1(datasource, spec);
	    mapreduce::results result;

		// job1.run<mapreduce::schedule_policy::sequential<job> >(result);
		job1.run<mapreduce::schedule_policy::cpu_parallel<job> >(result);

		for(auto it=job1.begin_results(); it!=job1.end_results(); ++it){
			GI[it->first] = it->second[0].second;
	    }

		for(int i=0;i<n;i++){
			GI[i] += randomDanglingSum;
		}

		memcpy(I, GI, n*sizeof(double));

		index++;
	}

	// printMatrix(I, n);
	auto stop = high_resolution_clock::now();
	auto duration = duration_cast<microseconds>(stop - start);
	cout << "Time taken by function: " << (double)duration.count()/1000000 << " seconds" << endl;
	writeToFile((string)argv[1], 0, I, n);
    return 0;
}


//
// $Id$
//
// 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 <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "pwiz/utility/misc/Std.hpp"
//#include <execinfo.h>

#include "pwiz/utility/misc/unit.hpp"
#include "Types.hpp"
#include "qr.hpp"

using namespace boost::numeric::ublas;
using namespace pwiz::math;
using namespace pwiz::math::types;
using namespace pwiz::util;

ostream* os_ = 0;
const double epsilon = 1e-12;

template<class matrix_type>
bool isUpperTriangular(const matrix_type& A, double eps)
{
    typedef typename matrix_type::size_type size_type;
    bool upper = true;

    for (size_type i=1; i<A.size2() && upper; i++)
    {
        for (size_type j=0; j<i && upper; j++)
        {
            upper = fabs(A(i,j)) < eps;
        }
    }

    return upper;
}

void testReflector()
{
    if (os_) *os_ << "testReflector() begin" << endl;

    dmatrix F(3,3);
    dvector x(3);

    x(0) = 1;
    x(1) = 1;
    x(2) = 0;

    Reflector(x, F);

    dvector v = prod(F, x);

    unit_assert_equal(fabs(v(0)), norm_2(x), epsilon);
    unit_assert_equal(v(1), 0, epsilon);
    unit_assert_equal(v(2), 0, epsilon);

    x(0) = -1;
    x(1) = 1;
    x(2) = 0;

    if (os_) *os_ << "testReflector() end" << endl;
}

void testQR()
{
    if (os_) *os_ << "testQR() begin" << endl;

    dmatrix A(3,3);
    dmatrix Q(3,3);
    dmatrix R(3,3);

    for (dmatrix::size_type i=0; i< A.size1(); i++)
    {
        for (dmatrix::size_type j=0; j<A.size2(); j++)
        {
            A(i,j) = ((i==j) || (j == 0));
        }
    }

    try {
        qr<dmatrix>(A, Q, R);

        unit_assert(isUpperTriangular(R, epsilon));

        dmatrix diff = (A - prod(Q, R));

        unit_assert_equal(norm_1(diff), 0, epsilon);

        identity_matrix<dmatrix::value_type> eye(Q.size1());
        diff = prod(Q, herm(Q)) - eye;

        unit_assert_equal(norm_1(diff), 0, epsilon);
    }
    catch (boost::numeric::ublas::bad_argument ba)
    {
        if (os_) *os_ << "exception: " << ba.what() << endl;
    }

    if (os_) *os_ << "testQR() end" << endl;
}

void testRectangularQR()
{
    if (os_) *os_ << "testRectangularQR() begin" << endl;

    dmatrix A(5,3);
    dmatrix Q;
    dmatrix R;

    for (dmatrix::size_type i=0; i< A.size1(); i++)
    {
        for (dmatrix::size_type j=0; j<A.size2(); j++)
        {
            A(i,j) = ((i==j) || (j == 0));
        }
    }

    try {
        qr<dmatrix>(A, Q, R);

        unit_assert(isUpperTriangular(R, epsilon));

        dmatrix diff = (A - prod(Q, R));

        unit_assert_equal(norm_1(diff), 0, epsilon);

        identity_matrix<dmatrix::value_type> eye(Q.size1());
        diff = prod(Q, herm(Q)) - eye;

        unit_assert_equal(norm_1(diff), 0, epsilon);
    }
    catch (boost::numeric::ublas::bad_argument ba)
    {
        if (os_) *os_ << "exception: " << ba.what() << endl;
    }

    if (os_) *os_ << "testRectangularQR() end" << endl;
}

int main(int argc, char** argv)
{
    if (argc>1 && !strcmp(argv[1],"-v")) os_ = &cout;
    if (os_) *os_ << "qrTest\n";
    testReflector();
    testQR();
    testRectangularQR();

    return 0;
}


//
// Copyright (c) 2012 Juan Palacios juan.palacios.puyana@gmail.com
// This file is part of minimathlibs.
// Subject to the BSD 2-Clause License
// - see < http://opensource.org/licenses/BSD-2-Clause>
//
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestRotation3DZYX
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <cmath>
#include "minimath/rotation3d.hpp"
#include "minimath/point3d.hpp"
#include "Defines.h"
#include "TestRotation3DUtils.h"

using namespace minimath;

BOOST_AUTO_TEST_SUITE(TestRotation3DZYX)

namespace
{

//
// Check that a RotationZYX defined by angles phi, theta, psi has the same
// effect on a point as individual Z, Y and X rotations.
//
bool testRotationZYX(const minimath::pointxyzd& point,
                     double phi,
                     double theta,
                     double psi)
{
  rotation3dzyx<double> rot1(phi, theta, psi);
  pointxyzd ref = rotation3dx<double>(psi) *
      ( rotation3dy<double>(theta) * (rotation3dz<double>(phi)*point) );
  return rot1*point == ref;
}

} // namespace

BOOST_AUTO_TEST_CASE(testInstantiation)
{
  rotation3dzyx<double> rot1, rot2;
}

BOOST_AUTO_TEST_CASE(testDefaultEquality)
{
  BOOST_CHECK(rotation3dzyx<double>() == rotation3dzyx<double>());
}

BOOST_AUTO_TEST_CASE(testCopyConstruction)
{
  rotation3dzyx<double> rot1;
  rotation3dzyx<double> rot2(rot1);
  BOOST_CHECK(rot1 == rot2);
}

BOOST_AUTO_TEST_CASE(testAssignment)
{
  rotation3dzyx<double> rot1, rot2;
  rot2 = rot1;
  BOOST_CHECK(rot1 == rot2);
}

BOOST_AUTO_TEST_CASE(testNullRotation)
{
  rotation3dzyx<double> rot;
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint45X)
{
  rotation3dzyx<double> rot(0, 0, PI/4.); // 45 degree rotation about X
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., cos45, cos45));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., -cos45, cos45));
}

BOOST_AUTO_TEST_CASE(testRotatePoint90X)
{
  rotation3dzyx<double> rot(0, 0, PI/2.); // 90 degree rotation about X
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint180X)
{
  rotation3dzyx<double> rot(0, 0, PI); // 180 degree rotation about X

  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint45Y)
{
  rotation3dzyx<double> rot(0, PI/4., 0); // 45 degree rotation about X
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(cos45, 0., -cos45));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(cos45, 0, cos45));
}

BOOST_AUTO_TEST_CASE(testRotatePoint90Y)
{
  rotation3dzyx<double> rot(0, PI/2., 0); // 90 degree rotation about X
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint180Y)
{
  rotation3dzyx<double> rot(0, PI, 0); // 180 degree rotation about X

  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint45Z)
{
  rotation3dzyx<double> rot(PI/4., 0, 0); // 45 degree rotation about Z
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(cos45, cos45, 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(-cos45, cos45, 0));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0, 0, 1));
}

BOOST_AUTO_TEST_CASE(testRotatePoint90Z)
{
  rotation3dzyx<double> rot(PI/2., 0, 0); // 90 degree rotation about Z
  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testRotatePoint180Z)
{
  rotation3dzyx<double> rot(PI, 0, 0); // 180 degree rotation about Z

  pointxyzd pTest = rot*p100;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = rot*p010;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));

  pTest = rot*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testAssignRotation3DX)
{
  for (int i = 1; i < 9; ++i) {
    BOOST_CHECK(rotation3dzyx<double>(0, 0, PI/i) == rotation3dx<double>(PI/i));
  }
}

BOOST_AUTO_TEST_CASE(testAssignRotation3DY)
{
  for (int i = 1; i < 9; ++i) {
    BOOST_CHECK(rotation3dzyx<double>(0, PI/i, 0) == rotation3dy<double>(PI/i));
  }
}

BOOST_AUTO_TEST_CASE(testAssignRotation3DZ)
{
  for (int i = 1; i < 9; ++i) {
    BOOST_CHECK(rotation3dzyx<double>(PI/i, 0, 0) == rotation3dz<double>(PI/i));
  }
}

BOOST_AUTO_TEST_CASE(testZYXRotations)
{
  for (int i = 1; i < 9; ++i) {

    const double angle = PI/i;

    BOOST_CHECK(testRotationZYX(p100, angle, angle, angle));
    BOOST_CHECK(testRotationZYX(p010, angle, angle, angle));
    BOOST_CHECK(testRotationZYX(p001, angle, angle, angle));

    BOOST_CHECK(testRotationZYX(p100, -angle, angle, angle));
    BOOST_CHECK(testRotationZYX(p010, angle, angle, angle));
    BOOST_CHECK(testRotationZYX(p001, angle, angle, angle));

    BOOST_CHECK(testRotationZYX(p100, angle, angle, angle));
    BOOST_CHECK(testRotationZYX(p010, -angle, angle, angle));
    BOOST_CHECK(testRotationZYX(p001, angle, angle, angle));

    BOOST_CHECK(testRotationZYX(p100, angle, -angle, angle));
    BOOST_CHECK(testRotationZYX(p010, angle, -angle, angle));
    BOOST_CHECK(testRotationZYX(p001, angle, -angle, angle));

    BOOST_CHECK(testRotationZYX(p100, -angle, -angle, -angle));
    BOOST_CHECK(testRotationZYX(p010, -angle, -angle, -angle));
    BOOST_CHECK(testRotationZYX(p001, -angle, -angle, -angle));

  }
}

BOOST_AUTO_TEST_CASE(testInverse)
{
  BOOST_CHECK(TestUtils::testInverseRotation3DZYX<rotation3dx<double> >());
  BOOST_CHECK(TestUtils::testInverseRotation3DZYX<rotation3dy<double> >());
  BOOST_CHECK(TestUtils::testInverseRotation3DZYX<rotation3dz<double> >());
}

BOOST_AUTO_TEST_CASE(testInvert)
{
  BOOST_CHECK(TestUtils::testInvertRotation3DZYX<rotation3dx<double> >());
  BOOST_CHECK(TestUtils::testInvertRotation3DZYX<rotation3dy<double> >());
  BOOST_CHECK(TestUtils::testInvertRotation3DZYX<rotation3dz<double> >());
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Nick Thompson, 2017
// 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)

#include <iostream>
#include <cmath>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <boost/math/quadrature/sinh_sinh.hpp>
#include <boost/math/quadrature/exp_sinh.hpp>

using boost::math::quadrature::tanh_sinh;
using boost::math::quadrature::sinh_sinh;
using boost::math::quadrature::exp_sinh;
using boost::math::constants::pi;
using boost::math::constants::half_pi;
using boost::math::constants::half;
using boost::math::constants::third;
using boost::math::constants::root_pi;
using std::log;
using std::cos;
using std::cosh;
using std::exp;
using std::sqrt;

int main()
{
    std::cout << std::setprecision(std::numeric_limits<double>::digits10);
    double tol = sqrt(std::numeric_limits<double>::epsilon());
    // For an integral over a finite domain, use tanh_sinh:
    tanh_sinh<double> tanh_integrator(tol, 10);
    auto f1 = [](double x) { return log(x)*log(1-x); };
    double Q = tanh_integrator.integrate(f1, (double) 0, (double) 1);
    double Q_expected = 2 - pi<double>()*pi<double>()*half<double>()*third<double>();

    std::cout << "tanh_sinh quadrature of log(x)log(1-x) gives " << Q << std::endl;
    std::cout << "The exact integral is                        " << Q_expected << std::endl;

    // For an integral over the entire real line, use sinh-sinh quadrature:
    sinh_sinh<double> sinh_integrator(tol, 10);
    auto f2 = [](double t) { return cos(t)/cosh(t);};
    Q = sinh_integrator.integrate(f2);
    Q_expected = pi<double>()/cosh(half_pi<double>());
    std::cout << "sinh_sinh quadrature of cos(x)/cosh(x) gives " << Q << std::endl;
    std::cout << "The exact integral is                        " << Q_expected << std::endl;

    // For half-infinite intervals, use exp-sinh.
    // Endpoint singularities are handled well:
    exp_sinh<double> exp_integrator(tol, 10);
    auto f3 = [](double t) { return exp(-t)/sqrt(t); };
    Q = exp_integrator.integrate(f3, 0, std::numeric_limits<double>::infinity());
    Q_expected = root_pi<double>();
    std::cout << "exp_sinh quadrature of exp(-t)/sqrt(t) gives " << Q << std::endl;
    std::cout << "The exact integral is                        " << Q_expected << std::endl;

}


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 06.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "solve.h"

#include <Eigen/Core>
#include <iostream>

#include "mylinearfeelementmatrix.h"
#include "mylinearloadvector.h"

namespace ElementMatrixComputation {

/* SAM_LISTING_BEGIN_2 */
Eigen::VectorXd solvePoissonBVP() {
  // Convert the globally defined function f to a LehrFEM++ mesh function object
  lf::mesh::utils::MeshFunctionGlobal mf_f{f};

  // The basis expansion coefficient vector for the finite-element solution
  Eigen::VectorXd solution = Eigen::VectorXd::Zero(1);

  //====================
  // Your code goes here
  // that is to call solve() in order to compute the basis expansion coefficients of a solution
  // of the Neumann boundary value problem
  // make use of the LehrFEM++'s local PROVIDER types for element matrices for the bilinear form
  //for the weak-laplacian
  // and element vectors for the linear form
  // initialize the lf::uscalfe::LinearFELocalLoadVector appropriately so that the right-hand side source function
  // provided by the funtion f(Eigen::Vector2d x);

  // define the element matrix and element vector builders and solve the system
  lf::uscalfe::LinearFELaplaceElementMatrix elmat_builder;
  lf::uscalfe::LinearFELocalLoadVector<double, decitype(mf_f)> elvec_builder(mf_f);
  Eigen::VectorXd solve(elmat_builder, elvec_provider)；
  //====================

  return solution;
}
/* SAM_LISTING_END_2 */

Eigen::VectorXd solveNeumannEq() {
  // Define the solution vector
  Eigen::VectorXd solution;

  //====================
  // Your code goes here
  //====================

  return solution;
}

}  // namespace ElementMatrixComputation


#ifndef _INCLUDES_HPP_
#define _INCLUDES_HPP_

#define _USE_MATH_DEFINES		// The get M_PI and similar constants from cmath

#include <iostream>			// For console I/O
#include <fstream>			// For file I/O
#include <stdio.h>			// For getchar
#include <math.h>			// Basic math functions
#include <iomanip>			// For setprecision
#include <vector>			// For stl vectors
#include <string>			// For stl strings
#include <time.h>			// For getting the time (e.g., and a RNG seed)
#include <sstream>			// Stringstream
#include <stdarg.h>			// For functions with different numbers of inputs
#include <functional>		// For passing functions
#include <random>			// For random numbers
#include <thread>			// For creating multiple threads
#include <unistd.h>			// To get the working directory
#include <iterator>			// Used by things like istream_iterator

// For getting the current directory
//#include <unistd.h>
//#define GetCurrentDir getcwd

#include <Eigen/Dense>			// For matrix math
#include <Eigen/SVD>			// For writing our Moore-Penrose pseudo-inverse implementation
#include <Eigen/Eigenvalues>		// For getting eigenvalues
#include <Eigen/Core>
#include <Eigen/Cholesky>

using namespace std;
using namespace Eigen;

#include <IOStringUtils.hpp>				// Basic file and string I/O functions
#include <MathUtils.hpp>					// Some useful math functions that aren't in other included libraries
#include <MarsagliaGenerator.hpp>

#endif


// Copyright 2015-2022 Tier IV, Inc. All rights reserved.
//
// 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 <boost/lexical_cast.hpp>
#include <random>
#include <rclcpp/rclcpp.hpp>

auto main(const int argc, char const * const * const argv) -> int
{
  rclcpp::init(argc, argv);

  auto node = std::make_shared<rclcpp::Node>("uniform_distribution");

  auto number = [&]() {
    auto engine = std::default_random_engine(std::random_device()());
    switch (argc) {
      case 3:
        return std::uniform_real_distribution<double>(
          boost::lexical_cast<double>(argv[1]), boost::lexical_cast<double>(argv[2]))(engine);
      default:
        return std::uniform_real_distribution<double>(0, 1)(engine);
    }
  };

  std::cout << number() << std::endl;

  return EXIT_SUCCESS;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the empirical_kernel_map
    from the dlib C++ Library.

    This example program assumes you are familiar with some general elements of
    the library.  In particular, you should have at least read the svm_ex.cpp
    and matrix_ex.cpp examples.

    Most of the machine learning algorithms in dlib are some flavor of "kernel machine".
    This means they are all simple linear algorithms that have been formulated such
    that the only way they look at the data given by a user is via dot products between
    the data samples.  These algorithms are made more useful via the application of the
    so-called kernel trick.  This trick is to replace the dot product with a user
    supplied function which takes two samples and returns a real number.  This function
    is the kernel that is required by so many algorithms.  The most basic kernel is the
    linear_kernel which is simply a normal dot product.  More interesting, however,
    are kernels which first apply some nonlinear transformation to the user's data samples
    and then compute a dot product.  In this way, a simple algorithm that finds a linear
    plane to separate data (e.g. the SVM algorithm) can be made to solve complex
    nonlinear learning problems.

    An important element of the kernel trick is that these kernel functions perform
    the nonlinear transformation implicitly.  That is, if you look at the implementations
    of these kernel functions you won't see code that transforms two input vectors in
    some way and then computes their dot products.  Instead you will see a simple function
    that takes two input vectors and just computes a single real number via some simple
    process.  You can basically think of this as an optimization.  Imagine that originally
    we wrote out the entire procedure to perform the nonlinear transformation and then
    compute the dot product but then noticed we could cancel a few terms here and there
    and simplify the whole thing down into a more compact and easily evaluated form.
    The result is a nice function that computes what we want but we no longer get to see
    what those nonlinearly transformed input vectors are.

    The empirical_kernel_map is a tool that undoes this.  It allows you to obtain these
    nonlinearly transformed vectors.  It does this by taking a set of data samples from
    the user (referred to as basis samples), applying the nonlinear transformation to all
    of them, and then constructing a set of orthonormal basis vectors which spans the space
    occupied by those transformed input samples.  Then if we wish to obtain the nonlinear
    version of any data sample we can simply project it onto this orthonormal basis and
    we obtain a regular vector of real numbers which represents the nonlinearly transformed
    version of the data sample.  The empirical_kernel_map has been formulated to use only
    dot products between data samples so it is capable of performing this service for any
    user supplied kernel function.

    The empirical_kernel_map is useful because it is often difficult to formulate an
    algorithm in a way that uses only dot products.  So the empirical_kernel_map lets
    us easily kernelize any algorithm we like by using this object during a preprocessing
    step.  However, it should be noted that the algorithm is only practical when used
    with at most a few thousand basis samples.  Fortunately, most datasets live in
    subspaces that are relatively low dimensional.  So for these datasets, using the
    empirical_kernel_map is practical assuming an appropriate set of basis samples can be
    selected by the user.  To help with this dlib supplies the linearly_independent_subset_finder.
    I also often find that just picking a random subset of the data as a basis works well.

    In what follows, we walk through the process of creating an empirical_kernel_map,
    projecting data to obtain the nonlinearly transformed vectors, and then doing a
    few interesting things with the data.
*/

#include <dlib/svm.h>
#include <dlib/rand.h>
#include <iostream>
#include <vector>

using namespace std;
using namespace dlib;

// ----------------------------------------------------------------------------------------

// First let's make a typedef for the kind of samples we will be using.
typedef matrix<double, 0, 1> sample_type;

// We will be using the radial_basis_kernel in this example program.
typedef radial_basis_kernel<sample_type> kernel_type;

// ----------------------------------------------------------------------------------------

void generate_concentric_circles (
    std::vector<sample_type>& samples,
    std::vector<double>& labels,
    const int num_points
);
/*!
    requires
        - num_points > 0
    ensures
        - generates two circles centered at the point (0,0), one of radius 1 and
          the other of radius 5.  These points are stored into samples.  labels will
          tell you if a given samples is from the smaller circle (its label will be 1)
          or from the larger circle (its label will be 2).
        - each circle will be made up of num_points
!*/

// ----------------------------------------------------------------------------------------

void test_empirical_kernel_map (
    const std::vector<sample_type>& samples,
    const std::vector<double>& labels,
    const empirical_kernel_map<kernel_type>& ekm
);
/*!
    This function computes various interesting things with the empirical_kernel_map.
    See its implementation below for details.
!*/

// ----------------------------------------------------------------------------------------

int main()
{
    std::vector<sample_type> samples;
    std::vector<double> labels;

    // Declare an instance of the kernel we will be using.
    const kernel_type kern(0.1);

    // create a dataset with two concentric circles.  There will be 100 points on each circle.
    generate_concentric_circles(samples, labels, 100);

    empirical_kernel_map<kernel_type> ekm;

    // Here we create an empirical_kernel_map using all of our data samples as basis samples.
    cout << "\n\nBuilding an empirical_kernel_map with " << samples.size() << " basis samples." << endl;
    ekm.load(kern, samples);
    cout << "Test the empirical_kernel_map when loaded with every sample." << endl;
    test_empirical_kernel_map(samples, labels, ekm);

    // create a new dataset with two concentric circles.  There will be 1000 points on each circle.
    generate_concentric_circles(samples, labels, 1000);
    // Rather than use all 2000 samples as basis samples we are going to use the
    // linearly_independent_subset_finder to pick out a good basis set.  The idea behind this
    // object is to try and find the 40 or so samples that best spans the subspace containing all the
    // data.
    linearly_independent_subset_finder<kernel_type> lisf(kern, 40);
    // populate lisf with samples.  We have configured it to allow at most 40 samples but this function
    // may determine that fewer samples are necessary to form a good basis.  In this example program
    // it will select only 26.
    fill_lisf(lisf, samples);

    // Now reload the empirical_kernel_map but this time using only our small basis
    // selected using the linearly_independent_subset_finder.
    cout << "\n\nBuilding an empirical_kernel_map with " << lisf.size() << " basis samples." << endl;
    ekm.load(lisf);
    cout << "Test the empirical_kernel_map when loaded with samples from the lisf object." << endl;
    test_empirical_kernel_map(samples, labels, ekm);

    cout << endl;
}

// ----------------------------------------------------------------------------------------

void test_empirical_kernel_map (
    const std::vector<sample_type>& samples,
    const std::vector<double>& labels,
    const empirical_kernel_map<kernel_type>& ekm
)
{

    std::vector<sample_type> projected_samples;

    // The first thing we do is compute the nonlinearly projected vectors using the
    // empirical_kernel_map.
    for (unsigned long i = 0; i < samples.size(); ++i)
    {
        projected_samples.push_back(ekm.project(samples[i]));
    }

    // Note that a kernel matrix is just a matrix M such that M(i,j) == kernel(samples[i],samples[j]).
    // So below we are computing the normal kernel matrix as given by the radial_basis_kernel and the
    // input samples.  We also compute the kernel matrix for all the projected_samples as given by the
    // linear_kernel.  Note that the linear_kernel just computes normal dot products.  So what we want to
    // see is that the dot products between all the projected_samples samples are the same as the outputs
    // of the kernel function for their respective untransformed input samples.  If they match then
    // we know that the empirical_kernel_map is working properly.
    const matrix<double> normal_kernel_matrix = kernel_matrix(ekm.get_kernel(), samples);
    const matrix<double> new_kernel_matrix = kernel_matrix(linear_kernel<sample_type>(), projected_samples);

    cout << "Max kernel matrix error: " << max(abs(normal_kernel_matrix - new_kernel_matrix)) << endl;
    cout << "Mean kernel matrix error: " << mean(abs(normal_kernel_matrix - new_kernel_matrix)) << endl;
    /*
        Example outputs from these cout statements.
        For the case where we use all samples as basis samples:
            Max kernel matrix error: 7.32747e-15
            Mean kernel matrix error: 7.47789e-16

        For the case where we use only 26 samples as basis samples:
            Max kernel matrix error: 0.000953573
            Mean kernel matrix error: 2.26008e-05

        Note that if we use enough basis samples we can perfectly span the space of input samples.
        In that case we get errors that are essentially just rounding noise (Moreover, using all the
        samples is always enough since they are always within their own span).  Once we start
        to use fewer basis samples we may begin to get approximation error.  In the second case we
        used 26 and we can see that the data doesn't really lay exactly in a 26 dimensional subspace.
        But it is pretty close.
    */

    // Now let's do something more interesting.  The following loop finds the centroids
    // of the two classes of data.
    sample_type class1_center;
    sample_type class2_center;
    for (unsigned long i = 0; i < projected_samples.size(); ++i)
    {
        if (labels[i] == 1)
            class1_center += projected_samples[i];
        else
            class2_center += projected_samples[i];
    }

    const int points_per_class = samples.size()/2;
    class1_center /= points_per_class;
    class2_center /= points_per_class;

    // Now classify points by which center they are nearest.  Recall that the data
    // is made up of two concentric circles.  Normally you can't separate two concentric
    // circles by checking which points are nearest to each center since they have the same
    // centers.  However, the kernel trick makes the data separable and the loop below will
    // perfectly classify each data point.
    for (unsigned long i = 0; i < projected_samples.size(); ++i)
    {
        double distance_to_class1 = length(projected_samples[i] - class1_center);
        double distance_to_class2 = length(projected_samples[i] - class2_center);

        bool predicted_as_class_1 = (distance_to_class1 < distance_to_class2);

        // Now print a message for any misclassified points.
        if (predicted_as_class_1 == true && labels[i] != 1)
            cout << "A point was misclassified" << endl;

        if (predicted_as_class_1 == false && labels[i] != 2)
            cout << "A point was misclassified" << endl;
    }

    // Next, note that classifying a point based on its distance between two other
    // points is the same thing as using the plane that lies between those two points
    // as a decision boundary.  So let's compute that decision plane and use it to classify
    // all the points.

    sample_type plane_normal_vector = class1_center - class2_center;
    // The point right in the center of our two classes should be on the deciding plane, not
    // on one side or the other.  This consideration brings us to the formula for the bias.
    double bias = dot((class1_center+class2_center)/2, plane_normal_vector);

    // Now classify points by which side of the plane they are on.
    for (unsigned long i = 0; i < projected_samples.size(); ++i)
    {
        double side = dot(plane_normal_vector, projected_samples[i]) - bias;

        bool predicted_as_class_1 = (side > 0);

        // Now print a message for any misclassified points.
        if (predicted_as_class_1 == true && labels[i] != 1)
            cout << "A point was misclassified" << endl;

        if (predicted_as_class_1 == false && labels[i] != 2)
            cout << "A point was misclassified" << endl;
    }

    // It would be nice to convert this decision rule into a normal decision_function object and
    // dispense with the empirical_kernel_map.  Happily, it is possible to do so.  Consider the
    // following example code:
    decision_function<kernel_type> dec_funct = ekm.convert_to_decision_function(plane_normal_vector);
    // The dec_funct now computes dot products between plane_normal_vector and the projection
    // of any sample point given to it.  All that remains is to account for the bias.
    dec_funct.b = bias;

    // now classify points by which side of the plane they are on.
    for (unsigned long i = 0; i < samples.size(); ++i)
    {
        double side = dec_funct(samples[i]);

        // And let's just check that the dec_funct really does compute the same thing as the previous equation.
        double side_alternate_equation = dot(plane_normal_vector, projected_samples[i]) - bias;
        if (abs(side-side_alternate_equation) > 1e-14)
            cout << "dec_funct error: " << abs(side-side_alternate_equation) << endl;

        bool predicted_as_class_1 = (side > 0);

        // Now print a message for any misclassified points.
        if (predicted_as_class_1 == true && labels[i] != 1)
            cout << "A point was misclassified" << endl;

        if (predicted_as_class_1 == false && labels[i] != 2)
            cout << "A point was misclassified" << endl;
    }

}

// ----------------------------------------------------------------------------------------

void generate_concentric_circles (
    std::vector<sample_type>& samples,
    std::vector<double>& labels,
    const int num
)
{
    sample_type m(2,1);
    samples.clear();
    labels.clear();

    dlib::rand rnd;

    // make some samples near the origin
    double radius = 1.0;
    for (long i = 0; i < num; ++i)
    {
        double sign = 1;
        if (rnd.get_random_double() < 0.5)
            sign = -1;
        m(0) = 2*radius*rnd.get_random_double()-radius;
        m(1) = sign*sqrt(radius*radius - m(0)*m(0));

        samples.push_back(m);
        labels.push_back(1);
    }

    // make some samples in a circle around the origin but far away
    radius = 5.0;
    for (long i = 0; i < num; ++i)
    {
        double sign = 1;
        if (rnd.get_random_double() < 0.5)
            sign = -1;
        m(0) = 2*radius*rnd.get_random_double()-radius;
        m(1) = sign*sqrt(radius*radius - m(0)*m(0));

        samples.push_back(m);
        labels.push_back(2);
    }
}

// ----------------------------------------------------------------------------------------


#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/boost/graph/graph_traits_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>

#include <boost/chrono.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/pending/disjoint_sets.hpp>

#include "GraphDefs.h"
#include "Timer.h"
#include "GraphUtil.h"
#include "GraphParse.h"
#include "GraphGen.h"

#include <fstream>
#include <string>

#define SHOW_DEBUG false

void printGraph(const char* title, VertexVector* vertices, EdgeVector* edges, bool printVertices = false) {
	std::cout << std::endl << "=== " << title << std::endl;

	if (printVertices) {
		// Iterate through the vertices and print them out
		for (int i = 0; i < vertices->size(); i++) {
			CGALPoint* v = (*vertices)[i];
			std::cout << "index: " << i << " (" << v->x() << ", " << v->y() << ")" << std::endl;
		}
	}

	// Iterate through the edges and print them out
	std::cout << std::endl << "Edges: " << std::endl;
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* e = (*edges)[i];
		CGALPoint* src = (*vertices)[e->u];
		CGALPoint* tar = (*vertices)[e->v];
		//std::cout << edgeWeightMap->at(e).weight << " (" << (*g)[src].pt << ") (" << (*g)[tar].pt << ")" << std::endl;
		std::cout << e->weight << " (" << (*src) << ") (" << (*tar) << ")" << std::endl;
	}

	std::cout << std::endl;
}

void printGDFGraph(const char* fileName, VertexVector* vertices, EdgeVector* edges) {
	std::ofstream myfile;
	myfile.open(fileName, std::ios::out | std::ios::in);

	myfile << "nodedef> name VARCHAR,label VARCHAR,width DOUBLE,height DOUBLE,x DOUBLE,y DOUBLE,color VARCHAR" << std::endl;

	// Iterate through the vertices and print them out
	boost::unordered_map<CGALPoint*, VertexIndex> vertexHandles;
	for (int i = 0; i < vertices->size(); i++) {
		CGALPoint* v = (*vertices)[i];
		myfile << i << ",,10.0,10.0," << (*v).x() << "," << (*v).y() << ",'153,153,153'" << std::endl;
		vertexHandles.emplace(v, i);
	}

	myfile << "edgedef> node1,node2,weight DOUBLE,directed BOOLEAN,color VARCHAR" << std::endl;

	// Iterate through the edges and print them out
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* e = (*edges)[i];
		CGALPoint* src = (*vertices)[e->u];
		CGALPoint* tar = (*vertices)[e->v];
		VertexIndex srcInd = vertexHandles[src];
		VertexIndex tarInd = vertexHandles[tar];
		//EdgeWeight weight = sqrt(CGAL::squared_distance(*src, *tar));
		EdgeWeight weight = 1.0;
		myfile << srcInd << "," << tarInd << "," << weight << ",false,'128,128,128'" << std::endl;
	}

	myfile.close();
}

CDT* computeCdt(VertexVector* vertices, EdgeVector* edges, boost::unordered_map<TriVertexHandle, VertexIndex>** handlesToIndex) {
	CDT* cdt = new CDT();

	(*handlesToIndex) = new boost::unordered_map<TriVertexHandle, VertexIndex>();
	boost::unordered_map<VertexIndex, TriVertexHandle> vertexHandles;
	for (int i = 0; i < vertices->size(); i++) {
		CGALPoint* pt = (*vertices)[i];
		TriVertexHandle vHandle = cdt->insert(*pt);
		vertexHandles.emplace(i, vHandle);
		(*handlesToIndex)->emplace(vHandle, i);
	}

	// Insert constraint edges
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* edge = (*edges)[i];
		VertexIndex u = edge->u;
		VertexIndex v = edge->v;
		TriVertexHandle uH = vertexHandles[u];
		TriVertexHandle vH = vertexHandles[v];
		cdt->insert_constraint(uH, vH);
	}

	assert(cdt->is_valid());

	return cdt;
}

// Given 3 colinear points, if the 2 furthest points are constrained, how is this delaunay triangulated?
// It seems the outcome is implementation dependent. CGAL will split the constraint into 2 edges.
// Other implementations might allow for overlapping, collinear edges. Either way, the current plan
// is to assume that an input forest, F, with collinear edges will be replaced by smaller constraint edges.
EdgeVector* newConstraintSetFromCdt(CDT* cdt, VertexVector* originalVertices) {
	EdgeVector* newEdgeVector = new EdgeVector();

	boost::unordered_map<CGALPoint, VertexIndex> vertexIndex;
	for (int i = 0; i < originalVertices->size(); i++) {
		vertexIndex[*(*originalVertices)[i]] = i;
	}

	// Add edges to graph
	for (CDT::Edge_iterator eit = cdt->edges_begin(); eit != cdt->edges_end(); ++eit) {
		CDT::Edge cgal_e = *eit;

		if (cdt->is_constrained(cgal_e)) {
			// Assumes point coord are unique
			CGALSegment segement = cdt->segment(cgal_e);
			CGALPoint cgal_u = segement.point(0);
			CGALPoint cgal_v = segement.point(1);
			VertexIndex u = vertexIndex[cgal_u];
			VertexIndex v = vertexIndex[cgal_v];

			SimpleEdge* edge = new SimpleEdge(u, v, 0);
			newEdgeVector->push_back(edge);
		}
	}
	return newEdgeVector;
}

TriVertexHandle OppositeOfEdge(TriVertexHandle ev0, TriVertexHandle ev1, TriFaceHandle f) {
	TriVertexHandle v0 = f->vertex(0);
	if (v0 != ev0 && v0 != ev1) {
		return v0;
	}

	TriVertexHandle v1 = f->vertex(1);
	if (v1 != ev0 && v1 != ev1) {
		return v1;
	}

	TriVertexHandle v2 = f->vertex(2);
	if (v2 != ev0 && v2 != ev1) {
		return v2;
	}

	assert(false); // Unreachable
	return NULL;
}

bool IsInsideCircle(CGALPoint* p, CGALPoint* q, CGALPoint* t) {
	if (SHOW_DEBUG) {
		std::cout << "(" << (*p) << ") (" << (*q) << ") (" << (*t) << ")" << std::endl;
	}
	CGALCircle c(*p, *q);
	CGAL::Bounded_side side = c.bounded_side(*t);

	// v is outside or on the circumcircle of f
	return side == CGAL::Bounded_side::ON_BOUNDED_SIDE
		|| side == CGAL::Bounded_side::ON_BOUNDARY;
}

void computeNonLocallyGabriel(
	VertexVector* vertices,
	EdgeVector* edges,
	EdgeVector** NewEdges,
	EdgeVector** S_Edges) {

	boost::chrono::high_resolution_clock::time_point start;
	boost::chrono::high_resolution_clock::time_point end;
	boost::chrono::milliseconds duration(0);
	boost::chrono::milliseconds total(0);

	// Compute CDT(F)
	start = boost::chrono::high_resolution_clock::now();
	boost::unordered_map<TriVertexHandle, VertexIndex>* handlesToIndex;
	CDT* cdt = computeCdt(vertices, edges, &handlesToIndex);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("CDT(F)", duration);

	// Replace F with NewF
	start = boost::chrono::high_resolution_clock::now();
	(*NewEdges) = newConstraintSetFromCdt(cdt, vertices);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("F -> NewF", duration);

	// Compute Non-Locally Gabriel edges
	start = boost::chrono::high_resolution_clock::now();
	boost::unordered_set<TriEdge>* S = new boost::unordered_set<TriEdge>();
	TriVertexHandle infiniteVertex = cdt->infinite_vertex();
	int edgeCount = 0;
	for (FiniteEdgeIter iter = cdt->finite_edges_begin(); iter != cdt->finite_edges_end(); ++iter) {
		edgeCount++;

		// typedef std::pair<Face_handle, int> Edge;
		TriEdge e = *iter;
		int eIndex = e.second;

		// Edge shared by faces f0 and f1
		TriFaceHandle f0 = e.first;
		TriFaceHandle f1 = e.first->neighbor(eIndex);

		// Vertex opposite of edge e in f0, f1
		TriVertexHandle opp0 = f0->vertex(eIndex);

		// Vertex of edge e
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		TriVertexHandle opp1 = OppositeOfEdge(e0, e1, f1);

		if (SHOW_DEBUG) {
			// Vertex endpoint v0, v1 of edge e for f0 (doesn't seem to be possible to identify the same edge for f1 in a similar way)
			std::cout << eIndex << std::endl;
			std::cout << "(" << *e0 << ") (" << *e1 << ")" << std::endl;
			std::cout << "(" << *opp0 << ")" << std::endl;
		}

		if (e0 != infiniteVertex && e1 != infiniteVertex) {
			CGALPoint p = e0->point();
			CGALPoint q = e1->point();

			bool addToConstraint = false;

			if (opp0 != infiniteVertex) {
				CGALPoint t = opp0->point();
				if (IsInsideCircle(&p, &q, &t)) {
					addToConstraint = true;
				}
			}

			if (opp1 != infiniteVertex) {
				CGALPoint t = opp1->point();
				if (IsInsideCircle(&p, &q, &t)) {
					addToConstraint = true;
				}
			}

			if (addToConstraint) {
				S->emplace(e);
			}
		}
	}

	(*S_Edges) = new EdgeVector();
	for (boost::unordered_set<TriEdge>::iterator iter = S->begin(); iter != S->end(); ++iter) {
		TriEdge e = *iter;
		int eIndex = e.second;
		TriFaceHandle f0 = e.first;
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		VertexIndex u = (*handlesToIndex)[e0];
		VertexIndex v = (*handlesToIndex)[e1];

		(*S_Edges)->push_back(new SimpleEdge(u, v, 0));
	}

	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("computeNonLocallyGabriel duration", duration);

	printDuration("Total", total);

	delete S;
	delete cdt;
	delete handlesToIndex;
}

void computeCgg(
	VertexVector* vertices,
	EdgeVector* edges,
	EdgeVector** NewEdges,
	EdgeVector** gabriel) {

	boost::unordered_set<SimpleEdge>* constraintEdgesSet = createSimpleEdgeSet(edges);

	boost::unordered_map<TriVertexHandle, VertexIndex>* handlesToIndex;
	CDT* cdt = computeCdt(vertices, edges, &handlesToIndex);
	(*NewEdges) = newConstraintSetFromCdt(cdt, vertices);

	boost::unordered_set<TriEdge>* S = new boost::unordered_set<TriEdge>();

	TriVertexHandle infiniteVertex = cdt->infinite_vertex();
	int edgeCount = 0;

	boost::chrono::high_resolution_clock::time_point startTotal = boost::chrono::high_resolution_clock::now();

	for (FiniteEdgeIter iter = cdt->finite_edges_begin(); iter != cdt->finite_edges_end(); ++iter) {
		edgeCount++;

		// typedef std::pair<Face_handle, int> Edge;
		TriEdge e = *iter;
		int eIndex = e.second;

		// Edge shared by faces f0 and f1
		TriFaceHandle f0 = e.first;
		TriFaceHandle f1 = e.first->neighbor(eIndex);

		// Vertex opposite of edge e in f0
		TriVertexHandle opp0 = f0->vertex(eIndex);

		// Vertex of edge e
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		TriVertexHandle opp1 = OppositeOfEdge(e0, e1, f1);

		if (SHOW_DEBUG) {
			// Vertex endpoint v0, v1 of edge e for f0 (doesn't seem to be possible to identify the same edge for f1 in a similar way)
			std::cout << eIndex << std::endl;
			std::cout << "(" << *e0 << ") (" << *e1 << ")" << std::endl;
			std::cout << "(" << *opp0 << ")" << std::endl;
		}

		if (e0 != infiniteVertex && e1 != infiniteVertex) {
			CGALPoint p = e0->point();
			CGALPoint q = e1->point();
			VertexIndex u = (*handlesToIndex)[e0];
			VertexIndex v = (*handlesToIndex)[e1];

			bool addToS = true;

			if (opp0 != infiniteVertex) {
				CGALPoint t = opp0->point();
				// Is not locally gabriel
				// Is not a constraint edge
				if (IsInsideCircle(&p, &q, &t)
					&& (constraintEdgesSet->count(SimpleEdge(u, v, 0)) < 1)) {
					addToS = false;
				}
			}

			if (opp1 != infiniteVertex) {
				CGALPoint t = opp1->point();
				// Is not locally gabriel
				// Is not a constraint edge
				if (IsInsideCircle(&p, &q, &t)
					&& (constraintEdgesSet->count(SimpleEdge(u, v, 0)) < 1)) {
					addToS = false;
				}
			}

			if (addToS) {
				S->emplace(e);
			}
		}
	}

	(*gabriel) = new EdgeVector();
	for (boost::unordered_set<TriEdge>::iterator iter = S->begin(); iter != S->end(); ++iter) {
		TriEdge e = *iter;
		int eIndex = e.second;
		TriFaceHandle f0 = e.first;
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		VertexIndex u = (*handlesToIndex)[e0];
		VertexIndex v = (*handlesToIndex)[e1];

		(*gabriel)->push_back(new SimpleEdge(u, v, 0));
	}

	boost::chrono::high_resolution_clock::time_point endTotal = boost::chrono::high_resolution_clock::now();
	boost::chrono::milliseconds total = (boost::chrono::duration_cast<boost::chrono::milliseconds>(endTotal - startTotal));
	printDuration("computeCgg duration", total);

	delete S;
	delete cdt;
	delete handlesToIndex;
	delete constraintEdgesSet;
}

EdgeVector* intersectInputSetWithConstraintSet(EdgeVector* inputSet, EdgeVector* constraintSet) {
	boost::unordered_set<SimpleEdge>* NewEdges_Hashset = new boost::unordered_set<SimpleEdge>();
	for (int i = 0; i < inputSet->size(); i++) {
		SimpleEdge* e = (*inputSet)[i];
		NewEdges_Hashset->emplace((*e));
	}

	EdgeVector* intersect = new EdgeVector();
	for (int i = 0; i < constraintSet->size(); i++) {
		SimpleEdge* se = (*constraintSet)[i];
		if (NewEdges_Hashset->count(*se) > 0) {
			intersect->push_back(new SimpleEdge(se->u, se->v, se->weight));
		}
	}

	return intersect;
}

bool containsEdge(boost::unordered_set<SimpleEdge>* edgeSet, SimpleEdge* edge) {
	SimpleEdge se(edge->u, edge->v, 0);
	return edgeSet->count(se) > 0;
}

// True if A a subgraph of B
bool isSubgraph(VertexVector* vertices, EdgeVector* a, EdgeVector* b) {
	boost::unordered_set<SimpleEdge>* bEdgeSet = createSimpleEdgeSet(b);

	// Iterate through the edges
	for (int i = 0; i < a->size(); i++) {
		SimpleEdge* edge = (*a)[i];
		if (!containsEdge(bEdgeSet, edge)) {
			CGALPoint* u = (*vertices)[edge->u];
			CGALPoint* v = (*vertices)[edge->v];
			//EdgeWeight weight = CGAL::squared_distance(*u, *v);
			CGAL::Lazy_exact_nt<CGAL::Gmpq> exactWeight = CGAL::squared_distance(*u, *v);
			EdgeWeight weight = CGAL::to_double(exactWeight);
			std::cout << "b contains edge not in a: " << weight << " (" << *u << ") (" << *v << ")" << std::endl;
			return false;
		}
	}

	return true;
}

EdgeVector* convertCdtToGraph(VertexVector* vertices, CDT* cdt) {
	EdgeVector* edgeVec = new EdgeVector();
	// edgeVec->reserve(cdt->number_of_faces() * 3); // Upper bound on number of edges (typically too much)

	// Map CGALPoint -> VertexIndex
	boost::unordered_map<CGALPoint, VertexIndex> vertexIndex;
	for (int i = 0; i < vertices->size(); i++) {
		vertexIndex[*(*vertices)[i]] = i;
	}

	// Add edges to graph
	for (CDT::Edge_iterator eit = cdt->edges_begin(); eit != cdt->edges_end(); ++eit) {
		CDT::Edge cgal_e = *eit;
		CGALSegment segement = cdt->segment(cgal_e);
		CGALPoint cgal_u = segement.point(0);
		CGALPoint cgal_v = segement.point(1);
		VertexIndex u = vertexIndex[cgal_u];
		VertexIndex v = vertexIndex[cgal_v];

		SimpleEdge* edge = new SimpleEdge(u, v, 0);
		edgeVec->push_back(edge);
	}

	return edgeVec;
}

bool isCdtSubgraph(VertexVector* vertices, EdgeVector* edgesF, EdgeVector* edgesS) {
	boost::unordered_map<TriVertexHandle, VertexIndex>* handlesToIndex;
	CDT* cdtS = computeCdt(vertices, edgesS, &handlesToIndex); // CDT of mimimum edge constraint
	EdgeVector* ev_cdtS = convertCdtToGraph(vertices, cdtS);
	bool res = isSubgraph(vertices, edgesF, ev_cdtS);

	delete ev_cdtS;
	delete cdtS;
	delete handlesToIndex;

	return res;
}

bool isCggSubgraph(VertexVector* vertices, EdgeVector* edgesF, EdgeVector* edgesS) {
	EdgeVector* newEdgesS;
	EdgeVector* gabriel_S;
	computeCgg(vertices, edgesS, &newEdgesS, &gabriel_S);
	bool res = isSubgraph(vertices, edgesF, gabriel_S);

	delete gabriel_S;
	delete newEdgesS;

	return res;
}

int main(int argc, char* argv[]) {
	const char* vertFile = (argc > 2) ? argv[1] : NULL;
	const char* edgeFile = (argc > 2) ? argv[2] : NULL;

	VertexVector* vertices = NULL;
	EdgeVector* edges = NULL;

	if (vertFile == NULL || edgeFile == NULL) {
		// Random graph
		//createRandomCirclePlaneForest(1000, 1000, 100, &vertices, &edges);
		//createRandomMediumLengthPlaneForest(1000, 1000, 100, &vertices, &edges);
		createRandomNearTriangulation(1000, 1000, 100, &vertices, &edges);
	}
	else {
		// Load graph from file
		// e.g. D:\g\data\HI.nodes D:\g\data\HI.edges
		parseGraph(vertFile, edgeFile, &vertices, &edges);
	}

	if (SHOW_DEBUG) {
		printGraph("Input", vertices, edges, true);
	}

	// Compute CGG(V, E)
	EdgeVector* NewEdges;
	EdgeVector* cggS;
	computeNonLocallyGabriel(vertices, edges, &NewEdges, &cggS);
	EdgeVector* S = intersectInputSetWithConstraintSet(NewEdges, cggS);
	//std::cout << "Edges in E: " << NewEdges->size() << " Edges in S: " << S->size() << " Ratio: " << (double)((double)S->size() / (double)NewEdges->size()) << std::endl;
	std::cout << S->size() << std::endl;
	std::cout << (double)((double)S->size() / (double)NewEdges->size()) << std::endl;
	std::cout << std::endl;

	//printGDFGraph("D:\\g\\results\\graph examples\\hi_gg_S.gdf", vertices, S);

	// Other possible validatation
	// CGG ⊆ CDT
	// S ⊆ CDT

	// S ⊆ E
	if (!isSubgraph(vertices, S, NewEdges)) {
		std::cout << "Error: isSubgraph is false" << std::endl;
	}

	// F ⊆ CDT(V, S) (Note: CGG is contained in CDT)
	if (!isCdtSubgraph(vertices, NewEdges, S)) {
		std::cout << "Error: isCdtSubgraph is false" << std::endl;
	}

	// F ⊆ CGG(V, S), CGG is computed by running a CDT on S, followed by
	// a locally gabriel check on all edge from the CDT and eliminating edges
	// that are non-locally gabriel AND not a constraint edge
	if (!isCggSubgraph(vertices, NewEdges, S)) {
		std::cout << "Error: isCggSubgraph is false" << std::endl;
	}

	deleteEdgeVector(S);
	deleteEdgeVector(cggS);
	deleteEdgeVector(NewEdges);

	deleteEdgeVector(edges);
	deleteVerticesVector(vertices);

	return 0;
}


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

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

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

//[overlaps
//` Checks if two geometries overlap

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace bg = boost::geometry; /*< Convenient namespace alias >*/

int main()
{
    // Checks if the two geometries overlaps or not.
    bg::model::polygon<bg::model::d2::point_xy<double> > poly1;
    bg::read_wkt("POLYGON((0 0,0 4,4 4,4 0,0 0))", poly1);
    bg::model::polygon<bg::model::d2::point_xy<double> > poly2;
    bg::read_wkt("POLYGON((2 2,2 6,6 7,6 1,2 2))", poly2);
    bool check_overlap = bg::overlaps(poly1, poly2);
    if (check_overlap) {
         std::cout << "Overlaps: Yes" << std::endl;
    } else {
        std::cout << "Overlaps: No" << std::endl;
    }

    bg::model::polygon<bg::model::d2::point_xy<double> > poly3;
    bg::read_wkt("POLYGON((-1 -1,-3 -4,-7 -7,-4 -3,-1 -1))", poly3);
    check_overlap = bg::overlaps(poly1, poly3);
    if (check_overlap) {
         std::cout << "Overlaps: Yes" << std::endl;
    } else {
        std::cout << "Overlaps: No" << std::endl;
    }

    return 0;
}

//]

//[overlaps_output
/*`
Output:
[pre
Overlaps: Yes

[$img/algorithms/overlaps.png]

Overlaps: No
]
*/
//]


// Copyright(c) 2019-present, Alexander Silva Barbosa & bflib contributors.
// Distributed under the MIT License (http://opensource.org/licenses/MIT)

/**
 * @author Alexander Silva Barbosa <alexander.ti.ufv@gmail.com>
 * @date 2019
 * Particle Filter (Monte Carlo)
 */

#pragma once

#include <Eigen/Dense>
#include <random>
#include <chrono>
#include <thread>
#include <iostream>
#include <vector>

using namespace Eigen;
using namespace std;

template <typename dataType, int states, int inputs, int outputs, int outputSize>
class PF
{
    private:
        typedef Matrix<dataType, states, 1> MatNx1;
        typedef Matrix<dataType, inputs, 1> MatMx1;
        typedef Matrix<dataType, outputs, 1> MatPx1;
        typedef Matrix<dataType, states, states> MatNxN;

    public:
        typedef MatNx1 State;
        typedef MatMx1 Input;
        typedef MatPx1 Output;
        typedef Input Control;
        typedef Output Sensor;
        typedef MatPx1 SensorStd;
        typedef MatNx1 ResampleStd;

        double eps;

    private:
        typedef void (*ModelFunction)(State &x, Input &u, double dt);
        typedef void (*SensorFunction)(std::vector<Output> &y, State &x, double dt);

        std::default_random_engine gen;
        std::normal_distribution<double> distr{0.0, 1.0};
        std::chrono::time_point<std::chrono::high_resolution_clock> start;

        int N, NMax;
        std::vector<State> W;
        std::vector< std::vector<Output> > Y;
        std::vector<double> w;
        double wMax;

        State minState, maxState, meanState;

        State x;
        ResampleStd Q;
        SensorStd R;

        MatNxN resampleMatrix;
        MatNx1 randX;

        ModelFunction modelFn;
        SensorFunction sensorFn;

        void init()
        {
            modelFn = NULL;
            sensorFn = NULL;

            resampleMatrix = Q.asDiagonal();

            N = NMax;
            W.resize(N);
            Y.resize(N);
            w.resize(N);
            meanState = (maxState - minState) / 2;
            initW();

            eps = 1e-10;

            start = std::chrono::high_resolution_clock::now();
        }
    public:

        PF(int N, State minState, State maxState) : NMax(N), minState(minState), maxState(maxState)
        {
            Q.setIdentity();
            R.setIdentity();
            x.setZero();
            init();
        }

        PF(int N, State X, State minState, State maxState) : x(X), NMax(N), minState(minState), maxState(maxState)
        {
            Q.setIdentity();
            R.setIdentity();
            init();
        }

        PF(int N, State minState, State maxState, ResampleStd Q, SensorStd R) : Q(Q), R(R), NMax(N), minState(minState), maxState(maxState)
        {
            x.setZero();
            init();
        }

        PF(int N, State X, State minState, State maxState, ResampleStd Q, SensorStd R) : x(X), Q(Q), R(R), NMax(N), minState(minState), maxState(maxState)
        {
            init();
        }

        virtual ~PF()
        {

        }

        void seed()
        {
            unsigned int s = std::chrono::system_clock::now().time_since_epoch().count();
            seed(s);
        }

        void seed(unsigned int s)
        {
            gen = std::default_random_engine(s);
            srand (s);
            std::srand(s);
            initW();
        }

        State state()
        {
            MatNx1 x;
            x.setZero();
            return x;
        }

        Input input()
        {
            MatMx1 u;
            u.setZero();
            return u;
        }

        Output output()
        {
            MatPx1 y;
            y.setZero();
            return y;
        }

        SensorStd createR()
        {
            SensorStd R;
            R.setZero();
            return R;
        }

        void setR(SensorStd R)
        {
            this->R = R;
        }

        ResampleStd createQ()
        {
            ResampleStd Q;
            Q.setZero();
            return Q;
        }

        void setQ(ResampleStd Q)
        {
            this->Q = Q;
            resampleMatrix = Q.asDiagonal();
        }

        double time()
        {
            auto end = std::chrono::high_resolution_clock::now();
            std::chrono::duration<double> diff = end - start;
            start = std::chrono::high_resolution_clock::now();
            return diff.count();
        }

        double delay(double s)
        {
            double ellapsed = time();
            double remain = s - ellapsed;
            if(remain < 0)
                return ellapsed;
            std::this_thread::sleep_for(std::chrono::nanoseconds((long long)(remain * 1e9)));
            ellapsed += time();
            return ellapsed;
        }

        std::vector<State>& particles()
        {
            return W;
        }

        void setModel(ModelFunction fn)
        {
            modelFn = fn;
        }

        void setSensor(SensorFunction fn)
        {
            sensorFn = fn;
        }

        virtual void model(State &x, Input &u, double dt)
        {

        }

        virtual void sensor(std::vector<Output> &z, State &x, double dt)
        {

        }

        void simulate(State &x, std::vector<Output> &y, Input &u, double dt)
        {
            doModel(x, u, dt);

            y.resize(outputSize);
            doSensor(y, x, dt);
        }

        void run(State &xK, std::vector<Output> &y, Input &u, double dt)
        {
            y.resize(outputSize);

            applyControl(u, dt);
            sense(dt);
            compare(y);
            resample();
            measure();

            xK = x;
        }

	void VaryN(float error, int Nmin, int Ni, float error_lim, int Nmax)
	{
	    int Np=0;
	    if(error>error_lim)
	    {
	    	float l;

            	l = round(error/error_lim);
	    	int k = (int)l;
                Np = Nmin + Ni*k;
            	if(Np > Nmax)
		{
			this->N = Nmax;
		}
		else
		{
			this->N = Np;
		}
            }
	    else
	    {
                Np = Nmin;
		this->N = Np;
            }
	  cout << this-> N << endl;
	}

    private:
        void initW()
        {
            for(int n = 0; n < N; n++)
            {
                W[n] = State::Random();
                for (size_t i = 0; i < states; i++)
                {
                    W[n](i) = meanState(i) + W[n](i) * meanState(i);
                }
            }
        }

        void applyControl( Input &u, double dt)
        {
            for(int i = 0; i < N; i++)
            {
                doModel(W[i], u, dt);
            }
        }

        void sense(double dt)
        {
            for(int i = 0; i < N; i++)
            {
                Y[i].resize(outputSize);
                doSensor(Y[i], W[i], dt);
            }
        }

        void compare(std::vector<Output> &y)
        {
            double sum = 0;
            double lh = 0;
            for(int i = 0; i < N; i++)
            {
                w[i] = 1.0;
                for(int j = 0; j < outputSize; j++)
                {
                    lh = likelihood(y[j], Y[i][j]);
                    w[i] *= lh;
                }
                sum += w[i];
            }

            wMax = 0;
            for(int i = 0; i < N; i++)
            {
                w[i] /= sum;
                if(w[i] > wMax)
                    wMax = w[i];
            }
        }

        double likelihood(Output y, Output z)
        {
            double lh = 0;
            double df, sq, ex, pi2, m;

            pi2 = sqrt(2 * 3.14159265358979);

            for (size_t i = 0; i < outputs; i++)
            {
                if(R(i) == 0)
                    continue;

                m = 1.0 / ( R(i) * pi2 );

                df = (z(i) - y(i)) / R(i);

                sq = df*df;
                ex = exp( -0.5 * sq );

                lh += m * ex;
            }

            lh += eps;
        }

        void resample()
        {
            int index = rand() % N;
            double F;
            std::vector<State> nW(N);

            for(int i = 0; i < N; i++)
            {
                F = 10 * wMax * ( ( rand() % 100 ) / 100.0);
                while(F > w[index])
                {
                    F -= w[index];
                    index = ( index + 1 ) % N;
                }
                randn(randX);
                nW[i] = W[index] + resampleMatrix * randX;
                for (size_t j = 0; j < outputs; j++)
                {
                    if(nW[i](j) < minState(j))
                        nW[i](j) = maxState(j) - ( minState(j) - nW[i](j) );
                    else if(nW[i](j) > maxState(j))
                        nW[i](j) = minState(j) + ( nW[i](j) - maxState(j) );
                }
            }

            W = nW;
        }

        void measure()
        {
            x.setZero();
            for(int i = 0; i < N; i++)
            {
                x += W[i];
            }
            x /= N;
        }

        void doModel(State &x, Input &u, double dt)
        {
            if(modelFn != NULL)
                modelFn(x, u, dt);
            else
                model(x, u, dt);
        }

        void doSensor(std::vector<Output> &z, State &x, double dt)
        {
            if(sensorFn != NULL)
                sensorFn(z, x, dt);
            else
                sensor(z, x, dt);
        }

        template<class T>
        void randn(T &mat)
        {
            for (size_t i = 0; i < mat.rows(); i++)
            {
                for (size_t j = 0; j < mat.cols(); j++)
                {
                    mat(i, j) = distr(gen);
                }
            }
        }

};


// Arguments: Doubles, Doubles, Doubles
#include <stan/math/prim/scal.hpp>
#include <boost/utility/enable_if.hpp>

using stan::math::var;
using std::numeric_limits;
using std::vector;

class AgradDistributionsBeta : public AgradDistributionTest {
 public:
  void valid_values(vector<vector<double> >& parameters,
                    vector<double>& log_prob) {
    vector<double> param(3);

    param[0] = 0.4;  // y
    param[1] = 0.5;  // alpha
    param[2] = 0.5;  // beta
    parameters.push_back(param);
    log_prob.push_back(-0.4311717080293276382896);  // expected log_prob

    param[0] = 0.5;  // y
    param[1] = 2.0;  // alpha
    param[2] = 5.0;  // beta
    parameters.push_back(param);
    log_prob.push_back(-0.06453852113757105324332);  // expected log_prob
  }

  void invalid_values(vector<size_t>& index, vector<double>& value) {
    // y
    index.push_back(0U);
    value.push_back(-1.0);

    index.push_back(0U);
    value.push_back(2.0);

    // alpha
    index.push_back(1U);
    value.push_back(0.0);

    index.push_back(1U);
    value.push_back(-1.0);

    index.push_back(1U);
    value.push_back(numeric_limits<double>::infinity());

    index.push_back(1U);
    value.push_back(-numeric_limits<double>::infinity());

    // beta
    index.push_back(2U);
    value.push_back(0.0);

    index.push_back(2U);
    value.push_back(-1.0);

    index.push_back(2U);
    value.push_back(numeric_limits<double>::infinity());

    index.push_back(2U);
    value.push_back(-numeric_limits<double>::infinity());
  }

  template <typename T_y, typename T_scale1, typename T_scale2, typename T3,
            typename T4, typename T5>
  typename stan::return_type<T_y, T_scale1, T_scale2>::type log_prob(
      const T_y& y, const T_scale1& alpha, const T_scale2& beta, const T3&,
      const T4&, const T5&) {
    return stan::math::beta_log(y, alpha, beta);
  }

  template <bool propto, typename T_y, typename T_scale1, typename T_scale2,
            typename T3, typename T4, typename T5>
  typename stan::return_type<T_y, T_scale1, T_scale2>::type log_prob(
      const T_y& y, const T_scale1& alpha, const T_scale2& beta, const T3&,
      const T4&, const T5&) {
    return stan::math::beta_log<propto>(y, alpha, beta);
  }

  template <typename T_y, typename T_scale1, typename T_scale2, typename T3,
            typename T4, typename T5>
  typename stan::return_type<T_y, T_scale1, T_scale2, T3, T4, T5>::type
  log_prob_function(const T_y& y, const T_scale1& alpha, const T_scale2& beta,
                    const T3&, const T4&, const T5&) {
    using stan::math::log1m;
    using std::log;

    return (alpha - 1.0) * log(y) + (beta - 1.0) * log1m(y)
           + lgamma(alpha + beta) - lgamma(alpha) - lgamma(beta);
  }
};


#include <iostream>
#include <Eigen/Lgsm>

int main(int argc, char* argv [])
{
  Eigen::Vector3d pos(1, 0, 0);
  Eigen::Displacementd H_1_0 = Eigen::Displacementd(pos);
  Eigen::Displacementd H_2_1 = Eigen::Displacementd(pos);

  Eigen::Vector3d axis(0, 0, 1);

  Eigen::Twistd tw(Eigen::AngularVelocityd(axis), pos.cross(axis));

  tw *=  1 * M_PI / 2.0;

  // See Murray Lie Sastry page 50.
  Eigen::Displacementd H_2_0 = tw.exp()  * H_1_0 * H_2_1;
  //std::cout << "twist : " << tw.transpose() << std::endl;
  std::cout << "H_1_0 : " << H_1_0 << std::endl;
  std::cout << "eTw " << tw.exp() << std::endl;
  //std::cout << "H_2_1 : " << H_2_1 << std::endl;
  //std::cout << "Brockett factor : " << tw.exp()  * H_1_0 << std::endl;
  std::cout << "H_2_0 " << H_2_0 << std::endl;
  //std::cout <<  tw.exp()  * H_1_0 * H_2_1 << std::endl;
  std::cout <<  tw.exp()  * H_1_0 << std::endl;

  return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#include <iostream>
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE distributions test
#include <boost/test/unit_test.hpp>

#include <dpMM/sphere.hpp>
#include <dpMM/clTGMMDataGpu.hpp>
#include <dpMM/karcherMean.hpp>
#include <dpMM/normalSphere.hpp>
#include <dpMM/timer.hpp>

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

BOOST_AUTO_TEST_CASE( sphere_test)
{
  cout<<"----------------------- sphere ----------------------"<<endl;

  boost::mt19937 rndGen(1);
  Sphere<double> Sd(3);

  cout<<"sampled point: "<<Sd.sampleUnif(&rndGen).transpose()<<endl;

  VectorXd a(3);
  VectorXd b(3);
  a << 0.0,0.0,1.0;
  b << 0.0,0.0,1.0;
  MatrixXd bRa = rotationFromAtoB<double>(a,b);
  MatrixXd aRb = rotationFromAtoB<double>(b,a);
  BOOST_CHECK((bRa.array() == aRb.transpose().array()).all());
  BOOST_CHECK((bRa.array() == MatrixXd::Identity(3,3).array()).all());

  a << 0.0,0.0,1.0;
  b << 0.0,1.0,0.0;
  bRa = rotationFromAtoB<double>(a,b);
  aRb = rotationFromAtoB<double>(b,a);
  MatrixXd bRaSoll(3,3);
  bRaSoll << 1.0,0.0,0.0,
           0.0,0.0,1.0,
           0.0,-1.0,0.0;
  BOOST_CHECK((bRa.array() == aRb.transpose().array()).all());
//  cout<<bRa<<endl;
//  cout<<bRaSoll<<endl;
  BOOST_CHECK(((bRa - bRaSoll).array() < 1e-6).all());

  VectorXd p = Sd.sampleUnif(&rndGen);
  VectorXd q = Sd.sampleUnif(&rndGen);
  cout<< "q             : " << q.transpose()<< endl;
  cout<< "q in TpS north: " << Sd.Log_p_north(p,q).transpose()<< endl;
  cout<< "q (Log->Exp)  : " << Sd.Exp_p(p,Sd.Log_p(p,q)).transpose()<< endl;

  Sphere<double> Sd10(10);
  p = Sd10.sampleUnif(&rndGen);
  q = Sd10.sampleUnif(&rndGen);
  cout<< "x in TpS: " << Sd10.Log_p_north(p,q).transpose()<< endl;

  uint32_t D = 3;
  uint32_t K = 5;
  uint32_t N = 20;
  boost::shared_ptr<Matrix<double,Dynamic,Dynamic> > sq(
      new Matrix<double,Dynamic,Dynamic>(D,N));
  Matrix<double,Dynamic,Dynamic> xMu(D,N);
  Matrix<double,Dynamic,Dynamic> x(D-1,N);
  Matrix<double,Dynamic,Dynamic> p0s(D,K);
  for (uint32_t k=0; k<K; ++k)
     p0s.col(k) =  Sd.sampleUnif(&rndGen);
  VectorXu z(N);
  Matrix<double,Dynamic,Dynamic> mus = sampleClustersOnSphere<double>(*sq, z, K);

  Matrix<double,Dynamic,Dynamic> muEst = karcherMeanMultiple(
      p0s, *sq, xMu, z, K, 100);

  cout<<"muTrue="<<endl<<mus<<endl;
  cout<<"muEst ="<<endl<<muEst<<endl;
  cout<<"muInit ="<<endl<<p0s<<endl;

  Sphere<double> S(D);
  S.rotate_p2north(muEst,xMu, x, z, K);

  for(uint32_t i=0; i<N; ++i)
    BOOST_CHECK(
        (x.col(i) - S.Log_p_north(muEst.col(z(i)),sq->col(i))).norm() < 1e-6);

  p = VectorXd(3);
  q = VectorXd(3);
  p << 1,0,0;
  q <<-1,0,0;
  VectorXd qp = S.Log_p_single(p,q) ;
  cout<<"mapping "<<q.transpose()<<" to TpS at"<<p.transpose()
    <<endl<< qp.transpose() <<endl;
  cout<<"Exp ing it back down: "<<S.Exp_p_single(p,qp).transpose()<<endl;

}

typedef float myFlt;

BOOST_AUTO_TEST_CASE( sphereGpu_test)
{
  cout<<"----------------------- sphere Gpu ----------------------"<<endl;
  uint32_t D = 3;
  uint32_t K = 6;
  uint32_t N = 20;
  boost::mt19937 rndGen(1112);
  Sphere<myFlt> Sd(D);

  spVectorXu sz(new VectorXu(N));
  sz->topRows(N/2) = VectorXu::Zero(N/2);
  sz->bottomRows(N/2) = VectorXu::Ones(N/2);
  boost::shared_ptr<Matrix<myFlt,Dynamic,Dynamic> > sq(
      new Matrix<myFlt,Dynamic,Dynamic>(D,N));

  Matrix<myFlt,Dynamic,Dynamic> mus = sampleClustersOnSphere<myFlt>(*sq, 2);
//  for (uint32_t i=0; i<N; ++i)
//  {
//    sq->col(i) = Sd.sampleUnif(&rndGen);
//    if(i<N/2)
//      (*sz)(i) = 0;
//    else
//      (*sz)(i) = 1;
//  }
  Matrix<myFlt,Dynamic,Dynamic> ps(D,K);
  for (uint32_t k=0; k<K; ++k)
     ps.col(k) =  Sd.sampleUnif(&rndGen);
//  ps.leftCols<2>() << 0.0,0.0,
//        1.0,-1.0,
//        0.0,0.0;
  Matrix<myFlt,Dynamic,1> w(N/2);
  w.setOnes(N/2);
  cout<<" karcher means on CPU"<<endl;
  cout<< karcherMeanWeighted<myFlt>(ps.col(0), sq->leftCols(N/2), w, 20) <<endl;
  cout<< karcherMeanWeighted<myFlt>(ps.col(1), sq->rightCols(N/2), w, 20) <<endl;

//  assert(false);

  ClTGMMDataGpu<myFlt> sphere(sq,sz,K);
  //sphere.updateClusters(ps,z);
  Matrix<myFlt,Dynamic,Dynamic> karcherMeans = sphere.karcherMeans(ps);
  cout<< karcherMeans <<endl;
  cout<< "true centers"<<endl << mus <<endl;
  BOOST_CHECK(((karcherMeans.leftCols(2) - mus).array().abs() < 1e-2).all());

  Timer t;
  Matrix<myFlt,Dynamic,Dynamic> x = sphere.Log_p_north(ps);
  t.toctic("Log_p_north GPU");
  BOOST_CHECK(x.rows() == static_cast<myFlt>(D-1));
  BOOST_CHECK(x.cols() == static_cast<myFlt>(N));

  for(uint32_t i=0; i<N; ++i)
  {
    BOOST_CHECK(x.col(i).norm() < 3.141592653589793);
    BOOST_CHECK(fabs(x.col(i).norm()
          -acosf(ps.col((*sz)(i)).transpose()*sq->col(i)) ) < 1e-4);
    myFlt err = fabs(x.col(i).norm()
        -acosf(ps.col((*sz)(i)).transpose()*sq->col(i)));
    if(err >= 1e-4)
      cout<< err<<endl;
  }

  //cout<<x.transpose()<<endl;

  Matrix<myFlt,Dynamic,Dynamic> xx(D-1,N);
  t.tic();
  xx.leftCols(N/2)  = Sd.Log_p_north(ps.col(0),sq->leftCols(N/2));
  xx.rightCols(N/2) = Sd.Log_p_north(ps.col(1),sq->rightCols(N/2));
  t.toctic("Log_p_north CPU");

  //cout<< xx.transpose()<<endl;
  BOOST_CHECK( ((x-xx).array().abs() < 1.0e-4).all());

  cout<<"sufficient statistics test --------------------"<<endl;
  sphere.relinearize(karcherMeans);
  sphere.computeSufficientStatistics();
  for (uint32_t k=0; k<K; ++k)
    cout<<"S_"<<k<<endl<<sphere.S(k)<<endl;

  cout<<"pdf test --------------------------------------"<<endl;

  Matrix<myFlt,Dynamic,1> pi(K);
  pi.setOnes(); pi /= K;
  SamplerGpu<myFlt> sampler(N,K,&rndGen);
  vector<Matrix<myFlt,Dynamic,Dynamic> > Sigmas(K);
  for (uint32_t k=0; k<K; ++k)
    if(sphere.counts()(k) > 0)
      Sigmas[k] = sphere.S(k)/sphere.counts()(k);
    else
      Sigmas[k] = Matrix<myFlt,Dynamic,Dynamic>::Identity(D-1,D-1)*0.01;
  Matrix<myFlt,Dynamic,1> logNormalizers(K);
  for(uint32_t k=0; k<K; ++k)
  {
    NormalSphere<myFlt> normS(karcherMeans.col(k),Sigmas[k],&rndGen);
    logNormalizers(k) = -0.5*normS.logDetSigma();
  }

  sphere.sampleGMMpdf(pi, Sigmas , logNormalizers, &sampler);
  Matrix<myFlt,Dynamic,Dynamic> pdfs = sphere.pdfs();

  for(uint32_t i=0; i<N; ++i)
  {
    Matrix<myFlt,Dynamic,1> logPdf(K);
    for(uint32_t k=0; k<K; ++k)
    {
      NormalSphere<myFlt> normS(karcherMeans.col(k),Sigmas[k],&rndGen);
      logPdf(k) = 0.5*(LOG_2PI*D+normS.logDetSigma()) + normS.logPdf(sq->col(i));;
      //log(pi(k)) + normS.logPdf(sq->col(i));
//      logPdf(k) = normS.normal_.; //log(pi(k)) + normS.logPdf(sq->col(i));
    }
    Matrix<myFlt,Dynamic,1> A = logPdf;
    myFlt maxLog = logPdf.maxCoeff();
    logPdf = logPdf.array() - log((logPdf.array() - maxLog).exp().matrix().sum())
      - maxLog;
//    cout<<pdfs(i,0)<<" vs "<< exp(logPdf(0))<<endl;
    cout<<"--------------------------------"<<endl;
    cout<<pdfs.row(i)<<endl;
//    cout<<logPdf.array().exp().matrix().transpose()<<endl;
//    cout<<"-"<<endl;
//    cout<<(pdfs.row(i)-logPdf.array().exp().matrix().transpose())<<endl;
//    cout<<"log"<<endl;
//    cout<<logPdf.transpose()<<endl;

//    cout<<pi.array().log().matrix().transpose()<<endl; // DONE: same!
    cout<<(A.array()).matrix().transpose()<<endl;
  }
};


/**
 * @file IPCA.hpp
 * @brief Following the paper,
 * Juyang Weng, Yilu Zhang and Wey-Shiuan Hwang, "Candid Covariance-free Incremental Principal Component Analysis", TPAMI
 * @author Haoxiang Li
 * @version 1.1
 * @date 2015-01-11
 */
#include <Eigen/Eigen>

typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMatrixXf;

/**
 * @brief Candid convariance-free IPCA
 */
class CCIPCA {
public:

    /**
     * @brief Initialize CCIPCA
     *
     * @param dim_subspace number of eigen-vectors expected
     * @param dim_data dimensionality of the data points (K)
     * @param p_mean pre-computed data mean vector (D)
     * @param p_init_pca pre-computed eigen-vectors (D -by- K)
     * @param num_data_points number of data points used in pre-computation
     */
    CCIPCA(int dim_subspace, int dim_data, const float *p_mean, const float *p_init_pca, int num_data_points);

    /**
     * @brief Update the eigen-vectors given
     */
    void update(const float *pp);

    /**
     * @brief Obtain sorted (descending) eigen vectors && values
     */
    void sorted_eigen(RowMatrixXf& eigenvectors, Eigen::RowVectorXf& eigenvalues, int k) const;

private:
    int m_num_data_points;
    RowMatrixXf m_eigenvecs;
    Eigen::RowVectorXf m_eigenvecs_norms;
    Eigen::RowVectorXf m_mean;
};


#include <fastad>
#include <benchmark/benchmark.h>

#ifdef USE_ADEPT
#include <adept_arrays.h>
#endif

// Finite-difference

static inline double f_test1(const std::vector<double>& x)
{
    double sum = 0.;
    for (size_t i = 0; i < x.size(); ++i) {
        sum += x[i];
    }

    double w0 = x[0] * x[1] - x[2] * sin(x[0]);
    double w1 = x[1] * w0 - cos(w0) + sum;
    return w1 + exp(w1 - w0);
}

static void BM_test1_fd(benchmark::State& state)
{
    constexpr double h = 1e-10;
    for (auto _ : state) {
        std::vector<double> x(100);
        for (size_t i = 0; i < x.size(); ++i) {
            x[i] = i / 100.;
        }
        double f = f_test1(x);
        for (size_t i = 0; i < x.size(); ++i) {
            x[i] += h;
            double f_h = f_test1(x);
            double dfdx_i = (f_h - f) / h;
            benchmark::DoNotOptimize(dfdx_i);
            x[i] -= h;
        }
    }
}

BENCHMARK(BM_test1_fd);

// FastAD
static void BM_test1_fastad(benchmark::State& state)
{
    using namespace ad;
    std::vector<Var<double>> x;
    for (size_t i = 0; i < 100; ++i) {
        x.emplace_back(i / 100.);
    }
    std::vector<Var<double>> w(3);
    auto expr = ad::bind(
                (w[0] = x[0] * x[1] - x[2] * sin(x[0]),
                 w[1] = x[1] * w[0] - cos(w[0]) +
                        ad::sum(x.begin(), x.end(), [](const auto& xi) {return xi;}),
                 w[2] = w[1] + ad::exp(w[1] - w[0]))
    );

    for (auto _ : state) {
        autodiff(expr);
        benchmark::DoNotOptimize(expr);
    }
}

BENCHMARK(BM_test1_fastad);

#ifdef USE_ADEPT

// Adept
static void BM_test1_adept(benchmark::State& state)
{
    using namespace adept;
    for (auto _ : state) {
        Stack stack;
        aVector x(100);
        for (size_t i = 0; i < 100; ++i) {
            x << i / 100.;
        }
        stack.new_recording();
        aReal w_0 = x[0] * x[1] - x[2] * sin(x[0]);
        aReal w_1 = x[1] * w_0 - cos(w_0) + sum(x);
        aReal J = w_1 + exp(w_1 - w_0);
        J.set_gradient(1.);
        stack.reverse();
        benchmark::ClobberMemory();
    }
}

BENCHMARK(BM_test1_adept);

#endif


#include <iostream>
#include <chrono>
#include <sstream>
#include <fstream>
#include <Eigen/Dense>
#include "Timer.h"
#include "Projection.h"
#include "Volumetric_helper.h"
#include "KinectFrame.h"
#include "KinectSpecs.h"
#include "ICP.h"

typedef double Decimal;

void compute_vertices(std::vector<Eigen::Matrix<Decimal, 4, 1>>& vertices, const KinectFrame& frame, Decimal fovy, Decimal aspect_ratio, Decimal near_plane, Decimal far_plane)
{
	Eigen::Matrix<Decimal, 3, 1> vert_uv, vert_u1v, vert_uv1;

	for (int x = 0; x < frame.depth_width() - 1; ++x)
	{
		for (int y = 0; y < frame.depth_height() - 1; ++y)
		{
			const Decimal depth = frame.depth[y * frame.depth_width() + x];

			if (depth > 0.01)
			{
				vert_uv = window_coord_to_3d(Eigen::Matrix<Decimal, 2, 1>(x, y), depth, fovy, aspect_ratio, near_plane, far_plane, frame.depth_width(), frame.depth_height());

				int i = y * frame.depth_width() + x;

#if 1
				vertices[i] = vert_uv.homogeneous();
#else

				if (!vert_uv.isZero(0.0001))
				{
					vertices.push_back(vert_uv.homogeneous());
				}
#endif
			}

		}
	}
}

// Usage: ./Icp.exe ../../data/frame_0.knt ../../data/frame_1.knt 0 10 3
int main(int argc, char **argv)
{

	if (argc < 3)
	{
		std::cerr
			<< "Missing parameters. Abort."
			<< std::endl
			<< "Usage: ./Icp.exe <knt_frame> <knt_frame> <filter_width> <max_distance_threshold> "
			<< "Usage: ./Icp.exe ../../data/frame_1.knt ../../data/frame_1.knt 10 3"
			<< std::endl;
		return EXIT_FAILURE;
	}

	Timer timer;
	const std::string input_filename[] = { argv[1], argv[2] };

	const int filter_width = (argc > 2) ? atoi(argv[3]) : 10;
	const Decimal max_distance = (Decimal)((argc > 3) ? atof(argv[4]) : 100);

	try
	{
		timer.start();
		KinectFrame frame[2] =
		{
			KinectFrame(input_filename[0]),
			KinectFrame(input_filename[1])
		};
		timer.print_interval("Importing frames          : ");

		if (frame[0].depth_width() != frame[1].depth_width() ||
			frame[0].depth_height() != frame[1].depth_height())
		{
			std::cout
				<< "Frame size doens not match: " << std::endl
				<< frame[0].depth_width() << ", " << frame[0].depth_height() << std::endl
				<< frame[1].depth_width() << ", " << frame[1].depth_height() << std::endl
				<< std::endl;
			return EXIT_FAILURE;
		}

		const uint16_t width = frame[0].depth_width();
		const uint16_t height = frame[0].depth_height();
		const uint32_t pixel_count = width * height;

		Decimal fovy = KINECT_V2_FOVY;
		Decimal aspect_ratio = KINECT_V2_DEPTH_ASPECT_RATIO;
		Decimal near_plane = KINECT_V2_DEPTH_MIN;
		Decimal far_plane = KINECT_V2_DEPTH_MAX;

		if (width != 512)	// it is not kinect version 2. It is version 1
		{
			fovy = KINECT_V1_FOVY;
			aspect_ratio = KINECT_V1_ASPECT_RATIO;
			near_plane = KINECT_V1_DEPTH_MIN;
			far_plane = KINECT_V1_DEPTH_MAX;
		}

		std::vector<Eigen::Matrix<Decimal, 4, 1>> vertices[2] =
		{
			std::vector<Eigen::Matrix<Decimal, 4, 1>>(pixel_count, Eigen::Matrix<Decimal, 4, 1>(0, 0, 0, 0)),
			std::vector<Eigen::Matrix<Decimal, 4, 1>>(pixel_count, Eigen::Matrix<Decimal, 4, 1>(0, 0, 0, 0))
		};

		for (int i = 0; i < 2; ++i)
		{
			timer.start();
			compute_vertices(vertices[i], frame[i], fovy, aspect_ratio, near_plane, far_plane);
			timer.print_interval("Depth map back projection : ");
		}

		ICP<Decimal> icp;
		icp.setInputCloud(vertices[0]);
		icp.setTargetCloud(vertices[1]);

		Eigen::Matrix<Decimal, 4, 4> identity = Eigen::Matrix<Decimal, 4, 4>::Identity();
		Eigen::Matrix<Decimal, 4, 4> rigidTransform = Eigen::Matrix<Decimal, 4, 4>::Identity();
		Eigen::Matrix<Decimal, 3, 3> R;
		Eigen::Matrix<Decimal, 3, 1> t;

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

			timer.start();
			bool icp_success = icp.align_iteration(vertices[0], vertices[1],
				filter_width, max_distance, width, height,
				R, t);

			timer.print_interval("Computing icp             : ");

			if (icp_success)
			{
				std::cout << "Icp Success" << std::endl;
			}
			else
			{
				std::cout << "Icp Failed" << std::endl;
			}

			std::cout
				<< std::fixed << std::endl
				<< R << std::endl << std::endl
				<< t.transpose() << std::endl << std::endl;
		}

	}
	catch (const std::exception& ex)
	{
		std::cerr << "Error: " << ex.what() << std::endl;
	}

	return 0;
}


/* boost random/generate_canonical.hpp header file
 *
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 */

#ifndef BOOST_RANDOM_GENERATE_CANONICAL_HPP
#define BOOST_RANDOM_GENERATE_CANONICAL_HPP

#include <algorithm>
#include <boost/assert.hpp>
#include <boost/config/no_tr1/cmath.hpp>
#include <boost/limits.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/random/detail/signed_unsigned_tools.hpp>
#include <boost/random/detail/generator_bits.hpp>

namespace boost {
namespace random {

namespace detail {

template<class RealType, std::size_t bits, class URNG>
RealType generate_canonical_impl(URNG& g, boost::mpl::true_ /*is_integral*/)
{
    using std::pow;
    typedef typename URNG::result_type base_result;
    std::size_t digits = std::numeric_limits<RealType>::digits;
    RealType R = RealType((g.max)()) - RealType((g.min)()) + 1;
    RealType mult = R;
    RealType limit =
        pow(RealType(2),
            RealType((std::min)(static_cast<std::size_t>(bits), digits)));
    RealType S = RealType(detail::subtract<base_result>()(g(), (g.min)()));
    while(mult < limit) {
        RealType inc = RealType(detail::subtract<base_result>()(g(), (g.min)()));
        S += inc * mult;
        mult *= R;
    }
    return S / mult;
}

template<class RealType, std::size_t bits, class URNG>
RealType generate_canonical_impl(URNG& g, boost::mpl::false_ /*is_integral*/)
{
    using std::pow;
    using std::floor;
    BOOST_ASSERT((g.min)() == 0);
    BOOST_ASSERT((g.max)() == 1);
    std::size_t digits = std::numeric_limits<RealType>::digits;
    std::size_t engine_bits = detail::generator_bits<URNG>::value();
    std::size_t b = (std::min)(bits, digits);
    RealType R = pow(RealType(2), RealType(engine_bits));
    RealType mult = R;
    RealType limit = pow(RealType(2), RealType(b));
    RealType S = RealType(g() - (g.min)());
    while(mult < limit) {
        RealType inc(floor((RealType(g()) - RealType((g.min)())) * R));
        S += inc * mult;
        mult *= R;
    }
    return S / mult;
}

}

/**
 * Returns a value uniformly distributed in the range [0, 1)
 * with at least @c bits random bits.
 */
template<class RealType, std::size_t bits, class URNG>
RealType generate_canonical(URNG& g)
{
    RealType result = detail::generate_canonical_impl<RealType, bits>(
        g, boost::is_integral<typename URNG::result_type>());
    BOOST_ASSERT(result >= 0);
    BOOST_ASSERT(result <= 1);
    if(result == 1) {
        result -= std::numeric_limits<RealType>::epsilon() / 2;
        BOOST_ASSERT(result != 1);
    }
    return result;
}

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_GENERATE_CANONICAL_HPP


#include <string>
#include <iostream>
#include <fstream>
#include <CGAL/Aff_transformation_3.h>
#include <CGAL/Cartesian.h>
#include <CGAL/IO/STL_reader.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/intersection.h>
#include <CGAL/Polygon_mesh_processing/transform.h>
#include <ctime>
#include <Eigen/Dense>

typedef CGAL::Simple_cartesian<double> K;
typedef CGAL::Surface_mesh<K::Point_3> Mesh;

namespace PMP = CGAL::Polygon_mesh_processing;

int main(int argc, char *argv[])
{
  const char* filename1 = "../meshes/BIG_SCREW.off";
  std::ifstream input1(filename1);
  Mesh mesh1;
  if (!input1 || !(input1 >> mesh1) || !CGAL::is_triangle_mesh(mesh1))
  {
    std::cerr << "Not a valid input file." << std::endl;
    return 1;
  }

  const char* filename2 = "../meshes/fingertip_8faces.off";
  std::ifstream input2(filename2);
  Mesh mesh2;
  if (!input2 || !(input2 >> mesh2) || !CGAL::is_triangle_mesh(mesh2))
  {
    std::cerr << "Not a valid input file." << std::endl;
    return 1;
  }

  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  for (int i = 0; i < 10; ++i) {
    double begin = std::clock();
    Eigen::Vector3d p;
    p << 0, 2*i, 0;
    K::Aff_transformation_3 transform_cgal(
        R(0,0), R(0,1), R(0,2), p(0),
        R(1,0), R(1,1), R(1,2), p(1),
        R(2,0), R(2,1), R(2,2), p(2), 1);
    // K::Aff_transformation_3 transform_cgal(CGAL::TRANSLATION, K::Vector_3(0,i*2,0));
    PMP::transform (transform_cgal, mesh1);
    bool intersecting = PMP::do_intersect (mesh1, mesh2);
    std::cout << "Computation Time: " << (std::clock() - begin) / CLOCKS_PER_SEC << " sec" << std::endl;
    std::cout << "intersecting: " << intersecting << std::endl;
  }
}