#include <map>
#include <sstream>
#include <fstream>
#include <iostream>
#include <chrono>
#include <unordered_map>

#define TINYOBJLOADER_IMPLEMENTATION
#include "tiny_obj_loader.h"

#include <glm/gtx/norm.hpp>
#include <boost/lexical_cast.hpp>

#include "mesh.hpp"

void genNormals(Mesh& mesh)
{
	std::vector<glm::vec3> normals(mesh.v.size());

	for (int i = 0; i < mesh.t.size(); i += 3)
	{
		auto v0 = mesh.v[mesh.t[i + 0]];
		auto v1 = mesh.v[mesh.t[i + 1]];
		auto v2 = mesh.v[mesh.t[i + 2]];

		auto n = glm::normalize(glm::cross(v1 - v0, v2 - v0));

		normals[mesh.t[i + 0]] += n;
		normals[mesh.t[i + 1]] += n;
		normals[mesh.t[i + 2]] += n;
	}

	for (auto& normal : normals)
	{
		normal = glm::normalize(normal);
	}

	mesh.n = normals;
}

void genRandomColors(Mesh& mesh) {
	for (int i = 0; i < mesh.v.size(); i++) {
		mesh.AddColor(glm::vec4(
			(float)rand() / (float)RAND_MAX,
			(float)rand() / (float)RAND_MAX,
			(float)rand() / (float)RAND_MAX,
			1.0f
		));
	}
}

Mesh genIcosahedron()
{
	Mesh mesh;

	const float t = (1.0f + std::sqrt(5.0f)) / 2.0f;

	mesh.AddVert(glm::vec3(-1.0f, t, 0.0f));
	mesh.AddVert(glm::vec3(1.0f, t, 0.0f));
	mesh.AddVert(glm::vec3(-1.0f, -t, 0.0f));
	mesh.AddVert(glm::vec3(1.0f, -t, 0.0f));
	mesh.AddVert(glm::vec3(0.0f, -1.0, t));
	mesh.AddVert(glm::vec3(0.0f, 1.0, t));
	mesh.AddVert(glm::vec3(0.0f, -1.0, -t));
	mesh.AddVert(glm::vec3(0.0f, 1.0, -t));
	mesh.AddVert(glm::vec3(t, 0.0f, -1.0f));
	mesh.AddVert(glm::vec3(t, 0.0f, 1.0f));
	mesh.AddVert(glm::vec3(-t, 0.0f, -1.0f));
	mesh.AddVert(glm::vec3(-t, 0.0f, 1.0f));

	mesh.AddTri(0, 11, 5);
	mesh.AddTri(0, 5, 1);
	mesh.AddTri(0, 1, 7);
	mesh.AddTri(0, 7, 10);
	mesh.AddTri(0, 10, 11);
	mesh.AddTri(1, 5, 9);
	mesh.AddTri(5, 11, 4);
	mesh.AddTri(11, 10, 2);
	mesh.AddTri(10, 7, 6);
	mesh.AddTri(7, 1, 8);
	mesh.AddTri(3, 9, 4);
	mesh.AddTri(3, 4, 2);
	mesh.AddTri(3, 2, 6);
	mesh.AddTri(3, 6, 8);
	mesh.AddTri(3, 8, 9);
	mesh.AddTri(4, 9, 5);
	mesh.AddTri(2, 4, 11);
	mesh.AddTri(6, 2, 10);
	mesh.AddTri(8, 6, 7);
	mesh.AddTri(9, 8, 1);

	mesh.Normalize();
	mesh.radius = 1.0f;

	return mesh;
}

struct Edge
{
	int v0;
	int v1;

	Edge(int v0, int v1)
		: v0(v0 < v1 ? v0 : v1), v1(v0 < v1 ? v1 : v0) {}

	bool operator <(const Edge& rhs) const
	{
		return v0 < rhs.v0 || (v0 == rhs.v0 && v1 < rhs.v1);
	}
};

int subdivideEdge(int f0, int f1, const glm::vec3& v0, const glm::vec3& v1, Mesh& io_mesh, std::map<Edge, int>& io_divisions)
{
	const Edge edge(f0, f1);
	auto it = io_divisions.find(edge);
	if (it != io_divisions.end())
	{
		return it->second;
	}

	const glm::vec3 v = normalize((v0 + v1) * 0.5f);
	const int f = io_mesh.v.size();
	io_mesh.v.emplace_back(v);
	io_divisions.emplace(edge, f);
	return f;
}

void SubdivideMesh(const Mesh& meshIn, Mesh& meshOut)
{
	meshOut.v = meshIn.v;

	std::map<Edge, int> divisions; // Edge -> new vertex

	for (uint32_t i = 0; i < meshIn.t.size() / 3; ++i)
	{
		const int f0 = meshIn.t[i * 3];
		const int f1 = meshIn.t[i * 3 + 1];
		const int f2 = meshIn.t[i * 3 + 2];

		const glm::vec3 v0 = meshIn.v[f0];
		const glm::vec3 v1 = meshIn.v[f1];
		const glm::vec3 v2 = meshIn.v[f2];

		const int f3 = subdivideEdge(f0, f1, v0, v1, meshOut, divisions);
		const int f4 = subdivideEdge(f1, f2, v1, v2, meshOut, divisions);
		const int f5 = subdivideEdge(f2, f0, v2, v0, meshOut, divisions);

		meshOut.AddTri(f0, f3, f5);
		meshOut.AddTri(f3, f1, f4);
		meshOut.AddTri(f4, f2, f5);
		meshOut.AddTri(f3, f4, f5);
	}
}

Mesh genIcosphere(int subdivisions)
{
	Mesh m = genIcosahedron();
	Mesh m2;

	for (int i = 0; i < subdivisions; i++)
	{
		SubdivideMesh (m, m2);
		m.v = m2.v;
		m.t = m2.t;
		m2.Clear();
	}
	return m;
}

Mesh genRing(const int points, const glm::vec4 color)
{
	Mesh m;
	for (int i = 0; i < points; i++)
	{
		m.AddVert({ std::cosf(2.f * PI * ((float)i / points)), 0.f, std::sinf(2 * PI * ((float)i / points)) });
		m.AddVert({ std::cosf(2.f * PI * (((float)i + 1) / points)), 0.f, std::sinf(2 * PI * (((float)i + 1) / points)) });
		m.AddColor(color);
		m.AddColor(color);
	}
	m.radius = 1.f;

	return m;
}

void genGrid(Mesh& mesh, const float dim, const int segments, const int subgrids, const glm::vec4 color)
{
	float start = -0.5f * dim;
	float pos = start;
	const float step = dim / segments;
	for (int i = 0; i <= segments; i++)
	{
		mesh.AddVert(glm::vec3(start, 0, pos));
		mesh.AddVert(glm::vec3(-start, 0, pos));
		mesh.AddVert(glm::vec3(pos, 0, start));
		mesh.AddVert(glm::vec3(pos, 0, -start));
		pos += step;

		for (int j = 0; j < 4; j++)
			mesh.AddColor(color);
	}

	if (subgrids > 0)
		genGrid(mesh, dim, segments * 10, subgrids - 1, glm::vec4(color[0], color[1], color[2], 0.5f * color[3]));
}

Mesh genGrid(const float dim, const int segments, const int subgrids, const glm::vec4 color)
{
	Mesh m;
	genGrid(m, dim, segments, subgrids, color);

	if (subgrids > 0)
	{
		genGrid(m,
			dim,
			segments * 10,
			subgrids - 1,
			glm::vec4(color[0], color[1], color[2], 0.5f * color[3])
		);
	}
	return m;
}

Mesh genVector(const glm::vec3 start, const glm::vec3 end, const glm::vec4 color)
{
	Mesh m;
	m.AddVert(start);
	m.AddVert(end);
	m.AddColor(color);
	m.AddColor(color);
	return m;
}

// seek till digit, sign or eof
// signless for int
void seekNextNumber(std::istringstream& iss)
{
	while (!std::isdigit(iss.peek()) && iss.peek() != '-' && iss.peek() != EOF)
		iss.ignore(1, EOF);
}

bool extractFloats(std::istringstream& iss, float* coord)
{
	for (int i = 0; i < 3; i++)
	{
		seekNextNumber(iss);

		char next = iss.peek();
		std::string s;
		std::getline(iss, s, ' ');
		try
		{
			coord[i] = boost::lexical_cast<float>(s);
		}
		catch (boost::bad_lexical_cast&)
		{
			std::cout << "Bad cast: " << s << " : '" << next << "', eof: " << (next == EOF) << std::endl;
		}
	}

	return true;
}

bool getInt(std::istringstream& iss, std::string& s)
{
	// Look ahead til we get to end of integer
	auto start = iss.tellg();
	while (std::isdigit(iss.peek()))
		iss.ignore(1, EOF);

	char end = iss.peek();

	// Seek back
	iss.clear();
	iss.seekg(start);
	std::getline(iss, s, end);

	// Seek till digit, space or eof
	while (!std::isdigit(iss.peek()) && !std::isspace(iss.peek()) && iss.peek() != EOF)
		iss.ignore(1, EOF);

	// Return whether next is a slash
	return end == '/';
}

bool extractInts(std::istringstream& iss, float* values)
{
	std::string s;
	bool proceed;

	for (int i = 0; i < 3; i++)
	{
		seekNextNumber(iss);
		proceed = true;
		if (proceed)
		{
			proceed = getInt(iss, s);
			values[i + 0] = boost::lexical_cast<float>(s);
		}
		if (proceed)
		{
			proceed = getInt(iss, s);
			values[i + 3] = boost::lexical_cast<float>(s);
		}
		if (proceed)
		{
			proceed = getInt(iss, s);
			values[i + 6] = boost::lexical_cast<float>(s);
		}
	}

	return true;
}

Mesh LoadOBJ(const std::string path, const glm::vec3 offset)
{
	Mesh m;

	auto start = std::chrono::system_clock::now();
	std::ifstream infile(path);

	int lineCount = 0;
	int lineTotal = std::count(std::istreambuf_iterator<char>(infile),
		std::istreambuf_iterator<char>(), '\n');

	infile.seekg(0, std::ios::beg);

	std::string line;
	glm::dvec3 avg(0.0);

	int v = 0;
	int t = 0;

	while (std::getline(infile, line))
	{
		std::istringstream iss(line);
		if (iss.peek() == 'v')
		{
			float coord[3];
			iss.ignore(1, EOF);

			if (iss.peek() == 'n')
			{
				extractFloats(iss, coord);
				// TODO: vertex normal support
			}
			else
			{
				extractFloats(iss, coord);
				glm::vec3 vec = glm::vec3(coord[0], coord[1], coord[2]);
				m.AddVert(vec);
				avg += vec;
				v++;
			}
		}
		else if (iss.peek() == 'f')
		{
			float values[9];
			extractInts(iss, values);
			m.AddTri(values[0] - 1, values[1] - 1, values[2] - 1);
			t++;
		}
	}

	avg = avg / static_cast<double>(v);

	glm::vec3 avgVec = glm::vec3(avg);
	float maxlen = 0;
	for (auto& vert : m.v)
	{
		// Center to mid
		vert = (vert - avgVec);
		// Find furthest vertice (mesh radius)
		if (glm::length2(vert) > maxlen) maxlen = glm::length2(vert);
	}

	m.radius = sqrtf(maxlen);
	m.mid = glm::vec3(0.f);// avgVec;

	auto end = std::chrono::system_clock::now();
	std::chrono::duration<double> elapsed_seconds = end - start;

	std::cout
		<< "Loaded OBJ mesh, vertices: "
		<< m.v.size() << "/" << v
		<< ", triangles: "
		<< m.t.size() / 3 << "/" << t
		<< ", time: "
		<< elapsed_seconds.count() << "s"
		<< std::endl;

	return m;
}

// For identical vertex matching
struct Vertex
{
	size_t vx, vy, vz;
	size_t nx, ny, nz;
	size_t tu, tv;

	bool operator==(const Vertex& other) const
	{
		return (
			this->vx == other.vx &&
			this->vy == other.vy &&
			this->vz == other.vz &&
			this->nx == other.nx &&
			this->ny == other.ny &&
			this->nz == other.nz &&
			+this->tu == other.tu &&
			this->tv == other.tv
			);
	}
};

// Hash function for Vertex struct (sum of powers of two)
namespace std {
	template<> struct hash<Vertex>
	{
		std::size_t operator()(const Vertex& v) const noexcept
		{
			return (
				(v.vx * v.vx) + (v.vy * v.vy) + (v.vz * v.vz) +
				(v.nx * v.nx) + (v.ny * v.ny) + (v.nz * v.nz) +
				(v.tu * v.tu) + (v.tv * v.tv)
				);
		}
	};
}

