#include "moebius_registration.h"

/*
Copyright (c) 2018, Michael Kazhdan, Alex Baden, and Keenan Crane
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer. Redistributions in binary form must reproduce
the above copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the distribution.

Neither the name of the Johns Hopkins University nor the names of its contributors
may be used to endorse or promote products derived from this software without specific
prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
TO, PROCUREMENT OF SUBSTITUTE  GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
DAMAGE.
*/

#include <stdio.h>
#include <stdlib.h>
#include <omp.h>
#include <map>
#include <unordered_map>
#include <Eigen/Dense>
#include <Misha/CmdLineParser.h>
#include <Misha/Ply.h>
#include <Misha/SparseMatrix.h>
#include <Misha/Solver.h>
#include <Misha/Timer.h>
#include <Misha/MemoryUsage.h>
#include <Misha/SphericalGeometry.h>

enum
{
	MESH_TYPE_TRIANGLE,
	MESH_TYPE_POLYGON,
	MESH_TYPE_TRIANGULATED_POLYGON,
	MESH_TYPE_COUNT
};
enum
{
	HOLE_FILL_TYPE_NONE,
	HOLE_FILL_TYPE_CENTER_TRIANGULATION,
	HOLE_FILL_TYPE_MINIMAL_AREA_TRIANGULATION,
	HOLE_FILL_TYPE_POLYGON,
	HOLE_FILL_TYPE_COUNT
};
const char *MeshTypeNames[] = { "triangle mesh" , "polygon mesh" , "implicitly triangulated polygon mesh" };
const char* HoleFillTypeNames[] = { "none" , "center triangulation" , "minimal area triangulation" , "polygon" };
cmdLineParameter< char* > In("in"), Out("out"), OutGrid("outG"), OutTessellation("outT");
cmdLineParameter< int > Iterations("iters", 100), Threads("threads", omp_get_num_procs()), Resolution("res", 256), SHDegree("degree", 1), AdvectionSteps("aSteps", 10), MeshType("mesh", MESH_TYPE_TRIANGLE + 1), HoleFillType("fill", HOLE_FILL_TYPE_NONE), CenterToInversion("c2i", 2);
cmdLineParameter< float > StepSize("stepSize", 0.1f), CutOff("cutOff", 1e-10f), Smooth("smooth", 5e-4f), AdvectionStepSize("aStepSize", 0.05f), GSSTolerance("gssTolerance", (float)1e-6), PoincareMaxNorm("poincareMaxNorm", 2.f);
cmdLineReadable Verbose("verbose"), FullVerbose("fullVerbose"), ASCII("ascii"), Randomize("random"), NoCenter("noCenter"), Collapse("collapse"), NoOrient("noOrient"), Lump("lump"), Spherical("spherical");

void Usage(const char* ex)
{
	printf("Usage %s:\n", ex);
	printf("\t --%s <input mesh>\n", In.name);
	printf("\t[--%s <output mesh>]\n", Out.name);
	printf("\t[--%s <output spherical grid>]\n", OutGrid.name);
	printf("\t[--%s <output spherical tessellation>]\n", OutTessellation.name);
	printf("\t[--%s <mesh type>=%d]\n", MeshType.name, MeshType.value);
	for (int i = 0; i < MESH_TYPE_COUNT; i++) printf("\t\t%d] %s\n", i + 1, MeshTypeNames[i]);
	printf("\t[--%s <hole fill type>=%d]\n", HoleFillType.name, HoleFillType.value);
	for (int i = 0; i < HOLE_FILL_TYPE_COUNT; i++) printf("\t\t%d] %s\n", i, HoleFillTypeNames[i]);
	printf("\t[--%s <CMCF iterations>=%d]\n", Iterations.name, Iterations.value);
	printf("\t[--%s <CMCF step size>=%f]\n", StepSize.name, StepSize.value);
	printf("\t[--%s <Moebius centering cut-off>=%g]\n", CutOff.name, CutOff.value);
	printf("\t[--%s <spherical harmonic degree>=%d]\n", SHDegree.name, SHDegree.value);
	printf("\t[--%s <advection steps>=%d]\n", AdvectionSteps.name, AdvectionSteps.value);
	printf("\t[--%s <advection step size>=%f]\n", AdvectionStepSize.name, AdvectionStepSize.value);
	printf("\t[--%s <spherical resolution>=%d]\n", Resolution.name, Resolution.value);
	printf("\t[--%s <spherical diffusion time>=%g]\n", Smooth.name, Smooth.value);
	printf("\t[--%s <center to inversion type>=%d]\n", CenterToInversion.name, CenterToInversion.value);
	printf("\t\t0] Trivial\n");
	printf("\t\t1] Golden section search\n");
	printf("\t\t2] Poincare\n");
	printf("\t[--%s <golden section search tolerance>=%e]\n", GSSTolerance.name, GSSTolerance.value);
	printf("\t[--%s <Poincare max norm>=%e]\n", PoincareMaxNorm.name, PoincareMaxNorm.value);
	printf("\t[--%s]\n", Randomize.name);
	printf("\t[--%s]\n", NoCenter.name);
	printf("\t[--%s]\n", ASCII.name);
	printf("\t[--%s]\n", Collapse.name);
	printf("\t[--%s]\n", NoOrient.name);
	printf("\t[--%s]\n", Spherical.name);
	printf("\t[--%s]\n", Lump.name);
	printf("\t[--%s]\n", Verbose.name);
	printf("\t[--%s]\n", FullVerbose.name);
}
cmdLineReadable* params[] = { &In , &Out , &OutGrid , &OutTessellation , &Iterations , &StepSize , &Threads , &Verbose , &FullVerbose , &Randomize , &ASCII , &NoCenter , &Resolution , &Smooth , &Collapse , &NoOrient , &SHDegree , &AdvectionSteps , &AdvectionStepSize , &CenterToInversion , &GSSTolerance , &PoincareMaxNorm , &Lump , &MeshType , &HoleFillType , &Spherical , NULL };

template< class Real >
Point3D< Real > NormalColor(Point3D< Real > n)
{
	Point3D< Real > c = (-n + Point3D< Real >(1, 1, 1)) * Real(128);
	for (int d = 0; d < 3; d++)
	{
		if (c[d] > Real(255)) c[d] = Real(255);
		if (c[d] < Real(0)) c[d] = Real(0);
	}
	return c;
}

template< typename Real >
SquareMatrix< Real, 3 > MassMatrix(const Point3D< Real > vertices[])
{
	SquareMatrix< Real, 3 > mass;
	Real area = (Real)Length(Point3D< Real >::CrossProduct(vertices[1] - vertices[0], vertices[2] - vertices[0])) / 2;
	mass(1, 0) = mass(0, 1) = mass(2, 0) = mass(0, 1) = mass(0, 2) = mass(1, 2) = mass(2, 1) = area / 12;
	mass(0, 0) = mass(1, 1) = mass(2, 2) = area / 6;
	return mass;
}
template< typename Real >
SquareMatrix< Real, 3 > StiffnessMatrix(const Point3D< Real > vertices[])
{
	SquareMatrix< Real, 3 > stiffness;
	Real area = (Real)Length(Point3D< Real >::CrossProduct(vertices[1] - vertices[0], vertices[2] - vertices[0])) / 2;
	for (int i = 0; i < 3; i++)
	{
		int i0 = (i + 1) % 3, i1 = (i + 2) % 3;
		Real dot = Point3D< Real >::Dot(vertices[i0] - vertices[i], vertices[i1] - vertices[i]);
		stiffness(i0, i1) = stiffness(i1, i0) = -dot / area;
	}
	stiffness(0, 0) = -(stiffness(0, 1) + stiffness(0, 2));
	stiffness(1, 1) = -(stiffness(1, 2) + stiffness(1, 0));
	stiffness(2, 2) = -(stiffness(2, 0) + stiffness(2, 1));
	return stiffness;
}

template< typename Real >
struct Mesh
{
	virtual int faces(void) const = 0;
	virtual Point3D< Real > normal(int f) const = 0;
	virtual Point3D< Real > center(int f) const = 0;
	virtual Real area(int f) const = 0;
	virtual SparseMatrix< Real, int > massMatrix(bool lump) const = 0;
	virtual SparseMatrix< Real, int > stiffnessMatrix(void) const = 0;
	virtual std::vector< Real > vertexAreas(void) const = 0;
	virtual std::vector< Point3D< Real > > vertexNormals(void) const = 0;
	virtual void push_back(int v1, int v2, int v3) = 0;
	virtual void pop_back(void) = 0;
	virtual void update(void) {}

	std::vector< Point3D< Real > > vertices;

	Real area(void) const
	{
		Real a = 0;
#pragma omp parallel for reduction( + : a )
		for (int f = 0; f < faces(); f++) a += area(f);
		return a;
	}
	Point3D< Real > center(void) const
	{
		Real area = 0, x = 0, y = 0, z = 0;
#pragma omp parallel for reduction( + : area , x , y , z )
		for (int f = 0; f < faces(); f++)
		{
			Real a = this->area(f);
			Point3D< Real > c = center(f)*a;
			area += a, x += c[0], y += c[1], z += c[2];
		}
		return Point3D< Real >(x, y, z) / area;
	}
	Real makeUnitArea(void)
	{
		Real scl = (Real)(1. / sqrt(area()));
#pragma omp parallel for
		for (int i = 0; i < vertices.size(); i++) vertices[i] *= scl;
		return (Real)1. / scl;
	}
	void pose(Point3D< Real >& center, Real& scale)
	{
		center = this->center();
		for (int i = 0; i < vertices.size(); i++) vertices[i] -= center;
		scale = 0;
		for (int i = 0; i < vertices.size(); i++) for (int j = 0; j < 3; j++) scale = std::max< Real >(scale, (Real)fabs(vertices[i][j]));
		scale *= 2;
		for (int i = 0; i < vertices.size(); i++) vertices[i] /= scale;
	}
	void pose(void)
	{
		Point3D< Real > center;
		Real scale;
		pose(center, scale);
	}

	Real radialDeviation(void) const
	{
		Real area = 0;
		std::vector< Real > vertexAreas = this->vertexAreas();
#pragma omp parallel for reduction( + : area )
		for (int i = 0; i < vertexAreas.size(); i++) area += vertexAreas[i];
		Point3D< Real > center;
		for (int i = 0; i < vertices.size(); i++) center += vertices[i] * vertexAreas[i];
		center /= area;

		// set v = \sum_i[ w_i * v_i ]
		// var = \sum_i[ w_i * ( v_i -v )^2 ]
		//     = \sum_i[ w_i * ( v_i^2 + v^2 - 2 * v_i * v ) ]
		//     = \sum_i[ w_i * v_i^2 ] + \sum_i[ w_i * v^2 ] - \sum_i[ 2 * w_i * v_i * v ) ]
		//     = \sum_i[ w_i * v_i^2 ] + v^2 - 2 * v^2
		//     = \sum_i[ w_i * v_i^2 ] - v^2
		Real average = 0, var = 0;
#pragma omp parallel for reduction( + : average , var )
		for (int i = 0; i < vertices.size(); i++)
		{
			Real r = (Real)sqrt(Point3D< Real >::SquareNorm(Point3D< Real >(vertices[i] - center)));
			average += r * vertexAreas[i];
			var += r * r * vertexAreas[i];
		}
		average /= area, var /= area;
		var -= average * average;
		var = (Real)sqrt(var);
		return var / average;
	}
};

template< typename Real >
struct TriangleMesh : public Mesh< Real >
{
	std::vector< TriangleIndex > triangles;
	using Mesh< Real >::center;
	using Mesh< Real >::vertices;

	int faces(void) const { return (int)triangles.size(); }
	void push_back(int v1, int v2, int v3) { triangles.push_back(TriangleIndex(v1, v2, v3)); }
	void pop_back(void) { triangles.pop_back(); }

