//
// Project: Delaunay
// File: Delaunay.hpp
//
// Copyright (c) 2021 Miika 'Lehdari' Lehtimäki
// You may use, distribute and modify this code under the terms
// of the licence specified in file LICENSE which is distributed
// with this source code package.
//

#ifndef DELAUNAY_DELAUNAY_HPP
#define DELAUNAY_DELAUNAY_HPP

#include <vector>
#include <cstdint>

#define DELAUNAY_BACKEND_EIGEN 1
//#define DELAUNAY_BACKEND_OTHER 2 // add more enumerations for more supported backends

// definitions for Eigen backend
#if DELAUNAY_BACKEND == DELAUNAY_BACKEND_EIGEN
    #include <Eigen/Dense>

    #define DELAUNAY_VEC Eigen::Matrix<T_Scalar, 2, 1>
    #define DELAUNAY_VEC_ACCESS(V, D) V(D)
    #define DELAUNAY_DETERMINANT
#endif

// Check for macro definitions
#ifndef DELAUNAY_VEC
    #error "DELAUNAY_VEC not defined"
#endif
#ifndef DELAUNAY_VEC_ACCESS
    #error "DELAUNAY_VEC_ACCESS not defined"
#endif
#ifndef DELAUNAY_DETERMINANT
    #error "DELAUNAY_DETERMINANT not defined"
#endif

#ifdef DELAUNAY_INLINE
    #error "DELAUNAY_INLINE already defined"
#endif
#define DELAUNAY_INLINE inline __attribute__((always_inline))

namespace delaunay {

namespace {

struct Triangle {
    int neighbours[3]; // indices of neighbouring triangles
    int vertices[3]; // indices of corner vertices