Mesh LoadOBJFast(const std::string filename, const std::string path)
{
	auto start = std::chrono::system_clock::now();

	tinyobj::attrib_t attrib;
	std::vector<tinyobj::shape_t> shapes;
	std::vector<tinyobj::material_t> materials;

	std::string warn;
	std::string err;

	Mesh m;
	glm::dvec3 avg(0.0);

	bool loaded = tinyobj::LoadObj(&attrib, &shapes, &materials, &warn, &err, filename.c_str(), path.c_str());

	if (!warn.empty()) std::cout << warn << std::endl;
	if (!err.empty()) std::cerr << err << std::endl;
	if (!loaded)
	{
		std::cerr << "Failed to load OBJ: " << path << std::endl;
		return m;
	}
	if (shapes.empty())
	{
		std::cerr << "No shapes in OBJ: " << path << std::endl;
		return m;
	}
	if (shapes[0].mesh.num_face_vertices.empty())
	{
		std::cerr << "No vertices in OBJ: " << path << std::endl;
		return m;
	}
	if (shapes[0].mesh.num_face_vertices[0] != 3)
	{
		std::cerr << "Unsupported non-triangle primitives in OBJ: " << path << std::endl;
		return m;
	}

	unsigned idx = 0u;
	unsigned offset = 0u;
	std::unordered_map<Vertex, int> previous;

	for (unsigned face = 0; face < shapes[0].mesh.num_face_vertices.size(); face++)
	{
		for (unsigned vertex = 0; vertex < 3; vertex++)
		{
			auto v_idx = shapes[0].mesh.indices[offset + vertex];
			Vertex v = {
				3 * v_idx.vertex_index + 0,
				3 * v_idx.vertex_index + 1,
				3 * v_idx.vertex_index + 2,
				3 * v_idx.normal_index + 0,
				3 * v_idx.normal_index + 1,
				3 * v_idx.normal_index + 2,
				2 * v_idx.texcoord_index + 0,
				2 * v_idx.texcoord_index + 1
			};

			auto match = previous.find(v);
			if (match == previous.end()) // New vertex
			{
				glm::vec3 vec = glm::vec3(
					attrib.vertices[v.vx],
					attrib.vertices[v.vy],
					attrib.vertices[v.vz]
				);
				m.v.emplace_back(vec);
				avg += vec;

				if (v_idx.normal_index != -1)
				{
					m.n.emplace_back(glm::vec3(
						attrib.normals[v.nx],
						attrib.normals[v.ny],
						attrib.normals[v.nz]
					));
				}

				if (v_idx.texcoord_index != -1)
				{
					m.tx.emplace_back(glm::vec2(
						attrib.texcoords[v.tu],
						attrib.texcoords[v.tv]
					));
				}
				size_t index = previous.size();
				previous.insert({ v, index });
				m.t.emplace_back(index);
			}
			else
			{
				m.t.emplace_back(match->second);
			}
		}
		offset += 3;
	}

	float maxlen = 0;
	avg = avg / static_cast<double>(m.v.size());
	for (auto& vert : m.v)
	{
		// Center to mid
		vert = (vert - glm::vec3(avg));
		// Find furthest vertice (mesh radius)
		if (glm::length2(vert) > maxlen) maxlen = glm::length2(vert);
	}
	m.radius = sqrtf(maxlen);
	m.mid = glm::vec3(0.f);

	auto end = std::chrono::system_clock::now();
	std::chrono::duration<double> elapsed_seconds = end - start;

	std::cout
		<< "Loaded OBJ mesh, vertices: " << m.v.size()
		<< ", triangles: " << (float)m.t.size() / 3.f
		<< ", normals: " << m.n.size()
		//<< ", texcoords: " << m.tx.size()
		<< ", colors: " << m.c.size()
		<< ", time: " << elapsed_seconds.count() << "s"
		<< std::endl;

	return m;
}


#pragma once
#include <iostream>

#include <utility>

#include <memory>
#include <tuple>
#include <iostream>
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <functional>
#include <vector>

namespace gsimp {
// geometric types
typedef std::vector<double> point_t;
typedef std::vector<size_t> cell_t;

// linear algebra types
typedef typename Eigen::SparseMatrix<double> matrix_t;
typedef typename Eigen::SparseVector<double> vector_t;

// chains
typedef typename std::pair<int, vector_t> chain_t;
typedef typename std::pair<int, std::vector<double>> chain_v;

int& chain_dim(chain_t& p) { return std::get<0>(p); }
vector_t& chain_rep(chain_t& p) { return std::get<1>(p); }
double& chain_val(chain_t& p, size_t i) { return std::get<1>(p).coeffRef(i); }
size_t chain_size(chain_t& p) {return std::get<1>(p).size();}

int& chain_dim(chain_v& p) { return std::get<0>(p); }
std::vector<double>& chain_rep_v(chain_v& p) { return std::get<1>(p); }
double& chain_val(chain_v& p, size_t i) { return std::get<1>(p)[i]; }
size_t chain_size(chain_v& p) {return std::get<1>(p).size();}

chain_t create_chain(int d,std::vector<double>& vec) {
    // no dimension checking
    // use at own risk
    std::cout << "creating the sparse vector\n";
    vector_t v(vec.size());
    std::cout << "creating the pair\n";
    chain_t c(d,v);
    std::cout << "made the pair starting conversion\n";
    for (size_t i = 0 ; i < vec.size(); ++i)
    {if (vec[i] != 0) chain_val(c,i) = vec[i];}
    return c;
}

chain_t add(chain_t chain1, chain_t chain2) {
    if (std::get<0>(chain1) == std::get<0>(chain2))
        return chain_t(std::get<0>(chain1),
                       std::get<1>(chain1) + std::get<1>(chain2));
    std::cout << "chains must have the same dimension to be added (+)";
    throw std::exception();
}

void add_to(chain_t chain1, chain_t chain2) {
    if (std::get<0>(chain1) == std::get<0>(chain2))
        std::get<1>(chain1) += std::get<1>(chain2);
    else {
        std::cout << "chains must have the same dimension to be added (+=)";
        throw std::exception();
    }
}

chain_t subtract(chain_t chain1, chain_t chain2) {
    if (std::get<0>(chain1) == std::get<0>(chain2))
        return chain_t(std::get<0>(chain1),
                       std::get<1>(chain1) - std::get<1>(chain2));
    std::cout << "chains must have the same dimension to be added (-)";
    throw std::exception();
}

void subtract_to(chain_t chain1, chain_t chain2) {
    if (std::get<0>(chain1) == std::get<0>(chain2))
        std::get<1>(chain1) -= std::get<1>(chain2);
    else {
        std::cout << "chains must have the same dimension to be added (-=)";
        throw std::exception();
    }
}

chain_t prod(double coef, chain_t chain) {
    return chain_t(std::get<0>(chain), std::get<1>(chain) * coef);
}

void prod_to(double coef, chain_t chain) { std::get<1>(chain) *= coef; }

Eigen::VectorXd point_to_eigen(point_t pt) {
    Eigen::VectorXd v0(pt.size());
    for (int j = 0; j < pt.size(); ++j) v0(j) = pt[j];
    return v0;
}

}  // namespace gsimp


// landmark.cpp
// Based on landmarks_maxmin.cpp in the 'landmark' R package by Matt Piekenbrock, Jason Cory Brunson, Yara Skaf
#include <carma>
#include <armadillo>

#include <vector>
#include <functional>
#include <numeric>
#include <algorithm>
#include <thread>

using std::size_t;
using std::vector;
using std::thread;

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/numpy.h>

using namespace pybind11::literals;
namespace py = pybind11;

template< typename InputIt >
inline double sq_euc_dist(InputIt x, InputIt y, const size_t d){
	double res = 0.0;
	for(size_t i = 0; i < d; ++i, ++x, ++y){
		res += ((*x) - (*y)) * ((*x) - (*y));
	}
	return(res);
}
// a Distance Function here just returns the distance between two points, given their *indices*
using DistFunction = typename std::function<double(size_t, size_t)>;

// dist_f  := distance function between two (indexed) points
// n_pts   := number of points in the data set
// eps     := distance threshold used as a stopping criterion for the maxmin procedure
// n       := cardinality threshold used as a stopping criterion for the maxmin procedure
// metric  := metric to use. If 0, uses `dist_f`, otherwise picks one of the available metrics.
// seed    := initial point (default is point at index 0)
// pick    := criterion to break ties. Possible values include 0 (first), 1 (random), or 2 (last).
// cover   := whether to report set membership for each point
void maxmin_f(DistFunction dist_f, const size_t n_pts,
              const double eps, const size_t n,
              const size_t seed, const size_t pick,
							vector< size_t >& indices,
							vector< double >& radii
							) {
  if (eps == -1.0 && n == 0){ throw std::invalid_argument("Must supply either positive 'eps' or positive 'n'."); }
  if (pick > 2){ throw std::invalid_argument("tiebreaker 'pick' choice must be in { 0, 1, 2 }."); }
  if (seed >= n_pts){ throw std::invalid_argument("Invalid seed index given."); }
	if (indices.size() == 0 || radii.size() == 0){ throw std::invalid_argument("Indices and radii must have at least one element to begin with."); }

  // Make a function that acts as a sentinel
  enum CRITERION { NUM, EPS, NUM_OR_EPS }; // These are the only ones that make sense
  const CRITERION stopping_criterion = (eps == -1.0) ? NUM : ((n == 0) ? EPS : NUM_OR_EPS);
  const auto is_finished = [stopping_criterion, eps, n](size_t n_landmarks, double c_eps) -> bool {
    switch(stopping_criterion){
      case NUM: return(n_landmarks >= n);
      case EPS: return(c_eps <= eps);
      case NUM_OR_EPS: return(n_landmarks >= n || c_eps <= eps);
			default: return(true); // should never happen, but gcc complains
    };
  };

  // Indices of possible candidate landmarks
  vector< size_t > candidate_pts(n_pts, 0);
  std::iota(begin(candidate_pts), end(candidate_pts), 0);
  candidate_pts.erase(begin(candidate_pts) + seed);

  // Preallocate distance vector for landmarks; one for each point
	double cover_radius = std::numeric_limits<double>::infinity();
  vector< double > lm_dist(n_pts, cover_radius);

  // Generate the landmarks
  bool stop_reached = false;
  while (!stop_reached){
    const size_t c_lm = indices.back(); // update current landmark

    // Update non-landmark points with distance to nearest landmark
    for (auto idx: candidate_pts){
      double c_dist = dist_f(c_lm, idx);
      if (c_dist < lm_dist[idx]){
        lm_dist[idx] = c_dist; // update minimum landmark distance
      }
    }

    // Of the remaining candidate points, find the one with the maximum landmark distance
    auto max_landmark = std::max_element(begin(candidate_pts), end(candidate_pts), [&lm_dist](size_t ii, size_t jj){
      return lm_dist[ii] < lm_dist[jj];
    });

    // If not greedily picking the first candidate point, partition the candidate points, then use corresponding strategy
    if (pick > 0 && max_landmark != end(candidate_pts)){
      double max_lm_dist = lm_dist[(*max_landmark)];
      auto it = std::partition(begin(candidate_pts), end(candidate_pts), [max_lm_dist, &lm_dist](size_t j){
        return lm_dist[j] == max_lm_dist;
      });
			max_landmark =
				pick == 1 ? (it != begin(candidate_pts) ? std::prev(it) : begin(candidate_pts)) :
				begin(candidate_pts) + (rand() % std::distance(begin(candidate_pts), it));
    }

    // If the iterator is valid, we have a new landmark, otherwise we're finished
    if (max_landmark != end(candidate_pts)){
      cover_radius = lm_dist[(*max_landmark)];
      stop_reached = is_finished(indices.size(), cover_radius);
      if (!stop_reached){
        indices.push_back(*max_landmark);
				radii.push_back(cover_radius);
				candidate_pts.erase(max_landmark);
      }
    } else {
      cover_radius = 0.0;
      stop_reached = true;
    }
  } // while(!finished())
}

// Point cloud wrapper - See 'maxmin_f' below for implementation
py::tuple maxmin_pc(
	const arma::mat& X,
	const double eps, const size_t n,
	const size_t metric = 1, const size_t seed = 0, const size_t pick = 0
){
  const size_t n_pts = X.n_cols, d = X.n_rows;
  if (seed >= n_pts){ throw std::invalid_argument("Invalid seed point."); }

	// Initial covering radius == Inf
	vector< double > cover_radii{ std::numeric_limits<double>::infinity() };

  // Choose the initial landmark
  vector< size_t > lm { seed };
  lm.reserve(n != 0 ? n : size_t(n_pts*0.15));

  // Choose the distance function
  DistFunction dist = [&X, d](size_t i, size_t j) {
		return(sq_euc_dist(X.begin_col(i), X.begin_col(j), d));
	};

  // Call the generalized procedure
  maxmin_f(dist, n_pts, eps, n, seed, pick, lm, cover_radii);

	return(py::make_tuple(lm, cover_radii));
}

// Converts (i,j) indices in the range { 0, 1, ..., n - 1 } to its 0-based position
// in a lexicographical ordering of the (n choose 2) combinations.
constexpr size_t to_nat_2(size_t i, size_t j, size_t n) noexcept {
  return i < j ? (n*i - i*(i+1)/2 + j - i - 1) : (n*j - j*(j+1)/2 + i - j - 1);
}

// X := (n_pts choose 2) pairwise distances
// n_pts := number of points in X
py::tuple maxmin_dist(
	const arma::vec& X, const size_t n_pts,
	const double eps, const size_t n,
	const size_t seed = 0, const size_t pick = 0){
  if (seed >= n_pts){ throw std::invalid_argument("Invalid seed point."); }

  // Parameterize the distance function
  DistFunction dist = [&X, n_pts](size_t i, size_t j) -> double {
    return X[to_nat_2(i,j,n_pts)];
  };

	// Initial covering radius == Inf
	vector< double > cover_radii{ std::numeric_limits<double>::infinity() };

  // Choose the initial landmark
  vector< size_t > lm { seed };
  lm.reserve(n != 0 ? n : size_t(n_pts*0.15));

  // Call the generalized procedure
  maxmin_f(dist, n_pts, eps, n, seed, pick, lm, cover_radii);
	return(py::make_tuple(lm, cover_radii));
}