	Point3D< Real > normal(int t) const { return Point3D< Real >::CrossProduct(vertices[triangles[t][1]] - vertices[triangles[t][0]], vertices[triangles[t][2]] - vertices[triangles[t][0]]); }
	Point3D< Real > center(int t) const { return (vertices[triangles[t][0]] + vertices[triangles[t][1]] + vertices[triangles[t][2]]) / 3; }
	Real area(int t) const { return (Real)Length(normal(t)) / 2.; }

#if 0
	template< typename TriangleMatrixFunctor >
	SparseMatrix< Real, int > matrix(TriangleMatrixFunctor F) const
#else
	SparseMatrix< Real, int > matrix(SquareMatrix< Real, 3 >(*F)(const Point3D< Real >[])) const
#endif
	{
		struct Entry
		{
			Entry(void) : row(-1), col(-1), value(0) {}
			Entry(int r, int c, Real v) : row(r), col(c), value(v) {}
			int row, col;
			Real value;
		};
		SparseMatrix< Real, int > M;
		M.resize((int)vertices.size());
		std::vector< std::vector< Entry > > entries(omp_get_max_threads());
#pragma omp parallel for
		for (int t = 0; t < triangles.size(); t++)
		{
			int thread = omp_get_thread_num();
			Point3D< Real > v[] = { vertices[triangles[t][0]] , vertices[triangles[t][1]] , vertices[triangles[t][2]] };
			SquareMatrix< Real, 3 > m = F(v);
			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) entries[thread].push_back(Entry(triangles[t][i], triangles[t][j], m(i, j)));
		}
		for (int i = 0; i < entries.size(); i++) for (int j = 0; j < entries[i].size(); j++)	M.rowSizes[entries[i][j].row]++;
#pragma omp parallel for
		for (int i = 0; i < M.rows; i++)
		{
			int rowSize = M.rowSizes[i];
			M.rowSizes[i] = 0;
			M.SetRowSize(i, rowSize);
			M.rowSizes[i] = 0;
		}
		for (int i = 0; i < entries.size(); i++) for (int j = 0; j < entries[i].size(); j++) M[entries[i][j].row][M.rowSizes[entries[i][j].row]++] = MatrixEntry< Real, int >(entries[i][j].col, entries[i][j].value);
#pragma omp parallel for
		for (int i = 0; i < M.rows; i++)
		{
			std::unordered_map< int, Real > row;
			for (int j = 0; j < M.rowSizes[i]; j++) row[M[i][j].N] += M[i][j].Value;
			M.SetRowSize(i, (int)row.size());
			int j = 0;
			for (std::unordered_map< int, Real >::const_iterator iter = row.begin(); iter != row.end(); iter++) M[i][j++] = MatrixEntry< Real, int >(iter->first, iter->second);
		}
		return M;
	}
	SparseMatrix< Real, int > massMatrix(bool lump) const
	{
		SparseMatrix< Real, int > mass = matrix(MassMatrix);
		if (lump)
		{
			for (int i = 0; i < mass.rows; i++)
			{
				Real sum = 0;
				for (int j = 0; j < mass.rowSizes[i]; j++) { sum += mass[i][j].Value; mass[i][j].Value = 0; }
				for (int j = 0; j < mass.rowSizes[i]; j++) if (mass[i][j].N == i) mass[i][j].Value = sum;
			}
		}
		return mass;
	}
	SparseMatrix< Real, int > stiffnessMatrix(void) const { return matrix(StiffnessMatrix); }

	std::vector< Real > vertexAreas(void) const
	{
		std::vector< Real > areas(vertices.size(), 0);
		for (int t = 0; t < triangles.size(); t++)
		{
			Real a = area(t) / 3;
			for (int j = 0; j < 3; j++) areas[triangles[t][j]] += a;
		}
		return areas;
	}
	std::vector< Point3D< Real > > vertexNormals(void) const
	{
		std::vector< Point3D< Real > > normals(vertices.size());
		for (int t = 0; t < triangles.size(); t++)
		{
			Point3D< Real > n = normal(t);
			Real a = area(t);
			n *= a / (Real)Length(n);
			for (int j = 0; j < 3; j++) normals[triangles[t][j]] += n;
		}
		return normals;
	}
};

template< typename Real >
struct PolygonMesh : public Mesh< Real >
{
protected:
	std::unordered_map< unsigned long long, int > _edges;
public:
	std::vector< std::vector< int > > polygons;
	using Mesh< Real >::center;
	using Mesh< Real >::vertices;

	int faces(void) const { return (int)polygons.size(); }
	void push_back(int v1, int v2, int v3) { polygons.push_back(std::vector< int >({ v1 , v2 , v3 })); }
	void pop_back(void) { polygons.pop_back(); }

	Point3D< Real > normal(int p) const
	{
		Point3D< Real > n;
		for (int i = 0; i < polygons[p].size(); i++)
		{
			Point3D< Real > b = vertices[polygons[p][(i + 1) % polygons[p].size()]] + vertices[polygons[p][i]];
			Point3D< Real > e = vertices[polygons[p][(i + 1) % polygons[p].size()]] - vertices[polygons[p][i]];
			n += Point3D< Real >::CrossProduct(b, e) / 2;
		}
		return n;
	}
	Point3D< Real > center(int p) const
	{
		Point3D< Real > c;
		for (int i = 0; i < polygons[p].size(); i++) c += vertices[polygons[p][i]];
		return c / (int)polygons[p].size();
	}
	Real area(int p) const { return (Real)Length(normal(p)) / 2.; }

	SparseMatrix< Real, int > massMatrix(bool lump) const
	{
		SparseMatrix< Real, int > mass;
		mass.resize((int)vertices.size());
		Eigen::DiagonalMatrix< double, Eigen::Dynamic > M0 = mass0();
#pragma omp parallel for
		for (int i = 0; i < mass.rows; i++)
		{
			mass.SetRowSize(i, 1);
			mass[i][0] = MatrixEntry< Real, int >(i, M0.diagonal()(i));
		}
		return mass;
	}

	SparseMatrix< Real, int > stiffnessMatrix(void) const
	{
		SparseMatrix< Real, int > stiffness;
		stiffness.resize((int)vertices.size());
		Eigen::SparseMatrix< double > D = coBoundary();
		Eigen::SparseMatrix< double > M = mass1();
		Eigen::SparseMatrix< double > S = D.transpose() * M * D;

		for (int i = 0; i < S.outerSize(); i++) for (Eigen::SparseMatrix< double >::InnerIterator it(S, i); it; ++it) stiffness.rowSizes[it.row()]++;
		for (int i = 0; i < stiffness.rows; i++)
		{
			int cols = stiffness.rowSizes[i];
			stiffness.rowSizes[i] = 0;
			stiffness.SetRowSize(i, cols);
			stiffness.rowSizes[i] = 1;
		}
		for (int i = 0; i < S.outerSize(); i++) for (Eigen::SparseMatrix< double >::InnerIterator it(S, i); it; ++it)
		{
			if (it.row() == it.col()) stiffness[it.row()][0] = MatrixEntry< Real, int >(it.col(), it.value());
			else
			{
				int idx = stiffness.rowSizes[it.row()]++;
				stiffness[it.row()][idx] = MatrixEntry< Real, int >(it.col(), it.value());
			}
		}
		return stiffness;
	}
	std::vector< Real > vertexAreas(void) const
	{
		std::vector< Real > areas(vertices.size(), 0);
		for (int p = 0; p < polygons.size(); p++)
		{
			Real a = area(p) / (int)polygons[p].size();
			for (int j = 0; j < polygons[p].size(); j++) areas[polygons[p][j]] += a;
		}
		return areas;
	}
	std::vector< Point3D< Real > > vertexNormals(void) const
	{
		std::vector< Point3D< Real > > normals(vertices.size());
		for (int p = 0; p < polygons.size(); p++)
		{
			Point3D< Real > n = normal(p);
			Real a = area(p);
			n *= a / (Real)Length(n);
			for (int j = 0; j < polygons.size(); j++) normals[polygons[p][j]] += n;
		}
		return normals;
	}

	void update(void)
	{
		for (int i = 0; i < polygons.size(); i++) for (int j = 0; j < polygons[i].size(); j++) for (int k = 0; k < polygons[i].size(); k++) if (j != k) _edges[EdgeKey(polygons[i][j], polygons[i][k])] = 0;
		int idx = 0;
		for (auto e = _edges.begin(); e != _edges.end(); e++) e->second = idx++;
	}

	Eigen::DiagonalMatrix< double, Eigen::Dynamic > mass0(void) const
	{
		Eigen::DiagonalMatrix< double, Eigen::Dynamic > M0(vertices.size());
		std::vector< double > areas(polygons.size());
#pragma omp parallel for
		for (int i = 0; i < polygons.size(); i++) areas[i] = area(i) / polygons[i].size();
#pragma omp parallel for
		for (int i = 0; i < vertices.size(); i++) M0.diagonal()(i) = 0;
		for (int i = 0; i < polygons.size(); i++) for (int j = 0; j < polygons[i].size(); j++) M0.diagonal()(polygons[i][j]) += areas[i];
		return M0;
	}
	Eigen::SparseMatrix< double > mass1(double lambda = 1) const
	{
		std::vector< Eigen::Triplet< double > > triplets;
		for (int i = 0; i < polygons.size(); i++)
		{
			Eigen::MatrixXd S = faceMass1(i, lambda);
			const std::vector< int > &p = polygons[i];
			int pSize = (int)p.size();
			for (int j = 0; j < pSize; j++) for (int k = 0; k < pSize; k++)
			{
				auto it1 = _edges.find(EdgeKey(p[j], p[(j + 1) % pSize]));
				auto it2 = _edges.find(EdgeKey(p[k], p[(k + 1) % pSize]));
				double sign1 = p[j] < p[(j + 1) % pSize] ? 1 : -1;
				double sign2 = p[k] < p[(k + 1) % pSize] ? 1 : -1;
				triplets.push_back(Eigen::Triplet< double >(it1->second, it2->second, S(j, k)*sign1*sign2));
			}
		}
		Eigen::SparseMatrix< double > M1((int)_edges.size(), (int)_edges.size());
		M1.setFromTriplets(triplets.begin(), triplets.end());
		return M1;
	}
	Eigen::SparseMatrix< double > coBoundary(void) const
	{
		std::vector< Eigen::Triplet< double > > triplets;
		triplets.reserve(_edges.size() * 2);

		int idx = 0;
		for (auto e = _edges.begin(); e != _edges.end(); e++)
		{
			int i, j;
			FactorEdgeKey(e->first, i, j);
			triplets.push_back(Eigen::Triplet< double >(e->second, i, -1));
			triplets.push_back(Eigen::Triplet< double >(e->second, j, 1));
		}
		Eigen::SparseMatrix< double > D((int)_edges.size(), (int)vertices.size());
		D.setFromTriplets(triplets.begin(), triplets.end());
		return D;
	}

	Eigen::MatrixXd faceMass1(int p, double lambda) const
	{
		std::vector< Point3D< double > > _vertices(polygons[p].size());
		for (int i = 0; i < polygons[p].size(); i++) _vertices[i] = Point3D< double >(vertices[polygons[p][i]]);

		Eigen::MatrixXd B(polygons[p].size(), 3), E(polygons[p].size(), 3), _E(polygons[p].size(), 3);
		for (int i = 0; i < polygons[p].size(); i++)
		{
			Point3D< double > v[] = { _vertices[i] , _vertices[(i + 1) % polygons[p].size()] };
			for (int j = 0; j < 3; j++)
			{
				E(i, j) = v[1][j] - v[0][j];
				B(i, j) = (v[1][j] + v[0][j]) / 2.;
			}
		}
		Eigen::MatrixXd A = E.transpose() * B;
		Eigen::MatrixXd _S = B * B.transpose() * sqrt(2) / A.norm();
		Point3D< double > n(-A(1, 2), A(0, 2), -A(0, 1));
		n /= Length(n);
		for (int i = 0; i < polygons[p].size(); i++) _vertices[i] -= n * Point3D< double >::Dot(n, _vertices[i]);
		for (int i = 0; i < polygons[p].size(); i++)
		{
			Point3D< double > v[] = { _vertices[i] , _vertices[(i + 1) % polygons[p].size()] };
			for (int j = 0; j < 3; j++) _E(i, j) = v[1][j] - v[0][j];
		}

		Eigen::MatrixXd C = _E.transpose().fullPivLu().kernel();
		return _S + C * C.transpose() * lambda;
	}
};