    Triangle(int t1, int t2, int t3, int v1, int v2, int v3) :
        neighbours  {t1, t2, t3},
        vertices    {v1, v2, v3}
    {}
};

template <typename T_Scalar>
DELAUNAY_INLINE bool ccw(
    const DELAUNAY_VEC& a,
    const DELAUNAY_VEC& b,
    const DELAUNAY_VEC& c)
{
#if DELAUNAY_BACKEND == DELAUNAY_BACKEND_EIGEN
    Eigen::Matrix<T_Scalar, 3, 3>   m;
    m <<
        a.transpose(),  1.0,
        b.transpose(),  1.0,
        c.transpose(),  1.0;
    return m.determinant() > 0.0;
#else
    return DELAUNAY_DETERMINANT(
        DELAUNAY_VEC_ACCESS(a,0), DELAUNAY_VEC_ACCESS(a,1), (T_Scalar)1.0,
        DELAUNAY_VEC_ACCESS(b,0), DELAUNAY_VEC_ACCESS(b,1), (T_Scalar)1.0,
        DELAUNAY_VEC_ACCESS(c,0), DELAUNAY_VEC_ACCESS(c,1), (T_Scalar)1.0) > 0.0;
#endif
}

// is d inside of circumcircle of abc?
template <typename T_Scalar>
DELAUNAY_INLINE bool inCircle(
    const DELAUNAY_VEC& a,
    const DELAUNAY_VEC& b,
    const DELAUNAY_VEC& c,
    const DELAUNAY_VEC& d)
{
    T_Scalar dx2 = DELAUNAY_VEC_ACCESS(d,0)*DELAUNAY_VEC_ACCESS(d,0);
    T_Scalar dy2 = DELAUNAY_VEC_ACCESS(d,1)*DELAUNAY_VEC_ACCESS(d,1);
#if DELAUNAY_BACKEND == DELAUNAY_BACKEND_EIGEN
    Eigen::Matrix<T_Scalar, 3, 3>   m;
    m <<
        DELAUNAY_VEC_ACCESS(a,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(a,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(a,0)*DELAUNAY_VEC_ACCESS(a,0)-dx2)+(DELAUNAY_VEC_ACCESS(a,1)*DELAUNAY_VEC_ACCESS(a,1)-dy2),
        DELAUNAY_VEC_ACCESS(b,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(b,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(b,0)*DELAUNAY_VEC_ACCESS(b,0)-dx2)+(DELAUNAY_VEC_ACCESS(b,1)*DELAUNAY_VEC_ACCESS(b,1)-dy2),
        DELAUNAY_VEC_ACCESS(c,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(c,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(c,0)*DELAUNAY_VEC_ACCESS(c,0)-dx2)+(DELAUNAY_VEC_ACCESS(c,1)*DELAUNAY_VEC_ACCESS(c,1)-dy2);
    return m.determinant() > 0.0;
#else
    return DELAUNAY_DETERMINANT(
        DELAUNAY_VEC_ACCESS(a,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(a,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(a,0)*DELAUNAY_VEC_ACCESS(a,0)-dx2)+(DELAUNAY_VEC_ACCESS(a,1)*DELAUNAY_VEC_ACCESS(a,1)-dy2),
        DELAUNAY_VEC_ACCESS(b,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(b,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(b,0)*DELAUNAY_VEC_ACCESS(b,0)-dx2)+(DELAUNAY_VEC_ACCESS(b,1)*DELAUNAY_VEC_ACCESS(b,1)-dy2),
        DELAUNAY_VEC_ACCESS(c,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(c,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(c,0)*DELAUNAY_VEC_ACCESS(c,0)-dx2)+(DELAUNAY_VEC_ACCESS(c,1)*DELAUNAY_VEC_ACCESS(c,1)-dy2))
        > 0.0;
#endif
}

// initial primitive construction functions for edges and triangles
template <template <typename, typename> class T_Vector, typename T_Scalar, typename T_Allocator>
DELAUNAY_INLINE void createEdge(const T_Vector<DELAUNAY_VEC, T_Allocator>& points,
    std::vector<Triangle>& triangles, int v1, int v2, int& firstTriangle, int& lastTriangle)
{
    int s = triangles.size();
    triangles.emplace_back(s+1, s+1, s+1, v1, v2, -1); // edge is presented by 2 ghost triangles (3rd vertex -1)
    triangles.emplace_back(s, s, s, v2, v1, -1);

    firstTriangle = s;
    lastTriangle = s;
}

template <template <typename, typename> class T_Vector, typename T_Scalar, typename T_Allocator>
DELAUNAY_INLINE void createTriangle(
    T_Vector<DELAUNAY_VEC, T_Allocator>& points,
    std::vector<Triangle>& triangles, int v1, int v2, int v3, int& firstTriangle, int& lastTriangle)
{
    int s = triangles.size();
    if (!ccw(points[v1], points[v2], points[v3])) {
        std::swap(v2, v3);
        lastTriangle = s+2;
    }
    else
        lastTriangle = s+3;
    triangles.emplace_back(s+1, s+2, s+3, v1, v2, v3);
    triangles.emplace_back(s, s+3, s+2, v2, v1, -1);
    triangles.emplace_back(s, s+1, s+3, v3, v2, -1);
    triangles.emplace_back(s, s+2, s+1, v1, v3, -1);

    firstTriangle = s+3;
}

// update neighbour of a triangle
// t: triangle neighbour of which is to be updated
// current: current neighbour triangle id to be changed
// updated: triangle id of the new neighbour
DELAUNAY_INLINE void updateNeighbour(Triangle& t, int current, int updated)
{
    if (t.neighbours[0] == current) {
        t.neighbours[0] = updated;
        return;
    }
    if (t.neighbours[1] == current) {
        t.neighbours[1] = updated;
        return;
    }
    if (t.neighbours[2] == current) {
        t.neighbours[2] = updated;
        return;
    }
}

// rotate triangle indices so that vertex id of -1 is at index 2, no-op for non-ghost or correct ghost triangles
DELAUNAY_INLINE void correctGhost(Triangle& t)
{
    if (t.vertices[0] == -1) {
        int tempVertex = t.vertices[2];
        int tempNeighbour = t.neighbours[2];
        t.vertices[2] = t.vertices[0];
        t.neighbours[2] = t.neighbours[0];
        t.vertices[0] = t.vertices[1];
        t.neighbours[0] = t.neighbours[1];
        t.vertices[1] = tempVertex;
        t.neighbours[1] = tempNeighbour;
    }
    else if (t.vertices[1] == -1) {
        int tempVertex = t.vertices[2];
        int tempNeighbour = t.neighbours[2];
        t.vertices[2] = t.vertices[1];
        t.neighbours[2] = t.neighbours[1];
        t.vertices[1] = t.vertices[0];
        t.neighbours[1] = t.neighbours[0];
        t.vertices[0] = tempVertex;
        t.neighbours[0] = tempNeighbour;
    }
}

// flip an edge between two triangles
inline void flip(std::vector<Triangle>& triangles, int t1, int t2)
{
    Triangle& triangle1 = triangles[t1];
    Triangle& triangle2 = triangles[t2];

    int t1EdgeId = 0; // id of the t1 edge connecting to t2
    if (triangle1.neighbours[1] == t2)
        t1EdgeId = 1;
    else if (triangle1.neighbours[2] == t2)
        t1EdgeId = 2;

    int t2EdgeId = 0; // id of the t2 edge connecting to t1
    if (triangle2.neighbours[1] == t1)
        t2EdgeId = 1;
    else if (triangle2.neighbours[2] == t1)
        t2EdgeId = 2;

    Triangle triangle1temp = triangle1;
    Triangle triangle2temp = triangle2;

    // update neighbour indexing
    updateNeighbour(triangles[triangle1temp.neighbours[(t1EdgeId+1)%3]], t1, t2);
    updateNeighbour(triangles[triangle2temp.neighbours[(t2EdgeId+1)%3]], t2, t1);

    // update vertex and neighbour indexing of t1 and t2
    triangle1.vertices[(t1EdgeId+1)%3] = triangle2temp.vertices[(t2EdgeId+2)%3];
    triangle1.neighbours[t1EdgeId] = triangle2temp.neighbours[(t2EdgeId+1)%3];
    triangle1.neighbours[(t1EdgeId+1)%3] = t2;

    triangle2.vertices[(t2EdgeId+1)%3] = triangle1temp.vertices[(t1EdgeId+2)%3];
    triangle2.neighbours[t2EdgeId] = triangle1temp.neighbours[(t1EdgeId+1)%3];
    triangle2.neighbours[(t2EdgeId+1)%3] = t1;

    // correct ghost indexing in case either of the triangles is a ghost
    correctGhost(triangle1);
    correctGhost(triangle2);
}

// get vertex of triangle t opposing neighbour neighbour
DELAUNAY_INLINE int getOpposingVertex(Triangle& t, int neighbour)
{
    if (t.neighbours[0] == neighbour)
        return t.vertices[2];
    if (t.neighbours[1] == neighbour)
        return t.vertices[0];
    if (t.neighbours[2] == neighbour)
        return t.vertices[1];

    // should never be reached
    return -1; // neighbour not neighbour of t
}

// get the triangle ID of right neighbour w.r.t to a vertex id
DELAUNAY_INLINE int getRightNeighbour(Triangle& t, int v)
{
    if (t.vertices[0] == v)
        return t.neighbours[2];
    if (t.vertices[1] == v)
        return t.neighbours[0];
    if (t.vertices[2] == v)
        return t.neighbours[1];

    // should never be reached
    return -1; // v not a vertex of t
}

// get the triangle ID of left neighbour w.r.t to a vertex id
DELAUNAY_INLINE int getLeftNeighbour(Triangle& t, int v)
{
    if (t.vertices[0] == v)
        return t.neighbours[0];
    if (t.vertices[1] == v)
        return t.neighbours[1];
    if (t.vertices[2] == v)
        return t.neighbours[2];

    // should never be reached
    return -1; // v not a vertex of t
}

// ghost triangle adding functions required in ends of a "seam"
DELAUNAY_INLINE int addGhostTriangle(std::vector<Triangle>& triangles, int v1, int v2, int n2, int n3)
{
    int s = triangles.size();
    triangles.emplace_back(-1, n2, n3, v1, v2, -1);
    triangles[n2].neighbours[2] = s;
    triangles[n3].neighbours[1] = s;
    return s;
}

DELAUNAY_INLINE int addGhostTriangle(std::vector<Triangle>& triangles, int v1, int v2, int n1, int n2, int n3)
{
    int s = triangles.size();
    triangles.emplace_back(n1, n2, n3, v1, v2, -1);
    triangles[n2].neighbours[2] = s;
    triangles[n3].neighbours[1] = s;
    return s;
}

template <template <typename, typename> class T_Vector, typename T_Scalar, typename T_Allocator>
void merge(T_Vector<DELAUNAY_VEC, T_Allocator>& points,
    std::vector<Triangle>& triangles, int& firstLeft, int lastLeft, int firstRight, int& lastRight)
{
    // keep track of end vertices in case the end triangles change
    int firstVertex = triangles[firstLeft].vertices[0];
    int lastVertex = triangles[lastRight].vertices[1];

    // find the lower common tangent
    int iterId = 0;
    while (true) {
        int nOps = 0;
        int leftNext = triangles[lastLeft].neighbours[1]; // next ghost triangle on the left mesh boundary
        int rightNext = triangles[firstRight].neighbours[2]; // next ghost triangle on the right mesh boundary
        if (ccw(
            points[triangles[leftNext].vertices[0]],
            points[triangles[leftNext].vertices[1]],
            points[triangles[firstRight].vertices[0]])) {
            lastLeft = leftNext;
            ++nOps;
        }
        if (ccw(
            points[triangles[rightNext].vertices[0]],
            points[triangles[rightNext].vertices[1]],
            points[triangles[lastLeft].vertices[1]])) {
            firstRight = rightNext;
            ++nOps;
        }
        if (nOps == 0) {
            // opposing triangles are right of the edge of both ghost triangles,
            // lower common tangent has been found
            break;
        }
        if (++iterId > points.size()) // maximum number of iterations reached, sign of numerical instability
            break;
    }

    bool leftValid = true;
    bool rightValid = true;
    int baseLeftVertex = triangles[lastLeft].vertices[1];
    int baseRightVertex = triangles[firstRight].vertices[0];

    // add new ghost triangle to the beginning of the seam
    int firstGhost = addGhostTriangle(triangles, baseRightVertex, baseLeftVertex,
        triangles[lastLeft].neighbours[1], triangles[firstRight].neighbours[2]);
    // check if the ghost overrides either of the end triangles - in that case update their indices
    if (triangles[firstGhost].vertices[0] == firstVertex)
        firstLeft = firstGhost;
    if (triangles[firstGhost].vertices[1] == lastVertex)
        lastRight = firstGhost;

    int lastConnectedSide = -1; // required for adding the end ghost, 0: left, 1: right
    int lastConnectedTriangle = -1;

    // merge loop
    iterId = 0;
    while (leftValid || rightValid) {
        auto& baseLeft = points[baseLeftVertex];
        auto& baseRight = points[baseRightVertex];

        // find connective candidates for both sides, delete non-delaunay edges by flipping
        int leftCand = triangles[lastLeft].vertices[0];
        leftValid = ccw(baseLeft, baseRight, points[leftCand]);
        if (leftValid) {
            int leftNeighbour = getRightNeighbour(triangles[lastLeft], baseLeftVertex);
            int leftNeighbourCand = getOpposingVertex(triangles[leftNeighbour], lastLeft);
            while (leftNeighbourCand != -1 &&
                inCircle(baseLeft, baseRight, points[leftCand], points[leftNeighbourCand])) {
                flip(triangles, lastLeft, leftNeighbour);
                lastLeft = leftNeighbour;
                leftCand = leftNeighbourCand;
                leftNeighbour = getRightNeighbour(triangles[lastLeft], baseLeftVertex);
                leftNeighbourCand = getOpposingVertex(triangles[leftNeighbour], lastLeft);
            }
        }

        int rightCand = triangles[firstRight].vertices[1];
        rightValid = ccw(baseLeft, baseRight, points[rightCand]);
        if (rightValid) {
            int rightNeighbour = getLeftNeighbour(triangles[firstRight], baseRightVertex);
            int rightNeighbourCand = getOpposingVertex(triangles[rightNeighbour], firstRight);
            while (rightNeighbourCand != -1 &&
                inCircle(baseLeft, baseRight, points[rightCand], points[rightNeighbourCand])) {
                flip(triangles, firstRight, rightNeighbour);
                rightCand = rightNeighbourCand;
                rightNeighbour = getLeftNeighbour(triangles[firstRight], baseRightVertex);
                rightNeighbourCand = getOpposingVertex(triangles[rightNeighbour], firstRight);
            }
        }

        if (!leftValid && !rightValid)
            break;

        if (!leftValid || (rightValid && inCircle(points[leftCand], baseLeft, baseRight, points[rightCand]))) {
            int rightNext = triangles[firstRight].neighbours[1];

            if (firstGhost != -1) { // update first ghost
                triangles[firstGhost].neighbours[0] = firstRight;
                triangles[firstRight].neighbours[2] = firstGhost;
                firstGhost = -1;
            }
            else { // connect left side triangle in case it was previously added
                if (lastConnectedSide == 0) {
                    triangles[lastConnectedTriangle].neighbours[2] = firstRight;
                    triangles[firstRight].neighbours[2] = lastConnectedTriangle;
                }
            }

            // connect edge
            triangles[firstRight].vertices[2] = baseLeftVertex;
            lastConnectedTriangle = firstRight;
            firstRight = rightNext;
            baseRightVertex = rightCand;

            lastConnectedSide = 1;
        }
        else {
            int leftNext = triangles[lastLeft].neighbours[2];

            if (firstGhost != -1) { // update first ghost
                triangles[firstGhost].neighbours[0] = lastLeft;
                triangles[lastLeft].neighbours[1] = firstGhost;
                firstGhost = -1;
            }
            else { // connect right side triangle in case it was previously added
                if (lastConnectedSide == 1) {
                    triangles[lastConnectedTriangle].neighbours[1] = lastLeft;
                    triangles[lastLeft].neighbours[1] = lastConnectedTriangle;
                }
            }

            // connect edge
            triangles[lastLeft].vertices[2] = baseRightVertex;
            lastConnectedTriangle = lastLeft;
            lastLeft = leftNext;
            baseLeftVertex = leftCand;

            lastConnectedSide = 0;
        }

        if (++iterId > points.size()) // maximum number of iterations reached, sign of numerical instability
            break;
    }

    // add new ghost triangle to the end of the seam
    int lastGhost;
    if (lastConnectedSide == 0) {
        int lastConnect = triangles[lastLeft].neighbours[1];
        lastGhost = addGhostTriangle(triangles, baseLeftVertex, baseRightVertex, lastConnect, firstRight, lastLeft);
        triangles[lastConnect].neighbours[2] = lastGhost;
    }
    else {
        int lastConnect = triangles[firstRight].neighbours[2];
        lastGhost = addGhostTriangle(triangles, baseLeftVertex, baseRightVertex, lastConnect, firstRight, lastLeft);
        triangles[lastConnect].neighbours[1] = lastGhost;
    }

    // check if the ghost overrides either of the end triangles - in that case update their indices
    if (triangles[lastGhost].vertices[0] == firstVertex)
        firstLeft = lastGhost;
    if (triangles[lastGhost].vertices[1] == lastVertex)
        lastRight = lastGhost;
}

template <template <typename, typename> class T_Vector, typename T_Scalar, typename T_Allocator>
void construct(T_Vector<DELAUNAY_VEC, T_Allocator>& points,
    int begin, int end, std::vector<Triangle>& triangles, int& firstTriangle, int& lastTriangle)
{
    int d = end-begin;
    if (d == 2) { // initial primitives, edge
        createEdge(points, triangles, begin, begin+1, firstTriangle, lastTriangle);
    }
    else if (d == 3) { // initial primitives, triangle
        createTriangle(points, triangles, begin, begin+1, begin+2, firstTriangle, lastTriangle);
    }
    else {
        // recursive construction and merge
        auto half = begin+d/2;
        int firstLeft; int lastLeft;
        construct(points, begin, half, triangles, firstLeft, lastLeft);
        int firstRight; int lastRight;
        construct(points, half, end, triangles, firstRight, lastRight);

        merge(points, triangles, firstLeft, lastLeft, firstRight, lastRight);

        firstTriangle = firstLeft;
        lastTriangle = lastRight;
    }
}

} // namespace

template <template <typename, typename> class T_Vector, typename T_Scalar, typename T_Allocator>
std::vector<int32_t> triangulate(T_Vector<DELAUNAY_VEC, T_Allocator>& points)
{
    // sort points primarily along x-axis (and along y-axis in case of equal x)
    std::sort(points.begin(), points.end(), [](
        const DELAUNAY_VEC& a,
        const DELAUNAY_VEC& b){
        return DELAUNAY_VEC_ACCESS(a,0) == DELAUNAY_VEC_ACCESS(b,0) ?
            DELAUNAY_VEC_ACCESS(a,1) < DELAUNAY_VEC_ACCESS(b,1) :
            DELAUNAY_VEC_ACCESS(a,0) < DELAUNAY_VEC_ACCESS(b,0);
    });

    std::vector<Triangle> triangles;
    triangles.reserve(2*points.size());

    int firstTriangle, lastTriangle;
    construct(points, 0, points.size(), triangles, firstTriangle, lastTriangle);

    // list indices into output vector
    std::vector<int32_t> indices;
    indices.reserve(triangles.size()*3);
    for (auto& t : triangles) {
        indices.emplace_back(t.vertices[0]);
        indices.emplace_back(t.vertices[1]);
        indices.emplace_back(t.vertices[2]);
    }
    return indices;
}

} // namespace delaunay

// undefine macros
#undef DELAUNAY_VEC
#undef DELAUNAY_VEC_ACCESS
#undef DELAUNAY_DETERMINANT
#undef DELAUNAY_INLINE

#endif //DELAUNAY_DELAUNAY_HPP


#include <iostream>
#include <vector>
#include <unordered_set>
#include <set>
#include <chrono>
#include <queue>
#include <utility>
#include <new>
#include <algorithm>
#include <stack>
#include <random>
#include <cmath>
#include <fstream>
/*
used for plotting routes
#include <boost/tuple/tuple.hpp>
#include "gnuplot-iostream.h"
*/

// writes a vector of pairs to a given file name
void WriteToFile(std::string file_name, std::vector< std::pair<double, double> > array) {
  std::ofstream fout(file_name);
  for(int i = 0;i < array.size(); i++) {
    fout<<array[i].first<<','<<array[i].second<<'\n';
  }
}

// represents each solution/chromosome
struct State{
  std::vector< std::pair <double,double> > permutation;
  double cost;
  int operator ==(State b) const {
    return (permutation == b.permutation) && (cost == b.cost);
  }
};

// a hash function for random_states
struct StateHash {
    size_t operator()(const State &v) const {
        size_t seed = 0;
      	for(auto t = v.permutation.begin();t != v.permutation.end();t++) {
            size_t h1 = std::hash<double>()((*t).first);
            size_t h2 = std::hash<double>()((*t).second);
            seed ^= (h1 ^ (h2 << 1));
        }
        return seed;
    }
};

// a hash function for random_states used for unordered_set
struct PairHash {
    size_t operator()(const std::pair<double,double> &v) const {
        size_t seed = 0;
        size_t h1 = std::hash<double>()(v.first);
        size_t h2 = std::hash<double>()(v.second);
        seed = (h1 ^ (h2 << 1));
        return seed;
    }
};

// two states are compared by their path cost
bool CompareState(State first, State second) {
    return (first.cost < second.cost);
}
// forward declaring functions
double StateCost(State);

State BeamSearch(State);

State SimulatedAnnealing(State);

State GeneticAlgorithm(State);

//  lists to store best, worst and best of each generation
std::vector<std::pair<double,double>> best_of_generation;

std::vector<std::pair<double,double>> worst_of_generation;

std::vector<std::pair<double,double>> average_of_generation;

// probability bins for rank selection
std::vector< int > pool_probability_bins;

int main() {
  // used for plotting route
  //Gnuplot gp;
  // recieve city Coordinates
  std::vector<std::pair<double, double> > points;
	int n;
	double x,y;
	std::string method;
	State nodes;
	State result;
	std::cout<<"Enter X and Y Coordinates of Initial Node:\n";
  std::cin>>x>>y;
  nodes.permutation.push_back(std::make_pair(x,y));
	std::cout<<"Enter Number of Nodes other than the Initial Node:\n";
	std::cin>>n;
    std::cout<<"Enter X and Y Coordinates of the Nodes:\n";
    for(int i = 0; i < n;i++) {
		std::cin>>x>>y;
		nodes.permutation.push_back(std::make_pair(x,y));
	}
  // create initial state / permutation
	nodes.cost = StateCost(nodes);
  // run the selected method
	std::cout<<"Enter Beam, SA or GA\n";
	std::cin>>method;
  // start timer
  auto start = std::chrono::high_resolution_clock::now();
	if(method == "Beam") {
		result = BeamSearch(nodes);
	}
	if(method == "SA") {
		result = SimulatedAnnealing(nodes);
	}
	if(method == "GA") {
		result = GeneticAlgorithm(nodes);
	}
  // stop timer and print the time it took and the best route cost
	auto finish = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double>elapsed = finish - start;
	std::cout<<"Duration: "<<elapsed.count()<<'\n'<<result.cost;

  // plot best route
  /*
  for(int i = 0;i<result.permutation.size();i++) {
        points.push_back(result.permutation[i]);
	}
	points.push_back(result.permutation[0]);
*/
  //gp << "set xrange [0:1000]\nset yrange [0:100]\n";
	//gp << "plot" << gp.file1d(average_of_generation) << "with lines title 'average',"<<std::endl;
	//gp << "plot" << gp.file1d(best_of_generation) << "with lines title 'best',"<<std::endl;
	//gp << "plot" << gp.file1d(worst_of_generation) << "with lines title 'worst',"<<std::endl;

  // write generation results to files
  //WriteToFile("average_of_generation1000k1000g.txt",average_of_generation);
  //WriteToFile("best_of_generation1000k1000g.txt",best_of_generation);
  //WriteToFile("worst_of_generation1000k1000g.txt",worst_of_generation);
  return 0;
}

int Factorial(int num) {
    int res = 1;
    for(int i = num;i>1;i--) {
        res*=i;
    }
    return res;
}

// computes cost of permutation
double StateCost (State nodes) {
    double cost = 0;
    for(int i = 0;i<nodes.permutation.size()-1;i++) {
        cost += sqrt(pow((nodes.permutation[i].first - nodes.permutation[i+1].first),2) + pow((nodes.permutation[i].second - nodes.permutation[i+1].second),2));
    }
    cost += sqrt(pow((nodes.permutation[0].first - nodes.permutation[nodes.permutation.size()-1].first),2) +
                 pow((nodes.permutation[0].second - nodes.permutation[nodes.permutation.size()-1].second),2));
    return cost;
}

// a binary search to find index/bin which the input belongs to
int FindIndex(int input) {
  int l = 0;
  int r = pool_probability_bins.size()-1;
  int m;
  while(l<r) {
    m = (l+r)/2;
    if(pool_probability_bins[m] == input) {
      break;
    }
    else if(input > pool_probability_bins[m]) {
      if(input < pool_probability_bins[m+1]) {
        break;
      }
      else {
        l = m+1;
      }
    }
    else {
      if(input > pool_probability_bins[m-1]) {
        m--;
        break;
      }
      else {
        r = m-1;
      }
    }
  }
  return m;
}

// creates random permutations and adds greedy state to poputlation
std::vector < State > RandomPopulation(State nodes , int size_of_population = -1) {
	std::unordered_set < State , StateHash > random_states;
	State new_state;
	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	std::default_random_engine generator (seed);
	if(size_of_population == -1) {
        size_of_population = std::min(std::max(1,int(1e5/pow(nodes.permutation.size(),2))),int((nodes.permutation.size()-1 > 9) ? 1e6 : Factorial(nodes.permutation.size()-1)));
	}
	else {
	    size_of_population = std::min(size_of_population,int((nodes.permutation.size()-1 > 9) ? 1e6 : Factorial(nodes.permutation.size()-1)));
	}
	for(int k = 0;k < size_of_population-1;k++) {
	    new_state = nodes;
	    for(int i = 1;i<nodes.permutation.size()-1;i++) {
	        std::uniform_int_distribution<int> distribution(i,nodes.permutation.size()-1);
	        int j = distribution(generator);
	        std::swap(new_state.permutation[i],new_state.permutation[j]);
	    }
	    new_state.cost = StateCost(new_state);
	    random_states.insert(new_state);
  }
  // computing greedy route/permutaion by finding closest city at each step
  std::set < int > visited;
  State greedy;
  visited.insert(0);
  int current = 0;
  greedy.permutation.push_back(nodes.permutation[0]);
  for(int i = 0; i < nodes.permutation.size()-1; i++){
    int closest = -1;
    double min_distnace = 99999;
    for(int j = 0; j < nodes.permutation.size()-1; j++){
      if(!visited.count(j)){
        if(pow((nodes.permutation[current].first - nodes.permutation[j].first),2) +
           pow((nodes.permutation[current].second - nodes.permutation[j].second),2) < min_distnace){
          min_distnace = pow((nodes.permutation[current].first - nodes.permutation[j].first),2) +
                        pow((nodes.permutation[current].second - nodes.permutation[j].second),2);
          closest = j;
         }
      }
    }
    visited.insert(closest);
    greedy.permutation.push_back(nodes.permutation[closest]);
    current = closest;
  }
  greedy.cost = StateCost(greedy);
  std::cerr<<greedy.cost<<'\n';
  std::vector < State > return_states;
  return_states.push_back(greedy);
  for(auto state = random_states.begin();state != random_states.end();state++) {
    return_states.push_back(*state);
  }
	return return_states;
}

// applies HillClimb on given state/permutation
// neighbours are produced by swapping two cities
State HillClimb (State nodes) {
    State min_neighbour;
    bool min_defined = false;
    do{
        if(min_defined == true) {
            nodes = min_neighbour;
            min_defined = false;
        }
        for(int i = 1;i<nodes.permutation.size();i++) {
            for(int j = i+1;j<nodes.permutation.size();j++) {
                State neighbour_state = nodes;
                swap(neighbour_state.permutation[i],neighbour_state.permutation[j]);
                neighbour_state.cost = StateCost(neighbour_state);
                if(min_defined == false) {
                    min_neighbour = neighbour_state;
                    min_defined = true;
                }
                else if(neighbour_state.cost < min_neighbour.cost) {
                    min_neighbour = neighbour_state;
                }
            }
        }
    }while(min_neighbour.cost < nodes.cost);
    return nodes;
}

// missleading name. it basically does HillClimb on each state in RandomPopulation
State BeamSearch(State nodes) {
    std::vector < State > initial_states = RandomPopulation(nodes,1);
    State final_state;
    bool final_defined = false;
    double final_state_cost = 0;
    for(auto state = initial_states.begin();state != initial_states.end();state++) {
        State local_min = HillClimb(*state);
        if(final_defined == false) {
            final_state = local_min;
            final_defined = true;
        }
        else if(local_min.cost < final_state.cost) {
            final_state = local_min;
        }
    }
    return final_state;
}

// a simple mutation function swapping two indexes
State RandomNeighbour(State nodes) {
  // choose two random numbers to swap
	State new_state;
	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	std::default_random_engine generator (seed);
  new_state = nodes;
  std::uniform_int_distribution<int> distribution(1,nodes.permutation.size()-1);
  int j = distribution(generator);
  int i = distribution(generator);
  // swap chosen indexes
  std::swap(new_state.permutation[i],new_state.permutation[j]);
  new_state.cost = StateCost(new_state);
	return new_state;
}

// SimulatedAnnealing
State SimulatedAnnealing(State nodes) {
    double temperature = 400;
    double cooling_factor = 0.99999;
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis(0.0, 1.0);
    while(temperature > 1e-9) {
        State neighbour = RandomNeighbour(nodes);
        if(neighbour.cost < nodes.cost) {
            nodes = neighbour;
        }
        else if(exp((neighbour.cost-nodes.cost)/temperature) < dis(gen)) {
             nodes = neighbour;
        }
        temperature *= cooling_factor;
    }
    return nodes;
}

// uses rank selection to generate breeding_pool
std::vector< State > RankSelection(std::vector< State > population,int kPopulationSize) {
  std::vector<State> breeding_pool;
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator (seed);
  std::uniform_int_distribution<int> distribution(0,(kPopulationSize*(kPopulationSize+1))/2);
  for(int i = 0; i<kPopulationSize;i++) {
    int j = distribution(generator);
    // finds index of chosen number in pool_probability_bins
    int chosen = FindIndex(j);
    breeding_pool.push_back(population[chosen]);
  }
  return breeding_pool;
}

// an order recombination based Cross Over function
State CrossOver(State parent1 ,State parent2) {
  // choose two random points for recombination
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	std::default_random_engine generator (seed);
  std::uniform_int_distribution<int> distribution(1,parent1.permutation.size()-1);
  std::unordered_set < std::pair<double,double> , PairHash > seen;
  int a = distribution(generator);
  int b = distribution(generator);
  // fill offspring with the a-b cut of first parent
  State new_state;
  for(int i = std::min(a,b);i<=std::max(a,b);i++) {
    new_state.permutation.push_back(parent1.permutation[i]);
    seen.insert(parent1.permutation[i]);
  }
  // fill the rest of offspring with rest of second parent
  int k=-1;
  for(int i = 0;i<std::min(a,b);i++) {
    while(seen.count(parent2.permutation[++k])) {}
    new_state.permutation.insert(new_state.permutation.begin(),parent2.permutation[k]);
  }
  for(int i = new_state.permutation.size();i<parent1.permutation.size();i++) {
    while(seen.count(parent2.permutation[++k])) {}
    new_state.permutation.push_back(parent2.permutation[k]);
  }
  new_state.cost = StateCost(new_state);
  return new_state;
}

State GeneticAlgorithm(State nodes) {
  const double kElitePercentage = 0.1;
  const double kMutationRate = 0.01;
  const int kGenerations = 1000;
  const int kPopulationSize = 500;
  std::vector < State > population = RandomPopulation(nodes, kPopulationSize);
  const int kEliteSize = int(kElitePercentage*population.size());
  State min_state;
  min_state.cost = 1000000;
  // rank based selection bins
  pool_probability_bins.push_back(0);
  for(int k = 0;k<kPopulationSize;k++) {
    int temp = pool_probability_bins[k];
    pool_probability_bins.push_back(temp + kPopulationSize - k);
  }

  // execute GeneticAlgorithm with specified number of generations
  for(int k = 0;k<kGenerations;k++) {
    std::vector < State > new_population;
    std::vector < State >  breeding_pool;
    std::vector < State >  offsprings;
    // sort current population
    std::sort(population.begin(),population.end(),CompareState);
    // save best and worst of population
    best_of_generation.push_back(std::make_pair(k, (*(population.begin())).cost));
    worst_of_generation.push_back(std::make_pair(k, (population[population.size()-1]).cost));

    double temp_avg = 0;
    int temp_counter = 1;
    // calculate average fitness of population
    for(auto it = population.begin();it != population.end();it++) {
      temp_avg = temp_avg / temp_counter * (temp_counter-1) + (*it).cost / temp_counter;
      temp_counter++;
    }
    // save average fitness of population
    average_of_generation.push_back(std::make_pair(k, temp_avg));
    // add elites to new population
    for(int i = 0;i<kEliteSize;i++) {
      new_population.push_back(population[i]);
    }
    // get breeding pool with RankSelection
    breeding_pool = RankSelection(population,kPopulationSize);
    for(int t = 0;t<population.size()-kEliteSize;t++) {
      // choose two random states from breeding_pool
      unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
      std::default_random_engine generator (seed);
      std::uniform_int_distribution<int> distribution(0,breeding_pool.size()-1);
      int j = distribution(generator);
      int i = distribution(generator);
      // create offspring of two parents using CrossOver
      State offspring = CrossOver(breeding_pool[i],breeding_pool[j]);
      offsprings.push_back(offspring);
    }
    // mutates an offspring with given probability
    for(auto it = offsprings.begin();it != offsprings.end();it++) {
      std::random_device rd;
      std::mt19937 gen(rd());
      std::uniform_real_distribution<> dis(0.0, 1.0);
      if(dis(gen) < kMutationRate) {
        State mutant = RandomNeighbour(*it);
        new_population.push_back(mutant);
      }
      else {
        new_population.push_back(*it);
      }
    }
    breeding_pool.erase(breeding_pool.begin(),breeding_pool.end());
    for(int it = 0;it < population.size();it++) {
      population[it]=new_population[it];
    }
    new_population.erase(new_population.begin(),new_population.end());
  }
  State result = population[0];
  for(int it = 0;it < population.size();it++) {
    if (result.cost > population[it].cost) {
      result = population[it];
    }
  }
  return result;
}


/**
 * UnscentedKalmanFilterX.hpp
 * @author koide
 * 16/02/01
 **/
#ifndef KKL_UNSCENTED_KALMAN_FILTER_X_HPP
#define KKL_UNSCENTED_KALMAN_FILTER_X_HPP

#include <random>
#include <Eigen/Dense>
#include <kalman/kalman_filter.hpp>

/**
 * @brief Unscented Kalman Filter class
 * @param T        scaler type
 * @param System   system class to be estimated
 */
template<typename T, class System>
class UnscentedKalmanFilterX  : public KalmanFilter<T, System>
{
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXt;    //列向量
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXt;
  using KalmanFilter<T, System>::state_dim;
  using KalmanFilter<T, System>::N;
  using KalmanFilter<T, System>::input_dim;
  using KalmanFilter<T, System>::measurement_dim;
  using KalmanFilter<T, System>::M;
  using KalmanFilter<T, System>::mean;
  using KalmanFilter<T, System>::cov;
  using KalmanFilter<T, System>::system;
  using KalmanFilter<T, System>::process_noise;
  using KalmanFilter<T, System>::measurement_noise;
  using KalmanFilter<T, System>::kalman_gain;
public:
  /**
   * @brief constructor
   * @param system               system to be estimated
   * @param state_dim            state vector dimension
   * @param input_dim            input vector dimension
   * @param measurement_dim      measurement vector dimension
   * @param process_noise        process noise covariance (state_dim x state_dim)
   * @param measurement_noise    measurement noise covariance (measurement_dim x measuremend_dim)
   * @param mean                 initial mean
   * @param cov                  initial covariance
   */
  UnscentedKalmanFilterX(const System& _system, int _state_dim, int _input_dim, int _measurement_dim,
                         const MatrixXt& _process_noise, const MatrixXt& _measurement_noise,
                         const VectorXt& _mean, const MatrixXt& _cov):
    KalmanFilter<T, System>(_system, _state_dim, _input_dim, _measurement_dim, _process_noise, _measurement_noise, _mean, _cov),
    S(2 * state_dim + 1),
    lambda(1)
  {
    weights.resize(S, 1);
    sigma_points.resize(S, N);
    ext_weights.resize(2 * (N + M) + 1, 1);
    ext_sigma_points.resize(2 * (N + M) + 1, N + M);
    expected_measurements.resize(2 * (N + M) + 1, M);

    // initialize weights for unscented filter
    weights[0] = lambda / (N + lambda);
    for (int i = 1; i < 2 * N + 1; i++) {
      weights[i] = 1 / (2 * (N + lambda));
    }

    // weights for extended state space which includes error variances
    ext_weights[0] = lambda / (N + M + lambda);
    for (int i = 1; i < 2 * (N + M) + 1; i++) {
      ext_weights[i] = 1 / (2 * (N + M + lambda));
    }
  }

  /**
   * @brief predict  预测函数
   * @param control  input vector
   */
  virtual void predict(const VectorXt& control) override
  {
    // calculate sigma points
    this->ensurePositiveFinite(cov);
    computeSigmaPoints(mean, cov, sigma_points); //根据上一时刻的均值和方差计算sigma点
    for (int i = 0; i < S; i++) {
      sigma_points.row(i) = system.f(sigma_points.row(i), control); //根据系统方程传播sigma点
    }

    const auto& Q = process_noise; //系统噪声|过程噪声

    // unscented transform
    VectorXt mean_pred(mean.size());
    MatrixXt cov_pred(cov.rows(), cov.cols());

    mean_pred.setZero();
    cov_pred.setZero();
    for (int i = 0; i < S; i++) {
      mean_pred += weights[i] * sigma_points.row(i);   //传播后的sigma点集均值
    }
    for (int i = 0; i < S; i++) {
      VectorXt diff = sigma_points.row(i).transpose() - mean_pred;
      cov_pred += weights[i] * diff * diff.transpose(); //传播后的sigma点集方差
    }
    cov_pred += Q;                                      //加上过程噪声

    //得到预测值和预测协方差
    mean = mean_pred;
    cov = cov_pred;
  }

  /**
   * @brief correct      校正函数
   * @param measurement  观测值
   */
  virtual void correct(const VectorXt& measurement) override
  {
    // create extended state space which includes error variances
    VectorXt ext_mean_pred = VectorXt::Zero(N + M, 1);
    MatrixXt ext_cov_pred = MatrixXt::Zero(N + M, N + M);
    ext_mean_pred.topLeftCorner(N, 1) = VectorXt(mean);
    ext_cov_pred.topLeftCorner(N, N) = MatrixXt(cov);
    ext_cov_pred.bottomRightCorner(M, M) = measurement_noise;

    this->ensurePositiveFinite(ext_cov_pred);
    computeSigmaPoints(ext_mean_pred, ext_cov_pred, ext_sigma_points); //根据预测均值和协方差以及测量噪声计算sigma点
                                                                       //此时测量误差并未添加到sigama主体,而是存放于拓展部分

    // unscented transform
    expected_measurements.setZero();
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      expected_measurements.row(i) = system.h(ext_sigma_points.row(i).transpose().topLeftCorner(N, 1));     //观测方程传播sigama点集
      expected_measurements.row(i) += VectorXt(ext_sigma_points.row(i).transpose().bottomRightCorner(M, 1));//添加测量噪声
    }

    VectorXt expected_measurement_mean = VectorXt::Zero(M);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      expected_measurement_mean += ext_weights[i] * expected_measurements.row(i);  //传播后的sigama点集均值
    }
    MatrixXt expected_measurement_cov = MatrixXt::Zero(M, M);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      VectorXt diff = expected_measurements.row(i).transpose() - expected_measurement_mean;
      expected_measurement_cov += ext_weights[i] * diff * diff.transpose();        //传播后的sigama点集协方差
    }

    // calculated transformed covariance
    MatrixXt sigma = MatrixXt::Zero(N + M, M);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      auto diffA = (ext_sigma_points.row(i).transpose() - ext_mean_pred);
      auto diffB = (expected_measurements.row(i).transpose() - expected_measurement_mean);
      sigma += ext_weights[i] * (diffA * diffB.transpose());
    }

    kalman_gain = sigma * expected_measurement_cov.inverse();                       //计算卡尔曼增益

    VectorXt ext_mean = ext_mean_pred + kalman_gain * (measurement - expected_measurement_mean); //最优估计
    MatrixXt ext_cov = ext_cov_pred - kalman_gain * expected_measurement_cov * kalman_gain.transpose();    //最优估计的协方差

    mean = ext_mean.topLeftCorner(N, 1);
    cov = ext_cov.topLeftCorner(N, N);
  }

  const MatrixXt& getSigmaPoints() const { return sigma_points; }
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  T lambda;
  const int S; //sigma点个数
  VectorXt weights;
  MatrixXt sigma_points;
  VectorXt ext_weights;
  MatrixXt ext_sigma_points;
  MatrixXt expected_measurements;

private:
  /**
   * @brief compute sigma points
   * @param mean          mean
   * @param cov           covariance
   * @param sigma_points  calculated sigma points
   */
  void computeSigmaPoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& sigma_points) {
    const int n = mean.size();
    assert(cov.rows() == n && cov.cols() == n);

    Eigen::LLT<MatrixXt> llt;   //Cholesky分解，将对称正定矩阵表示成一个下三角矩阵L和其转置的乘积的分解 M = L*L.transpose()
    llt.compute((n + lambda) * cov);
    MatrixXt l = llt.matrixL();

    sigma_points.row(0) = mean;
    for (int i = 0; i < n; i++) {
      sigma_points.row(1 + i * 2) = mean + l.col(i);
      sigma_points.row(1 + i * 2 + 1) = mean - l.col(i);
    }
  }
};

#endif


// std includes
#include <vector>
#include <iostream>
#include <random> // random_device, default_random_engine, uniform_real_distribution
#include <memory> // shared_ptr
#include <cmath> // sin, cos
// thirdparty includes
#include <Eigen/Dense>
// lib includes
#include "m0sh/uniform.h"
#include "m0sh/structured_sub.h"
#include "p0l/interpolation.h"

using TypeScalar = double;
// Space
const unsigned int DIM = 2;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
// Mesh
template<typename ...Args>
using TypeContainer = std::vector<Args...>;
using TypeMeshStructured = m0sh::Structured<TypeVector, TypeRef, TypeContainer>;
using TypeMeshStructuredSub = m0sh::StructuredSub<TypeVector, TypeRef, TypeContainer>;
using TypeMeshStructuredUniform = m0sh::Uniform<TypeVector, TypeRef, TypeContainer>;
// Data
const std::size_t np = 5;
const std::size_t n = 100;
const double l = 2 * M_PI;

double f(const double x, const double y) {
    return std::cos(x) + std::sin(y);
}

void print(const std::shared_ptr<TypeMeshStructured>& sMesh, const TypeContainer<TypeScalar> q, std::uniform_real_distribution<TypeScalar>& uniform, std::default_random_engine& e) {
    const TypeVector x = {uniform(e), uniform(e)};
    const double analy = f(x[0], x[1]);
    double interp = p0l::lagrangeMeshPoint<TypeMeshStructured, TypeContainer, double, TypeVector, TypeRef, TypeMeshStructuredSub>(sMesh, q, x, np);
    double error = std::abs(analy - interp);
    double relative = error / analy;
    std::cout << "Interpolation using " << np << " grid points (" << std::pow(np, DIM) << " points)." << " x : \n" << x << "\n Analy : " << analy << " Result : " << interp << " | error = " << error << " | relative = " << relative << std::endl;
}

int main() {
    // Init
    std::shared_ptr<TypeMeshStructured> sMesh = std::make_shared<TypeMeshStructuredUniform>(TypeContainer<std::size_t>(DIM, n), TypeContainer<TypeScalar>(DIM, l), TypeVector::Constant(0.0), TypeContainer<bool>(DIM, true));
    TypeContainer<TypeScalar> q(sMesh->nbPoints());
    for(std::size_t pointIndex = 0; pointIndex < q.size(); pointIndex++) {
        TypeVector x = sMesh->positionPoint(pointIndex);
        q[pointIndex] = f(x[0], x[1]);
    }
    // Random setup
    std::random_device r;
    std::default_random_engine e(r());
    std::uniform_real_distribution<TypeScalar> uniform(0, l);
    // Print
    print(sMesh, q, uniform, e);
    print(sMesh, q, uniform, e);
    print(sMesh, q, uniform, e);
    print(sMesh, q, uniform, e);
    print(sMesh, q, uniform, e);
    print(sMesh, q, uniform, e);
}


// Author: Daisuke Kanaizumi
// Affiliation: Department of Applied Mathematics, Waseda University

// verification program for q-Laguerre polynomials
// References

// Ismail, M. E., & Zhang, R. (2016).
// Integral and Series Representations of $ q $-Polynomials and Functions: Part I.
// arXiv preprint arXiv:1604.08441.
// Y. Chen , M. E. Ismail, & K. A. Muttalib. Asymptotics of basic Bessel functions and q-Laguerre polynomials, Lemma 2
// Journal of Computational and Applied Mathematics, 54(3), 263-272 (1994).

//Koekoek, R., & Swarttouw, R. F. (1996). The Askey-scheme of hypergeometric orthogonal polynomials and its q-analogue. arXiv preprint math/9602214.

#ifndef QLAGUERRE_HPP
#define QLAGUERRE_HPP

#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/constants.hpp>
#include <kv/Pochhammer.hpp>
#include <kv/QHypergeometric.hpp>
#include <cmath>
#include <limits>
#include <kv/convert.hpp> // this was included to use complex numbers
#include <kv/complex.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/psa.hpp>
namespace ub = boost::numeric::ublas;
namespace kv {
  template <class T> complex<interval<T> > qLpoly(const complex<interval<T> >& x,const interval<T>& q,const int n,const complex<interval<T> >& alpha){
    complex<interval<T> >res,sum;
    if(n<=1000){
      sum=0.;
      for(int k=0;k<=n;k++){
	sum=sum+pow(q,alpha*k+k*k)*pow(-x,k)
	  /qPochhammer(q,q,k)/qPochhammer(q,q,n-k)/qPochhammer(pow(q,alpha+1),q,k);
      }
      sum=sum*qPochhammer(pow(q,alpha+1),q,n);
      res=sum;
    }
    else{
      ub::vector<complex<interval<T> > >a(1),b(2);
      a(0)=pow(q,alpha+n+1);
      b(0)=pow(q,alpha+1);
      b(1)=-x*pow(q,n+alpha+1);

      res=qPochhammer(pow(q,alpha+1),q,n)/qPochhammer(q,q,n)
	*infinite_qPochhammer(complex<interval<T> >(-x*pow(q,alpha+n+1)),interval<T>(q))
	*QHypergeom(ub::vector<complex<interval<T> > >(a),ub::vector<complex<interval<T> > >(b),interval<T>(q),complex<interval<T> >(-x*pow(q,alpha+1)));

      //std::cout<<QHypergeom(ub::vector<complex<interval<T> > >(a),ub::vector<complex<interval<T> > >(b),interval<T>(q),complex<interval<T> >(-x*pow(q,alpha+1)))<<std::endl;
    }
    if((abs(res)).upper()==std::numeric_limits<T>::infinity()){
      res=infinite_qPochhammer(complex<interval<T> >(-x*pow(q,n+alpha+1)),interval<T> (q))
	/infinite_qPochhammer(complex<interval<T> >(pow(q,n+alpha+1)),interval<T> (q))/qPochhammer(q,q,n)
	*_1phi_1(complex<interval<T> >(-x),complex<interval<T> >(-x*pow(q,n+alpha+1)),interval<T>(q),complex<interval<T> >(pow(q,alpha+1)));
    }
    return res;
  }
  template <class T> interval<T>  qLpoly(const interval<T> & x,const interval<T>& q,const int n,const interval<T> & alpha){
    interval<T> res,sum;
    if(n<=1000){
      sum=0.;
      for(int k=0;k<=n;k++){
	sum=sum+pow(q,alpha*k+k*k)*pow(-x,k)
	  /qPochhammer(q,q,k)/qPochhammer(q,q,n-k)/qPochhammer(pow(q,alpha+1),q,k);
      }
      sum=sum*qPochhammer(pow(q,alpha+1),q,n);
      res=sum;
    }
    else{
      ub::vector<interval<T>  >a(1),b(2);
      a(0)=pow(q,alpha+n+1);
      b(0)=pow(q,alpha+1);
      b(1)=-x*pow(q,n+alpha+1);

      res=qPochhammer(pow(q,alpha+1),q,n)/qPochhammer(q,q,n)
	*infinite_qPochhammer(interval<T> (-x*pow(q,alpha+n+1)),interval<T>(q))
	*QHypergeom(ub::vector<interval<T>  >(a),ub::vector<interval<T>  >(b),interval<T>(q),interval<T> (-x*pow(q,alpha+1)));

      //std::cout<<QHypergeom(ub::vector<complex<interval<T> > >(a),ub::vector<complex<interval<T> > >(b),interval<T>(q),complex<interval<T> >(-x*pow(q,alpha+1)))<<std::endl;
    }
    if((abs(res)).upper()==std::numeric_limits<T>::infinity()){
      res=infinite_qPochhammer(interval<T> (-x*pow(q,n+alpha+1)),interval<T> (q))
	/infinite_qPochhammer(interval<T> (pow(q,n+alpha+1)),interval<T> (q))/qPochhammer(q,q,n)
	*_1phi_1(interval<T> (-x),interval<T> (-x*pow(q,n+alpha+1)),interval<T>(q),interval<T> (pow(q,alpha+1)));
    }
    return res;
  }
  template <class T> interval<T>  qLpoly_psa(const interval<T> & x,const interval<T>& q,const int n,const interval<T> & alpha){
    interval<T> res;

    kv::psa<interval<T> >a,b,c;
    a.v.resize(n+1);
    b.v.resize(n+1);
    c.v.resize(n+1);
    for(int i=0;i<=n;i++){
      a.v(i)=qPochhammer(-x,q,i)*std::pow(-1,i)*pow(q,i*(i+1)*0.5)*pow(q,alpha*i)/qPochhammer(q,q,i);
    }
    for(int j=0;j<=n;j++){
      b.v(j)=std::pow(-1,j)*pow(q,j*(j-1)*0.5)/qPochhammer(q,q,j);
    }
    c=a/b;
    res=c.v(n);
    return res;
}
  template <class T> interval<T>  qLpoly_rec(const interval<T> & x,const interval<T>& q,const int n,const interval<T> & alpha){
    interval<T> res,xx;
    xx=mid(x);
    ub::vector<interval<T>  >L(2*n);
    L(0)=1.;
    L(1)=(1-pow(q,alpha+1))*(1+pow(q,alpha+1)/(1-pow(q,alpha+1))*(-x))/(1-q);
    for(int i=1;i<=n-1;i++){
      L(i+1)=((1-pow(q,i+1)+q*(1-pow(q,i+alpha))-pow(q,2*i+alpha+1)*x)*L(i)-q*(1-pow(q,i+alpha))*L(i-1))/(1-pow(q,i+1));
    }
  res=L(n);
  return res;
  }
}
#endif


#ifndef SOPHUS_ROTATION_MATRIX_HPP
#define SOPHUS_ROTATION_MATRIX_HPP

#include <Eigen/Dense>
#include <Eigen/SVD>

#include "types.hpp"

namespace Sophus {

// Takes in arbiray square matrix and returns true if it is
// orthogonal.
template <class D>
SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> const& R) {
  using Scalar = typename D::Scalar;
  static int const N = D::RowsAtCompileTime;
  static int const M = D::ColsAtCompileTime;

  static_assert(N == M, "must be a square matrix");
  static_assert(N >= 2, "must have compile time dimension >= 2");

  return (R * R.transpose() - Matrix<Scalar, N, N>::Identity()).norm() <
         Constants<Scalar>::epsilon();
}

// Takes in arbiray square matrix and returns true if it is
// "scaled-orthogonal" with positive determinant.
//
template <class D>
SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> const& sR) {
  using Scalar = typename D::Scalar;
  static int const N = D::RowsAtCompileTime;
  static int const M = D::ColsAtCompileTime;
  using std::pow;
  using std::sqrt;

  Scalar det = sR.determinant();

  if (det <= Scalar(0)) {
    return false;
  }

  Scalar scale_sqr = pow(det, Scalar(2. / N));

  static_assert(N == M, "must be a square matrix");
  static_assert(N >= 2, "must have compile time dimension >= 2");

  return (sR * sR.transpose() - scale_sqr * Matrix<Scalar, N, N>::Identity())
             .template lpNorm<Eigen::Infinity>() <
         sqrt(Constants<Scalar>::epsilon());
}