// Maxmin procedure O(n^2)
// x := pairwise distances (not a distance matrix!) if pairwise = True, else (d x n) matrix representing a point cloud
// eps := radius to cover 'x' with, otherwise -1.0 to use 'n'
// n := number of landmarks requested
// pairwise_dist := whether input is a set of pairwise distances or a point cloud
py::tuple maxmin(const py::array_t<double>& x, const double eps, const size_t n, bool pairwise_dist, int seed){
	if (pairwise_dist){
		const arma::vec dx = carma::arr_to_col< double >(x);
		const size_t N = dx.size();

		// Find n such that choose(n, 2) == N
		size_t lb = std::sqrt(2*N);
		size_t n_pts = size_t(floor(lb));
		for (; n_pts <= size_t(std::ceil(lb+2)); ++n_pts){
			if (N == ((n_pts * (n_pts - 1))/2)){ break; }
		}
		return(maxmin_dist(dx, n_pts, eps, n, seed, 0));
	} else {
		const arma::mat X = carma::arr_to_mat< double >(x);
		return(maxmin_pc(X, eps, n, 1, seed, 0));
	}
}

void doSomething(int thread_id, vector< double >& output) {
	output[thread_id] = std::sqrt(static_cast< double >(thread_id));
}

// Spawns n threads
auto spawnThreads(int n) -> vector< double > {
	vector< thread > threads(n);
	vector< double > output(n, 0.0);
	for (int i = 0; i < n; i++) {
		threads[i] = thread(doSomething, i, std::ref(output));
	}
	for (auto& th : threads) { th.join(); } // each thread blocks until it's finished, sequentially
	return(output);
}

// Classical MDS
// D := distance matrix
void cmds_eig(const arma::mat& D, const size_t d, arma::vec& w, arma::mat& v){
	const size_t n = D.n_rows;
	arma::mat H(n, n, arma::fill::zeros);
	double fill_value = 1.0/double(n);
	H.fill(-fill_value);
	H.diag().fill(1.0 - fill_value);
	bool success = arma::eig_sym(w, v, -0.5 * H * D * H, "std");
	if (!success){
		throw std::invalid_argument("Eigenvalues failed to converge.");
	}
}

void cmds(const arma::mat& D, const size_t d, arma::mat& out){
	arma::mat v;
	arma::vec w;
	cmds_eig(D, d, w, v);
	out = arma::fliplr(v);
	arma::vec eigenvalues = arma::sort(w, "descend");
	out.resize(out.n_rows, d);
	for (size_t j = 0; j < d; ++j){
		if (eigenvalues[j] > 0){
			out.col(j) *= std::sqrt(eigenvalues[j]);
		} else {
			out.col(j).fill(0.0);
		}
	}
}

constexpr auto rank_comb2(size_t i, size_t j, size_t n) noexcept -> size_t {
  if (j < i){ std::swap(i,j); }
  return(size_t(n*i - i*(i+1)/2 + j - i - 1));
}

inline std::array< size_t, 2 > unrank_comb2(const size_t x, const size_t n) noexcept {
	auto i = static_cast< size_t >( (n - 2 - floor(sqrt(-8*x + 4*n*(n-1)-7)/2.0 - 0.5)) );
	auto j = static_cast< size_t >( x + i + 1 - n*(n-1)/2 + (n-i)*((n-i)-1)/2 );
	return (std::array< size_t, 2 >{ i, j });
}

// Measure all pairwise distances between columns of matrix 'x'
// template< typename OutputIt, typename Lambda >
// void dist(const arma::mat& x, OutputIt out){
// 	const size_t N = x.n_rows*(x.n_rows - 1)/2;
// 	for (size_t c = 0; c < N; ++c){
// 		std::array< size_t, 2 > p = unrank_comb2(c, x.n_rows);
// 		size_t i = p[0], j = p[1];
// 		*out++ = (double) arma::dot(x.col(i) - x.col(j));
// 	}
// }

// Measure all pairwise distances between columns of matrix 'x'
void dist_matrix(const arma::mat& x, arma::mat& D){
	const size_t N = x.n_cols*(x.n_cols - 1)/2;
	// py::print("n = ", N);
	for (size_t c = 0; c < N; ++c){
		std::array< size_t, 2 > p = unrank_comb2(c, x.n_cols);
		size_t i = p[0], j = p[1];
		arma::vec diff = x.col(i) - x.col(j);
		D(i,j) = (double) std::pow(arma::norm(diff), 2.0);
		D(j,i) = D(i,j);
	}
}

// Each C++11 thread should be running in their function with an infinite loop, constantly waiting for new tasks to grab and run.
#include "threadpool.h"
void parallel_mds_threadpool(const arma::mat& X, const vector< arma::uvec >& cover_sets, const size_t d, const size_t n_threads, vector< arma::mat >& out){
	// Allocate max number of threads
	ctpl::thread_pool p(n_threads);

	// Prepare the models
	const size_t n_opens = cover_sets.size();
	auto models = vector< arma::mat >(n_opens, arma::mat());

	// Launch the threads
	std::vector<std::future<void>> results(n_opens);

	for (size_t j = 0; j < n_opens; ++j) {
		results[j] = p.push([&models, &X, &cover_sets, j, d](int thread_id){
			const size_t n = cover_sets.at(j).size();
			const arma::mat X_j = X.cols(cover_sets.at(j));
			arma::mat D = arma::mat(n, n, arma::fill::zeros);
			dist_matrix(X_j, D);
			cmds(D, d, models.at(j));
		});
	}

	// Join them
	for (size_t j = 0; j < n_opens; ++j) { results[j].get(); }

	// Copy the local euclidean models (transposed)
	for (size_t j = 0; j < models.size(); ++j){
		out[j] = models[j];
	}
};

auto parallel_mds(const py::array_t< double >& x, const py::list& cover_sets, const size_t d, const size_t n_threads) -> py::list {

	// Conversions
	const arma::mat X = carma::arr_to_mat< double >(x).t();
	auto indices = vector< arma::uvec >(cover_sets.size());
	for (size_t j = 0; j < cover_sets.size(); ++j){
		indices[j] = cover_sets[j].cast< arma::uvec >();
	}

	// Do the parallel mds
	std::cout << "The GIL state is " << PyGILState_Check() <<std::endl;
	py::gil_scoped_release release;
	std::cout << "The GIL state is " << PyGILState_Check() <<std::endl;
	vector< arma::mat > results(indices.size(), arma::mat());
	parallel_mds_threadpool(X, indices, d, n_threads, results);
	py::gil_scoped_acquire acquire;

	// py::list output(indices.size());
	vector< py::array_t< double > > output(indices.size());
	for (size_t j = 0; j < indices.size(); ++j){
		output[j] = carma::mat_to_arr(results[j]);
	}
	return(py::cast(output));
} // parallel_mds

// auto parallel_mds_blocks(const py::array_t< double >& x, const py::list& cover_sets, const size_t d, 	const vector< size_t >& blocks, const size_t n_threads) -> py::list {
// 	// Conversions
// 	const arma::mat X = carma::arr_to_mat< double >(x).t();
// 	auto indices = vector< arma::uvec >(cover_sets.size());
// 	for (size_t j = 0; j < cover_sets.size(); ++j){
// 		indices[j] = cover_sets[j].cast< arma::uvec >();
// 	}

// 	// Do the parallel mds
// 	py::gil_scoped_release release;
// 	vector< arma::mat > results(indices.size(), arma::mat());
// 	parallel_mds_simple(X, indices, d, n_threads, results);
// 	py::gil_scoped_acquire acquire;

// 	// py::list output(indices.size());
// 	vector< py::array_t< double > > output(indices.size());
// 	for (size_t j = 0; j < indices.size(); ++j){
// 		output[j] = carma::mat_to_arr(results[j]);
// 	}
// 	return(py::cast(output));
// }

// Simple parallelization of MDS on each open of the cover
// Assumes n_threads > 1
// X := (d x n) column major matrix of points
// blocks := (n_threads+1) vector of offsets such that indicating a range [blocks[i], blocks[i+1]) of open for thread i to handle
void parallel_mds_simple(
	const arma::mat& X,
	const vector< vector< arma::uword > >& cover_sets,
	const size_t d,
	const size_t n_threads,
	const vector< size_t >& blocks,
	vector< arma::mat >& out
){
	if (blocks.size() != (n_threads+1)){ throw std::invalid_argument("Block vector size must match number of threads."); }

	// Prepare the lambda to do the work
	const auto do_mds = [&out, &X, &cover_sets, d](int i, const int j) -> void {
		for (; i < j; ++i){
			// py::print("subset: ", i, "/", cover_sets.size());
			const size_t n = cover_sets.at(i).size();
			//vector< arma::uword >& c_open = cover_sets.at(i);
			arma::uvec ind(cover_sets.at(i));// unfortunately this copy is required
			const arma::mat X_i = X.cols(ind);
			arma::mat D = arma::mat(n, n, arma::fill::zeros);
			dist_matrix(X_i, D);
			cmds(D, d, out.at(i));
		}
	};

	// Do sequential computation
	// for (size_t j = 0; j < n_threads; ++j){
	// 	// py::print("Starting thread: ", j);
	// 	do_mds(blocks.at(j), blocks.at(j+1));
	// }

	// Launch the threads
	auto tt = vector< thread >(n_threads - 1);
	for (size_t j = 0; j < (n_threads-1); ++j){
		tt.at(j) = thread(do_mds, blocks.at(j), blocks.at(j+1));
	}
	// Have the main thread do some work as well
	do_mds(blocks.at(n_threads-1), blocks.at(n_threads));

	// Join the threads
	for (size_t j = 0; j < (n_threads-1); ++j) { tt.at(j).join(); }
}

// When threads are created using the dedicated Python APIs (such as the threading module), a thread state is automatically associated to
// them and the code showed above is therefore correct. However, when threads are created from C (for example by a third-party library
// with its own thread management), they don’t hold the GIL, nor is there a thread state structure for them.

// If you need to call Python code from these threads (often this will be part of a callback API provided by the aforementioned third-party
// library), you must first register these threads with the interpreter by creating a thread state data structure, then acquiring the GIL,
// and finally storing their thread state pointer, before you can start using the Python/C API. When you are done, you should reset the
// thread state pointer, release the GIL, and finally free the thread state data structure.

// trust pybind11/carma to do the automatic conversions here
// Parallelization notes:
// (1) DO NOT PASS AN ARMA::MAT DIRECTLY AS A PARAMETER; convert manually w/ carma
// (2) The GIL lock seems to require spawning threads, if unlocked and reacquired. Only unlock if threads are spawned.
// (3) Can actually return vector< arma::mat > fine, surprisingly
auto parallel_mds_blocks(
	const py::array_t< double >& X,
	const vector< vector< arma::uword >  >& cover_sets,
	const size_t d,
	const size_t n_threads,
	const vector< size_t >& blocks
) -> vector< arma::mat > {

	// Allocate the output
	vector< arma::mat > results(cover_sets.size(), arma::mat(1,1, arma::fill::zeros));

	// Do the parallel mds
	// std::cout << "INIT: The GIL state is " << PyGILState_Check() <<std::endl;
	const arma::mat points = carma::arr_to_mat(X);
	py::gil_scoped_release release;
	vector< double > res = spawnThreads(n_threads);
	// std::cout << "BEGIN: The GIL state is " << PyGILState_Check() <<std::endl;
	parallel_mds_simple(points, cover_sets, d, n_threads, blocks, results);
	// PyGILState_Ensure();
	// std::cout << "FINISHED: The GIL state is " << PyGILState_Check() <<std::endl;
	py::gil_scoped_acquire acquire;
	// std::cout << "END: The GIL state is " << PyGILState_Check() <<std::endl;
	// py::cast(results)

	// vector< py::array_t< double > > output(cover_sets.size());
	// for (size_t j = 0; j < cover_sets.size(); ++j){
	// 	output[j] = carma::mat_to_arr(results[j], true);
	// }
	// return(output);
	return(results);
	// return(py::list(1));
}

PYBIND11_MODULE(landmark, m) {
	m.def("maxmin", &maxmin, "finds maxmin landmarks");
	m.def("do_parallel", [](size_t n_threads) -> py::array_t< double > {
		/* Release GIL before calling into (potentially long-running) C++ code */
		py::gil_scoped_release release;
		vector< double > res = spawnThreads(n_threads);
		py::gil_scoped_acquire acquire;
		return(carma::col_to_arr(arma::vec(res)));
	});
	m.def("cmds", [](const py::array_t< double >& X, const size_t d) -> py::array_t< double > {
		const arma::mat D = carma::arr_to_mat< double >(X);
		arma::mat emb;
		cmds(D, d, emb);
		return carma::mat_to_arr(emb);
	});
	// void parallel_mds(const py::array_t< double >& x, const py::list& cover_sets, const size_t d){
	m.def("parallel_cmds", parallel_mds);
	m.def("parallel_mds_blocks", parallel_mds_blocks);
	m.def("dist_matrix", [](const py::array_t< double >& x) -> py::array_t< double > {
		const arma::mat X = carma::arr_to_mat(x).t();
		arma::mat D = arma::mat(X.n_cols, X.n_cols, arma::fill::zeros);
		dist_matrix(X, D);
		return(carma::mat_to_arr(D));
	});
};


//  (C) Copyright Nick Thompson 2018.
//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_STATISTICS_BIVARIATE_STATISTICS_HPP
#define BOOST_MATH_STATISTICS_BIVARIATE_STATISTICS_HPP