// Triangulate the polygons by adding the mid-point but constrain the value at the mid-point to be the average of the values along the vertices of the face
template< typename Real >
struct TriangulatedPolygonMesh : public Mesh< Real >
{
	std::vector< std::vector< int > > polygons;
	using Mesh< Real >::center;
	using Mesh< Real >::vertices;

	int faces(void) const { return (int)polygons.size(); }
	void push_back(int v1, int v2, int v3) { polygons.push_back(std::vector< int >({ v1 , v2 , v3 })); }
	void pop_back(void) { polygons.pop_back(); }

	Point3D< Real > normal(int p) const
	{
		Point3D< Real > n;
		for (int i = 0; i < polygons[p].size(); i++)
		{
			Point3D< Real > b = vertices[polygons[p][(i + 1) % polygons[p].size()]] + vertices[polygons[p][i]];
			Point3D< Real > e = vertices[polygons[p][(i + 1) % polygons[p].size()]] - vertices[polygons[p][i]];
			n += Point3D< Real >::CrossProduct(b, e) / 2;
		}
		return n;
	}
	Point3D< Real > center(int p) const
	{
		Point3D< Real > c;
		for (int i = 0; i < polygons[p].size(); i++) c += vertices[polygons[p][i]];
		return c / (int)polygons[p].size();
	}
	Real area(int p) const
	{
		if (polygons[p].size() == 3)
		{
			Point3D< Real > v[] = { vertices[polygons[p][0]] , vertices[polygons[p][1]] , vertices[polygons[p][2]] };
			Real a = (Real)Length(Point3D< Real >::CrossProduct(v[1] - v[0], v[2] - v[0])) / 2;
			return a;
		} else
		{
			Real a = 0;
			Point3D< Real > c = center(p);
			for (int i0 = 0; i0 < polygons[p].size(); i0++)
			{
				int i1 = (i0 + 1) % polygons[p].size();
				Point3D< Real > v[] = { vertices[polygons[p][i0]] , vertices[polygons[p][i1]] , c };
				a += (Real)Length(Point3D< Real >::CrossProduct(v[1] - v[0], v[2] - v[0])) / 2;
			}
			return a;
		}
	}

	std::vector< Real > vertexAreas(void) const
	{
		std::vector< Real > areas(vertices.size(), 0);
		for (int p = 0; p < polygons.size(); p++)
		{
			if (polygons[p].size() == 3)
			{
				Point3D< Real > v[] = { vertices[polygons[p][0]] , vertices[polygons[p][1]] , vertices[polygons[p][2]] };
				Real a = (Real)Length(Point3D< Real >::CrossProduct(v[1] - v[0], v[2] - v[0])) / 2;
				a /= 3;
				for (int i = 0; i < 3; i++) areas[polygons[p][i]] += a;
			} else
			{
				Point3D< Real > c = center(p);
				for (int i0 = 0; i0 < polygons[p].size(); i0++)
				{
					int i1 = (i0 + 1) % polygons[p].size();
					Point3D< Real > v[] = { vertices[polygons[p][i0]] , vertices[polygons[p][i1]] , c };
					Real a = (Real)Length(Point3D< Real >::CrossProduct(v[1] - v[0], v[2] - v[0])) / 2;
					a /= 2;
					areas[polygons[p][i0]] += a, areas[polygons[p][i1]] += a;
				}
			}
		}
		return areas;
	}
	std::vector< Point3D< Real > > vertexNormals(void) const
	{
		std::vector< Point3D< Real > > normals(vertices.size());
		for (int p = 0; p < polygons.size(); p++)
		{
			Point3D< Real > n = normal(p);
			Real a = area(p);
			n *= a / (Real)Length(n);
			for (int j = 0; j < polygons.size(); j++) normals[polygons[p][j]] += n;
		}
		return normals;
	}

	SparseMatrix< Real, int > massMatrix(bool lump) const
	{
		SparseMatrix< Real, int > mass = matrix(MassMatrix);
		if (lump)
		{
#pragma omp parallel for
			for (int i = 0; i < mass.rows; i++)
			{
				Real sum = 0;
				for (int j = 0; j < mass.rowSizes[i]; j++) { sum += mass[i][j].Value; mass[i][j].Value = 0; }
				for (int j = 0; j < mass.rowSizes[i]; j++) if (mass[i][j].N == i) { mass[i][j].Value = sum; break; }
			}
		}
		return mass;
	}
	SparseMatrix< Real, int > stiffnessMatrix(void) const { return matrix(StiffnessMatrix); }

#if 0
	template< typename TriangleMatrixFunctor >
	SparseMatrix< Real, int > matrix(TriangleMatrixFunctor F) const
#else
	SparseMatrix< Real, int > matrix(SquareMatrix< Real, 3 >(*F)(const Point3D< Real >[])) const
#endif
	{
		struct Entry
		{
			Entry(void) : row(-1), col(-1), value(0) {}
			Entry(int r, int c, Real v) : row(r), col(c), value(v) {}
			int row, col;
			Real value;
		};
		SparseMatrix< Real, int > M;
		M.resize((int)vertices.size());
		std::vector< std::vector< Entry > > entries(omp_get_max_threads());
#pragma omp parallel for
		for (int p = 0; p < polygons.size(); p++)
		{
			int thread = omp_get_thread_num();
			int pSize = (int)polygons[p].size();
			Eigen::MatrixXd m = faceMatrix(p, F);
			for (int i = 0; i < pSize; i++) for (int j = 0; j < pSize; j++) entries[thread].push_back(Entry(polygons[p][i], polygons[p][j], m(i, j)));
		}
		for (int i = 0; i < entries.size(); i++) for (int j = 0; j < entries[i].size(); j++)	M.rowSizes[entries[i][j].row]++;
#pragma omp parallel for
		for (int i = 0; i < M.rows; i++)
		{
			int rowSize = M.rowSizes[i];
			M.rowSizes[i] = 0;
			M.SetRowSize(i, rowSize);
			M.rowSizes[i] = 0;
		}
		for (int i = 0; i < entries.size(); i++) for (int j = 0; j < entries[i].size(); j++) M[entries[i][j].row][M.rowSizes[entries[i][j].row]++] = MatrixEntry< Real, int >(entries[i][j].col, entries[i][j].value);
#pragma omp parallel for
		for (int i = 0; i < M.rows; i++)
		{
			std::unordered_map< int, Real > row;
			for (int j = 0; j < M.rowSizes[i]; j++) row[M[i][j].N] += M[i][j].Value;
			M.SetRowSize(i, (int)row.size());
			int j = 0;
			for (std::unordered_map< int, Real >::const_iterator iter = row.begin(); iter != row.end(); iter++) M[i][j++] = MatrixEntry< Real, int >(iter->first, iter->second);
		}
		return M;
	}

	Eigen::MatrixXd centerConstraintMatrix(int p) const
	{
		int pSize = (int)polygons[p].size();
		Eigen::MatrixXd C(pSize + 1, pSize);
		for (int i = 0; i < C.rows(); i++) for (int j = 0; j < C.cols(); j++) C(i, j) = 0;
		for (int i = 0; i < pSize; i++) C(i, i) = 1, C(pSize, i) = 1. / pSize;
		return C;
	}

	template< typename TriangleMatrixFunctor >
	Eigen::MatrixXd faceMatrix(int p, TriangleMatrixFunctor F) const
	{
		int pSize = (int)polygons[p].size();
		if (pSize == 3)
		{
			Eigen::MatrixXd M(pSize, pSize);
			Point3D< Real > v[] = { vertices[polygons[p][0]] , vertices[polygons[p][1]] , vertices[polygons[p][2]] };
			SquareMatrix< Real, 3 > m = F(v);
			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) M(i, j) = m(i, j);
			return M;
		} else
		{
			Eigen::MatrixXd M(pSize + 1, pSize + 1), C = centerConstraintMatrix(p);
			for (int i = 0; i < M.rows(); i++) for (int j = 0; j < M.cols(); j++) M(i, j) = 0;

			Point3D< Real > c = center(p);
			int idx[] = { 0 , 0 , pSize };
			for (int i0 = 0; i0 < pSize; i0++)
			{
				int i1 = (i0 + 1) % pSize;
				idx[0] = i0, idx[1] = i1;
				Point3D< Real > v[] = { vertices[polygons[p][i0]] , vertices[polygons[p][i1]] , c };
				SquareMatrix< Real, 3 > m = F(v);
				for (int j = 0; j < 3; j++) for (int k = 0; k < 3; k++) M(idx[j], idx[k]) += m(j, k);
			}
			return C.transpose() * M * C;
		}
	}
};

template< typename Real >
struct QuasiConformalRatio
{
	static const Real ConformalCutOff;
	std::vector< TriangleIndex > triangles;
	std::vector< SquareMatrix< Real, 2 > > massInvs;
	std::vector< Real > areas;

	QuasiConformalRatio(const            TriangleMesh< Real > & tMesh, int tCount) : QuasiConformalRatio(&tMesh.vertices[0], &tMesh.triangles[0], tCount) { }
	QuasiConformalRatio(const             PolygonMesh< Real > & pMesh, int tCount) : QuasiConformalRatio(&pMesh.vertices[0], &pMesh.polygons[0], tCount) { }
	QuasiConformalRatio(const TriangulatedPolygonMesh< Real > &tpMesh, int tCount) : QuasiConformalRatio(&tpMesh.vertices[0], &tpMesh.polygons[0], tCount) { }

	QuasiConformalRatio(const Point3D< Real > *v, const TriangleIndex *t, int tCount)
	{
		triangles.resize(tCount);
		for (int i = 0; i < tCount; i++) triangles[i] = t[i];
		_init(v);
	}
	QuasiConformalRatio(const Point3D< Real > *v, const std::vector< int > *p, int pCount)
	{
		MinimalAreaTriangulation< Real > MAT;
		int count = 0;
		for (int i = 0; i < pCount; i++) count += (int)p[i].size() - 2;
		triangles.reserve(count);
		for (int i = 0; i < pCount; i++)
		{
			const std::vector< int >& poly = p[i];
			std::vector< Point3D< Real > > _vertices(poly.size());
			std::vector< TriangleIndex > _triangles;
			for (int j = 0; j < poly.size(); j++) _vertices[j] = v[poly[j]];
			MAT.GetTriangulation(_vertices, _triangles);
			for (int j = 0; j < _triangles.size(); j++) triangles.push_back(TriangleIndex(poly[_triangles[j][0]], poly[_triangles[j][1]], poly[_triangles[j][2]]));
		}
		_init(v);
	}