// Takes in arbiray square matrix (2x2 or larger) and returns closest
// orthogonal matrix with positive determinant.
template <class D>
SOPHUS_FUNC enable_if_t<
    std::is_floating_point<typename D::Scalar>::value,
    Matrix<typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime>>
makeRotationMatrix(Eigen::MatrixBase<D> const& R) {
  using Scalar = typename D::Scalar;
  static int const N = D::RowsAtCompileTime;
  static int const M = D::ColsAtCompileTime;

  static_assert(N == M, "must be a square matrix");
  static_assert(N >= 2, "must have compile time dimension >= 2");

  Eigen::JacobiSVD<Matrix<Scalar, N, N>> svd(
      R, Eigen::ComputeFullU | Eigen::ComputeFullV);

  // Determine determinant of orthogonal matrix U*V'.
  Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant();
  // Starting from the identity matrix D, set the last entry to d (+1 or
  // -1),  so that det(U*D*V') = 1.
  Matrix<Scalar, N, N> Diag = Matrix<Scalar, N, N>::Identity();
  Diag(N - 1, N - 1) = d;
  return svd.matrixU() * Diag * svd.matrixV().transpose();
}

}  // namespace Sophus

#endif  // SOPHUS_ROTATION_MATRIX_HPP


#include <iostream>
#include <functional>