#include <iterator>
#include <tuple>
#include <type_traits>
#include <stdexcept>
#include <future>
#include <thread>
#include <vector>
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <boost/assert.hpp>

// Support compilers with P0024R2 implemented without linking TBB
// https://en.cppreference.com/w/cpp/compiler_support
#ifndef BOOST_NO_CXX17_HDR_EXECUTION
#include <execution>
#define EXEC_COMPATIBLE
#endif

namespace boost{ namespace math{ namespace statistics { namespace detail {

// See Equation III.9 of "Numerically Stable, Single-Pass, Parallel Statistics Algorithms", Bennet et al.
template<typename ReturnType, typename ForwardIterator>
ReturnType means_and_covariance_seq_impl(ForwardIterator u_begin, ForwardIterator u_end, ForwardIterator v_begin, ForwardIterator v_end)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    Real cov = 0;
    ForwardIterator u_it = u_begin;
    ForwardIterator v_it = v_begin;
    Real mu_u = *u_it++;
    Real mu_v = *v_it++;
    std::size_t i = 1;

    while(u_it != u_end && v_it != v_end)
    {
        Real u_temp = (*u_it++ - mu_u)/(i+1);
        Real v_temp = *v_it++ - mu_v;
        cov += i*u_temp*v_temp;
        mu_u = mu_u + u_temp;
        mu_v = mu_v + v_temp/(i+1);
        i = i + 1;
    }

    if(u_it != u_end || v_it != v_end)
    {
        throw std::domain_error("The size of each sample set must be the same to compute covariance");
    }

    return std::make_tuple(mu_u, mu_v, cov/i, i);
}

// Numerically stable parallel computation of (co-)variance
// https://dl.acm.org/doi/10.1145/3221269.3223036
template<typename ReturnType, typename ForwardIterator>
ReturnType means_and_covariance_parallel_impl(ForwardIterator u_begin, ForwardIterator u_end, ForwardIterator v_begin, ForwardIterator v_end)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    const auto u_elements = std::distance(u_begin, u_end);
    const auto v_elements = std::distance(v_begin, v_end);

    if(u_elements != v_elements)
    {
        throw std::domain_error("The size of each sample set must be the same to compute covariance");
    }

    const unsigned max_concurrency = std::thread::hardware_concurrency() == 0 ? 2u : std::thread::hardware_concurrency();
    unsigned num_threads = 2u;

    // 5.16 comes from benchmarking. See boost/math/reporting/performance/bivariate_statistics_performance.cpp
    // Threading is faster for: 10 + 5.16e-3 N/j <= 5.16e-3N => N >= 10^4j/5.16(j-1).
    const auto parallel_lower_bound = 10e4*max_concurrency/(5.16*(max_concurrency-1));
    const auto parallel_upper_bound = 10e4*2/5.16; // j = 2

    // https://lemire.me/blog/2020/01/30/cost-of-a-thread-in-c-under-linux/
    if(u_elements < parallel_lower_bound)
    {
        return means_and_covariance_seq_impl<ReturnType>(u_begin, u_end, v_begin, v_end);
    }
    else if(u_elements >= parallel_upper_bound)
    {
        num_threads = max_concurrency;
    }
    else
    {
        for(unsigned i = 3; i < max_concurrency; ++i)
        {
            if(parallel_lower_bound < 10e4*i/(5.16*(i-1)))
            {
                num_threads = i;
                break;
            }
        }
    }

    std::vector<std::future<ReturnType>> future_manager;
    const auto elements_per_thread = std::ceil(static_cast<double>(u_elements)/num_threads);

    ForwardIterator u_it = u_begin;
    ForwardIterator v_it = v_begin;

    for(std::size_t i = 0; i < num_threads - 1; ++i)
    {
        future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [u_it, v_it, elements_per_thread]() -> ReturnType
        {
            return means_and_covariance_seq_impl<ReturnType>(u_it, std::next(u_it, elements_per_thread), v_it, std::next(v_it, elements_per_thread));
        }));
        u_it = std::next(u_it, elements_per_thread);
        v_it = std::next(v_it, elements_per_thread);
    }

    future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [u_it, u_end, v_it, v_end]() -> ReturnType
    {
        return means_and_covariance_seq_impl<ReturnType>(u_it, u_end, v_it, v_end);
    }));

    ReturnType temp = future_manager[0].get();
    Real mu_u_a = std::get<0>(temp);
    Real mu_v_a = std::get<1>(temp);
    Real cov_a = std::get<2>(temp);
    Real n_a = std::get<3>(temp);

    for(std::size_t i = 1; i < future_manager.size(); ++i)
    {
        temp = future_manager[i].get();
        Real mu_u_b = std::get<0>(temp);
        Real mu_v_b = std::get<1>(temp);
        Real cov_b = std::get<2>(temp);
        Real n_b = std::get<3>(temp);

        const Real n_ab = n_a + n_b;
        const Real delta_u = mu_u_b - mu_u_a;
        const Real delta_v = mu_v_b - mu_v_a;

        cov_a = cov_a + cov_b + (-delta_u)*(-delta_v)*((n_a*n_b)/n_ab);
        mu_u_a = mu_u_a + delta_u*(n_b/n_ab);
        mu_v_a = mu_v_a + delta_v*(n_b/n_ab);
        n_a = n_ab;
    }

    return std::make_tuple(mu_u_a, mu_v_a, cov_a, n_a);
}

template<typename ReturnType, typename ForwardIterator>
ReturnType correlation_coefficient_seq_impl(ForwardIterator u_begin, ForwardIterator u_end, ForwardIterator v_begin, ForwardIterator v_end)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using std::sqrt;

    Real cov = 0;
    ForwardIterator u_it = u_begin;
    ForwardIterator v_it = v_begin;
    Real mu_u = *u_it++;
    Real mu_v = *v_it++;
    Real Qu = 0;
    Real Qv = 0;
    std::size_t i = 1;

    while(u_it != u_end && v_it != v_end)
    {
        Real u_tmp = *u_it++ - mu_u;
        Real v_tmp = *v_it++ - mu_v;
        Qu = Qu + (i*u_tmp*u_tmp)/(i+1);
        Qv = Qv + (i*v_tmp*v_tmp)/(i+1);
        cov += i*u_tmp*v_tmp/(i+1);
        mu_u = mu_u + u_tmp/(i+1);
        mu_v = mu_v + v_tmp/(i+1);
        ++i;
    }

    // If both datasets are constant, then they are perfectly correlated.
    if (Qu == 0 && Qv == 0)
    {
        return std::make_tuple(mu_u, Qu, mu_v, Qv, cov, Real(1), i);
    }
    // If one dataset is constant and the other isn't, then they have no correlation:
    if (Qu == 0 || Qv == 0)
    {
        return std::make_tuple(mu_u, Qu, mu_v, Qv, cov, Real(0), i);
    }

    // Make sure rho in [-1, 1], even in the presence of numerical noise.
    Real rho = cov/sqrt(Qu*Qv);
    if (rho > 1) {
        rho = 1;
    }
    if (rho < -1) {
        rho = -1;
    }

    return std::make_tuple(mu_u, Qu, mu_v, Qv, cov, rho, i);
}

// Numerically stable parallel computation of (co-)variance:
// https://dl.acm.org/doi/10.1145/3221269.3223036
//
// Parallel computation of variance:
// http://i.stanford.edu/pub/cstr/reports/cs/tr/79/773/CS-TR-79-773.pdf
template<typename ReturnType, typename ForwardIterator>
ReturnType correlation_coefficient_parallel_impl(ForwardIterator u_begin, ForwardIterator u_end, ForwardIterator v_begin, ForwardIterator v_end)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    const auto u_elements = std::distance(u_begin, u_end);
    const auto v_elements = std::distance(v_begin, v_end);

    if(u_elements != v_elements)
    {
        throw std::domain_error("The size of each sample set must be the same to compute covariance");
    }

    const unsigned max_concurrency = std::thread::hardware_concurrency() == 0 ? 2u : std::thread::hardware_concurrency();
    unsigned num_threads = 2u;

    // 3.25 comes from benchmarking. See boost/math/reporting/performance/bivariate_statistics_performance.cpp
    // Threading is faster for: 10 + 3.25e-3 N/j <= 3.25e-3N => N >= 10^4j/3.25(j-1).
    const auto parallel_lower_bound = 10e4*max_concurrency/(3.25*(max_concurrency-1));
    const auto parallel_upper_bound = 10e4*2/3.25; // j = 2

    // https://lemire.me/blog/2020/01/30/cost-of-a-thread-in-c-under-linux/
    if(u_elements < parallel_lower_bound)
    {
        return correlation_coefficient_seq_impl<ReturnType>(u_begin, u_end, v_begin, v_end);
    }
    else if(u_elements >= parallel_upper_bound)
    {
        num_threads = max_concurrency;
    }
    else
    {
        for(unsigned i = 3; i < max_concurrency; ++i)
        {
            if(parallel_lower_bound < 10e4*i/(3.25*(i-1)))
            {
                num_threads = i;
                break;
            }
        }
    }

    std::vector<std::future<ReturnType>> future_manager;
    const auto elements_per_thread = std::ceil(static_cast<double>(u_elements)/num_threads);

    ForwardIterator u_it = u_begin;
    ForwardIterator v_it = v_begin;

    for(std::size_t i = 0; i < num_threads - 1; ++i)
    {
        future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [u_it, v_it, elements_per_thread]() -> ReturnType
        {
            return correlation_coefficient_seq_impl<ReturnType>(u_it, std::next(u_it, elements_per_thread), v_it, std::next(v_it, elements_per_thread));
        }));
        u_it = std::next(u_it, elements_per_thread);
        v_it = std::next(v_it, elements_per_thread);
    }

    future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [u_it, u_end, v_it, v_end]() -> ReturnType
    {
        return correlation_coefficient_seq_impl<ReturnType>(u_it, u_end, v_it, v_end);
    }));

    ReturnType temp = future_manager[0].get();
    Real mu_u_a = std::get<0>(temp);
    Real Qu_a = std::get<1>(temp);
    Real mu_v_a = std::get<2>(temp);
    Real Qv_a = std::get<3>(temp);
    Real cov_a = std::get<4>(temp);
    Real n_a = std::get<6>(temp);

    for(std::size_t i = 1; i < future_manager.size(); ++i)
    {
        temp = future_manager[i].get();
        Real mu_u_b = std::get<0>(temp);
        Real Qu_b = std::get<1>(temp);
        Real mu_v_b = std::get<2>(temp);
        Real Qv_b = std::get<3>(temp);
        Real cov_b = std::get<4>(temp);
        Real n_b = std::get<6>(temp);

        const Real n_ab = n_a + n_b;
        const Real delta_u = mu_u_b - mu_u_a;
        const Real delta_v = mu_v_b - mu_v_a;

        cov_a = cov_a + cov_b + (-delta_u)*(-delta_v)*((n_a*n_b)/n_ab);
        mu_u_a = mu_u_a + delta_u*(n_b/n_ab);
        mu_v_a = mu_v_a + delta_v*(n_b/n_ab);
        Qu_a = Qu_a + Qu_b + delta_u*delta_u*((n_a*n_b)/n_ab);
        Qv_b = Qv_a + Qv_b + delta_v*delta_v*((n_a*n_b)/n_ab);
        n_a = n_ab;
    }

    // If both datasets are constant, then they are perfectly correlated.
    if (Qu_a == 0 && Qv_a == 0)
    {
        return std::make_tuple(mu_u_a, Qu_a, mu_v_a, Qv_a, cov_a, Real(1), n_a);
    }
    // If one dataset is constant and the other isn't, then they have no correlation:
    if (Qu_a == 0 || Qv_a == 0)
    {
        return std::make_tuple(mu_u_a, Qu_a, mu_v_a, Qv_a, cov_a, Real(0), n_a);
    }

    // Make sure rho in [-1, 1], even in the presence of numerical noise.
    Real rho = cov_a/sqrt(Qu_a*Qv_a);
    if (rho > 1) {
        rho = 1;
    }
    if (rho < -1) {
        rho = -1;
    }

    return std::make_tuple(mu_u_a, Qu_a, mu_v_a, Qv_a, cov_a, rho, n_a);
}

} // namespace detail

#ifdef EXEC_COMPATIBLE

template<typename ExecutionPolicy, typename Container, typename Real = typename Container::value_type>
inline auto means_and_covariance(ExecutionPolicy&& exec, Container const & u, Container const & v)
{
    if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
    {
        if constexpr (std::is_integral_v<Real>)
        {
            using ReturnType = std::tuple<double, double, double, double>;
            ReturnType temp = detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
            return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
        }
        else
        {
            using ReturnType = std::tuple<Real, Real, Real, Real>;
            ReturnType temp = detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
            return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
        }
    }
    else
    {
        if constexpr (std::is_integral_v<Real>)
        {
            using ReturnType = std::tuple<double, double, double, double>;
            ReturnType temp = detail::means_and_covariance_parallel_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
            return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
        }
        else
        {
            using ReturnType = std::tuple<Real, Real, Real, Real>;
            ReturnType temp = detail::means_and_covariance_parallel_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
            return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
        }
    }
}

template<typename Container>
inline auto means_and_covariance(Container const & u, Container const & v)
{
    return means_and_covariance(std::execution::seq, u, v);
}

template<typename ExecutionPolicy, typename Container>
inline auto covariance(ExecutionPolicy&& exec, Container const & u, Container const & v)
{
    return std::get<2>(means_and_covariance(exec, u, v));
}