	Real operator()(const std::vector< Point3D< Real > > &vertices) const
	{
		Real error = 0, area = 0;
#pragma omp parallel for reduction( + : error , area )
		for (int i = 0; i < triangles.size(); i++)
		{
			Point3D< Real > v[] = { vertices[triangles[i][0]] , vertices[triangles[i][1]] , vertices[triangles[i][2]] };
			SquareMatrix< Real, 2 > m = massInvs[i] * _MassMatrix(v);
			Real det = m.determinant(), trace = m.trace();
			trace /= 2;
			if (det / 4 <= ConformalCutOff * ConformalCutOff) continue;
			// The eigenvectors of this matrix are the roots of
			// P(x) = [x-d(0,0)]*[x-d(1,1)] - d(1,0)*d(0,1)
			//      = x^2 - x * Tr(d) + Det(d)
			// x = Tr(d)/2 +/- sqrt( Tr^2(d)/4 - Det(d) )
			Real disc = trace * trace - det;
			if (disc <= Real(0)) disc = 0;
			else                disc = sqrt(disc);
			Real x1 = trace - disc;
			Real x2 = trace + disc;
			if (x1 <= 0) continue;
			Real tError = sqrt(x2 / x1) * areas[i];
			error += tError;
			area += areas[i];
		}
		return error / area;
	}

protected:
	static SquareMatrix< Real, 2 > _MassMatrix(const Point3D< Real > v[3])
	{
		Point3D< Real > t[] = { v[1] - v[0] , v[2] - v[0] };
		SquareMatrix< Real, 2 > mass;
		for (int j = 0; j < 2; j++) for (int k = 0; k < 2; k++) mass(j, k) = Point3D< Real >::Dot(t[j], t[k]);
		return mass;
	}
	void _init(const Point3D< Real > *vertices)
	{
		massInvs.resize(triangles.size());
		areas.resize(triangles.size(), 0);

		Real area = 0;
#pragma omp parallel for reduction( + : area )
		for (int i = 0; i < triangles.size(); i++)
		{
			Point3D< Real > v[] = { vertices[triangles[i][0]] , vertices[triangles[i][1]] , vertices[triangles[i][2]] };
			areas[i] = (Real)Length(Point3D< Real >::CrossProduct(v[1] - v[0], v[2] - v[0])) / 2;
			area += areas[i];
			if (areas[i] <= ConformalCutOff) continue;
			massInvs[i] = _MassMatrix(v).inverse();
		}
#pragma omp parallel for
		for (int i = 0; i < areas.size(); i++) areas[i] /= area;

	}
};
template< typename Real > const Real QuasiConformalRatio< Real >::ConformalCutOff = (Real)1e-15;

int FaceSize(const TriangleIndex& tri) { return 3; }
int FaceSize(const std::vector< int > &poly) { return (int)poly.size(); }

template< typename Face >
std::vector< std::vector< int > > BoundaryLoops(const std::vector< Face > &faces)
{
	auto EdgeKey = [](unsigned int v1, unsigned int v2) { return ((unsigned long long)v1) << 32 | ((unsigned long long)v2); };
	auto FactorEdgeKey = [](unsigned long long key, int& v1, int& v2) { v1 = (int)(key >> 32), v2 = (int)((key << 32) >> 32); };

	std::unordered_set< unsigned long long > edges;
	for (int f = 0; f < faces.size(); f++)
	{
		int fSize = FaceSize(faces[f]);
		for (int v = 0; v < fSize; v++)
		{
			int v1 = faces[f][v], v2 = faces[f][(v + 1) % fSize];
			edges.insert(EdgeKey(v1, v2));
		}
	}

	std::vector< std::pair< int, int > > boundaryEdges;
	for (auto i = edges.begin(); i != edges.end(); i++)
	{
		int v1, v2;
		FactorEdgeKey(*i, v1, v2);
		if (edges.find(EdgeKey(v2, v1)) == edges.end()) boundaryEdges.push_back(std::pair< int, int >(v2, v1));
	}

	std::vector< std::vector< int > > boundaryLoops;
	while (boundaryEdges.size())
	{
		std::vector< int > boundaryLoop;
		std::pair< int, int > boundaryEdge = boundaryEdges.back();
		boundaryEdges.pop_back();
		int start = boundaryEdge.first, v = boundaryEdge.second;
		boundaryLoop.push_back(v);
		while (v != start)
		{
			for (int i = 0; i < boundaryEdges.size(); i++) if (boundaryEdges[i].first == v)
			{
				v = boundaryEdges[i].second;
				boundaryLoop.push_back(v);
				boundaryEdges[i] = boundaryEdges.back();
				boundaryEdges.pop_back();
				break;
			}
		}
		boundaryLoops.push_back(boundaryLoop);
	}
	return boundaryLoops;
}
template< typename Real >
std::vector< std::vector< int > > BoundaryLoops(const TriangleMesh< Real > &mesh) { return BoundaryLoops(mesh.triangles); }
template< typename Real >
std::vector< std::vector< int > > BoundaryLoops(const PolygonMesh< Real > &mesh) { return BoundaryLoops(mesh.polygons); }
template< typename Real >
std::vector< std::vector< int > > BoundaryLoops(const TriangulatedPolygonMesh< Real > &mesh) { return BoundaryLoops(mesh.polygons); }

template< class Real, typename Mesh >
void CMCF(Mesh &mesh, int iters, bool lump, int fCount)
{
	double stepSize = StepSize.value;
	QuasiConformalRatio< Real > qcRatio(mesh, fCount);
	std::vector< Point3D< Real > > &vertices = mesh.vertices;
	std::vector< Point3D< Real > > b(vertices.size()), oldX(vertices.size()), n(vertices.size());
	Vector< Real > _b(b.size()), _x(vertices.size());
	Point3D< Real > center;
	Real scale = 1.;
	mesh.pose(center, scale);
	scale *= mesh.makeUnitArea();

	// Initialize the matrices.
	// M is the current matrix which is initialized with the sparse matrix topology.
	// D stores the mass matrix.
	// L stores the stiffness matrix.
	// The stiffness matrix is unchanged throughout the flow so we can initialize it once.
	SparseMatrix< Real, int > D, L, M;
	M = L = mesh.stiffnessMatrix();
	if (Randomize.set) for (int i = 0; i < vertices.size(); i++) vertices[i] = RandomSpherePoint< Real >();
	EigenSolverCholeskyLLt< Real >* solver = NULL;

	auto SetStats = [&](Real &deformationScale, Real &quasiConformalRatio, Real &radialDeviation)
	{
		Real differenceNorm = 0, oldNorm = 0;
#pragma omp parallel for
		for (int j = 0; j < D.rows; j++)
		{
			differenceNorm += Point3D< Real >::Dot(oldX[j] - vertices[j], oldX[j] - vertices[j]) * D[j][0].Value;
			oldNorm += Point3D< Real >::Dot(oldX[j], oldX[j]) * D[j][0].Value;
		}
		deformationScale = sqrt(differenceNorm / oldNorm);
		quasiConformalRatio = qcRatio(vertices);
		radialDeviation = mesh.radialDeviation();
	};
	Timer timer;
	for (int i = 0; i < iters; i++)
	{
		double t;
		double tt = Timer::Time();

		t = Timer::Time();
		D = mesh.massMatrix(lump);
		double mTime = Timer::Time() - t;
		// Set the new constraint vector: b = D * x
		D.MultiplyParallel(&vertices[0], &b[0], Threads.value, MULTIPLY_CLEAR);
		// Set the system matrix: M = D + t * L
#pragma omp parallel for num_threads( Threads.value )
		for (int j = 0; j < M.rows; j++)
		{
			for (int k = 0; k < M.rowSizes[j]; k++) M[j][k].Value = L[j][k].Value * stepSize;
			for (int k = 0; k < D.rowSizes[j]; k++) M[j][k].Value += D[j][k].Value;
		}

		t = Timer::Time();
		// If this is the first solve perform both the symbolic and numerical factorization.
		if (!solver) solver = new EigenSolverCholeskyLLt< Real >(M);
		// Otherwise, if the system matrix has changed, update the numerical factorization.
		else solver->update(M);
		double sTime1 = Timer::Time() - t;

		t = Timer::Time();
		// Solve for each of the x, y, z coefficients independently
		for (int d = 0; d < 3; d++)
		{
#pragma omp parallel for num_threads( Threads.value )
			for (int j = 0; j < b.size(); j++) _b[j] = b[j][d], _x[j] = oldX[j][d] = vertices[j][d];
			solver->solve(&_b[0], &_x[0]);
#pragma omp parallel for num_threads( Threads.value )
			for (int j = 0; j < vertices.size(); j++) vertices[j][d] = _x[j];
		}
		double sTime2 = Timer::Time() - t;

		double mem = MemoryInfo::UsageMB();
		// Re-scale/center the mesh
		mesh.pose(), mesh.makeUnitArea();

		if (FullVerbose.set)
		{
			tt = Timer::Time() - tt;
			Real deformationScale, quasiConformalRatio, radialDeviation;
			SetStats(deformationScale, quasiConformalRatio, radialDeviation);
			printf("CMCF[%d / %d] %4.2f = %4.2f + %4.2f + %4.2f(s): D-Norm=%6.5f / QC-Ratio=%6.5f / R-Deviation=%6.5f\r", i + 1, iters, tt, mTime, sTime1, sTime2, deformationScale, quasiConformalRatio, radialDeviation);
		}
	}
	if (FullVerbose.set && iters > 0) printf("\n");
	if (Verbose.set && iters > 0)
	{
		Real deformationScale, quasiConformalRatio, radialDeviation;
		SetStats(deformationScale, quasiConformalRatio, radialDeviation);
		printf("CMCF[%d] %4.2f(s): D-Norm=%6.5f / QC-Ratio=%6.5f / R-Deviation=%6.5f\n", iters, timer.elapsed(), deformationScale, quasiConformalRatio, radialDeviation);
	}
	mesh.pose();

	for (int i = 0; i < vertices.size(); i++) vertices[i] = vertices[i] * scale + center;
	if (solver) delete solver;
}

template< typename Real >
void Center(SphericalGeometry::Mesh< Real >& mesh)
{
	static const int MAX_ITERATIONS = 50;
	int iters;
	int verbose = FullVerbose.set ? 2 : (Verbose.set ? 1 : 0);
	switch (CenterToInversion.value)
	{
	case 0:  iters = mesh.normalize(MAX_ITERATIONS, CutOff.value, true, typename SphericalGeometry::Mesh< Real >::CenterToInversion(), verbose); break;
	case 1:  iters = mesh.normalize(MAX_ITERATIONS, CutOff.value, true, typename SphericalGeometry::Mesh< Real >::GSSCenterToInversion((Real)GSSTolerance.value), verbose); break;
	default: iters = mesh.normalize(MAX_ITERATIONS, CutOff.value, true, typename SphericalGeometry::Mesh< Real >::PoincareCenterToInversion((Real)PoincareMaxNorm.value), verbose); break;
	}
	if (iters == MAX_ITERATIONS && CenterToInversion.value != 1)
	{
		fprintf(stderr, "[WARNING] Failed to meet centering threshold after %d iterations, trying golden-section search\n", MAX_ITERATIONS);
		iters = mesh.normalize(MAX_ITERATIONS, CutOff.value, true, typename SphericalGeometry::Mesh< Real >::GSSCenterToInversion((Real)GSSTolerance.value), verbose);
	}
	if (iters == MAX_ITERATIONS) fprintf(stderr, "[WARNING] Failed to meet centering threshold after %d iterations\n", MAX_ITERATIONS);
}
template< unsigned int SHDegree, typename Real >
void SHCenter(SphericalGeometry::Mesh< Real >& mesh)
{
	static const int MAX_ITERATIONS = 50;
	int verbose = FullVerbose.set ? 2 : (Verbose.set ? 1 : 0);
	if (mesh.template normalizeSH< SHDegree >(MAX_ITERATIONS, AdvectionSteps.value, AdvectionStepSize.value, CutOff.value, true, verbose) == MAX_ITERATIONS) fprintf(stderr, "[WARNING] Failed to meet centering threshold after %d iterations\n", MAX_ITERATIONS);
}