#include <Eigen/Dense>
#include <EigenRand/EigenRand>

using namespace std;
using namespace Eigen;

int main()
{
	Rand::Vmt19937_64 urng{4212};
	Vector4f mean{0, 1, 2, 3};
	Matrix4f cov;
	cov << 1, 1, 0, 0,
		1, 2, 0, 0,
		0, 0, 3, 1,
		0, 0, 1, 2;

	// constructs MvNormalGen with Scalar=float, Dim=4
	Rand::MvNormalGen<float, 4> gen1{mean, cov};

	// or you can use `make-` helper function. It can deduce the type of generator to be created.
	auto gen2 = Rand::makeMvNormGen(mean, cov);

	// generates one sample ( shape (4, 1) )
	Vector4f sample = gen1.generate(urng);

	// generates 10 samples ( shape (4, 10) )
	MatrixXf samples = gen1.generate(urng, 10);
	// or you can just use `MatrixXf` type

	cout << sample << endl;
	cout << samples << endl;

	Rand::StdNormalGen<float> stdnorm;
	for (int i = 0; i < 10; ++i)
	{
		cout << stdnorm.generate<Matrix<float, 4, -1>>(4, 1, urng) << endl;
	};
};


/*--
  Open3DMotion
  Copyright (c) 2004-2012.
  All rights reserved.
  See LICENSE.txt for more information.
--*/