template<typename Container>
inline auto covariance(Container const & u, Container const & v)
{
    return covariance(std::execution::seq, u, v);
}

template<typename ExecutionPolicy, typename Container, typename Real = typename Container::value_type>
inline auto correlation_coefficient(ExecutionPolicy&& exec, Container const & u, Container const & v)
{
    if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
    {
        if constexpr (std::is_integral_v<Real>)
        {
            using ReturnType = std::tuple<double, double, double, double, double, double, double>;
            return std::get<5>(detail::correlation_coefficient_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
        }
        else
        {
            using ReturnType = std::tuple<Real, Real, Real, Real, Real, Real, Real>;
            return std::get<5>(detail::correlation_coefficient_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
        }
    }
    else
    {
        if constexpr (std::is_integral_v<Real>)
        {
            using ReturnType = std::tuple<double, double, double, double, double, double, double>;
            return std::get<5>(detail::correlation_coefficient_parallel_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
        }
        else
        {
            using ReturnType = std::tuple<Real, Real, Real, Real, Real, Real, Real>;
            return std::get<5>(detail::correlation_coefficient_parallel_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
        }
    }
}

template<typename Container, typename Real = typename Container::value_type>
inline auto correlation_coefficient(Container const & u, Container const & v)
{
    return correlation_coefficient(std::execution::seq, u, v);
}

#else // C++11 bindings

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto means_and_covariance(Container const & u, Container const & v) -> std::tuple<double, double, double>
{
    using ReturnType = std::tuple<double, double, double, double>;
    ReturnType temp = detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
    return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto means_and_covariance(Container const & u, Container const & v) -> std::tuple<Real, Real, Real>
{
    using ReturnType = std::tuple<Real, Real, Real, Real>;
    ReturnType temp = detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v));
    return std::make_tuple(std::get<0>(temp), std::get<1>(temp), std::get<2>(temp));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline double covariance(Container const & u, Container const & v)
{
    using ReturnType = std::tuple<double, double, double, double>;
    return std::get<2>(detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline Real covariance(Container const & u, Container const & v)
{
    using ReturnType = std::tuple<Real, Real, Real, Real>;
    return std::get<2>(detail::means_and_covariance_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline double correlation_coefficient(Container const & u, Container const & v)
{
    using ReturnType = std::tuple<double, double, double, double, double, double, double>;
    return std::get<5>(detail::correlation_coefficient_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline Real correlation_coefficient(Container const & u, Container const & v)
{
    using ReturnType = std::tuple<Real, Real, Real, Real, Real, Real, Real>;
    return std::get<5>(detail::correlation_coefficient_seq_impl<ReturnType>(std::begin(u), std::end(u), std::begin(v), std::end(v)));
}

#endif

}}} // namespace boost::math::statistics

#endif


/* -*-C++-*-
   (c) Copyright 1993-2005, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    Simple statistics functions for single variables
*/

#include <stdlib.h>
#include <cstdio>
#include <string>
#include <float.h>
#include <string.h>

#include <boost/format.hpp>

#include <Lintel/AssertBoost.hpp>
#include <Lintel/Stats.hpp>
#include <Lintel/Double.hpp>

///////////////////////////////////////////////////////////////////////////
// Functions specific to the Statistics base class
///////////////////////////////////////////////////////////////////////////

StatsBase::StatsBase() : reset_count(0), is_assigned(true) {
    reset();
    reset_count = 0; // this is really is the first time
};

StatsBase::~StatsBase() {
    DEBUG_SINVARIANT(checkInvariants());
    is_assigned = false;
};

bool StatsBase::checkInvariants() const {
    return is_assigned;
}

void StatsBase::reset() {
    DEBUG_SINVARIANT(checkInvariants());
    reset_count++;
}

double StatsBase::stddev() const {
    DEBUG_SINVARIANT(checkInvariants());
    double sigsq = variance();

    if (sigsq <= 0.0) {
	return 0.0;
    }

    DEBUG_SINVARIANT(sigsq > 0.0);
    return sqrt(sigsq);
}

double StatsBase::relconf95() const {
    DEBUG_SINVARIANT(checkInvariants());
    return conf95()/mean();
}

///////////////////////////////////////////////////////////////////////////
// Functions related to the Stats singe-variable statistic class
///////////////////////////////////////////////////////////////////////////

// Create a new one
//
Stats::Stats() : StatsBase() {
  reset();
};

Stats::~Stats()	{};

void Stats::reset() {
    DEBUG_SINVARIANT(checkInvariants());
    StatsBase::reset();
    number = 0;
    sum = 0.0;
    sumsq = 0.0;
    min_value = Double::Inf;
    max_value = -Double::Inf;
}

void Stats::add(const double value) {
    DEBUG_INVARIANT(value == value, "You tried to add a NaN to the stats object.");

    ++number;
    sum += value;
    sumsq += value*value;

    if (value < min_value) {
	min_value = value;
    }
    if (value > max_value) {
	max_value = value;
    }
}

void Stats::add(const Stats &stat) {
    number += stat.number;
    sum += stat.sum;
    sumsq += stat.sumsq;

    if (stat.min_value < min_value) {
	min_value = stat.min_value;
    }
    if (stat.max_value > max_value) {
	max_value = stat.max_value;
    }
}

void Stats::addTimeSeq(const double value, const double) {
    this->add(value);
}

// Accessor functions

double Stats::mean() const {
    DEBUG_SINVARIANT(checkInvariants());
    if (number == 0) {
	return 0.0;
    } else {
	return double(sum)/double(number);
    }
};

double Stats::variance() const {
    DEBUG_SINVARIANT(checkInvariants());
    if (number == 0) return 0.0;
    double m = mean();
    return double(sumsq)/double(number) - m*m;
}

// TODO-future: Add in an optional invariant that checks statistical validity,
// i.e., that number>30. Consider also adding a generic conf function that
// takes a z-constant as input so that we can easily add more/different conf
// intervals. Could also add an interface that checks if a specific value is
// within some specified confidence interval.
// Potential z constants of interest:
// conf90 z= 1.645
// conf95 z= 1.960
// conf99 z= 2.576
double Stats::conf95() const {
    DEBUG_SINVARIANT(checkInvariants());
    if (number == 0) return DBL_MAX; // **** Should really be NaN if count==0
    DEBUG_SINVARIANT(number > 0); // **** Could be number > 30 for statistical
                                  // **** validity.
    return 1.96*stddev()/sqrt((double)number);
}

// Summarize contents as a string

std::string Stats::debugString() const {
    DEBUG_SINVARIANT(checkInvariants());

    if (count() == 0) {
	return "count 0";
    } else {
	return str(boost::format("count %d mean %G stddev %G var %G 95%%conf %G rel95%%conf %G"
				 " min %G max %G") % count() % mean() % stddev() % variance()
		   % conf95() % relconf95() % min() % max());
    }
};

void Stats::printRome(int depth, std::ostream &out) const {
    DEBUG_SINVARIANT(checkInvariants());

    std::string spaces;
    for(int i = 0; i < depth; i++) {
	spaces += " ";
    }

    out << spaces << "{ count " << countll() << " }\n";
    if (count() > 0) {
	out << spaces << "{ min " << min() << " }\n";
	out << spaces << "{ max " << max() << " }\n";
	out << spaces << "{ mean " << mean() << " }\n";
	out << spaces << "{ stddev " << stddev() << " }\n";
	out << spaces << "{ variance " << variance() << " }\n";
	out << spaces << "{ conf95 " << conf95() << " }\n";
	out << spaces << "{ total " << total() << " }\n";
	out << spaces << "{ total_sq " << total_sq() << " }\n";
    }
}

void Stats::printTabular(int depth, std::ostream &out) const {
  DEBUG_SINVARIANT(checkInvariants());

  std::string spaces;
  for(int i = 0; i < depth; i++) {
    spaces += " ";
  }

  out << spaces << "count " << countll() << "\n";
  if (count() > 0) {
    out << spaces << "min " << min() << "\n";
    out << spaces << "max " << max() << "\n";
    out << spaces << "mean " << mean() << "\n";
    out << spaces << "stddev " << stddev() << "\n";
    out << spaces << "variance " << variance() << "\n";
    out << spaces << "conf95 " << conf95() << "\n";
    out << spaces << "total " << total() << "\n";
    out << spaces << "total_sq " << total_sq() << "\n";
  }
  // This is kind of a hack, but I had problems when the printout of
  // Stats changed for an empty list.
  else
    {
      out << spaces << "min " << min() << "\n";
      out << spaces << "max " << max() << "\n";
      out << spaces << "mean " << 0 << "\n";
      out << spaces << "stddev " << 0 << "\n";
      out << spaces << "variance " << 0 << "\n";
      out << spaces << "conf95 " << 0 << "\n";
      out << spaces << "total " << total() << "\n";
      out << spaces << "total_sq " << total_sq() << "\n";
    }
}

void Stats::printText(std::ostream &out) const {
    out << boost::format("count=%d, mean=%.8g, stddev=%.8g, min=%.8g, max=%.8g\n")
	% countll() % mean() % stddev() % min() % max();
}

Stats *Stats::another_new() const {
    return new Stats();
}


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

#include <Eigen/Dense>

#include "openbr_internal.h"

#include "openbr/core/common.h"
#include "openbr/core/eigenutils.h"
#include "openbr/core/opencvutils.h"

namespace br
{

/*!
 * \ingroup initializers
 * \brief Initialize Eigen
 * http://eigen.tuxfamily.org/dox/TopicMultiThreading.html
 * \author Scott Klum \cite sklum
 */
class EigenInitializer : public Initializer
{
    Q_OBJECT

    void initialize() const
    {
        Eigen::initParallel();
    }
};

BR_REGISTER(Initializer, EigenInitializer)

/*!
 * \ingroup transforms
 * \brief Projects input into learned Principal Component Analysis subspace.
 * \author Brendan Klare \cite bklare
 * \author Josh Klontz \cite jklontz
 */
class PCATransform : public Transform
{
    Q_OBJECT
    friend class DFFSTransform;
    friend class LDATransform;

protected:
    Q_PROPERTY(float keep READ get_keep WRITE set_keep RESET reset_keep STORED false)
    Q_PROPERTY(int drop READ get_drop WRITE set_drop RESET reset_drop STORED false)
    Q_PROPERTY(bool whiten READ get_whiten WRITE set_whiten RESET reset_whiten STORED false)

    /*!
     *     keep <  0: All eigenvalues are retained.
     *     keep =  0: No PCA performed, eigenvectors form an identity matrix.
     * 0 < keep <  1: Fraction of the variance to retain.
     *     keep >= 1: Number of leading eigenvectors to retain.
     */
    BR_PROPERTY(float, keep, 0.95)
    BR_PROPERTY(int, drop, 0)
    BR_PROPERTY(bool, whiten, false)

    Eigen::VectorXf mean, eVals;
    Eigen::MatrixXf eVecs;

    int originalRows;

public:
    PCATransform() : keep(0.95), drop(0), whiten(false) {}

private:
    double residualReconstructionError(const Template &src) const
    {
        Template proj;
        project(src, proj);

        Eigen::Map<const Eigen::VectorXf> srcMap(src.m().ptr<float>(), src.m().rows*src.m().cols);
        Eigen::Map<Eigen::VectorXf> projMap(proj.m().ptr<float>(), keep);

        return (srcMap - mean).squaredNorm() - projMap.squaredNorm();
    }

    void train(const TemplateList &trainingSet)
    {
        if (trainingSet.first().m().type() != CV_32FC1)
            qFatal("Requires single channel 32-bit floating point matrices.");

        originalRows = trainingSet.first().m().rows;
        int dimsIn = trainingSet.first().m().rows * trainingSet.first().m().cols;
        const int instances = trainingSet.size();

        // Map into 64-bit Eigen matrix
        Eigen::MatrixXd data(dimsIn, instances);
        for (int i=0; i<instances; i++)
            data.col(i) = Eigen::Map<const Eigen::MatrixXf>(trainingSet[i].m().ptr<float>(), dimsIn, 1).cast<double>();

        trainCore(data);
    }

    void project(const Template &src, Template &dst) const
    {
        dst = cv::Mat(1, keep, CV_32FC1);

        // Map Eigen into OpenCV
        Eigen::Map<const Eigen::MatrixXf> inMap(src.m().ptr<float>(), src.m().rows*src.m().cols, 1);
        Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(), keep, 1);

        // Do projection
        outMap = eVecs.transpose() * (inMap - mean);
    }

    void store(QDataStream &stream) const
    {
        stream << keep << drop << whiten << originalRows << mean << eVals << eVecs;
    }

    void load(QDataStream &stream)
    {
        stream >> keep >> drop >> whiten >> originalRows >> mean >> eVals >> eVecs;
    }

protected:
    void trainCore(Eigen::MatrixXd data)
    {
        int dimsIn = data.rows();
        int instances = data.cols();
        const bool dominantEigenEstimation = (dimsIn > instances);

        Eigen::MatrixXd allEVals, allEVecs;
        if (keep != 0) {
            // Compute and remove mean
            mean = Eigen::VectorXf(dimsIn);
            for (int i=0; i<dimsIn; i++) mean(i) = data.row(i).sum() / (float)instances;
            for (int i=0; i<dimsIn; i++) data.row(i).array() -= mean(i);

            // Calculate covariance matrix
            Eigen::MatrixXd cov;
            if (dominantEigenEstimation) cov = data.transpose() * data / (instances-1.0);
            else                         cov = data * data.transpose() / (instances-1.0);

            // Compute eigendecomposition. Returns eigenvectors/eigenvalues in increasing order by eigenvalue.
            Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eSolver(cov);
            allEVals = eSolver.eigenvalues();
            allEVecs = eSolver.eigenvectors();
            if (dominantEigenEstimation) allEVecs = data * allEVecs;
        } else {
            // Null case
            mean = Eigen::VectorXf::Zero(dimsIn);
            allEVecs = Eigen::MatrixXd::Identity(dimsIn, dimsIn);
            allEVals = Eigen::VectorXd::Ones(dimsIn);
        }

        if (keep <= 0) {
            keep = dimsIn - drop;
        } else if (keep < 1) {
            // Keep eigenvectors that retain a certain energy percentage.
            const double totalEnergy = allEVals.sum();
            if (totalEnergy == 0) {
                keep = 0;
            } else {
                double currentEnergy = 0;
                int i=0;
                while ((currentEnergy / totalEnergy < keep) && (i < allEVals.rows())) {
                    currentEnergy += allEVals(allEVals.rows()-(i+1));
                    i++;
                }
                keep = i - drop;
            }
        } else {
            if (keep + drop > allEVals.rows())
                qFatal("Insufficient samples, needed at least %d but only got %d.", (int)keep + drop, (int)allEVals.rows());
        }

        // Keep highest energy vectors
        eVals = Eigen::VectorXf((int)keep, 1);
        eVecs = Eigen::MatrixXf(allEVecs.rows(), (int)keep);
        for (int i=0; i<keep; i++) {
            int index = allEVals.rows()-(i+drop+1);
            eVals(i) = allEVals(index);
            eVecs.col(i) = allEVecs.col(index).cast<float>() / allEVecs.col(index).norm();
            if (whiten) eVecs.col(i) /= sqrt(eVals(i));
        }

        // Debug output
        if (Globals->verbose) qDebug() << "PCA Training:\n\tDimsIn =" << dimsIn << "\n\tKeep =" << keep;
    }

    void writeEigenVectors(const Eigen::MatrixXd &allEVals, const Eigen::MatrixXd &allEVecs) const
    {
        const int originalCols = mean.rows() / originalRows;

        { // Write out mean image
            cv::Mat out(originalRows, originalCols, CV_32FC1);
            Eigen::Map<Eigen::MatrixXf> outMap(out.ptr<float>(), mean.rows(), 1);
            outMap = mean.col(0);
            // OpenCVUtils::saveImage(out, Globals->Debug+"/PCA/eigenVectors/mean.png");
        }

        // Write out sample eigen vectors (16 highest, 8 lowest), filename = eigenvalue.
        for (int k=0; k<(int)allEVals.size(); k++) {
            if ((k < 8) || (k >= (int)allEVals.size()-16)) {
                cv::Mat out(originalRows, originalCols, CV_64FC1);
                Eigen::Map<Eigen::MatrixXd> outMap(out.ptr<double>(), mean.rows(), 1);
                outMap = allEVecs.col(k);
                // OpenCVUtils::saveImage(out, Globals->Debug+"/PCA/eigenVectors/"+QString::number(allEVals(k),'f',0)+".png");
            }
        }
    }
};

BR_REGISTER(Transform, PCATransform)

/*!
 * \ingroup transforms
 * \brief PCA on each row.
 * \author Josh Klontz \cite jklontz
 */
class RowWisePCATransform : public PCATransform
{
    Q_OBJECT

    void train(const TemplateList &trainingSet)
    {
        if (trainingSet.first().m().type() != CV_32FC1)
            qFatal("Requires single channel 32-bit floating point matrices.");

        originalRows = trainingSet.first().m().rows;
        const int dimsIn = trainingSet.first().m().cols;
        int instances = 0;
        foreach (const Template &t, trainingSet)
            instances += t.m().rows;

        // Map into 64-bit Eigen matrix
        Eigen::MatrixXd data(dimsIn, instances);
        int index = 0;
        foreach (const Template &t, trainingSet)
            for (int i=0; i<t.m().rows; i++)
                data.col(index++) = Eigen::Map<const Eigen::MatrixXf>(t.m().ptr<float>(i), dimsIn, 1).cast<double>();

        PCATransform::trainCore(data);
    }

    void project(const Template &src, Template &dst) const
    {
        dst = cv::Mat(src.m().rows, keep, CV_32FC1);

        for (int i=0; i<src.m().rows; i++) {
            Eigen::Map<const Eigen::MatrixXf> inMap(src.m().ptr<float>(i), src.m().cols, 1);
            Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(i), keep, 1);
            outMap = eVecs.transpose() * (inMap - mean);
        }
    }
};

BR_REGISTER(Transform, RowWisePCATransform)

/*!
 * \ingroup transforms
 * \brief Computes Distance From Feature Space (DFFS) \cite moghaddam97.
 * \author Josh Klontz \cite jklontz
 */
class DFFSTransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(float keep READ get_keep WRITE set_keep RESET reset_keep STORED false)
    BR_PROPERTY(float, keep, 0.95)

    PCATransform pca;
    Transform *cvtFloat;

    void init()
    {
        pca.keep = keep;
        cvtFloat = make("CvtFloat");
    }

    void train(const TemplateList &data)
    {
        pca.train((*cvtFloat)(data));
    }

    void project(const Template &src, Template &dst) const
    {
        dst = src;
        dst.file.set("DFFS", sqrt(pca.residualReconstructionError((*cvtFloat)(src))));
    }

    void store(QDataStream &stream) const
    {
        pca.store(stream);
    }

    void load(QDataStream &stream)
    {
        pca.load(stream);
    }
};

BR_REGISTER(Transform, DFFSTransform)

/*!
 * \ingroup transforms
 * \brief Projects input into learned Linear Discriminant Analysis subspace.
 * \author Brendan Klare \cite bklare
 * \author Josh Klontz \cite jklontz
 */
class LDATransform : public Transform
{
    friend class SparseLDATransform;

    Q_OBJECT
    Q_PROPERTY(float pcaKeep READ get_pcaKeep WRITE set_pcaKeep RESET reset_pcaKeep STORED false)
    Q_PROPERTY(bool pcaWhiten READ get_pcaWhiten WRITE set_pcaWhiten RESET reset_pcaWhiten STORED false)
    Q_PROPERTY(int directLDA READ get_directLDA WRITE set_directLDA RESET reset_directLDA STORED false)
    Q_PROPERTY(float directDrop READ get_directDrop WRITE set_directDrop RESET reset_directDrop STORED false)
    Q_PROPERTY(QString inputVariable READ get_inputVariable WRITE set_inputVariable RESET reset_inputVariable STORED false)
    Q_PROPERTY(bool isBinary READ get_isBinary WRITE set_isBinary RESET reset_isBinary STORED false)
    Q_PROPERTY(bool normalize READ get_normalize WRITE set_normalize RESET reset_normalize STORED false)
    BR_PROPERTY(float, pcaKeep, 0.98)
    BR_PROPERTY(bool, pcaWhiten, false)
    BR_PROPERTY(int, directLDA, 0)
    BR_PROPERTY(float, directDrop, 0.1)
    BR_PROPERTY(QString, inputVariable, "Label")
    BR_PROPERTY(bool, isBinary, false)
    BR_PROPERTY(bool, normalize, true)

    int dimsOut;
    Eigen::VectorXf mean;
    Eigen::MatrixXf projection;
    float stdDev;

    void train(const TemplateList &_trainingSet)
    {
        // creates "Label"
        TemplateList trainingSet = TemplateList::relabel(_trainingSet, inputVariable, isBinary);
        int instances = trainingSet.size();

        // Perform PCA dimensionality reduction
        PCATransform pca;
        pca.keep = pcaKeep;
        pca.whiten = pcaWhiten;
        pca.train(trainingSet);
        mean = pca.mean;

        TemplateList ldaTrainingSet;
        static_cast<Transform*>(&pca)->project(trainingSet, ldaTrainingSet);

        int dimsIn = ldaTrainingSet.first().m().rows * ldaTrainingSet.first().m().cols;

        // OpenBR ensures that class values range from 0 to numClasses-1.
        // Label exists because we created it earlier with relabel
        QList<int> classes = File::get<int>(trainingSet, "Label");
        QMap<int, int> classCounts = trainingSet.countValues<int>("Label");
        const int numClasses = classCounts.size();

        // Map Eigen into OpenCV
        Eigen::MatrixXd data = Eigen::MatrixXd(dimsIn, instances);
        for (int i=0; i<instances; i++)
            data.col(i) = Eigen::Map<const Eigen::MatrixXf>(ldaTrainingSet[i].m().ptr<float>(), dimsIn, 1).cast<double>();

        // Removing class means
        Eigen::MatrixXd classMeans = Eigen::MatrixXd::Zero(dimsIn, numClasses);
        for (int i=0; i<instances; i++)  classMeans.col(classes[i]) += data.col(i);
        for (int i=0; i<numClasses; i++) classMeans.col(i) /= classCounts[i];
        for (int i=0; i<instances; i++)  data.col(i) -= classMeans.col(classes[i]);

        PCATransform space1;

        if (!directLDA)
        {
            // The number of LDA dimensions is limited by the degrees
            // of freedom of scatter matrix computed from 'data'. Because
            // the mean of each class is removed (lowering degree of freedom
            // one per class), the total rank of the covariance/scatter
            // matrix that will be computed in PCA is bound by instances - numClasses.
            space1.keep = std::min(dimsIn, instances-numClasses);
            space1.trainCore(data);

            // Divide each eigenvector by sqrt of eigenvalue.
            // This has the effect of whitening the within-class scatter.
            // In effect, this minimizes the within-class variation energy.
            for (int i=0; i<space1.keep; i++) space1.eVecs.col(i) /= pow((double)space1.eVals(i),0.5);
        }
        else if (directLDA == 2)
        {
            space1.drop = instances - numClasses;
            space1.keep = std::min(dimsIn, instances) - space1.drop;
            space1.trainCore(data);
        }
        else
        {
            // Perform (modified version of) Direct LDA

            // Direct LDA uses to the Null space of the within-class scatter.
            // Thus, the lower rank, is used to our benefit. We are not discarding
            // these vectors now (in non-direct code we use the keep parameter
            // to discard Null space). We keep the Null space b/c this is where
            // the within-class scatter goes to zero, i.e. it is very useful.
            space1.keep = dimsIn;
            space1.trainCore(data);

            if (dimsIn > instances - numClasses) {
                // Here, we are replacing the eigenvalue of the  null space
                // eigenvectors with the eigenvalue (divided by 2) of the
                // smallest eigenvector from the row space eigenvector.
                // This allows us to scale these null-space vectors (otherwise
                // it is a divide by zero.
                double null_eig = space1.eVals(instances - numClasses - 1) / 2;
                for (int i = instances - numClasses; i < dimsIn; i++)
                    space1.eVals(i) = null_eig;
            }

            // Drop the first few leading eigenvectors in the within-class space
            QList<float> eVal_list; eVal_list.reserve(dimsIn);
            float fmax = -1;
            for (int i=0; i<dimsIn; i++) fmax = std::max(fmax, space1.eVals(i));
            for (int i=0; i<dimsIn; i++) eVal_list.append(space1.eVals(i)/fmax);

            QList<float> dSum = Common::CumSum(eVal_list);
            int drop_idx;
            for (drop_idx = 0; drop_idx<dimsIn; drop_idx++)
                if (dSum[drop_idx]/dSum[dimsIn-1] >= directDrop)
                    break;

            drop_idx++;
            space1.keep = dimsIn - drop_idx;

            Eigen::MatrixXf new_vecs = Eigen::MatrixXf(space1.eVecs.rows(), (int)space1.keep);
            Eigen::MatrixXf new_vals = Eigen::MatrixXf((int)space1.keep, 1);

            for (int i = 0; i < space1.keep; i++) {
                new_vecs.col(i) = space1.eVecs.col(i + drop_idx);
                new_vals(i) = space1.eVals(i + drop_idx);
            }

            space1.eVecs = new_vecs;
            space1.eVals = new_vals;

            // We will call this "agressive" whitening. Really, it is not whitening
            // anymore. Instead, we are further scaling the small eigenvalues and the
            // null space eigenvalues (to increase their impact).
            for (int i=0; i<space1.keep; i++) space1.eVecs.col(i) /= pow((double)space1.eVals(i),0.15);
        }

        // Now we project the mean class vectors into this second
        // subspace that minimizes the within-class scatter energy.
        // Inside this subspace we learn a subspace projection that
        // maximizes the between-class scatter energy.
        Eigen::MatrixXd mean2 = Eigen::MatrixXd::Zero(dimsIn, 1);

        // Remove means
        for (int i=0; i<dimsIn; i++)     mean2(i) = classMeans.row(i).sum() / numClasses;
        for (int i=0; i<numClasses; i++) classMeans.col(i) -= mean2;

        // Project into second subspace
        Eigen::MatrixXd data2 = space1.eVecs.transpose().cast<double>() * classMeans;

        // The rank of the between-class scatter matrix is bound by numClasses - 1
        // because each class is a vector used to compute the covariance,
        // but one degree of freedom is lost removing the global mean.
        int dim2 = std::min((int)space1.keep, numClasses-1);
        PCATransform space2;
        space2.keep = dim2;
        space2.trainCore(data2);

        // Compute final projection matrix
        projection = ((space2.eVecs.transpose() * space1.eVecs.transpose()) * pca.eVecs.transpose()).transpose();
        dimsOut = dim2;

        stdDev = 1; // default initialize
        if (isBinary) {
            assert(dimsOut == 1);
            float posVal = 0;
            float negVal = 0;
            Eigen::MatrixXf results(trainingSet.size(),1);
            for (int i = 0; i < trainingSet.size(); i++) {
                Template t;
                project(trainingSet[i],t);
                //Note: the positive class is assumed to be 0 b/c it will
                // typically be the first gallery template in the TemplateList structure
                if (classes[i] == 0)
                    posVal += t.m().at<float>(0,0);
                else if (classes[i] == 1)
                    negVal += t.m().at<float>(0,0);
                else
                    qFatal("Binary mode only supports two class problems.");
                results(i) = t.m().at<float>(0,0);  //used for normalization
            }
            posVal /= classCounts[0];
            negVal /= classCounts[1];

            if (posVal < negVal) {
                //Ensure positive value is supposed to be > 0 after projection
                Eigen::MatrixXf invert = Eigen::MatrixXf::Ones(dimsIn,1);
                invert *= -1;
                projection = invert.transpose() * projection;
            }

            if (normalize)
                stdDev = sqrt(results.array().square().sum() / trainingSet.size());
        }
    }

    void project(const Template &src, Template &dst) const
    {
        dst = cv::Mat(1, dimsOut, CV_32FC1);

        // Map Eigen into OpenCV
        Eigen::Map<Eigen::MatrixXf> inMap((float*)src.m().ptr<float>(), src.m().rows*src.m().cols, 1);
        Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(), dimsOut, 1);

        // Do projection
        outMap = projection.transpose() * (inMap - mean);
        if (normalize && isBinary)
            dst.m().at<float>(0,0) = dst.m().at<float>(0,0) / stdDev;
    }

    void store(QDataStream &stream) const
    {
        stream << pcaKeep;
        stream << directLDA;
        stream << directDrop;
        stream << dimsOut;
        stream << mean;
        stream << projection;
        if (normalize && isBinary)
            stream << stdDev;
    }

    void load(QDataStream &stream)
    {
        stream >> pcaKeep;
        stream >> directLDA;
        stream >> directDrop;
        stream >> dimsOut;
        stream >> mean;
        stream >> projection;
        if (normalize && isBinary)
            stream >> stdDev;
    }
};

BR_REGISTER(Transform, LDATransform)

/*!
 * \ingroup transforms
 * \brief Projects input into learned Linear Discriminant Analysis subspace
 *          learned on a sparse subset of features with the highest weight
 *          in the original LDA algorithm.
 * \author Brendan Klare \cite bklare
 */
class SparseLDATransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(float varThreshold READ get_varThreshold WRITE set_varThreshold RESET reset_varThreshold STORED false)
    Q_PROPERTY(float pcaKeep READ get_pcaKeep WRITE set_pcaKeep RESET reset_pcaKeep STORED false)
    Q_PROPERTY(bool normalize READ get_normalize WRITE set_normalize RESET reset_normalize STORED false)
    BR_PROPERTY(float, varThreshold, 1.5)
    BR_PROPERTY(float, pcaKeep, 0.98)
    BR_PROPERTY(bool, normalize, true)

    LDATransform ldaSparse;
    int dimsOut;
    QList<int> selections;

    Eigen::VectorXf mean;

    void init()
    {
        ldaSparse.init();
        ldaSparse.pcaKeep = pcaKeep;
        ldaSparse.inputVariable = "Label";
        ldaSparse.isBinary = true;
        ldaSparse.normalize = true;
    }

    void train(const TemplateList &_trainingSet)
    {

        LDATransform ldaOrig;
        ldaOrig.init();
        ldaOrig.inputVariable = "Label";
        ldaOrig.pcaKeep = pcaKeep;
        ldaOrig.isBinary = true;
        ldaOrig.normalize = true;

        ldaOrig.train(_trainingSet);

        //Only works on binary class problems for now
        assert(ldaOrig.projection.cols() == 1);
        float ldaStd = eigStd(ldaOrig.projection);
        for (int i = 0; i < ldaOrig.projection.rows(); i++)
            if (abs(ldaOrig.projection(i)) > varThreshold * ldaStd)
                selections.append(i);

        TemplateList newSet;
        for (int i = 0; i < _trainingSet.size(); i++) {
            cv::Mat x(_trainingSet[i]);
            cv::Mat y = cv::Mat(selections.size(), 1, CV_32FC1);
            int idx = 0;
            int cnt = 0;
            for (int j = 0; j < x.rows; j++)
                for (int k = 0; k < x.cols; k++, cnt++)
                    if (selections.contains(cnt))
                        y.at<float>(idx++,0) = x.at<float>(j, k);
            newSet.append(Template(_trainingSet[i].file, y));
        }
        ldaSparse.train(newSet);
        dimsOut = ldaSparse.dimsOut;
    }

    void project(const Template &src, Template &dst) const
    {
        Eigen::Map<Eigen::MatrixXf> inMap((float*)src.m().ptr<float>(), src.m().rows*src.m().cols, 1);
        Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(), dimsOut, 1);

        int d = selections.size();
        cv::Mat inSelect(d,1,CV_32F);
        for (int i = 0; i < d; i++)
            inSelect.at<float>(i) = src.m().at<float>(selections[i]);
        ldaSparse.project(Template(src.file, inSelect), dst);
    }

    void store(QDataStream &stream) const
    {
        stream << pcaKeep;
        stream << ldaSparse;
        stream << dimsOut;
        stream << selections;
    }

    void load(QDataStream &stream)
    {
        stream >> pcaKeep;
        stream >> ldaSparse;
        stream >> dimsOut;
        stream >> selections;
    }
};

BR_REGISTER(Transform, SparseLDATransform)

/*!
 * \ingroup distances
 * \brief L1 distance computed using eigen.
 * \author Josh Klontz \cite jklontz
 */
class L1Distance : public Distance
{
    Q_OBJECT

    float compare(const Template &a, const Template &b) const
    {
        const int size = a.m().rows * a.m().cols;
        Eigen::Map<Eigen::VectorXf> aMap((float*)a.m().data, size);
        Eigen::Map<Eigen::VectorXf> bMap((float*)b.m().data, size);
        return (aMap-bMap).cwiseAbs().sum();
    }
};

BR_REGISTER(Distance, L1Distance)

/*!
 * \ingroup distances
 * \brief L2 distance computed using eigen.
 * \author Josh Klontz \cite jklontz
 */
class L2Distance : public Distance
{
    Q_OBJECT

    float compare(const Template &a, const Template &b) const
    {
        const int size = a.m().rows * a.m().cols;
        Eigen::Map<Eigen::VectorXf> aMap((float*)a.m().data, size);
        Eigen::Map<Eigen::VectorXf> bMap((float*)b.m().data, size);
        return (aMap-bMap).squaredNorm();
    }
};

BR_REGISTER(Distance, L2Distance)

} // namespace br

#include "eigen3.moc"


#include "iri_geometry.h"
#include <math.h>
#include <iostream>
#include <Eigen/Dense>

// Spoint functions ------------------------------------------
Spoint::Spoint() : x(0) , y(0),  time_stamp(0) { }

Spoint::Spoint( double x_ , double y_ , double time_stamp_) :
		x(x_), y(y_), time_stamp(time_stamp_) { }
Spoint Spoint::operator+ (Spoint p2) const
{
	return Spoint(  x + p2.x , y + p2.y , time_stamp );
}
Spoint Spoint::operator- (Spoint p2) const
{
	return Spoint(  x - p2.x , y - p2.y , time_stamp);
}
Spoint Spoint::operator* (double k) const
{
	return Spoint(  x *k , y *k , time_stamp);
}
double Spoint::distance(Spoint p2) const
{
	return sqrt( distance2(p2)  );
}
double Spoint::distance2(Spoint p2) const
{
	return (x-p2.x)*(x-p2.x) + (y-p2.y)*(y-p2.y);
}
Spoint Spoint::propagate( double dt , Sforce f, double desired_velocity) const
{
	return *this;
}
void Spoint::print() const
{
	std::cout << "(x , y ,t) = (" << x << " , " << y  << " | " << time_stamp << " )" << std::endl;
}

//Spoint_cov methods -----------------------------------------------------------
Spoint_cov::Spoint_cov():
		Spoint()
{
	cov.reserve(4);
	cov.resize(4,0.0);
	cov[0] = 1.0;
	cov[3] = 1.0;
}
Spoint_cov::Spoint_cov( double x_ , double y_ , double time_stamp_,
		const std::vector<double>& cov_ ):
		Spoint( x_,y_,time_stamp_ )
{
	if ( cov_.empty() || cov_.size() < (std::size_t) 4 )
	{
		cov.reserve(4);
		cov.resize(4,0.0);
		cov[0] = 1.0;
		cov[3] = 1.0;
	}
	else
	{
		cov = cov_;
	}
}
double Spoint_cov::cov_dist( Spoint p2 ) const
{
	//analitical inversion of the 2x2 matrix
	//       det =   cov_xx * cov_yy - cov_xy * cov_xy
	double cov_det = cov[0] * cov[3] - cov[1] * cov[2];
	double dx = x - p2.x;
	double dy = y - p2.y;
	return ( dx*dx*cov[3] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/cov_det;
}
Spoint_cov Spoint_cov::operator+ (Spoint_cov p2) const
{
    return Spoint_cov(  x + p2.x , y + p2.y , time_stamp , cov);
}
Spoint_cov Spoint_cov::operator- (Spoint_cov p2) const
{
    return Spoint_cov(  x - p2.x , y - p2.y , time_stamp , cov);
}
Spoint_cov Spoint_cov::operator* (double k) const
{
    return Spoint_cov(  x*k , y*k , time_stamp , cov);
}
double Spoint_cov::cov_xx() const
{
	return cov[0];
}
double Spoint_cov::cov_yy() const
{
	return cov[3];
}
double Spoint_cov::cov_xy() const
{
	return cov[1];
}
void Spoint_cov::print() const
{
    Spoint::print();
    std::cout << "cov = [" << cov[0] << " , " << cov[1] << std::endl;
    std::cout << "       " << cov[2] << " , " << cov[3] << " ]" << std::endl;
}

//SpointV methods -----------------------------------------------------------
SpointV::SpointV() : Spoint(), vx(0), vy(0) { }

SpointV::SpointV( double x_ , double y_, double time_stamp_, double vx_, double vy_ ) :
		Spoint(x_,y_,time_stamp_), vx(vx_),vy(vy_) { }
SpointV SpointV::operator+ (SpointV p2) const
{
	return SpointV(  x + p2.x , y + p2.y , time_stamp,  vx + p2.vx , vy + p2.vy );
}
SpointV SpointV::operator- (SpointV p2) const
{
	return SpointV(  x - p2.x , y - p2.y , time_stamp-p2.time_stamp,  vx - p2.vx , vy - p2.vy );
}
SpointV SpointV::operator* ( double k) const
{
	return SpointV(  x * k , y * k, time_stamp,  vx * k , vy * k );
}
void SpointV::print() const
{
	std::cout << "(x , y ,t) = (" << x << " , " << y  << " | " << time_stamp << " )" <<
			"     (vx,vy) = ( " << vx << " , " << vy << " )" << std::endl;
}
SpointV SpointV::propagate( double dt , Sforce f, double desired_velocity) const
{
	//linear propagation (uniformly accelerated system)
	double vxn = vx + f.fx*dt;
	double vyn = vy + f.fy*dt;
	double dx = vx*dt + f.fx*dt*dt*0.5;
	double dy = vy*dt + f.fy*dt*dt*0.5;

	//apply constrains:
	double v = sqrt( vxn*vxn + vyn*vyn );
	if ( v > desired_velocity)
	{
		vxn *= desired_velocity /v;
		vyn *= desired_velocity /v;
		dx = vxn*dt;
		dy = vyn*dt;
	}

	return SpointV( x+dx,y+dy, time_stamp+dt, vxn, vyn);
}
double SpointV::orientation() const
{
	return atan2( vy, vx );
}

double SpointV::v() const
{
	return sqrt( vy*vy + vx*vx );
}

void SpointV::norm_v(double v)
{
	double theta = this->orientation();
	vx = cos(theta)*v;
	vy = sin(theta)*v;
}

double SpointV::angle_heading_point( Spoint p2 ) const
{
	double dx = p2.x - x;
	double dy = p2.y - y;
	return diffangle( atan2( dy , dx ), orientation() );
}

//SpointV_cov methods -----------------------------------------------------------
SpointV_cov::SpointV_cov() :
		SpointV()
{
	cov.reserve(16);
	cov.resize(16,0.0);
	cov[0] = 0.4;
	cov[5] = 0.4;
	cov[10] = 0.1;
	cov[15] = 0.1;
}
SpointV_cov::SpointV_cov(double x_ , double y_, double time_stamp_, double vx_, double vy_,
		const std::vector<double>& cov_) :
		SpointV(x_,y_,time_stamp_,vx_,vy_)
{
	if ( cov_.empty() || cov_.size() < (std::size_t) 16 )
	{
		cov.reserve(16);
		cov.resize(16,0.0);
		cov[0] = 0.4;
		cov[5] = 0.4;
		cov[10] = 0.1;
		cov[15] = 0.1;
	}
	else
	{
		cov = cov_;
	}
}
SpointV_cov::SpointV_cov(SpointV p, const std::vector<double>& cov_) :
		SpointV( p.x , p.y, p.time_stamp, p.vx, p.vy)
{
	if ( cov_.empty() || cov_.size() < (std::size_t) 16 )
	{
		cov.reserve(16);
		cov.resize(16,0.0);
		cov[0] = 0.4;
		cov[5] = 0.4;
		cov[10] = 0.1;
		cov[15] = 0.1;
	}
	else
	{
		cov = cov_;
	}
}
SpointV_cov::SpointV_cov(Spoint_cov p ):
		SpointV( p.x , p.y, p.time_stamp, 0.0, 0.0)
{
	cov.reserve(16);
	cov.resize(16,0.0);
	cov[0] = p.cov[0];
	cov[1] = p.cov[1];
	cov[4] = p.cov[2];
	cov[5] = p.cov[3];
	cov[10] = 0.1;
	cov[15] = 0.1;
}

double SpointV_cov::cov_dist( Spoint p2 ) const
{
	// TODO: consider velocities, right now is a covariance distance of positions
	//       det =   cov_xx * cov_yy - cov_xy * cov_xy
	double cov_det = cov[0] * cov[5] - cov[1] * cov[4];
	double dx = x - p2.x;
	double dy = y - p2.y;
	//
	return ( dx*dx*cov[5] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/cov_det;
}
double SpointV_cov::cov_dist( Spoint p2 , double &det) const
{
	det = cov[0] * cov[5] - cov[1] * cov[4];
	double dx = x - p2.x;
	double dy = y - p2.y;
	return ( dx*dx*cov[5] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/det;
}

double SpointV_cov::cov_distV( SpointV_cov p2, double &distV, double &distxv ) const
{
	// TODO: consider velocities, right now is a covariance distance of positions
	//       det =   cov_xx * cov_yy - cov_xy * cov_xy
	// SpointV_cov external for actual track, SpointV_cov internal for the initial Spoint_cov for this track in the first time that the cross is made.
	double cov_det = cov[0] * cov[4] - cov[1] * cov[5];
	double dx = x - p2.x;
	double dy = y - p2.y;
	double distx=( dx*dx*cov[5] - 2*dx*dy*cov[1] + dy*dy*cov[0] )/cov_det;

	double cov_detV = cov[10] * cov[14] - cov[11] * cov[15];
	double dvx = vx - p2.vx;
	double dvy = vy - p2.vy;
	distV=( dvx*dvx*cov[15] - 2*dvx*dvy*cov[11] + dvy*dvy*cov[10] )/cov_detV;

	distxv=distx+distV;

	return distx;

}

SpointV_cov SpointV_cov::operator+ (SpointV_cov p2) const
{
    return SpointV_cov(  x + p2.x , y + p2.y , time_stamp , vx + p2.vx , vy + p2.vy ,cov);
}
SpointV_cov SpointV_cov::operator- (SpointV_cov p2) const
{
    return SpointV_cov(  x - p2.x , y - p2.y , time_stamp-p2.time_stamp,  vx - p2.vx , vy - p2.vy ,cov);
}
SpointV_cov SpointV_cov::operator* (double k) const
{
    return SpointV_cov(  x * k , y * k, time_stamp,  vx * k , vy * k , cov);
}
SpointV_cov SpointV_cov::propagate( double dt , Sforce f, double desired_velocity) const
{
	SpointV new_point = SpointV::propagate(dt,f,desired_velocity);
	Eigen::MatrixXd cov_prev(4 , 4 );
	for (unsigned int i = 0; i < 4; ++i)
	{
		cov_prev.row(i) << cov[i*4],cov[i*4+1],cov[i*4+2],cov[i*4+3];
	}
	//std::cout << "cov_ prev = \n" <<  cov_prev << std::endl;
	Eigen::MatrixXd Phi(4 , 4 );
	Phi = Eigen::MatrixXd::Identity(4,4);
	Phi(0,2) = dt;
	Phi(1,3) = dt;
	//std::cout << "Phi = " << Phi << std::endl;
	Eigen::MatrixXd G(4 , 2 );
	G = Eigen::MatrixXd::Zero(4,2);
	G(0,0) = dt*dt/2;
	G(1,1) = dt*dt/2;
	G(2,0) = dt;
	G(3,1) = dt;
	//std::cout << "G = " << G << std::endl;
	Eigen::MatrixXd new_cov(4,4);
	Eigen::MatrixXd cov_f(2,2);
	cov_f = Eigen::MatrixXd::Zero(2,2);
	cov_f(0,0) = 0.3;//TODO this covariance should be associated to the corresponding force and not generic
	cov_f(1,1) = 0.3;
	//std::cout << "f = " << cov_f << std::endl;
	new_cov = Phi*cov_prev*Phi.transpose() + G*cov_f*G.transpose();
	std::vector<double> new_cov_std(16,0.0);
	for(unsigned int i = 0; i<16; ++i)
	{
		new_cov_std[i] = new_cov( i/4 , i%4 );
	}
	//std::cout << "new cov = \n" <<  new_cov << std::endl;
	return SpointV_cov( new_point, new_cov_std );
}
double SpointV_cov::cov_xx() const
{
	return cov[0];
}
double SpointV_cov::cov_yy() const
{
	return cov[5];
}
double SpointV_cov::cov_xy() const
{
	return cov[1];
}
Spoint_cov SpointV_cov::toSpoint_cov() const
{
	std::vector<double> cov_point;
	cov_point.reserve(4);
	cov_point.resize(4,0.0);
	cov_point[0] = cov[0];
	cov_point[1] = cov[1];
	cov_point[2] = cov[4];
	cov_point[3] = cov[5];
	return Spoint_cov( x,y,time_stamp, cov_point );
}
void SpointV_cov::print() const
{
    SpointV::print();
    std::cout << "covariance = [ " << cov[0] << " , " << cov[1] << " , " <<cov[2]  << " , " << cov[3] << std::endl <<
    		     "               " << cov[4] << " , " << cov[5] << " , " <<cov[6]  << " , " << cov[7] << std::endl <<
    		     "               " << cov[8] << " , " << cov[9] << " , " <<cov[10]  << " , " << cov[11] << std::endl <<
    		     "               " << cov[12] << " , " << cov[13] << " , " <<cov[14]  << " , " << cov[15] << std::endl;
}

//Spose methods -----------------------------------------------------------
Spose::Spose() : Spoint(), theta(0)  , v(0) , w(0)  { }
Spose::Spose( double x_ , double y_ , double time_stamp_ , double theta_ , double v_, double w_) :
	Spoint(x_,y_, time_stamp_), theta(theta_), v(v_), w(w_) { }
Spose Spose::operator+ (Spose p2) const
{
	return Spose(  x + p2.x , y + p2.y , theta + p2.theta, p2.time_stamp , p2.v , p2.w );
}
Spose Spose::operator- (Spose p2) const
{
	return Spose(  x - p2.x , y - p2.y , theta - p2.theta, p2.time_stamp , p2.v , p2.w );
}
Spose Spose::operator* ( double k) const
{
	return Spose(  x * k , y *k , theta, time_stamp , v , w );
}
double Spose::distance(Spoint p2) const
{
	return sqrt(  (x-p2.x)*(x-p2.x) + (y-p2.y)*(y-p2.y) );
}
double Spose::angle_heading_pose( Spose p2 ) const
{
	double dx = p2.x - x;
	double dy = p2.y - y;
	return diffangle( atan2( dy , dx ), theta );
}
double Spose::social_distance( Spose p2 ) const
{
	double lambda = 0.0;
	double phi = this->angle_heading_pose( p2 );
	double anisotropy = (lambda + (1-lambda)*(1 + cos(phi))/2 );
	return this->distance(p2)/anisotropy;
}
void Spose::print() const
{
	std::cout << "(x , y | theta) = (" << x << " , " << y  << " | " << theta <<
			" )  --  at t = " << time_stamp << std::endl <<
			"(v , w) = (" << v  << " , " << w << " )" << std::endl;
}

//Spose_cov methods -----------------------------------------------------------
Spose_cov::Spose_cov():	 Spose()
{
	cov.reserve(25);
	cov.resize(25,0.0);
	cov[0] = 0.5;//xx
	cov[6] = 0.5;//yy
	cov[12] = 0.05;//theta_theta
	cov[18] = 0.1;//cov_vv
	cov[24] = 0.1;//ww
}
Spose_cov::Spose_cov(double x_ , double y_ ,
		double time_stamp_ , double theta_ ,  double v_, double w_,
		const std::vector<double>& cov_ ):
		Spose( x_,y_,time_stamp_, theta_,v_,w_ )
{
	if ( cov_.empty() || cov_.size() < (std::size_t) 25 )
	{
		cov.reserve(25);
		cov.resize(25,0.0);
		cov[0] = 0.5;//xx
		cov[6] = 0.5;//yy
		cov[12] = 0.05;//theta_theta
		cov[18] = 0.1;//cov_vv
		cov[24] = 0.1;//ww
	}
	else
	{
		cov = cov_;
	}
}
Spose_cov::Spose_cov( Spose pose_, const std::vector<double>& cov_ ):
		Spose(pose_)
{
	if ( cov_.empty() || cov_.size() < (std::size_t) 25 )
	{
		cov.reserve(25);
		cov.resize(25,0.0);
		cov[0] = 0.5;//xx
		cov[6] = 0.5;//yy
		cov[12] = 0.05;//theta_theta
		cov[18] = 0.1;//cov_vv
		cov[24] = 0.1;//ww
	}
	else
	{
		cov = cov_;
	}
}
Spose_cov::Spose_cov( SpointV_cov point ):
		Spose(point.x,point.y,point.time_stamp,point.orientation(),point.v())
{
	cov.reserve(25);
	cov.resize(25,0.0);
	cov[0] = point.cov[0];//xx
	cov[6] = point.cov[5];//yy
	cov[12] = 0.05;//theta_theta
	cov[18] = 0.1;//cov_vv
	cov[24] = 0.1;//ww
}
double Spose_cov::distance( Spose_cov p2) const
{
	return Spose::distance( Spose(p2.x,p2.y) );
}
double Spose_cov::cov_dist( Spose p2 ) const
{
	//analitical inversion of the 2x2 matrix
	double cov_xx = cov[0];
	double cov_yy = cov[6];
	double cov_xy = cov[5];
	double cov_det = cov_xx * cov_yy - cov_xy * cov_xy;
	double dx = x - p2.x;
	double dy = y - p2.y;
	return ( dx*dx*cov_yy - 2*dx*dy*cov_xy + dy*dy*cov_xx )/cov_det;
}
double Spose_cov::cov_dist( Spose_cov p2 ) const
{
	return this->cov_dist( Spose(p2.x,p2.y) );
}

//Sdestinations methods -----------------------------------------------------------
Sdestination::Sdestination( int id_, double x_ , double y_ , double prob_ ,
		Sdestination::destination_type type_, std::vector<int> neighbours_ids_ ) :
	Spoint(x_,y_), id(id_), prob(prob_) ,
	type(type_), neighbours_ids(neighbours_ids_)  { }
Sdestination::~Sdestination(){}
bool Sdestination::operator== (Sdestination d2) const
{
	if ( x == d2.x && y == d2.y )
		return true;
	else
		return false;
}

void Sdestination::print() const
{
	std::cout << "destination " << id << "at (" << x << " , " << y  <<  ")  of type " << type  <<
		"   and probability = " << prob  << "neighbours " << neighbours_ids.size() << std::endl;
}

SdetectionObservation::SdetectionObservation() :
				SpointV_cov(), id(0) { }
SdetectionObservation::SdetectionObservation( int id_ , double time_stamp_ ,
		double x_, double y_, double vx_, double vy_, const std::vector<double>& cov_) :
			SpointV_cov(x_,y_,time_stamp_,vx_,vy_,cov_) , id(id_) { }
SdetectionObservation::SdetectionObservation( int id_ , SpointV_cov pointV_cov_) :
	SpointV_cov( pointV_cov_ ), id(id_) {}
void SdetectionObservation::print() const
{
	std::cout <<  "detection Observation = " << id << " th "  << std::endl;
	SpointV_cov::print();
}

//Sforce methods
Sforce::Sforce() : fx(0.0) , fy(0.0) {}
Sforce::Sforce(double fx_ , double fy_) : fx(fx_) , fy(fy_) {}
double Sforce::module() { return sqrt(fx*fx + fy*fy);}
double Sforce::module(double r2) { return sqrt(fx*fx + r2*fy*fy);}
double Sforce::module2( ) { return fx*fx + fy*fy;}
double Sforce::module2( double r2) { return fx*fx + r2*fy*fy;}
void Sforce::print() const
{
	std::cout <<  "Force = (" << fx << " , " << fy  << " )" << std::endl;
}
Sforce Sforce::operator+ (Sforce f2) const
{
	return Sforce(  fx + f2.fx , fy + f2.fy );
}
void Sforce::sum(Sforce f2)
//a method similar to +=
{
	fx += f2.fx;
	fy += f2.fy;
}
Sforce& Sforce::operator +=(Sforce f2)
{
	fx += f2.fx;
	fy += f2.fy;
	return *this;
}

Sforce Sforce::operator* (double k) const
{
	return Sforce(  fx *k , fy *k );
}

double Sforce::operator* (Spoint dr) const
{
	return this->fx * dr.x + this->fy * dr.y;
}
Sforce Sforce::operator- (Sforce f2) const
{
    return Sforce(  fx - f2.fx , fy - f2.fy );
}

double diffangle(double alpha , double beta)
{
	  double delta = alpha - beta;
	  if ( alpha >= beta )
	  {
	    while (delta >  PI)
	      delta -= 2*PI;
	  }
	  else
	  {
		  while (delta < -PI )
			  delta += 2*PI;
	  }
	  return delta;
}