template< class Real >
void Execute(SphericalGeometry::Mesh< Real > &mesh, const std::vector< Point3D< Real > > &vertices, const std::vector< Point3D< Real > > &colors)
{
	// Center the spherical mesh
	if (!NoCenter.set)
	{
		Timer t;
		Center(mesh);
		if (SHDegree.value >= 2) SHCenter< 2 >(mesh);
		if (SHDegree.value >= 3) SHCenter< 3 >(mesh);
		if (SHDegree.value >= 4) SHCenter< 4 >(mesh);
	}

	if (Out.set)
	{
		if (Spherical.set)
		{
			std::vector< PlyColorVertex< float > > outVertices(vertices.size());
			for (int i = 0; i < vertices.size(); i++)
			{
				outVertices[i].point = Point3D< float >(mesh.vertices[i]);
				outVertices[i].color = Point3D< float >(colors[i]);
			}
			PlyWritePolygons(Out.value, outVertices, mesh.polygons, PlyColorVertex< float >::WriteProperties, PlyColorVertex< float >::WriteComponents, ASCII.set ? PLY_ASCII : PLY_BINARY_NATIVE, NULL, 0);
		} else
		{
			std::vector< PlyParametrizedColorVertex< float, Real > > outVertices(vertices.size());
			for (int i = 0; i < vertices.size(); i++)
			{
				outVertices[i].point = Point3D< float >(vertices[i]);
				outVertices[i].color = Point3D< float >(colors[i]);
				outVertices[i].param = mesh.vertices[i];
			}
			PlyWritePolygons(Out.value, outVertices, mesh.polygons, PlyParametrizedColorVertex< float, Real >::WriteProperties, PlyParametrizedColorVertex< float, Real >::WriteComponents, ASCII.set ? PLY_ASCII : PLY_BINARY_NATIVE, NULL, 0);
		}
	}

	if (OutGrid.set || OutTessellation.set)
	{
		struct VertexAndColor
		{
			VertexAndColor(void) {}
			VertexAndColor(Point3D< Real > v, Point3D< Real > c) : vertex(v), color(c) {}
			Point3D< Real > vertex, color;
			VertexAndColor operator * (Real s) const { return VertexAndColor(vertex*s, color*s); }
			VertexAndColor operator / (Real s) const { return VertexAndColor(vertex / s, color / s); }
			VertexAndColor operator + (const VertexAndColor& vc) const { return VertexAndColor(vertex + vc.vertex, color + vc.color); }
		};
		Timer timer;

		std::vector< VertexAndColor > verticesAndColors(vertices.size());
		for (int i = 0; i < vertices.size(); i++) verticesAndColors[i].vertex = Point3D< Real >(vertices[i]), verticesAndColors[i].color = Point3D< Real >(colors[i]);
		SphericalGeometry::Tessellation< Real > tessellator(mesh, verticesAndColors, Resolution.value);
		if (Collapse.set) tessellator.collapseCells(verticesAndColors);

		if (OutTessellation.set)
		{
			std::vector< Point3D< Real > > _vertices(verticesAndColors.size()), _colors(verticesAndColors.size());
			for (int i = 0; i < verticesAndColors.size(); i++) _vertices[i] = verticesAndColors[i].vertex, _colors[i] = verticesAndColors[i].color;
			tessellator.write(OutTessellation.value, _vertices, _colors, !ASCII.set);
		}

		SphericalGrid< Real > sGrid;
		sGrid.resize(Resolution.value);
		tessellator.createSGrid(sGrid, Smooth.value);

		if (OutGrid.set)
		{
			char* ext = GetFileExtension(OutGrid.value);
			if (!strcasecmp(ext, "ply")) tessellator.writeGrid(OutGrid.value, Smooth.value, [](Real v) { return v < 0 ? (Real)-sqrt(-v) : (Real)sqrt(v); }, !ASCII.set);
			else if (!strcasecmp(ext, "sgrid"))
			{
				SphericalGrid< float > _sGrid;
				_sGrid.resize(sGrid.resolution());
				for (int i = 0; i < sGrid.resolution(); i++) for (int j = 0; j < sGrid.resolution(); j++) _sGrid(i, j) = (float)sGrid(i, j);
				_sGrid.write(OutGrid.value);
			}
			delete[] ext;
		}
		if (Verbose.set) printf("Tessellated: %.2f(s)\n", timer.elapsed());
	}
}
template< class Real >
void Execute(TriangleMesh< Real > &mesh, const std::vector< Point3D< Real > > &vertices, const std::vector< Point3D< Real > > &colors)
{
	SphericalGeometry::Mesh< Real > sMesh;
	{
		// Convert the triangles to polygons
		std::vector< std::vector< int > > polygons(mesh.triangles.size());
		for (int i = 0; i < mesh.triangles.size(); i++)
		{
			polygons[i].resize(3);
			for (int j = 0; j < 3; j++) polygons[i][j] = mesh.triangles[i][j];
		}
		sMesh = SphericalGeometry::Mesh< Real >(vertices, mesh.vertices, polygons);
		sMesh.makeUnitMass();
	}
	Execute(sMesh, vertices, colors);
}
template< class Real >
void Execute(PolygonMesh< Real > &mesh, const std::vector< Point3D< Real > > &vertices, const std::vector< Point3D< Real > > &colors)
{
	SphericalGeometry::Mesh< Real > sMesh;
	{
		sMesh = SphericalGeometry::Mesh< Real >(vertices, mesh.vertices, mesh.polygons);
		sMesh.makeUnitMass();
	}
	Execute(sMesh, vertices, colors);
}
template< class Real >
void Execute(TriangulatedPolygonMesh< Real > &mesh, const std::vector< Point3D< Real > > &vertices, const std::vector< Point3D< Real > > &colors)
{
	SphericalGeometry::Mesh< Real > sMesh;
	{
		sMesh = SphericalGeometry::Mesh< Real >(vertices, mesh.vertices, mesh.polygons);
		sMesh.makeUnitMass();
	}
	Execute(sMesh, vertices, colors);
}
template< typename Real >
void HoleFillCenterTriangulation(Mesh< Real > &mesh, const std::vector< int > &loop)
{
	Point3D< Real > c;
	for (int j = 0; j < loop.size(); j++) mesh.push_back(loop[j], loop[(j + 1) % loop.size()], (int)mesh.vertices.size()), c += mesh.vertices[loop[j]];
	c /= (int)loop.size();
	mesh.vertices.push_back(c);
}
template< typename Real >
void HoleFillMinimalAreaTriangulation(Mesh< Real > &mesh, const std::vector< int > &loop)
{
	MinimalAreaTriangulation< Real > MAT;
	std::vector< TriangleIndex > triangles(loop.size() - 2);
	std::vector< Point3D< Real > > vertices(loop.size());
	for (int j = 0; j < loop.size(); j++) vertices[j] = mesh.vertices[loop[j]];
	MAT.GetTriangulation(vertices, triangles);
	for (int j = 0; j < triangles.size(); j++) mesh.push_back(loop[triangles[j][0]], loop[triangles[j][1]], loop[triangles[j][2]]);;
}
template< typename Real > void HoleFillPolygon(TriangleMesh< Real >  &tMesh, const std::vector< int > &loop) { fprintf(stderr, "[WARNING] Polygon hole-filling not supported for triangle mesh\n"); }
template< typename Real > void HoleFillPolygon(PolygonMesh< Real >  &pMesh, const std::vector< int > &loop) { pMesh.polygons.push_back(loop); }
template< typename Real > void HoleFillPolygon(TriangulatedPolygonMesh< Real > &tpMesh, const std::vector< int > &loop) { tpMesh.polygons.push_back(loop); }

template< class Real, typename Mesh >
void Execute(Mesh &mesh, const std::vector< Point3D< Real > > &colors)
{
	int fCount = mesh.faces(), vCount = (int)mesh.vertices.size();
	if (HoleFillType.value != HOLE_FILL_TYPE_NONE)
	{
		Timer t;
		std::vector< std::vector< int > > boundaryLoops = BoundaryLoops(mesh);
		for (int i = 0; i < boundaryLoops.size(); i++)
		{
			if (FullVerbose.set) printf("\t%d] Hole size: %d\n", i + 1, (int)boundaryLoops[i].size());
			switch (HoleFillType.value)
			{
			case HOLE_FILL_TYPE_CENTER_TRIANGULATION:       HoleFillCenterTriangulation(mesh, boundaryLoops[i]); break;
			case HOLE_FILL_TYPE_MINIMAL_AREA_TRIANGULATION: HoleFillMinimalAreaTriangulation(mesh, boundaryLoops[i]); break;
			case HOLE_FILL_TYPE_POLYGON:                    HoleFillPolygon(mesh, boundaryLoops[i]); break;
			default: fprintf(stderr, "[WARNING] Unrecognizd hole-fill type:%d\n", HoleFillType.value);
			}
		}
		mesh.update();
		if (Verbose.set) printf("Filled in %d holes: %.2f(s)\n", (int)boundaryLoops.size(), t.elapsed());
	}
	std::vector< Point3D< Real > > vertices = mesh.vertices;

	CMCF< Real >(mesh, Iterations.value, Lump.set, fCount);

	if (mesh.faces() != fCount || mesh.vertices.size() != vCount)
	{
		while (mesh.faces() > fCount) mesh.pop_back();
		while (mesh.vertices.size() > vCount) mesh.vertices.pop_back();
		mesh.update();
	}

	// Normalize the parameterization
	{
		Point3D< Real > center = mesh.center();
#pragma omp parallel for
		for (int i = 0; i < mesh.vertices.size(); i++) mesh.vertices[i] -= center;
#pragma omp parallel for
		for (int i = 0; i < mesh.vertices.size(); i++) mesh.vertices[i] /= Length(mesh.vertices[i]);
	}

	// Orient the spherical parameterization so that it's outward facing
	if (!NoOrient.set)
	{
		Real pArea = 0, nArea = 0;
#pragma omp parallel for reduction ( + : pArea , nArea )
		for (int f = 0; f < mesh.faces(); f++)
		{
			Point3D< Real > c = mesh.center(f);
			Point3D< Real > n = mesh.normal(f);
			Real l = (Real)Length(n);
			if (Point3D< Real >::Dot(c, n) > 0) pArea += l;
			else                                  nArea += l;
		}
		if (nArea > pArea)
#pragma omp parallel for
			for (int i = 0; i < mesh.vertices.size(); i++) mesh.vertices[i] = -mesh.vertices[i];
	}
	Execute(mesh, vertices, colors);
}


/*
  Tencent is pleased to support the open source community by making
  Plato available.
  Copyright (C) 2019 THL A29 Limited, a Tencent company.
  All rights reserved.

  Licensed under the BSD 3-Clause License (the "License"); you may
  not use this file except in compliance with the License. You may
  obtain a copy of the License at

  https://opensource.org/licenses/BSD-3-Clause

  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.

  See the AUTHORS file for names of contributors.
*/

#ifndef __PLATO_ALGO_FAST_UNFOLDING_LOUVAIN_HPP__
#define __PLATO_ALGO_FAST_UNFOLDING_LOUVAIN_HPP__

#include <cstdint>
#include <cstdlib>
#include <unordered_map>
#include <algorithm>
#include <utility>
#include <vector>
#include <cmath>
#include <unordered_set>
#include <boost/format.hpp>
#include <boost/lockfree/queue.hpp>

#include "glog/logging.h"

#include "plato/util/perf.hpp"
#include "plato/util/atomic.hpp"
#include "plato/graph/graph.hpp"
#include "plato/engine/dualmode.hpp"

namespace plato { namespace algo {
struct louvain_opts_t {
  int alpha_ = -1;
  bool part_by_in_ = false;
  int outer_iteration_ = 3;
  int inner_iteration_ = 2;
};

template<typename GRAPH>
class louvain_epoch_t {
public:
  using partition_t = typename GRAPH::partition_t;
  using edge_value_t = typename GRAPH::edata_t;
  using louvain_value_t = plato::dense_state_t<edge_value_t, partition_t>;
  using adj_unit_list_spec_t = typename GRAPH::adj_unit_list_spec_t;
  struct epoch_msg_type_t {
    vid_t v_i;
    vid_t from;
    vid_t to;
    edge_value_t ki;
  };