#include "Open3DMotion/Maths/Matrix3x3.h"

#ifndef OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN
extern "C"
{
#include <f2clibs/f2c.h>
#include <clapack.h>
}
#else
#include <Eigen/Dense>
#include <Eigen/SVD>
#endif

namespace Open3DMotion
{
	void Matrix3x3::SVD(double* U, double* s, double* VT, const double* A)
  {
#ifndef OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN
    long three(3);
    Matrix3x3 Acpy(A);
    long lwork(256);
    double work[256];
    long info(0);

    // use lapack routine
    // - note U and VT are swapped
    // - this is because of the fortran column-major
    //   ordering for matrices - it turns out that
    //   using a row major matrix here corresponds to
    //   swapping U and VT
    dgesvd_(
      "A",  // all of U
      "A",  // all of VT
      &three, // rows
      &three, // cols
      Acpy,   // input/output matrix
      &three, // leading dimension of Acpy
      s,      // singular values
      VT,      // left orthonormal matrix
      &three, // leading dimension of left
      U,      // right orthonormal matrix
      &three, // leading dimension of right
      work,   // workspace
      &lwork, // size of workspace
      &info);   // returned error codes

#else
		Eigen::Map< const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > _A(A, 3, 3);
    Eigen::Map< Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > _U(U, 3, 3);
    Eigen::Map< Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > _VT(VT, 3, 3);
    Eigen::Map< Eigen::Matrix<double, 3, 1> > _s(s, 3, 1);
    Eigen::JacobiSVD< Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > svd(_A, Eigen::ComputeFullU | Eigen::ComputeFullV);
    _U = svd.matrixU();
    _VT = svd.matrixV().transpose();
    _s = svd.singularValues();
#endif

  }
}


#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <cstdlib>
#include <ctime>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <cfloat>

#include "time_bench.hpp"

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

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

template <class T>
inline double norm(ublas::vector<T> v){
  /* ベクトルのノルムを計算する */
  double sum = 0;
  for(unsigned int i = 0; i < v.size(); ++i){
    sum += v[i] * v[i];
  }
  return sqrt(sum);
}

template <class T>
inline double dist(ublas::vector<T> x, ublas::vector<T> y){
  /* ベクトルのノルムから距離を定義 */
  ublas::vector<T> v = x - y;
  return norm(v);
}

inline double dist(ublas::matrix<double> mat1, unsigned int r1,
		   ublas::matrix<double> mat2, unsigned int r2){
  /* mat1のr1行目のベクトルと, mat2のr2行目のベクトルの距離を計算 */
  if(mat1.size2() != mat2.size2()){
    return -1;
  }else{
    double sum = 0;
    for(unsigned int i = 0; i < mat1.size2(); ++i){
      sum += (mat1(r1, i) - mat2(r2, i)) * (mat1(r1, i) - mat2(r2, i));
    }
    return sqrt(sum);
  }
}

template <class T>
inline unsigned int argmin(ublas::vector<T> v){
  if(v.empty()){ return -1; /* エラー */
  }else{
    int argmin = 0; T minval = v[argmin];
    for(unsigned int i = 0; i < v.size(); ++i){
      if(v[i] < minval){
	argmin = i; minval = v[i];
      }
    }
    return argmin;
  }
}

template <class T>
inline unsigned int argmax(ublas::vector<T> v){
  if(v.empty()){ return -1; /* エラー */
  }else{
    int argmax = 0; T maxval = v[argmax];
    for(unsigned int i = 0; i < v.size(); ++i){
      if(v[i] > maxval){
	argmax = i; maxval = v[i];
      }
    }
    return argmax;
  }
}

template <class T>
inline T min(ublas::vector<T> v){
  if(v.empty()){ return -1; /* エラー */
  }else{
    T minval = v[0];
    for(unsigned int i = 0; i < v.size(); ++i){
      if(v.at(i) < minval){
	minval = v.at(i);
      }
    }
    return minval;
  }
}

template <class T>
inline T min2(ublas::vector<T> v){
  /* vector vの要素で2番目に小さい要素を見つける */
  if(v.size() < 2){ return -1; /* エラー */
  }else{
    T min1 = v[0], min2 = v[1];
    if(min1 > min2){ /* swap する */
      T temp = min1; min1 = min2; min2 = temp;
    }
    for(unsigned int i = 2; i < v.size(); ++i){
      if(v[i] <= min1){       /* 最小値が発見された */
	min2 = min1; min1 = v[i];
      }else if(v[i] <= min2){ /* 準最小値が発見された */
	min2 = v[i];
      }
    }
    return min2;
  }
}
inline void Hamerly_update_ul(const unsigned int n, const unsigned int k,
			      const ublas::matrix <double> data,
			      const ublas::matrix <double> c,       /* クラスタの重心 */
			      const ublas::vector <unsigned int> a, /* 各点x_iが属するクラスタ */
			      ublas::vector<double> &u,            /* upper bound */
			      ublas::vector<double> &l             /* lower bound */){
  ublas::vector<double> dist_xc(k, 0); /* x[i] とクラスター中心との距離*/
  for(unsigned int i = 0; i < n; ++i){
    for(unsigned int j = 0; j < k; ++j){
      dist_xc[j] = dist(data, i, c, j);
    }
    u[i] = dist_xc[a[i]]; /* u[i] = d(x[i], c[a[i]])*/
    l[i] = min2(dist_xc); /* l[i] = min2_j d(x[i], c[j]) */
    /* lはx[i]から2番目に近いクラスタ重心までの距離 */
    //cerr << i << ": " << u[i] << "\t" << l[i] << "\t" << dist_xc << endl;
  }
  //cerr << u << endl;
  //cerr << l << endl;
  return;
}

bool Hamerly_repeat(const unsigned int n, const unsigned int d, const unsigned int k,
		    const ublas::matrix <double> data,
		    ublas::vector<unsigned int> &q, /* クラスタ中の点の数 */
		    ublas::matrix<double> &c,       /* クラスタの重心 */
		    ublas::matrix<double> &c_sum,   /* クラスタのベクトル和*/
		    ublas::vector<double> &s,       /* 最近接クラスタの重心との距離 */
		    ublas::vector<unsigned int> &a, /* 各点x_iが属するクラスタ */
		    ublas::vector<double> &u,       /* upper bound */
		    ublas::vector<double> &l        /* lower bound */){
  /* Hamerlyの繰り返しステップ クラスタ重心の更新の有無をbooleanで返す */
  /* この繰り返しステップで何らかの更新があったか */
  volatile bool updated = false;

  /* クラスター中心の変化があったか */
  ublas::vector<bool> cluster_unchanged(k, true);

  /* まずs[j]を更新
   * s[j] = min_{jj != j} dist(c[jj], c[j])
   *      = min2_jj dist(c[j], c[jj]) */
  for(unsigned int j = 0; j < k; ++j){
    ublas::vector<double> distance(k, 0);
    for(unsigned int jj = 0; jj < k; ++jj){
      distance[jj] = dist(c, j, c, jj);
    }
    s[j] = min2(distance);
  }

  //cerr << "s: " << s << endl;

  /* 各点x[i]について，Hamerlyの命題の条件をみたすか確認しながら
   * (つまり，適宜枝刈りを実行しながら)
   * 最近接クラスタの再割当てを行う
   * 同時に，のちのクラスタ重心の更新のために
   * c_sum[], q[] を正しい値に保つ */
  for(unsigned int i = 0; i < n; ++i){
    double m = max((s[a[i]] / 2.0), l[i]);
    //cerr << "i = " << i << ", m = " << m << ", u[i] = " << u[i] << endl;
    if(u[i] > m){     /* Hamerlyの命題の条件を満たさない */
      //cerr << i << u[i] << " -> ";
      u[i] = dist(data,i,c,a[i]);  /* upper boundを厳密な値に更新 */
      //cerr << u[i] << endl;
      if(u[i] > m){   /* Hamerlyの命題の条件を満たさない */
	unsigned int aa = a[i]; /* 最近接クラスタが変化したか調べる */
	{ /* 最近接クラスタを再計算 */
	  ublas::vector<double> distance(k);
	  for(unsigned int j = 0; j < k; ++j){
	    distance[j] = dist(data,i,c,j);
	  }
	  a[i] = argmin(distance);
	}
	if(aa != a[i]){ /* 最近接クラスタの変化によるパラメタの更新*/
	  /* 点x[i]の最近接クラスタは，aa から a[i] に変化した */
	  updated = true; /* この繰り返しステップ更新があった */
	  /* 後のクラスター重心の更新のために q, c_sum の更新が必要
	   * 同時に、更新があったクラスターのindexをメモしておく */
	  row(c_sum,aa)   -= row(data,i); q[aa]--;
	  row(c_sum,a[i]) += row(data,i); q[a[i]]++;
	  cluster_unchanged[aa] = false;
	  cluster_unchanged[a[i]] = false;
	  {
	    ublas::vector<double> dist_xc(k, 0);
	    /* x[i] とクラスター中心との距離*/
	    for(unsigned int j = 0; j < k; ++j){
	      dist_xc[j] = dist(data, i, c, j);
	    }
	    u[i] = dist_xc[a[i]]; /* u[i] = d(x[i], c[a[i]])*/
	    l[i] = min2(dist_xc); /* l[i] = min2_j d(x[i], c[j]) */
	  }
	}
      }
    }
  }

  ublas::matrix<double> c_prev = c; /* 変化前のクラスタ重心 */
  if(updated){ /* いずれかのデータ点の最近接クラスタが変化した場合 */
    ublas::vector<double> p(k, 0); /* 各クラスタの重心の移動距離 */
    for(unsigned int j = 0; j < k; ++j){
      if(!cluster_unchanged[j]){ /* j番目のクラスタ重心が移動していた場合 */
	row(c,j) = row(c_sum,j) / q[j]; /* クラスタ重心を更新 */
	p[j] = dist(c_prev, j, c, j); /* クラスター重心の移動距離 */
      }
    }
    unsigned int r = argmax(p);
    for(unsigned int i = 0; i < n; ++i){
      u[i] += p[a[i]];  l[i] -= p[r];
    }
  }

  return updated; /* データ点の最近接クラスタに変化があったかを返す */
}