  struct sync_val_msg_type_t{
    vid_t v_i;
    edge_value_t val;
  };

public:
  /**
   * @brief
   * @param graph
   * @param graph_info
   * @param m
   * @param opts
   */
  explicit louvain_epoch_t(
    std::shared_ptr<GRAPH> graph, const graph_info_t& graph_info, double m,
    const louvain_opts_t& opts = louvain_opts_t());
  /**
   * @brief
   */
  ~louvain_epoch_t();

  /**
   * @brief
   */
  void compute();

  /**
   * @brief getter
   * @return
   */
  std::shared_ptr<GRAPH> graph() { return graph_; };
  /**
   * @brief getter
   * @return
   */
  std::vector<vid_t>& labels() { return labels_; };

private:
  std::shared_ptr<GRAPH> graph_;
  graph_info_t graph_info_;
  louvain_value_t ki_;
  std::vector<vid_t> labels_;
  std::vector<edge_value_t> sigma_tot_;
  bitmap_t<> local_bit_;
  double m_;
  louvain_opts_t opts_;
};

template <typename GRAPH>
louvain_epoch_t<GRAPH>::louvain_epoch_t(
  std::shared_ptr<GRAPH> graph,
  const graph_info_t& graph_info, double m, const louvain_opts_t& opts) :
  graph_(graph), graph_info_(graph_info), ki_(graph_info.max_v_i_, graph->partitioner()),
  labels_(graph_info.max_v_i_ + 1), sigma_tot_(graph_info.max_v_i_ + 1, 0),
  local_bit_(graph_info.max_v_i_ + 1), m_(2 * m), opts_(opts) {

  auto& cluster_info = plato::cluster_info_t::get_instance();
  plato::stop_watch_t watch;

  {// init labels
    watch.mark("t0");
#pragma omp parallel for num_threads(cluster_info.threads_)
    for (vid_t v_i = 0; v_i <= graph_info.max_v_i_; ++v_i) {
      labels_[v_i] = v_i;
    }
    LOG(INFO) << "epoch init labels: " << watch.show("t0") / 1000.0;
  }

  {//calc ki_ and sync sigma_tot_
    bitmap_t<> active_all(graph_info.max_v_i_ + 1);
    active_all.fill();
    auto active_view_all = plato::create_active_v_view(graph->partitioner()->self_v_view(), active_all);
    watch.mark("t0");
    edge_value_t max_ki = 0;
    using push_context_t = plato::template mepa_bc_context_t<sync_val_msg_type_t>;
    plato::broadcast_message<sync_val_msg_type_t, vid_t>(
      active_view_all,
      /**
       * @brief
       * @param context
       * @param v_i
       */
      [&](const push_context_t& context, vid_t v_i) {
        edge_value_t local_sum = 0;
        auto neighbours = graph->neighbours(v_i);
        for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
          local_sum += it->edata_;
        }
        if (neighbours.begin_ != neighbours.end_) {
          ki_[v_i] = local_sum;
          local_bit_.set_bit(v_i);
          plato::write_max(&max_ki, local_sum);
          context.send(sync_val_msg_type_t{ v_i, local_sum});
        }
      },
      /**
       * @brief
       * @param p_i
       * @param msg
       * @return
       */
      [&](int p_i, sync_val_msg_type_t& msg) {
        sigma_tot_[msg.v_i] = msg.val;
        return 0;
      }
    );
    LOG(INFO) << "epoch sync sigma_tot: " << watch.show("t0") / 1000.0;
    MPI_Allreduce(MPI_IN_PLACE, &max_ki, 1, get_mpi_data_type<edge_value_t>(), MPI_MAX, MPI_COMM_WORLD);
    LOG(INFO) << "max ki: " << max_ki;
  }
}

template <typename GRAPH>
louvain_epoch_t<GRAPH>::~louvain_epoch_t() {
}

template <typename GRAPH>
void louvain_epoch_t<GRAPH>::compute() {
  auto& cluster_info = plato::cluster_info_t::get_instance();
  auto try_change = [&](vid_t v_i, vid_t from, vid_t to, edge_value_t ki_in_from, edge_value_t ki_in_to) {
    double x = (double)ki_[v_i] + (double)sigma_tot_[to] - (double)sigma_tot_[from];
    return (double)ki_in_to - (double)ki_in_from - 2.0 * (double)ki_[v_i] * x / m_;
  };
  auto do_change = [&](epoch_msg_type_t& msg) {
    labels_[msg.v_i] = msg.to;
    plato::write_add(&sigma_tot_[msg.from], -msg.ki);
    plato::write_add(&sigma_tot_[msg.to], msg.ki);
  };

  auto active_view = plato::create_active_v_view(graph_->partitioner()->self_v_view(), local_bit_);
  plato::stop_watch_t watch;
  using push_context_t = plato::template mepa_bc_context_t<epoch_msg_type_t>;
  for (int try_time = 0; try_time < opts_.inner_iteration_; ++try_time){
    watch.mark("t0");
    auto exec_once = [&](std::function<bool(vid_t, vid_t)> condition) {
      plato::broadcast_message<epoch_msg_type_t, vid_t>(
        active_view,
        /**
         * @brief
         * @param context
         * @param v_i
         */
        [&](const push_context_t& context, plato::vid_t v_i) {
          vid_t target = (vid_t)-1;
          vid_t from = labels_[v_i];
          edge_value_t ki_in_from = 0;
          std::unordered_map<vid_t, edge_value_t> ki_in_map;
          auto neighbours = graph_->neighbours(v_i);
          edge_value_t self_cycle = 0;
          for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
            vid_t to = labels_[it->neighbour_];
            if (condition(from, to)) {
              ki_in_map[to] += it->edata_;
            } else if (to == from) {
              ki_in_from += it->edata_;
            }

            if (it->neighbour_ == v_i) {
              self_cycle = it->edata_;
            }
          }

          double best_delta = 0;
          for (auto& it: ki_in_map) {
            double delta = try_change(v_i, from, it.first, ki_in_from, it.second + self_cycle);
            if (delta > best_delta) {
              target = it.first;
              best_delta = delta;
            }
          }

          if (target != (vid_t)-1) {
            epoch_msg_type_t msg = epoch_msg_type_t{ v_i, from, target, ki_[v_i] };
            context.send(msg);
            do_change(msg);
          }
        },
        /**
         * @brief
         * @param p_i
         * @param msg
         * @return
         */
        [&](int p_i, epoch_msg_type_t& msg) {
          if (p_i != cluster_info.partition_id_) {
            do_change(msg);
          }
          return 0;
        }
      );
    };

    exec_once([&](vid_t from, vid_t to){
      if (from < to) return true;
      return false;
    });
    exec_once([&](vid_t from, vid_t to){
      if (from > to) return true;
      return false;
    });
    LOG(INFO) << "try_time: "  << try_time << " cost: " << watch.show("t0") / 1000.0;
  }
}

template<typename GRAPH>
class louvain_fast_unfolding_t {
public:
  using partition_t = typename GRAPH::partition_t;
  using edge_value_t = typename GRAPH::edata_t;
  using adj_unit_list_spec_t = plato::adj_unit_list_t<edge_value_t>;
  using louvain_value_t = plato::dense_state_t<vid_t, partition_t>;

  struct edge_sync_msg_type_t {
    vid_t src;
    vid_t dst;
    edge_value_t data;
  };

  struct degree_sync_msg_type_t {
    vid_t src;
    vid_t degree;
  };

public:
  /**
   * @brief
   * @param graph
   * @param graph_info
   * @param opts
   */
  explicit louvain_fast_unfolding_t(
    std::shared_ptr<GRAPH> graph, const graph_info_t& graph_info,
    const louvain_opts_t& opts = louvain_opts_t());

  /**
   * @brief destructor
   */
  ~louvain_fast_unfolding_t();

  /**
   * @brief compute
   */
  void compute();

  /**
   * @brief save to storage.
   * @tparam STREAM
   * @param streams
   */
  template <typename STREAM>
  void save(std::vector<STREAM*>& streams);

private:
  /**
   * @brief
   * @param graph
   * @param labels
   * @return
   */
  std::shared_ptr<GRAPH> rebuild(std::shared_ptr<GRAPH> graph, std::vector<vid_t>& labels);
  /**
   * @brief
   * @param labels
   */
  void update_local_label(std::vector<vid_t>& labels);
private:
  std::shared_ptr<GRAPH> graph_;
  graph_info_t graph_info_;
  louvain_value_t local_label_;
  louvain_value_t local_comm_size_;
  louvain_opts_t opts_;
};

template<typename GRAPH>
louvain_fast_unfolding_t<GRAPH>::louvain_fast_unfolding_t(
  std::shared_ptr<GRAPH> graph, const graph_info_t& graph_info,
  const louvain_opts_t& opts)
  : graph_(graph), graph_info_(graph_info),
    local_label_(graph_info.max_v_i_, graph->partitioner()),
    local_comm_size_(graph_info.max_v_i_, graph->partitioner()), opts_(opts) {
}

template<typename GRAPH>
louvain_fast_unfolding_t<GRAPH>::~louvain_fast_unfolding_t() {
}

template<typename GRAPH>
void louvain_fast_unfolding_t<GRAPH>::compute() {
  //first calc m and init label
  plato::stop_watch_t watch;
  watch.mark("t0");
  double m = local_label_.template foreach<double> (
    /**
     * @brief
     * @param v_i
     * @param pval
     * @return
     */
    [&](vid_t v_i, vid_t* pval) {
      *pval = v_i;
      double local_sum = 0;
      auto neighbours = graph_->neighbours(v_i);
      for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
        local_sum += it->edata_;
      }
      return local_sum;
    }
  );
  m /= 2; //undirected
  LOG(INFO) << "calc m: " << m << " cost: " << watch.show("t0") / 1000.0;

  //epoch
  louvain_epoch_t<GRAPH>* cur = new louvain_epoch_t<GRAPH>(graph_, graph_info_, m, opts_);
  for (int epoch = 0; epoch < opts_.outer_iteration_; epoch++) {
    LOG(INFO) << "epoch " << epoch << " begin!";
    watch.mark("compute");
    cur->compute();
    LOG(INFO) << "compute cost: " << watch.show("compute") / 1000.0;
    update_local_label(cur->labels());
    if (epoch == opts_.outer_iteration_ - 1) {
      delete cur;
      break;
    }
    //rebuild from current graph
    watch.mark("rebuild");
    graph_info_t graph_info_next(graph_info_);
    graph_info_next.is_directed_ = true;
    auto graph_next = rebuild(cur->graph(), cur->labels());
    louvain_epoch_t<GRAPH>* nxt = new louvain_epoch_t<GRAPH>(graph_next, graph_info_next, m, opts_);
    LOG(INFO) << "rebuild cost: " << watch.show("rebuild") / 1000.0;
    delete cur;
    cur = nxt;
  }
}

template<typename GRAPH>
std::shared_ptr<GRAPH> louvain_fast_unfolding_t<GRAPH>::rebuild(std::shared_ptr<GRAPH> graph, std::vector<vid_t>& labels) {
  plato::stop_watch_t watch;
  watch.mark("t0");
  eid_t total_edge = local_label_.template foreach<eid_t> (
    [&](vid_t v_i, vid_t* pval) {
      auto neighbours = graph->neighbours(v_i);
      return (eid_t)(neighbours.end_ - neighbours.begin_);
    }
  );
  LOG(INFO) << "rebuild before total edge: " << total_edge << " cost: " << watch.show("t0") / 1000.0;

  watch.mark("t0");
  auto& cluster_info = plato::cluster_info_t::get_instance();
  vid_t v_begin = graph->partitioner()->offset_[cluster_info.partition_id_];
  vid_t v_end = graph->partitioner()->offset_[cluster_info.partition_id_ + 1];
  std::vector<eid_t> edge_idx(v_end - v_begin + 1, 0);
  std::vector<eid_t> tmp_idx(v_end - v_begin + 1, 0);

  bitmap_t<> active_all(graph_info_.max_v_i_ + 1);
  active_all.fill();
  auto active_view_all = plato::create_active_v_view(graph->partitioner()->self_v_view(), active_all);

  {
    //first, calc degree
    using push_context_t = plato::template mepa_sd_context_t<degree_sync_msg_type_t>;
    plato::spread_message<degree_sync_msg_type_t, vid_t>(
      active_view_all,
      /**
       * @brief
       * @param context
       * @param v_i
       */
      [&](const push_context_t& context, vid_t v_i) {
        auto neighbours = graph->neighbours(v_i);
        if (neighbours.begin_ == neighbours.end_) return;
        vid_t src = labels[v_i];
        auto send_to = graph->partitioner()->get_partition_id(src);
        context.send(send_to, degree_sync_msg_type_t { src, (vid_t)(neighbours.end_ - neighbours.begin_) } );
      },
      /**
       * @brief
       * @param msg
       * @return
       */
      [&](degree_sync_msg_type_t& msg) {
        vid_t pos = msg.src - v_begin + 1;
        plato::write_add(&edge_idx[pos], (eid_t)msg.degree);
        return 0;
      }
    );

    for (int i = 1; i < (int)edge_idx.size(); ++i) {
      edge_idx[i] = edge_idx[i - 1] + edge_idx[i];
      tmp_idx[i] = edge_idx[i];
    }

    LOG(INFO) << "rebuild calc degree cost: " << watch.show("t0") / 1000.0;
  }
  eid_t total_local_edge = edge_idx[edge_idx.size() - 1];
  std::vector<std::pair<vid_t, edge_value_t> > edges(total_local_edge);
  LOG(INFO) << "rebuild local edge: " << edges.size();
  watch.mark("t0");
  {
    //second, transfer edge
    using push_context_t = plato::template mepa_sd_context_t<edge_sync_msg_type_t>;
    plato::spread_message<edge_sync_msg_type_t, vid_t>(
      active_view_all,
      /**
       * @brief
       * @param context
       * @param v_i
       */
      [&](const push_context_t& context, vid_t v_i) {
        auto neighbours = graph->neighbours(v_i);
        if (neighbours.begin_ == neighbours.end_) return;
        vid_t src = labels[v_i];
        auto send_to = graph->partitioner()->get_partition_id(src);
        for (auto it = neighbours.begin_; neighbours.end_ != it; ++it) {
          vid_t dst = labels[it->neighbour_];
          context.send(send_to, edge_sync_msg_type_t{ src, dst, it->edata_ });
        }
      },
      /**
       * @brief
       * @param msg
       * @return
       */
      [&](edge_sync_msg_type_t& msg) {
        vid_t pos = msg.src - v_begin;
        vid_t idx = __sync_fetch_and_add(&tmp_idx[pos], (eid_t)1);
        edges[idx].first = msg.dst;
        edges[idx].second = msg.data;
        return 0;
      }
    );
    LOG(INFO) << "rebuild transfer edge cost: " << watch.show("t0") / 1000.0;
  }

  watch.mark("t0");
  edge_cache_t<edge_value_t> edge_cache;
  {
    //third, sort and aggregate
#pragma omp parallel for num_threads(cluster_info.threads_)
    for (vid_t v_i = v_begin; v_i < v_end; ++v_i) {
      vid_t pos = v_i - v_begin;
      eid_t e_start = edge_idx[pos];
      eid_t e_end = edge_idx[pos + 1];
      if (e_start == e_end) continue;
      std::sort(edges.begin() + e_start, edges.begin() + e_end);
      vid_t pre = (vid_t)-1;
      edge_value_t local_sum = 0;
      for (eid_t e = e_start; e < e_end; ++e) {
        if (pre != edges[e].first) {
          if (pre != (vid_t)-1) {
            edge_cache.push_back(edge_unit_t<edge_value_t> { v_i, pre, local_sum });
          }
          pre = edges[e].first;
          local_sum = 0;
        }
        local_sum += edges[e].second;
      }
      if (pre != (vid_t)-1) {
        edge_cache.push_back(edge_unit_t<edge_value_t> { v_i, pre, local_sum });
      }
    }

    eid_t edge_num_new = edge_cache.size();
    MPI_Allreduce(MPI_IN_PLACE, &edge_num_new, 1, get_mpi_data_type<eid_t>(), MPI_SUM, MPI_COMM_WORLD);
    LOG(INFO) << "rebuild new edge num: " << edge_num_new;
    LOG(INFO) << "rebuild aggregate edge cost: " << watch.show("t0") / 1000.0;
  }

  watch.mark("t0");
  graph_info_t graph_info_next(graph_info_);
  graph_info_next.is_directed_ = true;
  std::shared_ptr<GRAPH> pgraph(new GRAPH(graph->partitioner()));
  pgraph->load_from_cache(graph_info_next, edge_cache);
  LOG(INFO) << "rebuild load cache cost: " << watch.show("t0") / 1000.0;

  return pgraph;
}

template<typename GRAPH>
void louvain_fast_unfolding_t<GRAPH>::update_local_label(std::vector<vid_t>& labels) {
  local_label_.template foreach<int> (
    /**
     * @brief
     * @param v_i
     * @param pval
     * @return
     */
    [&] (vid_t v_i, vid_t* pval) {
      if (labels[*pval] != *pval) {
        *pval = labels[*pval];
      }
      return 0;
    }
  );
}