void Hamerly_init(const unsigned int n, const unsigned int d, const unsigned int k,
		  const ublas::matrix <double> data,
		  ublas::vector<unsigned int> &q,   /* クラスタ中の点の数 */
		  ublas::matrix<double> &c,         /* クラスタの重心 */
		  ublas::matrix<double> &c_sum,     /* クラスタのベクトル和*/
		  ublas::vector<unsigned int> &a,   /* 各点x_iが属するクラスタ */
		  ublas::vector<double> &u,         /* upper bound */
		  ublas::vector<double> &l          /* lower bound */){
  /* 各クラスタの代表点をランダムに選択 */
  for(unsigned int j = 0; j < k; ++j){
    ublas::zero_vector<double> zero(d);
    row(c,j) = row(data, j);
    /* j番目のクラスタ中心をj番目のデータ点と一致させて初期化した */
  }

  /* 全てのデータ点について，初期クラスタa[i]を計算 */
  for(unsigned int i = 0; i < n; ++i){
    { /* argmin_j dist(x(i), c(j))の計算 */
      ublas::vector<double> distance(k);
      for(unsigned int j = 0; j < k; ++j){
	distance[j] = dist(data,i,c,j);
      }
      a[i] = argmin(distance);
    }
    /* クラスタ内の点の数，クラスタの点のベクトル和も更新 */
    q[a[i]]++;  row(c_sum,a[i]) += row(data,i);
  }

  /* upper bound, lower bound の更新 */
  Hamerly_update_ul(n, k, data, c, a, u, l);

  /* 初期クラスタ中心に対するデータ割り当てに対応して，
   * クラスター中心の更新や，各データ点のu[i],l[i]の更新が必要 */

  ublas::matrix<double> c_prev = c; /* 変化前のクラスタ重心 */

  ublas::vector<double> p(k, 0); /* 各クラスタの重心の移動距離 */
  for(unsigned int j = 0; j < k; ++j){
    row(c,j) = row(c_sum,j) / q[j]; /* クラスタ重心を更新 */
    p[j] = dist(c_prev, j, c, j); /* クラスター重心の移動距離 */
  }
  unsigned int r = argmax(p);

  for(unsigned int i = 0; i < n; ++i){
    u[i] += p[a[i]];  l[i] -= p[r];
  }

  return;
}

double Hamerly_sqerr(const unsigned int n, const unsigned int k,
		     const ublas::matrix<double>data,
		     const ublas::matrix<double> c,       /* クラスタの重心 */
		     const ublas::vector<unsigned int> a  /* 各点x_iが属するクラスタ */){
  /* クラスタリング結果に基づき二乗平均二乗誤差を計算して返す */
  double sum = 0;
  for(unsigned int i = 0; i < n; ++i){
    sum += dist(data,i,c,a[i]) * dist(data,i,c,a[i]);
  }
  return (sum / n);
}

void Hamerly(const unsigned int n, const unsigned int d, const unsigned int k,
	     const ublas::matrix<double> data){
  if(n < k){
    cerr << "data size n is smaller than cluster size d" << endl;
    exit(1);
  }else{
    struct timeval t_start, t_end;
    gettimeofday(&t_start, NULL); /* 時間計測開始 */

    /* j番目のクラスタを主語とする記号  */
    ublas::vector<unsigned int> q(k,0); /* クラスタ中の点の数 */
    ublas::matrix<double> c(k,d,0);     /* クラスタの重心 */
    ublas::matrix<double> c_sum(k,d,0); /* クラスタ中のベクトルの総和 */
    ublas::vector<double> s(k,0);       /* 最近接クラスタの重心との距離 */

    /* i番目のデータ点を主語とする記号 */
    ublas::vector<unsigned int> a(n, 0);  /* 各点x_iが属するクラスタ */
    ublas::vector<double> u(n, 0);        /* upper bound */
    ublas::vector<double> l(n, 0);        /* lower bound */

    Hamerly_init(n, d, k, data, q, c, c_sum, a, u, l);

    int t = 0; /* 繰り返しステップを何回繰り返したか */

    //cerr << "a: " << a << ": t = " << t << endl;

    while(Hamerly_repeat(n, d, k, data, q, c, c_sum, s, a, u, l)){
      t++;
      //cerr << "a: " << a << ": t = " << t << endl;
    }
    t++; /* 更新されなかった最後の1回も実行されている */

    gettimeofday(&t_end, NULL); /* 時間計測終了 */

    { /* 結果を出力 */
      ofstream fs("Hamerly_"
		  + std::to_string(n) + "_"
		  + std::to_string(d) + "_"
		  + std::to_string(k) + ".txt");
      if(fs.fail()){ exit(1); }
      fs << "a: " << a << endl;
      fs << c << endl;
      fs.close();
    }

    cout << n << "\t"                               /* サンプル数 */
	 << d << "\t"                               /* 次元 */
      	 << k << "\t"                               /* クラスタ数 */
	 << diff_timeval(t_start, t_end) << "\t"    /* 実行時間(us) */
	 << Hamerly_sqerr(n, k, data, c, a) << "\t" /* 平均二乗誤差 */
	 << t << endl;                              /* 繰り返し回数*/

    return;
  }
}

int main(int argc, char *argv[]){
  if(argc < 5){
    cerr << "usage: " << argv[0]
	 << " <n: data num> <d: dimension> <k: cluster num> <data file>" << endl;
    exit(1);
  }else{
    unsigned int n = atoi(argv[1]), d = atoi(argv[2]), k = atoi(argv[3]);
    char *file = argv[4];
    ublas::matrix<double> data(n, d); /* n個のd次元データを読み込む */
    {
      ifstream fs(file);
      if(fs.fail()){ exit(1); }
      for(unsigned int i = 0; i < n; ++i){
	for(unsigned int j = 0; j < d; ++j){
	  fs >> data(i,j);
	}
      }
      fs.close();
    }
    Hamerly(n, d, k, data);
    return 0;
  }
}


#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

int main() {
    //
    // check multiprecision/doc/html/boost_multiprecision/tut/ints/cpp_int.html
    // for checked & unchecked types
    //

    // integer number type
    using boost::multiprecision::cpp_int;   // unlimited precision
    using boost::multiprecision::int1024_t;
    using boost::multiprecision::uint1024_t;

    using boost::multiprecision::checked_cpp_int;
    using boost::multiprecision::checked_int1024_t;
    using boost::multiprecision::checked_uint1024_t;

    // rational number type
    using boost::multiprecision::cpp_rational;

    // floating point number type
    using boost::multiprecision::cpp_dec_float_100;
    using cpp_dec_float_200 = boost::multiprecision::number < boost::multiprecision::cpp_dec_float < 200 > > ;

    //
    // real code below
    //

    // calculate factorial of 50
    cpp_int factorial{ 1 };
    // cpp_int factorial{ "1" }; also works
    for (cpp_int i{ 1 }; i <= 50; ++i)
        factorial *= i;

    std::cout << "Factorial of 50 is\n\t" << factorial << "\n\n";

    cpp_rational rational{ cpp_int{ 321 }, cpp_int{ 123 } };
    // cpp_rational rational{ "321/123" }; also works
    rational *= 10;
    // print 50 numbers of rational
    std::cout << rational << " is \n\t"
        << rational.convert_to<cpp_dec_float_100>().str(50) << "\n\n";

    // calculate square root of 2
    std::cout << "sqrt(2)\n\t"
        << std::setprecision(std::numeric_limits<cpp_dec_float_200>::digits10)
        << boost::multiprecision::sqrt(cpp_dec_float_200{ 2 });
}


/*
 * solvers.cpp
 *
 *  Created on: Sep 29, 2016
 *      Author: george
 */

#include "solvers.hpp"

#include <gecode/int.hh>
#include <gecode/float.hh>
#include <gecode/search.hh>
#include <gecode/gist.hh>

#include <boost/numeric/ublas/io.hpp>

using namespace Gecode;

class LinearEqSysSolver : public Space {
protected:
  IntVarArray x;
public:
  LinearEqSysSolver(
      const std::vector<std::vector<int>>& A,
      const std::vector<int>& y) throw (InvalidArgumentException)
    : x(*this, y.size(), 1, Int::Limits::max) {

    const size_t n_actors = y.size();
    const size_t n_channels = A.size();

    for (size_t i=0; i<n_channels; i++ ) {
      IntArgs c(n_actors);
      IntVarArgs xs(n_actors);
      for (size_t j=0; j<n_actors; j++ ) {
        c[j]  = A[i][j];
        xs[j] = x[j];
      }

      linear(*this, c, xs, IRT_EQ, y[i]);
    }
    branch(*this, x, INT_VAR_SIZE_MIN(), INT_VAL_MIN());
  }

  LinearEqSysSolver(bool share, LinearEqSysSolver& s) : Space(share, s) {
    x.update(*this, share, s.x);
  }

  virtual Space* copy(bool share) {
    return new LinearEqSysSolver(share,*this);
  }

  void print(void) const {
    std::cout << x << std::endl;
  }

  vector<int> getSolution(void) const {
    vector<int> tmp_sol;
    for (int i=0; i<x.size(); i++) {
      //if x[i].assigned()
      tmp_sol.push_back(x[i].val());
      //...else something is wrong
    }
    return tmp_sol;
  }

  // constrain function
  virtual void constrain(const Space& _b) {
    const LinearEqSysSolver& b = static_cast<const LinearEqSysSolver&>(_b);

    int sum = 0;
    IntVarArgs xs(b.x.size());
    for (int i=0; i<b.x.size(); i++) {
      sum += b.x[i].val();
      xs[i] = x[i];
    }
    linear(*this, xs, IRT_LE, sum);
  }
};

class FloatSysSolver : public Space {
protected:
  FloatVarArray x;
public:
  FloatSysSolver(
      const std::vector<std::vector<int>>& A,
      const std::vector<int>& y) throw (InvalidArgumentException)
    : x(*this, y.size(), 1, Float::Limits::max) {

    const size_t n_actors = y.size();
    const size_t n_channels = A.size();

    for (size_t i=0; i<n_channels; i++ ) {
      FloatValArgs c(n_actors);
      FloatVarArgs xs(n_actors);
      for (size_t j=0; j<n_actors; j++ ) {
        c[j]  = A[i][j];
        xs[j] = x[j];
      }

      linear(*this, c, xs, FRT_EQ, y[i]);
    }
    branch(*this, x, FLOAT_VAR_SIZE_MIN(), FLOAT_VAL_SPLIT_MIN());
  }

  FloatSysSolver(bool share, FloatSysSolver& s) : Space(share, s) {
    x.update(*this, share, s.x);
  }

  virtual Space* copy(bool share) {
    return new FloatSysSolver(share,*this);
  }

  void print(void) const {
    std::cout << x << std::endl;
  }

  vector<double> getSolution(void) const {
    vector<double> tmp_sol;
    for (int i=0; i<x.size(); i++) {
      //if x[i].assigned()
      tmp_sol.push_back(x[i].val().min());
      //...else something is wrong
    }
    return tmp_sol;
  }

  // constrain function
  virtual void constrain(const Space& _b) {
    const FloatSysSolver& b = static_cast<const FloatSysSolver&>(_b);

    double sum = 0;
    FloatVarArgs xs(b.x.size());
    for (int i=0; i<b.x.size(); i++) {
      sum += b.x[i].val().max();
      xs[i] = x[i];
    }
    linear(*this, xs, FRT_LE, sum);
  }
};

vector<int> tools::solveLinearEqSys(
    const std::vector<std::vector<int>>& A,
    const std::vector<int>& y) throw (InvalidArgumentException) {

  LinearEqSysSolver* m = new LinearEqSysSolver(A,y);
  //Gist::bab(m);
  BAB<LinearEqSysSolver> e(m);
  delete m;

  std::vector<int> solutions;
  while (LinearEqSysSolver* s = e.next()) {
    s->print();
    solutions = s->getSolution();
    delete s;
  }
  //if solutions.size() = 0, SDFG is probably not consistent!
  return solutions;
}