template<typename GRAPH>
template<typename STREAM>
void louvain_fast_unfolding_t<GRAPH>::save(std::vector<STREAM*>& ss) {
  struct louvain_msg_type_t {
    vid_t src;
    vid_t label;
  };
  boost::lockfree::queue<louvain_msg_type_t> que(1024);
  LOG_IF(FATAL, !que.is_lock_free())
  << "boost::lockfree::queue is not lock free\n";

  // start a thread to pop and edge and write to output
  std::atomic<bool> done(false);
  std::thread pop_write([&done, &ss, &que](void) {
#pragma omp parallel num_threads(ss.size())
    {
      int tid = omp_get_thread_num();
      louvain_msg_type_t vb;
      while (!done) {
        if (que.pop(vb)) {
          *ss[tid] << vb.src << "," << vb.label << "\n";
        }
      }

      while (que.pop(vb)) {
        *ss[tid] << vb.src << "," << vb.label << "\n";
      }
    }
  });

  // traverse
  local_label_.template foreach<int> (
    /**
     * @brief
     * @param v_i
     * @param pval
     * @return
     */
    [&] (vid_t v_i, vid_t* pval) {
      while (!que.push(louvain_msg_type_t {v_i, *pval} )) {
      }
      return 0;
    }
  );

  done = true;
  pop_write.join();
}

}  // namespace plato
}  // namespace algo
#endif


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_POLY_HPP
#define BOOST_GEOMETRY_PROJECTIONS_POLY_HPP

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_mlfn.hpp>
#include <boost/geometry/srs/projections/impl/pj_msfn.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct poly {}; // Polyconic (American)

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace poly
    {

            static const double tolerance = 1e-10;
            static const double conv_tolerance = 1e-10;
            static const int n_iter = 10;
            static const int i_iter = 20;
            static const double i_tolerance = 1.e-12;

            template <typename T>
            struct par_poly
            {
                T ml0;
                detail::en<T> en;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_poly_ellipsoid
                : public base_t_fi<base_poly_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_poly<T> m_proj_parm;

                inline base_poly_ellipsoid(const Parameters& par)
                    : base_t_fi<base_poly_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    T  ms, sp, cp;

                    if (fabs(lp_lat) <= tolerance) {
                        xy_x = lp_lon;
                        xy_y = -this->m_proj_parm.ml0;
                    } else {
                        sp = sin(lp_lat);
                        ms = fabs(cp = cos(lp_lat)) > tolerance ? pj_msfn(sp, cp, this->m_par.es) / sp : 0.;
                        xy_x = ms * sin(lp_lon *= sp);
                        xy_y = (pj_mlfn(lp_lat, sp, cp, this->m_proj_parm.en) - this->m_proj_parm.ml0) + ms * (1. - cos(lp_lon));
                    }
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    xy_y += this->m_proj_parm.ml0;
                    if (fabs(xy_y) <= tolerance) {
                        lp_lon = xy_x;
                        lp_lat = 0.;
                    } else {
                        T r, c, sp, cp, s2ph, ml, mlb, mlp, dPhi;
                        int i;

                        r = xy_y * xy_y + xy_x * xy_x;
                        for (lp_lat = xy_y, i = i_iter; i ; --i) {
                            sp = sin(lp_lat);
                            s2ph = sp * ( cp = cos(lp_lat));
                            if (fabs(cp) < i_tolerance) {
                                BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                            }
                            c = sp * (mlp = sqrt(1. - this->m_par.es * sp * sp)) / cp;
                            ml = pj_mlfn(lp_lat, sp, cp, this->m_proj_parm.en);
                            mlb = ml * ml + r;
                            mlp = this->m_par.one_es / (mlp * mlp * mlp);
                            lp_lat += ( dPhi =
                                ( ml + ml + c * mlb - 2. * xy_y * (c * ml + 1.) ) / (
                                this->m_par.es * s2ph * (mlb - 2. * xy_y * ml) / c +
                                2.* (xy_y - ml) * (c * mlp - 1. / s2ph) - mlp - mlp ));
                            if (fabs(dPhi) <= i_tolerance)
                                break;
                        }
                        if (!i) {
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        }
                        c = sin(lp_lat);
                        lp_lon = asin(xy_x * tan(lp_lat) * sqrt(1. - this->m_par.es * c * c)) / sin(lp_lat);
                    }
                }

                static inline std::string get_name()
                {
                    return "poly_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_poly_spheroid
                : public base_t_fi<base_poly_spheroid<T, Parameters>, T, Parameters>
            {
                par_poly<T> m_proj_parm;

                inline base_poly_spheroid(const Parameters& par)
                    : base_t_fi<base_poly_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    T  cot, E;

                    if (fabs(lp_lat) <= tolerance) {
                        xy_x = lp_lon;
                        xy_y = this->m_proj_parm.ml0;
                    } else {
                        cot = 1. / tan(lp_lat);
                        xy_x = sin(E = lp_lon * sin(lp_lat)) * cot;
                        xy_y = lp_lat - this->m_par.phi0 + cot * (1. - cos(E));
                    }
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    T B, dphi, tp;
                    int i;

                    if (fabs(xy_y = this->m_par.phi0 + xy_y) <= tolerance) {
                        lp_lon = xy_x;
                        lp_lat = 0.;
                    } else {
                        lp_lat = xy_y;
                        B = xy_x * xy_x + xy_y * xy_y;
                        i = n_iter;
                        do {
                            tp = tan(lp_lat);
                            lp_lat -= (dphi = (xy_y * (lp_lat * tp + 1.) - lp_lat -
                                .5 * ( lp_lat * lp_lat + B) * tp) /
                                ((lp_lat - xy_y) / tp - 1.));
                        } while (fabs(dphi) > conv_tolerance && --i);
                        if (! i) {
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        }
                        lp_lon = asin(xy_x * tan(lp_lat)) / sin(lp_lat);
                    }
                }

                static inline std::string get_name()
                {
                    return "poly_spheroid";
                }

            };

            // Polyconic (American)
            template <typename Parameters, typename T>
            inline void setup_poly(Parameters& par, par_poly<T>& proj_parm)
            {
                if (par.es != 0.0) {
                    proj_parm.en = pj_enfn<T>(par.es);
                    proj_parm.ml0 = pj_mlfn(par.phi0, sin(par.phi0), cos(par.phi0), proj_parm.en);
                } else {
                    proj_parm.ml0 = -par.phi0;
                }
            }

    }} // namespace detail::poly
    #endif // doxygen

    /*!
        \brief Polyconic (American) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Conic
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_poly.gif
    */
    template <typename T, typename Parameters>
    struct poly_ellipsoid : public detail::poly::base_poly_ellipsoid<T, Parameters>
    {
        inline poly_ellipsoid(const Parameters& par) : detail::poly::base_poly_ellipsoid<T, Parameters>(par)
        {
            detail::poly::setup_poly(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Polyconic (American) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Conic
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_poly.gif
    */
    template <typename T, typename Parameters>
    struct poly_spheroid : public detail::poly::base_poly_spheroid<T, Parameters>
    {
        inline poly_spheroid(const Parameters& par) : detail::poly::base_poly_spheroid<T, Parameters>(par)
        {
            detail::poly::setup_poly(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::poly, poly_spheroid, poly_ellipsoid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class poly_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    if (par.es)
                        return new base_v_fi<poly_ellipsoid<T, Parameters>, T, Parameters>(par);
                    else
                        return new base_v_fi<poly_spheroid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void poly_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("poly", new poly_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_POLY_HPP


// #pragma GCC optimize ("O0")

#include <iostream>
#include <string>

using std::string;

#include <boost/log/trivial.hpp>

#include "eigenIncluder.hpp"

#include "streamTrace.hpp"
#include "navigation.hpp"
#include "gTime.hpp"
#include "common.hpp"
#include "enums.h"

/* satellite code to satellite system ----------------------------------------*/
E_Sys code2sys(char code)
{
	if (code=='G'||code==' ') return E_Sys::GPS;
	if (code=='R') return E_Sys::GLO;
	if (code=='E') return E_Sys::GAL; /* extension to sp3-c */
	if (code=='J') return E_Sys::QZS; /* extension to sp3-c */
	if (code=='C') return E_Sys::BDS; /* extension to sp3-c */
	if (code=='L') return E_Sys::LEO; /* extension to sp3-c */
	return E_Sys::NONE;
}

/** read an epoch of data from an sp3 precise ephemeris file
 */
bool readsp3(
	std::istream&	fileStream, 	///< stream to read content from
	list<Peph>&		pephList,		///< list of precise ephemerides for one epoch
	int				opt,			///< options options (1: only observed + 2: only predicted + 4: not combined)
	bool&			isUTC,			///< reported utc state in header
	double*			bfact)			///< bfact values from header
{
	GTime	time			= {};

	//fprintf(stdout,"\n SP3READ: Expanded  %s to %d files\n",file,n);

	//keep track of file number
	static int index = 0;
	index++;

	int hashCount	= 0;
	int cCount		= 0;
	int fCount		= 0;

	bool epochFound = false;
	string line;
	while (fileStream)
	{
		//return early when an epoch is complete
		int peek = fileStream.peek();
		if	(  peek == '*'
			&& epochFound)
		{
			return true;
		}

		getline(fileStream, line);

		char* buff = &line[0];

		if (buff[0] == '*')
		{
			//epoch line
			epochFound = true;

			bool error = str2time(buff, 3, 28, time);
			if (error)
			{
				printf("\nInvalid epoch line in sp3 file %s\n", line.c_str());
				return false;
			}

			if (isUTC)
			{
				time = utc2gpst(time); /* utc->gpst */
			}
			continue;
		}

		if (buff[0] == 'P')
		{
			//position line
			bool pred_p = false;
			bool pred_c = false;

			E_Sys sys = code2sys(buff[1]);
			int prn = (int)str2num(buff, 2, 2);

			if		(sys == +E_Sys::SBS)	prn += 100;
			else if (sys == +E_Sys::QZS)	prn += 192; /* extension to sp3-c */

			SatSys Sat;
			Sat.sys = sys;
			Sat.prn = prn;
			if (!Sat)
				continue;

			Peph peph 	= {};
			peph.time 	= time;
			peph.index	= index;
			peph.Sat	= Sat;
			bool valid	= false;

			if (buff[0] == 'P')
			{
				pred_c = strlen(buff)>=76 && buff[75]=='P';
				pred_p = strlen(buff)>=80 && buff[79]=='P';
			}

			//positions/rates
			for (int j = 0; j < 3; j++)
			{
				/* read option for predicted value */
				if (j < 3 	&& (opt&1)	&&  pred_p)		continue;
				if (j < 3 	&& (opt&2)	&& !pred_p)		continue;

				double val = str2num(buff, 4	+ j * 14,	14);
				double std = str2num(buff,61	+ j * 3,	2);

				if (buff[0]=='P')
				{
					/* position */
					if	( val != 0
						&&fabs(val - 999999.999999) >= 1E-6)
					{
						peph.Pos[j] = val * 1000;
						valid = true;
					}

					double base = bfact[0];
					if	(  base	> 0
						&& std	> 0)
					{
						peph.PosStd[j] = pow(base, std) * 1E-3;
					}
				}
				else if (valid)
				{
// 					/* velocity */
// 					if	( val !=0
// 						&&fabs(val - 999999.999999) >= 1E-6)
// 					{
// 						peph.Vel[j] = val * 0.1;
// 					}
//
// 					double base = bfact[j < 3 ? 0 : 1];
// 					if	(  base	> 0
// 						&& std	> 0)
// 					{
// 						peph.VelStd[j] = pow(base, std) * 1E-7;
// 					}
				}
			}

			//clocks / rates
			for (int j = 3; j < 4; j++)
			{
				/* read option for predicted value */
				if (j == 3	&& (opt&1)	&&  pred_c)		continue;
				if (j == 3	&& (opt&2)	&& !pred_c)		continue;

				double val = str2num(buff, 4	+ j * 14,	14);
				double std = str2num(buff,61	+ j * 3,	3);

				if (buff[0] == 'P')
				{
					/* position */
					if	( val != 0
						&&fabs(val - 999999.999999) >= 1E-6)
					{
						peph.Clk = val * 1E-6;
						valid = 1; /* valid epoch */
					}

					double base = bfact[1];
					if	(  base	> 0
						&& std	> 0)
					{
						peph.ClkStd = pow(base, std) * 1E-12;
					}
				}
				else if (valid)
				{
// 					/* velocity */
// 					if	( val !=0
// 						&&fabs(val - 999999.999999) >= 1E-6)
// 					{
// 						peph.dCk = val * 1E-10;
// 					}
//
// 					double base = bfact[j < 3 ? 0 : 1];
// 					if	(  base	> 0
// 						&& std	> 0)
// 					{
// 						peph.dCkStd = pow(base, std) * 1E-16;
// 					}
				}
			}

			if (valid)
			{
				pephList.push_back(peph);
// 				nav->pephMap[peph.Sat][peph.time] = peph;
			}

			continue;
		}

		if (buff[0] == '#')
		{
			hashCount++;

			if (hashCount == 1)
			{
				//first line is time and type
// 				type = buff[2];
				int error = str2time(buff, 3, 28, time);
				if (error)
					return false;

				continue;
			}
		}

		string twoChars = line.substr(0,2);

// 		if (twoChars == "+ ")
// 		{
//			plusCount++;
// 			//number and list of satellites included in the file - information only, sat ids are included in epoch lines..
// 			if (lineNum == 2)
// 			{
// 				ns = (int)str2num(buff,4,2);
// 			}
// 			for (int j = 0; j < 17 && k < ns; j++)
// 			{
// 				E_Sys sys=code2sys(buff[9+3*j]);
//
// 				int prn = (int)str2num(buff,10+3*j,2);
//
// 				if (k < MAXSAT)
// 					sats[k++] = SatSys(sys, prn);
// 			}
//			continue;
// 		}

// 		if (twoChars == "++")
// 		{
// 			pplusCount++;
// 			continue;
// 		}

		if (twoChars == "%c")
		{
			cCount++;

			if (cCount == 1)
			{
				char tsys[4] = "";
				strncpy(tsys, buff+9,3);
				tsys[3] = '\0';

				if (!strcmp(tsys, "UTC"))
				{
					isUTC = true;
// 					time = utc2gpst(time); /* utc->gpst */
				}
			}
			continue;
		}

		if (twoChars == "%f")
		{
			fCount++;

			if (fCount == 1)
			{
				bfact[0] = str2num(buff, 3,10);
				bfact[1] = str2num(buff,14,12);
			}
			continue;
		}

		if (line.substr(0,3) == "EOF")
		{
			//all done
			return true;
		}
	}

// 	printf("\nDidnt find eof in sp3 file\n");
	return false;
}

void readSp3ToNav(
	string&	file,
	nav_t*	nav,
	int		opt)
{
	std::ifstream fileStream(file);
	if (!fileStream)
	{
		printf("\nSp3 file open error %s\n", file.c_str());
	}

	list<Peph>	pephList;

	bool	isUTC		= false;
	double	bfact[2]	= {};
	while (readsp3(fileStream, pephList, opt, isUTC, bfact))
	{
		//keep reading until it fails
		for (auto& peph : pephList)
		{
			nav->pephMap[peph.Sat][peph.time] = peph;
		}
		pephList.clear();
	}
}

void orb2sp3(
	nav_t& nav)
{
	for (auto& [SatId,	satNav]	: nav.satNavMap)
	for (auto& [time,	orbitInfo]	: satNav.satOrbit.orbitInfoMap)
	{
		SatSys Sat;
		Sat.fromHash(SatId);

		Peph peph = {};

		peph.index			= 0;
		peph.time			= time;
		peph.Sat			= Sat;

		/* copy itrf coordinates */
		for (int k = 0; k < 3; k++)
		{
			peph.Pos[k] = orbitInfo.xtrf[k];
		}

		nav.pephMap[Sat][time] = peph;
	}
}


/*
 *  Copyright (c) 2009, 2010, Universitaet Bremen
 *  All rights reserved.
 *
 *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Universitaet Bremen nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef __UKF_MTKWRAP_HPP__
#define __UKF_MTKWRAP_HPP__

#include <cassert>

#include <Eigen/Core>
//#include <Eigen/LU>

//#include <Eigen/QR>

namespace ukfom {

// import most common Eigen types
USING_PART_OF_NAMESPACE_EIGEN

/**
 * mtkwrap<M> wraps an MTK-Manifold M to a ukf-compatible manifold.
 * M has to have an enum DOF and implement the methods boxplus and boxminus.
 */
template<class M>
struct mtkwrap : public M{
	typedef mtkwrap<M> self;
public:
	typedef double scalar; // MTK only works with double
	typedef scalar scalar_type;

	enum {
		DOF = M::DOF
	};

	typedef Matrix<scalar_type, DOF, 1> vectorized_type;

	mtkwrap(const M &m=M()) : M(m) {}

	/*
	 * manifold operator (+)
	 *
	 */
	self& operator+=(const vectorized_type &delta_state)
	{
		assert(delta_state.stride() == DOF);
		M::boxplus(delta_state.data());
		return *this;
	}

	const self operator+(const vectorized_type &delta_state) const
	{
		self result = *this;
		result += delta_state;

		return result;
	}

	/*
	 * manifold operator (-)
	 */
	const vectorized_type operator-(const self &other) const
	{
		vectorized_type result;
		assert(result.stride()==DOF);
		M::boxminus(result.data(), other);

		return result;
	}

	bool operator==(const self &other) const
	{
		vectorized_type diff = (*this) - other;
		return diff.isZero(1e-12);
	}

	bool operator!=(const self &other) const
	{
		return !(*this == other);
	}

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} // namespace ukfom
#endif /* __UKF_MTKWRAP_HPP__ */