vector<double> tools::solveFloatSys(
    const std::vector<std::vector<int>>& A,
    const std::vector<int>& y) throw (InvalidArgumentException) {

  FloatSysSolver* m = new FloatSysSolver(A,y);
  //Gist::bab(m);
  BAB<FloatSysSolver> e(m);
  delete m;

  std::vector<double> solutions;
  while (FloatSysSolver* s = e.next()) {
    s->print();
    solutions = s->getSolution();
    delete s;
  }
  //if solutions.size() = 0, SDFG is probably not consistent!
  return solutions;
}


// Copyright (c) 2017 Evan S Weinberg
// Test code for a real operator.

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>
#include <sstream>
#include <complex>
#include <random>

// Borrow dense matrix eigenvalue routines.
#include <Eigen/Dense>

#include "blas/generic_vector.h"

#include "square_laplace.h"

using namespace std;
using namespace Eigen;

typedef Matrix<std::complex<double>, Dynamic, Dynamic, ColMajor> cMatrix;
typedef Matrix<double, Dynamic, Dynamic, ColMajor> dMatrix;

int main(int argc, char** argv)
{
  double *rhs_real;
  double *rhs_real_indef;
  complex<double> *rhs_cplx;
  complex<double> *rhs_cplx_indef;
  complex<double> *gauge_links;

  // Set output precision to be long.
  cout << setprecision(10);

  // RNG related things.
  std::mt19937 generator (1337u); // RNG, 1337u is the seed.
  double inv_variance = 6.0; // inverse of variance for gaussian non-compact U(1) links.

  // Basic information about the lattice.
  int length = 8;
  double m_sq = 0.001;

  // Some start-up.
  int volume = length*length;
  int wilson_volume = 2*volume;

  // Create a random compact U(1) link.
  gauge_links = allocate_vector<complex<double>>(2*length*length);
  gaussian_real(gauge_links, 2*length*length, generator, 1.0/inv_variance);
  polar(gauge_links, 2*length*length);

  // Vectors.
  rhs_real = allocate_vector<double>(volume);
  rhs_real_indef = allocate_vector<double>(volume);
  rhs_cplx = allocate_vector<complex<double>>(volume);
  rhs_cplx_indef = allocate_vector<complex<double>>(wilson_volume);

  // Zero out the vector.
  zero_vector(rhs_real, length*length);
  zero_vector(rhs_real_indef, length*length);
  zero_vector(rhs_cplx, length*length);
  zero_vector(rhs_cplx_indef, 2*length*length);

  //////////////////////////
  // REAL, SYMMETRIC CASE //
  //////////////////////////

  std::cout << "Real, Symmetric case.\n\n";

  // Structure which gets passed to the function.
  laplace_struct lapstr;
  lapstr.length = length;
  lapstr.m_sq = m_sq;

  // Allocate a sufficiently gigantic matrix.
  dMatrix mat_real = dMatrix::Zero(volume, volume);

  // Form matrix elements. This is where it's important that
  // dMatrix and cMatrix are column major.
  // I should probably make this safer by using a "Map".
  for (int i = 0; i < volume; i++)
  {
    // Set a point on the rhs for a matrix element.
    zero_vector(rhs_real, volume);
    rhs_real[i] = 1.0;

    // Where we put the result of the matrix element.
    double* mptr = &(mat_real(i*volume));

    square_laplacian(mptr, rhs_real, &lapstr);
  }

  // This should be the matrix. Let's print it to make sure.

  if (volume <= 16)
  {
    std::cout << mat_real << "\n";
  }

  // Get the eigenvalues.
  SelfAdjointEigenSolver<dMatrix> eigsolve_real(volume);
  eigsolve_real.compute(mat_real);

  std::cout << "The eigenvalues are:\n" << eigsolve_real.eigenvalues() << "\n";

  /////////////////////////////
  // COMPLEX, HERMITIAN CASE //
  /////////////////////////////

  std::cout << "\n\nComplex, Symmetric case.\n\n";

  // Structure which gets passed to the function.
  laplace_gauged_struct lapstr_gauged;
  lapstr_gauged.length = length;
  lapstr_gauged.m_sq = m_sq;
  lapstr_gauged.gauge_links = gauge_links;

  // Allocate a sufficiently gigantic matrix.
  cMatrix mat_cplx = cMatrix::Zero(volume, volume);

  // Form matrix elements. This is where it's important that
  // dMatrix and cMatrix are column major.
  // I should probably make this safer by using a "Map".
  for (int i = 0; i < volume; i++)
  {
    // Set a point on the rhs for a matrix element.
    zero_vector(rhs_cplx, volume);
    rhs_cplx[i] = 1.0;

    // Where we put the result of the matrix element.
    complex<double>* mptr = &(mat_cplx(i*volume));

    square_laplacian_gauged(mptr, rhs_cplx, &lapstr_gauged);
  }

  // This should be the matrix. Let's print it to make sure.

  if (volume <= 9)
  {
    std::cout << mat_cplx << "\n";
  }

  // Get the eigenvalues.
  SelfAdjointEigenSolver<cMatrix> eigsolve_cplx(volume);
  eigsolve_cplx.compute(mat_cplx);

  std::cout << "The eigenvalues are:\n" << eigsolve_cplx.eigenvalues() << "\n";

  ///////////////////////////
  // REAL, INDEFINITE CASE //
  ///////////////////////////

  std::cout << "\n\nReal, Indefinite case.\n\n";

  // Allocate a sufficiently gigantic matrix.
  dMatrix mat_real_indef = dMatrix::Zero(volume, volume);

  // Form matrix elements. This is where it's important that
  // dMatrix and cMatrix are column major.
  // I should probably make this safer by using a "Map".
  for (int i = 0; i < volume; i++)
  {
    // Set a point on the rhs for a matrix element.
    zero_vector(rhs_real_indef, volume);
    rhs_real_indef[i] = 1.0;

    // Where we put the result of the matrix element.
    double* mptr = &(mat_real_indef(i*volume));

    square_staggered(mptr, rhs_real_indef, &lapstr);
  }

  // This should be the matrix. Let's print it to make sure.

  if (volume <= 16)
  {
    std::cout << mat_real_indef << "\n";
  }

  // Get the eigenvalues.
  EigenSolver<dMatrix> eigsolve_real_indef(wilson_volume);
  eigsolve_real_indef.compute(mat_real_indef);

  std::cout << "The eigenvalues are:\n" << eigsolve_real_indef.eigenvalues() << "\n";

  //////////////////////////////
  // COMPLEX, INDEFINITE CASE //
  //////////////////////////////

  std::cout << "\n\nComplex, Indefinite case.\n\n";

  // Allocate a sufficiently gigantic matrix.
  cMatrix mat_cplx_indef = cMatrix::Zero(wilson_volume, wilson_volume);

  // Form matrix elements. This is where it's important that
  // dMatrix and cMatrix are column major.
  // I should probably make this safer by using a "Map".
  for (int i = 0; i < wilson_volume; i++)
  {
    // Set a point on the rhs for a matrix element.
    zero_vector(rhs_cplx_indef, wilson_volume);
    rhs_cplx_indef[i] = 1.0;

    // Where we put the result of the matrix element.
    complex<double>* mptr = &(mat_cplx_indef(i*wilson_volume));

    square_wilson_gauged(mptr, rhs_cplx_indef, &lapstr_gauged);
  }

  // This should be the matrix. Let's print it to make sure.

  if (volume <= 9)
  {
    std::cout << mat_cplx_indef << "\n";
  }

  // Get the eigenvalues.
  ComplexEigenSolver<cMatrix> eigsolve_cplx_indef(wilson_volume);
  eigsolve_cplx_indef.compute(mat_cplx_indef);

  std::cout << "The eigenvalues are:\n" << eigsolve_cplx_indef.eigenvalues() << "\n";

  //////////////
  // CLEAN UP //
  //////////////

  // Free the lattice.
  //delete[] lattice;
  deallocate_vector(&rhs_real);
  deallocate_vector(&rhs_real_indef);
  deallocate_vector(&rhs_cplx);
  deallocate_vector(&rhs_cplx_indef);
  deallocate_vector(&gauge_links);
  return 0;
}


#ifndef TRIUMF_STATISTICAL_MECHANICS_FERMI_DIRAC_HPP
#define TRIUMF_STATISTICAL_MECHANICS_FERMI_DIRAC_HPP

#include <cmath>

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

#include <triumf/constants/codata_2018.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

//
namespace statistical_mechanics {

// Fermi-Dirac statistics
namespace fermi_dirac {

// chemical potential of a Fermi gas
template <typename T> T chemical_potential(T temperature, T E_0, T E_F) {
  /*
  // Sommerfeld expansion
  constexpr T k_B =
      triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<T>::value();
  T fraction = (k_B * temperature) / E_F;
  return E_0 +
         E_F * (1.0 -
                std::pow(boost::math::constants::pi<T>() * fraction, 2) / 12.0 -
                std::pow(boost::math::constants::pi<T>() * fraction, 4) / 80.0);
  */
  return E_0 + E_F;
}

// Fermi-Dirac distribution
template <typename T> T distribution(T temperature, T energy, T E_0, T E_F) {
  constexpr T k_B =
      triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<T>::value();
  T mu = chemical_potential<T>(temperature, E_0, E_F);
  T arg = (energy - mu) / (k_B * temperature);
  return 1.0 / (std::exp(arg) + 1.0);
}

// Fermi-Dirac function
template <typename T> T function(T temperature, T energy, T E_0, T E_F) {
  T f_E = distribution<T>(temperature, energy, E_0, E_F);
  return f_E * (1.0 - f_E);
}

} // namespace fermi_dirac

} // namespace statistical_mechanics

} // namespace triumf

#endif // TRIUMF_STATISTICAL_MECHANICS_FERMI_DIRAC_HPP


/// \file
// --------------------------------------------------------------------------
// This file is part of the reference implementation for the paper
//    QFib: Fast and Efficient Brain Tractogram Compression
//    C. Mercier*, S. Rousseau*, P. Gori, I. Bloch and T. Boubekeur
//    NeuroInformatics 2020
//    DOI: 10.1007/s12021-020-09452-0
//
// All rights reserved. Use of this source code is governed by a
// MIT license that can be found in the LICENSE file.
// --------------------------------------------------------------------------
#pragma once

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <bitset>

#include "oct.hpp"
#include "sfibonacci.h"
#include "bitvalue.hpp"
#include "parallel.hpp"

using namespace Eigen;

//struct CompressedVector {Bitvalue<T>pv;};
template<typename T>
using CompressedVector = Bitvalue<T>;

template<typename T>
struct CompressedFiber
{
	Vector3f origin;
	Vector3f second;
	std::vector<CompressedVector<T>> data;
};

float epsMapping = 0.08f;

enum QuantizationMethod
{
	OCTAHEDRAL,
	SPHERICALFIBONACCI
};

QuantizationMethod quantizationMethod = QuantizationMethod::SPHERICALFIBONACCI;

namespace fc
{
template<typename T>
Vector3f unquantize(const CompressedVector<T> & cvec)
{
	switch(quantizationMethod)
	{
	float vec[3];
	case(QuantizationMethod::OCTAHEDRAL):
		octDecode(cvec, vec);
		return Vector3f(vec[0], vec[1], vec[2]).normalized();
	case(QuantizationMethod::SPHERICALFIBONACCI):
		static uint32_t nbFiboPoints = 1 << (bitcount);
		return fib::SF(cvec, nbFiboPoints);
	default :
		std::cerr << "ERROR: Quantization Method not handled !" << std::endl;
		exit(EXIT_FAILURE);
		break;

	}
}

template<typename T>
void quantize(const Vector3f & vec, CompressedVector<T> & cvec)
{
	switch(quantizationMethod)
	{
	case(QuantizationMethod::OCTAHEDRAL) :
		octEncode(vec.data(), cvec);
		break;
	case(QuantizationMethod::SPHERICALFIBONACCI):
		static uint32_t nbFiboPoints = 1 << (bitcount);
		cvec.setCombinedValue(fib::inverseSF<T>(vec, nbFiboPoints));
		break;
	default:
		std::cerr << "ERROR: Quantization Method not handled !" << std::endl;
		exit(EXIT_FAILURE);
		break;
	}
}

///
/// \brief uniformMapping Uniform mapping from uniquant (Fast lossy compression of 3d unit vectors)
/// \param v Vector to map
/// \param axis Axis to use for the spherical cap
/// \param ratio Ratio to use for the mapping
/// \return The mapped vector
///
Vector3f uniformMapping(const Vector3f & v, const Vector3f & axis, const float ratio)
{
	Vector3d dv = v.cast<double>();
	Vector3d daxis = axis.cast<double>();
	dv.normalize();
	daxis.normalize();

	double K = 1.0 / ratio;
	double c = dv.dot(daxis);

	// numerical instabilities
	if (c > 1.0)
		c = 1.0;
	if (c < -1.0)
		c = -1.0;
	Vector3d p1 = (dv - c * daxis).normalized();
	double delta = (1.0 - ((1.0 - c) / K));
	return  ((p1 * std::sqrt(1.0 - delta * delta) + delta * daxis).cast<float>()).normalized();
}

///
/// \brief inverseUniformMapping Inverse uniform mapping from uniquant (Fast lossy compression of 3d unit vectors)
/// \param v Vector to unmap
/// \param axis Axis to use for the spherical cap
/// \param ratio Ratio to use for the mapping
/// \return The unmapped vector
///

Vector3f inverseUniformMapping(const Vector3f & v, const Vector3f & axis, const float ratio)
{
	//return v;
	Vector3d v2 = v.cast<double>();
	Vector3d axis2 = axis.cast<double>();
	v2.normalize();
	axis2.normalize();

	double K = ratio;
	double c = v2.dot(axis2);

	/// numerical instabilities
	if (c > 1.0)
		c = 1.0;
	if (c < -1.0)
		c = -1.0;
	Vector3d p1 = (v2 - c * axis2).normalized();
	double delta = (1.0 - ((1.0 - c) / K));
	return  ((p1 * std::sqrt(1.0 - delta * delta) + delta * axis2).cast<float>()).normalized();
}

///
/// \brief computeRatio Compute the minimum dot product of the input fiber (only one)
/// \param fiber Fiber from which to compute the minimum dot product
/// \return Minimum dot product of the unique fiber
///

float computeRatio(const std::vector<Vector3f> & fibers)
{
	Vector3f v1, v2;
	float mindot = 2.f;
	v1 = (fibers[1] - fibers[0]).normalized();
	for(unsigned p = 2; p < fibers.size(); ++p)
	{
		v2 = (fibers[p] - fibers[p-1]).normalized();
		float dot = v2.dot(v1);
		mindot = (dot <  mindot) ? dot : mindot;
		v1 = v2;
	}

	return mindot;
}

///
/// \brief computeRatioVerif Compute the ratio of the input fibers and verify that stepsizes are almost constant
/// \param fibers Fibers from which to compute the ratio
/// \param verbose Parameter to display the maximum angle if true
/// \param totalNbPts Variable that will contain the total number of points in the bundle, used for average error computation
/// \return Ratio of the bundle of fibers
///

float computeRatioVerif(const std::vector<std::vector<Vector3f> > & fibers, bool verbose, uint64_t & totalNbPts)
{
	unsigned maxThreads = std::max(unsigned(1), std::thread::hardware_concurrency());
	std::vector<uint64_t> nbPts(maxThreads, 0);
	std::vector<float> mindot(maxThreads, 2.0f);
	std::vector<float> maxStep(maxThreads, 0.0f);
	std::vector<float> minStep(maxThreads, std::numeric_limits<float>::max());
#ifdef USE_OPENMP
#pragma omp parallel for
	for(int f = 0; f < fibers.size(); ++f)
	{
		nbPts[omp_get_thread_num()] += fibers[f].size();
		Vector3f v1, v2;
		float v2norm;
		v1 = (fibers[f][1] - fibers[f][0]).normalized();
		for(unsigned p = 2; p < fibers[f].size(); ++p)
		{
			v2 = (fibers[f][p] - fibers[f][p-1]);
			v2norm = v2.norm();
			maxStep[omp_get_thread_num()] = (v2norm > maxStep[omp_get_thread_num()]) ? v2norm : maxStep[omp_get_thread_num()];
			minStep[omp_get_thread_num()] = (v2norm < minStep[omp_get_thread_num()]) ? v2norm : minStep[omp_get_thread_num()];
			float dot = (v2 / v2norm).dot(v1);
			mindot[omp_get_thread_num()] = (dot <  mindot[omp_get_thread_num()]) ? dot : mindot[omp_get_thread_num()];
			v1 = v2 / v2norm;
		}
	}
#else
	parallel.For<unsigned>(0, fibers.size(), 1, [&](unsigned f)
	{
		nbPts[parallel.getID()] += fibers[f].size();
		Vector3f v1, v2;
		float v2norm;
		v1 = (fibers[f][1] - fibers[f][0]).normalized();
		for(unsigned p = 2; p < fibers[f].size(); ++p)
		{
			v2 = (fibers[f][p] - fibers[f][p-1]);
			v2norm = v2.norm();
			maxStep[parallel.getID()] = (v2norm > maxStep[parallel.getID()]) ? v2norm : maxStep[parallel.getID()];
			minStep[parallel.getID()] = (v2norm < minStep[parallel.getID()]) ? v2norm : minStep[parallel.getID()];
			float dot = (v2 / v2norm).dot(v1);
			mindot[parallel.getID()] = (dot <  mindot[parallel.getID()]) ? dot : mindot[parallel.getID()];
			v1 = v2 / v2norm;
		}
	});
#endif
	float mdot = mindot[0], maxS = maxStep[0], minS = minStep[0];
	totalNbPts=nbPts[0];
	for (unsigned int i=1; i<mindot.size(); i++)
	{
		totalNbPts += nbPts[i];
		mdot = (mindot[i] < mdot) ? mindot[i] : mdot;
		maxS = (maxStep[i] > maxS) ? maxStep[i] : maxS;
		minS = (minStep[i] < minS) ? minStep[i] : minS;
	}
	float diff = maxS - minS;

	if (verbose)
	{
		std::cout << "Max angle: " << acos(mdot) * 180 / fib::PI << "°" << std::endl;
		std::cout << "Max length between segments: " << maxS << std::endl;
		std::cout << "Min length between segments: " << minS << std::endl;
		std::cout << "Difference: " << diff << std::endl;
	}

	if (diff > 0.1f * (maxS + minS) / 2.0f)
	{
		std::cerr << std::endl << "-------------!ERROR!--------------" << std::endl;
		std::cerr << "Difference in segments length too big (" << diff << "), no compression possible" << std::endl;
		std::cerr << "Try to resample your data with a constant stepsize before compression" << std::endl;
		std::cerr << "-------------!ERROR!--------------" << std::endl << std::endl;
		exit(EXIT_FAILURE);
	}

	epsMapping = 0.f;
	for(unsigned i = 0; i < 20; ++i)
		epsMapping = 3 * sqrt(2) * pow(2, -bitcount/2.f) * ((1 - (mdot))/2+epsMapping);
	//Error maximization proportionaly to itself (epsMapping < 1)
	epsMapping = sqrt(epsMapping);
	return std::min(1.f, (1.f - mdot) / 2.f + epsMapping);
}

///
/// \brief computeRatioVerif Compute the ratio of the input fibers without verifying that stepsizes are almost constant
/// \param fibers Fibers from which to compute the ratio
/// \param verbose Parameter to display the maximum angle if true
/// \param totalNbPts Variable that will contain the total number of points in the bundle, used for average error computation
/// \return Ratio of the bundle of fibers
///

float computeRatio(const std::vector<std::vector<Vector3f> > & fibers, bool verbose, uint64_t & totalNbPts)
{
	unsigned maxThreads = std::max(unsigned(1), std::thread::hardware_concurrency());
	std::vector<uint64_t> nbPts(maxThreads, 0);
	std::vector<float> mindot(maxThreads, 2.0f);

#ifdef USE_OPENMP
#pragma omp parallel for
	for(int f = 0; f < fibers.size(); ++f)
	{
		nbPts[omp_get_thread_num()] += fibers[f].size();
		Vector3f v1, v2;
		float v2norm;
		v1 = (fibers[f][1] - fibers[f][0]).normalized();
		for(unsigned p = 2; p < fibers[f].size(); ++p)
		{
			v2 = (fibers[f][p] - fibers[f][p-1]).normalized();
			float dot = v2.dot(v1);
			mindot[omp_get_thread_num()] = (dot <  mindot[omp_get_thread_num()]) ? dot : mindot[omp_get_thread_num()];
			v1 = v2;
		}
	}
#else
	parallel.For<unsigned>(0, fibers.size(), 1, [&](unsigned f)
	{
		nbPts[parallel.getID()] += fibers[f].size();
		Vector3f v1, v2;
		float v2norm;
		v1 = (fibers[f][1] - fibers[f][0]).normalized();
		for(unsigned p = 2; p < fibers[f].size(); ++p)
		{
			v2 = (fibers[f][p] - fibers[f][p-1]).normalized();
			float dot = v2.dot(v1);
			mindot[parallel.getID()] = (dot <  mindot[parallel.getID()]) ? dot : mindot[parallel.getID()];
			v1 = v2;
		}
	});
#endif
	float mdot=mindot[0];
	totalNbPts=nbPts[0];
	for (unsigned int i=1; i<mindot.size(); i++)
	{
		totalNbPts += nbPts[i];
		mdot = (mindot[i] < mdot) ? mindot[i] : mdot;
	}

	if (verbose)
		std::cout << "Max angle: " << acos(mdot) * 180 / fib::PI << "°" << std::endl;

	epsMapping = 0.f;
	for(unsigned i = 0; i < 20; ++i)
		epsMapping = 3 * sqrt(2) * pow(2, -bitcount/2.f) * ((1 - (mdot))/2+epsMapping);
	//Error maximization proportionaly to itself (epsMapping < 1)
	epsMapping = sqrt(epsMapping);
	return std::min(1.f, (1.f - mdot) / 2.f + epsMapping);
}

///
/// \brief compressFiber Compression of the fiber considering its ratio
/// \param fiber Fiber to compress
/// \param ratio Ratio to use for the compression
/// \param cfiber Fiber compressed
/// \param template T is the type of the compressed data, either int8 or int16 depending on the precision asked
///

template<typename T>
void compressFiber(const std::vector<Vector3f> & fiber, const float ratio, CompressedFiber<T> & cfiber)
{
	assert(fiber.size() > 1);

	//encode the first two points
	cfiber.origin = fiber[0];
	cfiber.second = fiber[1];

	cfiber.data.resize(fiber.size() - 2);

	//compute the first unit vector and the stepsize
	Vector3f axis = (fiber[1] - fiber[0]).normalized();
	float stepsize = (fiber[1] - fiber[0]).norm();
	Vector3f currentpoint = fiber[1];

	//compress the rest of the fiber
	Vector3f v;
	constexpr float eps = 0.00000001;
	for(unsigned i = 2; i < fiber.size(); ++i)
	{
		v = (fiber[i] - currentpoint).normalized();
		if(((1.f - v.dot(axis)) / 2.f) > ratio) //Conservative mapping in case the epsilon is not enough
		{
			fc::quantize((-((1.0 - eps) * axis) + (eps * v)).normalized(), cfiber.data[i-2]);
		}
		else //Otherwise, general case
		{
			if(abs(v.dot(axis)) < 1.f)
				fc::quantize(fc::inverseUniformMapping(v, axis, ratio), cfiber.data[i-2]);
			else
			{
				fc::quantize(v, cfiber.data[i-2]);
			}
		}
		axis = fc::uniformMapping(fc::unquantize(cfiber.data[i-2]), axis, ratio);
		currentpoint += (stepsize * axis);
		//currentpoint = fiber[i]; //Uncomment if you want to disable error propagation !!
	}
}

///
/// \brief compressFiber Compression of the fiber considering its ratio, computing the obtained error at the same time
/// \param fiber Fiber to compress
/// \param ratio Ratio to use for the compression
/// \param cfiber Fiber compressed
/// \param maxError Maximum error (at point level) when compressing the fiber
/// \param meanError Average error (at point level) when compressing the fiber
/// \param template T is the type of the compressed data, either int8 or int16 depending on the precision asked
///

template<typename T>
void compressFiber(const std::vector<Vector3f> & fiber, const float ratio, CompressedFiber<T> & cfiber, float & maxError, double & meanError)
{
	assert(fiber.size() > 1);

	//encode the first two points
	cfiber.origin = fiber[0];
	cfiber.second = fiber[1];

	cfiber.data.resize(fiber.size() - 2);

	//compute the first unit vector and the stepsize
	Vector3f axis = (fiber[1] - fiber[0]).normalized();
	float stepsize = (fiber[1] - fiber[0]).norm();
	Vector3f currentpoint = fiber[1];

	//compress the rest of the fiber
	Vector3f v;
	constexpr float eps = 0.00000001;
	for(unsigned i = 2; i < fiber.size(); ++i)
	{
		v = (fiber[i] - currentpoint).normalized();
		if(((1.f - v.dot(axis)) / 2.f) > ratio) //Conservative mapping in case the epsilon is not enough
		{
			fc::quantize((-((1.0 - eps) * axis) + (eps * v)).normalized(), cfiber.data[i-2]);
		}
		else //Otherwise, general case
		{
			if(abs(v.dot(axis)) < 1.f)
				fc::quantize(fc::inverseUniformMapping(v, axis, ratio), cfiber.data[i-2]);
			else
				fc::quantize(v, cfiber.data[i-2]);
		}
		axis = fc::uniformMapping(fc::unquantize(cfiber.data[i-2]), axis, ratio);
		currentpoint += (stepsize * axis);
		float err = (fiber[i] - currentpoint).norm();
		maxError = err > maxError ? err : maxError;
		meanError += err;
	}
}

///
/// \brief decompressFiber Decompress the input fiber
/// \param cfiber Compressed fiber to decompress
/// \param ratio Ratio to use for decompression (same than for compression)
/// \param fiber Decompressed fiber
///

template<typename T>
void decompressFiber(const CompressedFiber<T> & cfiber, const float ratio, std::vector<Vector3f> & fiber)
{
	fiber.reserve(cfiber.data.size() + 2);
	fiber.push_back(cfiber.origin);
	fiber.push_back(cfiber.second);

	float stepsize = (cfiber.second - cfiber.origin).norm();
	Vector3f axis = (fiber[1] - fiber[0]).normalized();

	for(unsigned i = 0; i < cfiber.data.size(); ++i)
	{
		Vector3f v = fc::unquantize(cfiber.data[i]);
		if(abs(v.dot(axis)) < 1.f)
			v = fc::uniformMapping(v, axis, ratio);

		fiber.push_back(fiber[i+1] + stepsize * v);
		axis = v;
	}
}
}