/*
arg min Sum_i Ei(Ri)
  s.t. ~Ri Ri = 1

Ei = Sum_j wij |Ri qji ~Ri - pji|^2 + A Sum_j cij |Ri - Rj|^2 + ei |Ri - R_{i-1}|^2

where:  Sum_j wij = 1    and     ei + Sum_j cij = 1

Ei = Sum_j wij |Ri qji - pji Ri|^2 + A Sum_j cij |Ri - Rj|^2 + ei |Ri - R_{i-1}|^2

Since: Sum_j wij |Ri qji - pji Ri|^2 = Ri^T Hi Ri

Ei = Ri^T Hi Ri + A Sum_j cij |Ri - Rj|^2 + ei |Ri - R_{i-1}|^2

dEi/Ri = 2 Hi Ri + 2 A sum_j(cij Ri - Rj) + 2 ei (Ri - R_{i-1})
dEi/Ri = Hi Ri + A sum_j(cij Ri) - A sum_j(cij Rj) + ei Ri - ei R_{i-1}
dEi/Ri = Hi Ri + A Ri sum_j(cij) - A sum_j(cij Rj) + ei Ri - ei R_{i-1}
dEi/Ri = (Hi + A sum_j(cij) I + ei I) Ri - sum_j(A cij Rj) - ei R_{i-1}

Setting dEi/Ri = 0

(Hi + A sum_j(cij) I + ei I) Ri - sum_j(A cij Rj) - ei R_{i-1} = 0

Setting Mi = Hi + (A sum_j(cij) + ei) I

And imposing: sum_j(cij) + ei = 1

Mi = Hi + A I

Mi Ri = sum_j(A cij Rj) + ei R_{i-1}

M R = L R + ei R_prev

Where
   M is stacking Mi at the diagonal
   L is laplacian matrix from sum_j(A cij Rj)
   R_prev is stacking feedback R_{i-1}

Solve (M - L) R = ei R_prev
s.t R = R_const,  at the constrained points.
*/
#ifdef WIN32
#define NOMINMAX
#include <windows.h>
#endif

#if defined (__APPLE__) || defined (OSX)
#include <OpenGL/gl.h>
#include <GLUT/glut.h>
#else
#include <GL/gl.h>
#include <GL/glut.h>
#endif

#include "GA/c3ga.h"
#include "GA/c3ga_util.h"
#include "GA/gl_util.h"

#include "primitivedraw.h"
#include "gahelper.h"
#include "Laplacian.h"

#include <memory>

#include <vector>
#include <queue>
#include <map>
#include "numerics.h"
#include "HalfEdge/Mesh.h"
#include "GARotorEstimator.h"

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

// #include <ppl.h>

#include "Benchs.h"

const char *WINDOW_TITLE = "Interactive 3D Shape Deformation using Conformal Geometric Algebra";

// GLUT state information
int g_viewportWidth = 800;
int g_viewportHeight = 600;

void display();
void reshape(GLint width, GLint height);
void MouseButton(int button, int state, int x, int y);
void MouseMotion(int x, int y);
void KeyboardUpFunc(unsigned char key, int x, int y);
void SpecialFunc(int key, int x, int y);
void SpecialUpFunc(int key, int x, int y);
void Idle();
void DestroyWindow();

//using namespace boost;
using namespace c3ga;
using namespace std;
using namespace numerics;

class Camera
{
public:
	float		pos[3];
	float		fw[3];
	float		up[3];
	float		translateVel;
	float		rotateVel;

	Camera()
	{
		float		_pos[] = { 0, 0, -2};
		float		_fw[] = { 0, 0, 1 };
		float		_up[] = { 0, 1, 0 };

		translateVel = 0.005;
		rotateVel = 0.005;
		memcpy(pos, _pos, sizeof(float)*3);
		memcpy(fw, _fw, sizeof(float)*3);
		memcpy(up, _up, sizeof(float)*3);
	}

	void glLookAt()
	{
		gluLookAt( pos[0], pos[1], pos[2], fw[0],  fw[1],  fw[2], up[0],  up[1],  up[2] );
	}
};

class VertexBuffer
{
public:
	std::vector<Eigen::Vector3d> deformedPositions; //deformed mesh positions
	std::map<int, Eigen::Vector3d> constrainedPositions; //positional constraints
	std::vector<Eigen::Vector3d> laplacianCoordinates; //laplacian Coordinates
	std::vector<Eigen::Vector3d> normals; //for rendering (lighting)
	std::vector<Eigen::Vector3d> normalsOrig; //original normals
	std::vector<Eigen::Quaterniond> rotors;
	int size;

	VertexBuffer() : size(0)
	{
	}

	void resize(int size)
	{
		this->size = size;
		deformedPositions.resize(size);
		laplacianCoordinates.resize(size);
		normals.resize(size);
		normalsOrig.resize(size);
		rotors.resize(size);
	}
	int get_size() { return size; }

};

class IndexBuffer {
public:
	std::vector<int> faces;
	int size;

	IndexBuffer() : size(0)
	{
	}

	void resize(int size)
	{
		this->size = size;
		faces.resize(size);
	}
	int get_size() { return size; }

};

class Handle
{
public:
	rotor R;
	translator T;
	translator Tcenter;
	dualSphere dS;
	std::set<int> constraints;

	Handle() {
		T = _rotor(1.0);
		R = _translator(1.0);
		Tcenter = _translator(1.0);
	}
	Handle(dualSphere dS)
	{
		normalizedPoint x = DualSphereCenter(dS);
		Tcenter = exp(-0.5 * _vectorE3GA(x) * ni);
		T = _translator(1.0);
		R = _rotor(1.0);
		this->dS = dS;
	}

	TRversor GetTRVersor()
	{
		return _TRversor(T * _TRversor(Tcenter * R * inverse(Tcenter)));
	}
};

Camera g_camera;
Mesh mesh, meshLow;
map<int, int> mapping;
vectorE3GA g_prevMousePos;
bool g_rotateModel = false;
bool g_rotateModelOutOfPlane = false;
rotor g_modelRotor = _rotor(1.0);
bool g_rotateKeyRotors = false;
bool g_translateKeyRotors = false;
bool g_computeBasis = false;
float g_dragDistance = -1.0f;
int g_dragObject;
std::shared_ptr<SparseMatrix> A;
std::shared_ptr<SparseMatrix> AHi;
Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solverLow, solverHi;
Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solverRotations;
Eigen::SparseMatrix<double> RotationMatrix;
vector<Eigen::Triplet<double>> triplets;
int systemType = LaplaceBeltrami; //MeanValue; //LaplaceBeltrami
bool g_showSpheres = true;
bool g_showWires = false;
bool g_iterateManyTimes = false;
Eigen::MatrixXd b3Low;
Eigen::MatrixXd xyzLow;
Eigen::MatrixXd b3Hi;
Eigen::MatrixXd xyzHi;
Eigen::VectorXd Rknown;
Eigen::MatrixXd RknownHi;
Eigen::MatrixXd RbestHi;

double g_meshArea = 0.0;

bool g_automaticAnimation = false;
bool g_convergence = false;
Benchmarks benchs;

VertexBuffer vertexDescriptors;
VertexBuffer vertexDescriptorsLow;
IndexBuffer trianglesHi;
std::vector<Handle> handles;
std::set<int> allconstraints;

Eigen::Affine3d MotorToMatrix(const TRversor& R) {
	TRversor Ri = inverse(R);

	// compute images of basis vectors:
	c3ga::flatPoint imageOfE1NI = _flatPoint(R * c3ga::e1ni * Ri);
	c3ga::flatPoint imageOfE2NI = _flatPoint(R * c3ga::e2ni * Ri);
	c3ga::flatPoint imageOfE3NI = _flatPoint(R * c3ga::e3ni * Ri);
	c3ga::flatPoint imageOfNONI = _flatPoint(R * c3ga::noni * Ri);

	// create matrix representation:
	Eigen::Affine3d M;
	M(0, 0) = imageOfE1NI.m_c[0];
	M(1, 0) = imageOfE1NI.m_c[1];
	M(2, 0) = imageOfE1NI.m_c[2];
	M(3, 0) = imageOfE1NI.m_c[3];
	M(0, 1) = imageOfE2NI.m_c[0];
	M(1, 1) = imageOfE2NI.m_c[1];
	M(2, 1) = imageOfE2NI.m_c[2];
	M(3, 1) = imageOfE2NI.m_c[3];
	M(0, 2) = imageOfE3NI.m_c[0];
	M(1, 2) = imageOfE3NI.m_c[1];
	M(2, 2) = imageOfE3NI.m_c[2];
	M(3, 2) = imageOfE3NI.m_c[3];
	M(0, 3) = imageOfNONI.m_c[0];
	M(1, 3) = imageOfNONI.m_c[1];
	M(2, 3) = imageOfNONI.m_c[2];
	M(3, 3) = imageOfNONI.m_c[3];
	return M;
}

void ComputeLaplacianCoordinates(std::shared_ptr<SparseMatrix> A, Mesh* mesh, std::vector<Eigen::Vector3d>& laplacianCoordinates)
{
	std::fill(laplacianCoordinates.begin(), laplacianCoordinates.end(), Eigen::Vector3d(0, 0, 0));

	auto numRows = A->numRows();

	for (int i = 0; i < numRows; ++i)
	{
		SparseMatrix::RowIterator aIter = A->iterator(i);
		for (; !aIter.end(); ++aIter)
		{
			auto j = aIter.columnIndex();
			laplacianCoordinates[i] += mesh->vertexAt(j).p * aIter.value();
		}
	}
}

bool is_constrained(std::set<int>& constraints, int vertex)
{
	return constraints.find(vertex) != constraints.end();
}

void PreFactor(std::shared_ptr<SparseMatrix> A, std::set<int>& constraints, Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>& solver)
{

	Eigen::SparseMatrix<double> Lc = Eigen::SparseMatrix<double>(A->numRows(), A->numColumns());

	auto numRows = A->numRows();
	for (int i = 0; i < numRows; ++i)
	{
		if (!is_constrained(constraints, i))
		{
			SparseMatrix::RowIterator aIter = A->iterator(i);
			for (; !aIter.end(); ++aIter)
			{
				auto j = aIter.columnIndex();
				Lc.insert(i, j) = (*A)(i, j);
			}
		}
		else
		{
			Lc.insert(i, i) = 1.0;
		}
	}

	Lc.makeCompressed();
	solver.compute(Lc);
	if (solver.info() != Eigen::Success) {
		// TODO: error handling
	}

}

double meshArea(Mesh *mesh)
{
	double area = 0.0;
	for(Face& face :  mesh->getFaces())
	{
		Eigen::Vector3d	u = face.edge->next->vertex->p - face.edge->vertex->p;
		Eigen::Vector3d	v = face.edge->next->next->vertex->p - face.edge->vertex->p;
		area += 0.5 * u.cross(v).norm();
	}
	return area;
}

void meshMapping(Mesh *meshLow, Mesh *mesh, map<int, int>& mapping)
{
	for (Vertex& vertexLow : meshLow->getVertices())
	{
		double minDist = 1e38;
		int mappedPoint = -1;
		Eigen::Vector3d& pLow = vertexLow.p;
		for (Vertex& vertexHi : mesh->getVertices())
		{
			Eigen::Vector3d& p = vertexHi.p;
			double dist = (pLow - p).norm();
			if (dist < minDist) {
				minDist = dist;
				mappedPoint = vertexHi.ID;
			}
		}
		mapping[vertexLow.ID] = mappedPoint;
	}
}

int main(int argc, char* argv[])
{
	mesh.readOBJ("cactus2.obj"); //cactus2.obj  armadillo.obj
	mesh.CenterAndNormalize();
	mesh.computeNormals();

	meshLow.readOBJ("cactus1.obj"); //cactus1.obj armadillo1.obj
	meshLow.CenterAndNormalize();
	meshLow.computeNormals();

	meshMapping(&meshLow, &mesh, mapping);

	// GLUT Window Initialization:
	glutInit (&argc, argv);
	glutInitWindowSize(g_viewportWidth, g_viewportHeight);
	glutInitDisplayMode( GLUT_RGB | GLUT_ALPHA | GLUT_DOUBLE | GLUT_DEPTH);
	glutCreateWindow(WINDOW_TITLE);

	// Register callbacks:
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutMouseFunc(MouseButton);
	glutMotionFunc(MouseMotion);
	glutKeyboardUpFunc(KeyboardUpFunc);
	glutSpecialFunc(SpecialFunc);
	glutSpecialUpFunc(SpecialUpFunc);
	glutIdleFunc(Idle);
	atexit(DestroyWindow);

	InitializeDrawing();

	//cactus.obj
	handles.push_back(Handle(_dualSphere(c3gaPoint(.0, .04, 0.7) - 0.5*SQR(0.15)*ni)));
	handles.push_back(Handle(_dualSphere(c3gaPoint(.0, .04, -0.8) - 0.5*SQR(0.15)*ni)));

	////cylinder.obj
	//handles.push_back(Handle(_dualSphere(c3gaPoint(.0, 0.9, .0) - 0.5*SQR(0.25)*ni)));
	//handles.push_back(Handle(_dualSphere(c3gaPoint(.0, -0.9, .0) - 0.5*SQR(0.25)*ni)));

	// //Armadillo foot and hand
	// handles.push_back(Handle(_dualSphere(c3gaPoint(-.5, 0.45, -.3) - 0.5*SQR(0.15)*ni))); // hand
	// handles.push_back(Handle(_dualSphere(c3gaPoint(.2, -0.6, .1) - 0.5*SQR(0.15)*ni))); // foot
	// handles.push_back(Handle(_dualSphere(c3gaPoint(.45, 0.45, -.3) - 0.5*SQR(0.15)*ni))); // hand
	// handles.push_back(Handle(_dualSphere(c3gaPoint(-.3, -0.6, .1) - 0.5*SQR(0.15)*ni))); // foot
	// //Armadillo hip y head
	// handles.push_back(Handle(_dualSphere(c3gaPoint(.0, 0.4, -.2) - 0.5*SQR(0.15)*ni)));
	// handles.push_back(Handle(_dualSphere(c3gaPoint(.0, -0.05, .1) - 0.5*SQR(0.15)*ni)));

	vertexDescriptorsLow.resize(meshLow.numVertices());
	vertexDescriptors.resize(mesh.numVertices());
	trianglesHi.resize(mesh.numFaces() * 3);

	for( Vertex& vertexLow : meshLow.getVertices())
	{
		vertexDescriptorsLow.rotors[vertexLow.ID] = Eigen::Quaterniond::Identity();
		normalizedPoint position = c3gaPoint(vertexLow.p.x(), vertexLow.p.y(), vertexLow.p.z());

		for (Handle& handle : handles)
		{
			TRversor TR = handle.GetTRVersor();

			if (_double(position << (TR * handle.dS * inverse(TR))) > 0) //inside the sphere
			{
				handle.constraints.insert(vertexLow.ID);
				vertexDescriptorsLow.constrainedPositions[vertexLow.ID] = vertexLow.p;
			}
		}
	}

	A = CreateLaplacianMatrix( &meshLow, systemType );

	ComputeLaplacianCoordinates(A, &meshLow, vertexDescriptorsLow.laplacianCoordinates);

	b3Low = Eigen::MatrixXd(A->numRows(), 3);

	for (Handle& handle : handles) {
		allconstraints.insert(handle.constraints.begin(), handle.constraints.end());
	}
	PreFactor(A, allconstraints, solverLow);

	g_meshArea = meshArea(&meshLow);

	for (Vertex& vertexHi : mesh.getVertices()) {
		vertexDescriptors.normalsOrig[vertexHi.ID] = vertexHi.n;
		vertexDescriptors.rotors[vertexHi.ID] = Eigen::Quaterniond::Identity();
	}

	for (Face& face : mesh.getFaces()) {
		int i = face.ID;
		int	v1 = face.edge->vertex->ID;
		int	v2 = face.edge->next->vertex->ID;
		int	v3 = face.edge->next->next->vertex->ID;
		trianglesHi.faces[i * 3 + 0] = v1;
		trianglesHi.faces[i * 3 + 1] = v2;
		trianglesHi.faces[i * 3 + 2] = v3;
	}

	AHi = CreateLaplacianMatrix(&mesh, systemType);
	ComputeLaplacianCoordinates(AHi, &mesh, vertexDescriptors.laplacianCoordinates);
	std::set<int> constraintsHi;
	for (map<int, int>::iterator iter = mapping.begin(); iter != mapping.end(); iter++){
		constraintsHi.insert(iter->second);
	}
	b3Hi = Eigen::MatrixXd(AHi->numRows(), 3);
	PreFactor(AHi, constraintsHi, solverHi);

  RotationMatrix = Eigen::SparseMatrix<double>(A->numRows()*4, A->numColumns()*4);
  triplets.resize(A->NonZeros()*16);
  Rknown = Eigen::VectorXd(A->numRows()*4);
  Rknown.setZero();
  RknownHi = Eigen::MatrixXd(AHi->numRows(), 4);
  RknownHi.setZero();

	glutMainLoop();

	return 0;
}

void SolvePoissonEquation(VertexBuffer& vertexDescriptors)
{
	int n = vertexDescriptors.get_size();

	for (int i = 0; i < n; ++i) {
		b3Low.row(i) = vertexDescriptors.laplacianCoordinates[i];
	}

	for (Handle& handle : handles) {
		Eigen::Affine3d M = MotorToMatrix(handle.GetTRVersor());
		for (int i : handle.constraints) {
			b3Low.row(i) = M * vertexDescriptors.constrainedPositions[i];
		}
	}

	xyzLow = solverLow.solve(b3Low);

	for( int i = 0 ; i < n ; ++i ) {
		vertexDescriptors.deformedPositions[i] = xyzLow.row(i);
	}
}

void SolvePoissonEquationHi(VertexBuffer& vertexDescriptors, VertexBuffer& vertexDescriptorsLow)
{
	int n = vertexDescriptors.get_size();

	for (int i = 0; i < n; ++i) {
		b3Hi.row(i) = vertexDescriptors.laplacianCoordinates[i];
	}

	for (auto&& pair : mapping){
		b3Hi.row(pair.second) = vertexDescriptorsLow.deformedPositions[pair.first];
	}

	xyzHi = solverHi.solve(b3Hi);

	for (int i = 0; i < n; ++i) {
		vertexDescriptors.deformedPositions[i] = xyzHi.row(i);
		vertexDescriptors.normals[i] = vertexDescriptors.rotors[i]._transformVector(vertexDescriptors.normalsOrig[i]);
	}
}

/*
Laplacian: L(Ri) => Ri = Sum_j wij Rj

Solve Sum_i wij (Ri - Rj) = 0
s.t Ri = R_const,  at the constrained points.

L Ri = 0
s.t Ri = R_const

Where L is discrete laplacian operator
*/
void LaplacianRotationsInterpolation(VertexBuffer& vertexDescriptors, VertexBuffer& vertexDescriptorsLow)
{
	int n = vertexDescriptors.get_size();

	for (auto&& pair : mapping){
		RknownHi.row(pair.second) = vertexDescriptorsLow.rotors[pair.first].coeffs();
	}

	RbestHi = solverHi.solve(RknownHi);

	for (int i = 0; i < n; ++i) {
		vertexDescriptors.rotors[i] = Eigen::Quaterniond((Eigen::Vector4d)RbestHi.row(i)).normalized();
	}
}

/*
Ei = sum_j |Ri qij ~Ri - pij|^2
Ei = sum_j |Ri qij - pij Ri|^2

Ri = w + B

(w + B) qij - pij (w + B)
w qij + B qij - pij w - pij B
w qij + B qij - pij w - pij B

pij B = pij . B + pij x B
B qij = B . qij + B x qij = qij . B - qij x B

w qij + (qij . B - qij x B) - pij w - (pij . B + pij x B)
w qij + qij . B - qij x B - pij w - pij . B - pij x B

(qij - pij) w + (qij - pij) . B - (qij + pij) x B

(qij - pij) w - (qij - pij)^T B - [qij + pij]_x B

|0            -(qij - pij)^T  | |w| = |-(qij - pij)^T B               |
|(qij - pij)   [qij + pij]^T_x| |B|   |(qij - pij) w - [qij + pij]_x B|

d = (qij - pij)
s = (qij + pij)
d w - s^T B - [s]_x B

|0    -d^T  | |w| = |-d^T B       |
|d   [s]^T_x| |B|   |d w - [s]_x B|

|0    -d^T  |^T |0    -d^T  | =
|d   [s]^T_x|   |d   [s]^T_x|

|0    d^T  | |0    -d^T  | = |d^T d    d^T [s]^T_x              |
|-d   [s]_x| |d   [s]^T_x|   |[s]_x d  -d (-d^T) + [s]_x [s]^T_x|

|0    d^T  | |0    -d^T  | = |d^T d    d^T [s]^T_x    |
|-d   [s]_x| |d   [s]^T_x|   |[s]_x d  d d^T - [s]^2_x|

| ||d||^2   (s x d)^T      |
| s x d     d d^T - [s]^2_x|

| ||d||^2   (s x d)^T                |
| s x d     d d^T - s s^T + ||s||^2 I|

d = (qij - pij)
s = (qij + pij)

||d||^2 = - 2 pij^T qij + ||pij||^2 + ||qij||^2
||s||^2 = + 2 pij^T qij + ||pij||^2 + ||qij||^2
s x d = 2 pij x qij
d d^T = qij qij^T - pij qij^T - qij pij^T + pij pij^T
s s^T = qij qij^T + pij qij^T + qij pij^T + pij pij^T
d d^T - s s^T = - 2 pij qij^T - 2 qij pij^T

| ||d||^2   (s x d)^T                |
| s x d     d d^T - s s^T + ||s||^2 I|

-2 | qij^T pij   (qij x pij)^T                      | + ||pij||^2 + ||qij||^2 I4x4
   | qij x pij   qij pij^T + pij qij^T - qij^T pij I|

COORDINATES:

qij pij^T = | qx | |px py pz | = | qx px  qx py  qx pz |
            | qy |               | qy px  qy py  qy pz |
            | qz |               | qz px  qz py  qz pz |

Trace(qij pij^T) = qij^T pij
qij x pij = [(qy pz - qz py) e2e3, (qz px - qx pz) e3e1, (qx py - qy px) e1e2]
pij qij^T = (qij pij^T)^T
qij pij^T + pij qij^T = qij pij^T + (qij pij^T)^T =

| qx px  qx py  qx pz | | qx px  qy px  qz px |   | qx px+qx px  qx py+qy px  qx pz+qz px |
| qy px  qy py  qy pz | | qx py  qy py  qz py | = | qy px+qx py  qy py+qy py  qy pz+qz py |
| qz px  qz py  qz pz | | qx pz  qy pz  qz pz |   | qz px+qx pz  qz py+qy pz  qz pz+qz pz |

qij = a e1 + b e2 + c e3
pij = d e1 + e e2 + f e3
(b f - c e) e2e3 + (c d - a f) e3e1 + (a e - b d) e1e2
(qy pz - qz py) e2e3 + (qz px - qx pz) e3e1 + (qx py - qy px) e1e2
*/
void E3GA_Prep(const vector<Eigen::Vector3d>& P, const vector<Eigen::Vector3d>& Q, const vector<double>& w, const int N, Eigen::Matrix4d &JtJ)
{
	Eigen::Matrix3d Sx;
	double wj;
	Sx.setZero();
	double S = 0;
	for (size_t j = 0; j < N; ++j) {
		wj = w[j];
		const Eigen::Vector3d& Qj = Q[j];
		const Eigen::Vector3d& Pj = P[j];
		S += wj * Pj.dot(Pj);
		S += wj * Qj.dot(Qj);
		Sx.noalias() += (wj * Pj) * Qj.transpose();
	}

	wj = 0.5 * S;
 	JtJ(0,0) = wj - Sx.trace();
	wj = wj + Sx.trace();
	JtJ(0,1) = -(Sx(1, 2) - Sx(2, 1));
	JtJ(0,2) = -(Sx(2, 0) - Sx(0, 2));
	JtJ(0,3) = -(Sx(0, 1) - Sx(1, 0));
	JtJ(1,1) = -2.0 * Sx(0, 0) + wj;
	JtJ(1,2) = -(Sx(0, 1) + Sx(1, 0));
	JtJ(1,3) = -(Sx(2, 0) + Sx(0, 2));
	JtJ(2,2) = -2.0 * Sx(1, 1) + wj;
	JtJ(2,3) = -(Sx(1, 2) + Sx(2, 1));
	JtJ(3,3) = -2.0 * Sx(2, 2) + wj;
  JtJ.selfadjointView<Eigen::Upper>().evalTo(JtJ);
}

/*
Ei = sum_j cij |Ri qij ~Ri - pij|^2 + alphaA sum_j wij |Rj - Ri|^2
Ei = sum_j cij |Ri qij - pij Ri|^2 + alphaA sum_j wij |Rj - Ri|^2

Ei = Ri^T Mi Ri + alphaA sum_j wij (Rj^T Rj - Rj^T Ri + Ri^T Ri)
d/Ri Ei = Mi Ri + sum_j alphaA wij (-Rj + Ri)

Mi Ri + sum_j alphaA wij (-Rj + Ri) = 0
Mi Ri - sum_j alphaA wij (Rj) + sum_j alphaA wij (Ri) = 0

Since sum_j alphaA wij = alphaA

Mi Ri + alphaA Ri - sum_j alphaA wij (Rj) = 0
(Mi + alphaA I) Ri = sum_j alphaA wij Rj
*/
Eigen::Quaterniond E3GA_Fast4(const vector<Eigen::Vector3d>& P, const vector<Eigen::Vector3d>& Q, const vector<double>& w, double alphaA, const vector<Eigen::Quaterniond>& Rq, const int N)
{
	Eigen::Matrix4d JtJ;
	Eigen::Matrix3d Sx;
  Eigen::Vector4d b;

  b.setZero();
  for (int i = 0; i < N; ++i)
	{
		const Eigen::Quaterniond& Qi = Rq[i];
		b(0) += Qi.w(); // 1.0
		b(1) += Qi.x(); // e1 ^ e2
		b(2) += Qi.y(); // e1 ^ e3
		b(3) += Qi.z(); // e2 ^ e3
	}

  b *= alphaA / (double) N;

	double wj;
	Sx.setZero();
	double S = 0;
	for (size_t j = 0; j < N; ++j) {
		wj = w[j];
		const Eigen::Vector3d& Qj = Q[j];
		const Eigen::Vector3d& Pj = P[j];
		S += wj * Pj.dot(Pj);
		S += wj * Qj.dot(Qj);
		Sx.noalias() += (wj * Pj) * Qj.transpose();
	}

	wj = 0.5 * S;
 	JtJ(0,0) = wj - Sx.trace() + alphaA;
	wj = wj + Sx.trace() + alphaA;
	JtJ(0,1) = -(Sx(1, 2) - Sx(2, 1));
	JtJ(0,2) = -(Sx(2, 0) - Sx(0, 2));
	JtJ(0,3) = -(Sx(0, 1) - Sx(1, 0));
	JtJ(1,1) = -2.0 * Sx(0, 0) + wj;
	JtJ(1,2) = -(Sx(0, 1) + Sx(1, 0));
	JtJ(1,3) = -(Sx(2, 0) + Sx(0, 2));
	JtJ(2,2) = -2.0 * Sx(1, 1) + wj;
	JtJ(2,3) = -(Sx(1, 2) + Sx(2, 1));
	JtJ(3,3) = -2.0 * Sx(2, 2) + wj;
  JtJ.selfadjointView<Eigen::Upper>().evalTo(JtJ);

  Eigen::Vector4d x = JtJ.inverse().eval() * b;
  return Eigen::Quaterniond(x(0), x(1), x(2), x(3)).normalized();
}

void SolveRotationsSystem(Mesh *mesh, std::shared_ptr<SparseMatrix> A, VertexBuffer& vertexDescriptors, std::set<int>& constraints, bool analyzeSystem)
{
	vector<Eigen::Vector3d> P;
	vector<Eigen::Vector3d> Q;
  vector<double> w;
  vector<int> neighbors;
  Eigen::VectorXd Rbest;
  Eigen::Matrix4d JtJ;
  vector<Eigen::Quaterniond> Rq;

	P.resize(32);
	Q.resize(32);
  neighbors.resize(32);
  triplets.reserve(A->NonZeros()*16);
  w.resize(32);
  Rq.resize(32);
	const double alpha = 0.14;
	const double ei = 0.2;
  const double cij = 1.0;
  const double alphaA = alpha * g_meshArea;
  int nnz = 0;

	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;
    int ii = i*4;

		Eigen::Vector3d &pi = vertex.p;
		Eigen::Vector3d &tpi = vertexDescriptors.deformedPositions[i];

		int vertexDegree = 0;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); vertexDegree < 32 && !edgeAroundIter.end(); edgeAroundIter++, vertexDegree++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;

			Eigen::Vector3d &tpj = vertexDescriptors.deformedPositions[j];
			Eigen::Vector3d &pj = mesh->vertexAt(j).p;
			P[vertexDegree] = pj - pi;
			Q[vertexDegree] = tpj - tpi;
      neighbors[vertexDegree] = j;
      w[vertexDegree] = (*A)(i, j);
		}
    if (!is_constrained(constraints, i)) {
  		double invVertexDegree = (alphaA * cij) / ((double)vertexDegree + ei); // Plus ei since we consider the feedback.
      for(int v = 0; v < vertexDegree; ++v) {
        int jj = neighbors[v]*4;
        triplets[nnz++] = Eigen::Triplet<double>(ii+0, jj+0, -invVertexDegree);
        triplets[nnz++] = Eigen::Triplet<double>(ii+1, jj+1, -invVertexDegree);
        triplets[nnz++] = Eigen::Triplet<double>(ii+2, jj+2, -invVertexDegree);
        triplets[nnz++] = Eigen::Triplet<double>(ii+3, jj+3, -invVertexDegree);
      }
      E3GA_Prep(P, Q, w, vertexDegree, JtJ);
      JtJ(0, 0) += alphaA + 1e-6;
      JtJ(1, 1) += alphaA + 1e-6;
      JtJ(2, 2) += alphaA + 1e-6;
      JtJ(3, 3) += alphaA + 1e-6;
      triplets[nnz++] = Eigen::Triplet<double>(ii+0, ii+0, JtJ(0, 0));
      triplets[nnz++] = Eigen::Triplet<double>(ii+0, ii+1, JtJ(0, 1));
      triplets[nnz++] = Eigen::Triplet<double>(ii+0, ii+2, JtJ(0, 2));
      triplets[nnz++] = Eigen::Triplet<double>(ii+0, ii+3, JtJ(0, 3));
      triplets[nnz++] = Eigen::Triplet<double>(ii+1, ii+0, JtJ(1, 0));
      triplets[nnz++] = Eigen::Triplet<double>(ii+1, ii+1, JtJ(1, 1));
      triplets[nnz++] = Eigen::Triplet<double>(ii+1, ii+2, JtJ(1, 2));
      triplets[nnz++] = Eigen::Triplet<double>(ii+1, ii+3, JtJ(1, 3));
      triplets[nnz++] = Eigen::Triplet<double>(ii+2, ii+0, JtJ(2, 0));
      triplets[nnz++] = Eigen::Triplet<double>(ii+2, ii+1, JtJ(2, 1));
      triplets[nnz++] = Eigen::Triplet<double>(ii+2, ii+2, JtJ(2, 2));
      triplets[nnz++] = Eigen::Triplet<double>(ii+2, ii+3, JtJ(2, 3));
      triplets[nnz++] = Eigen::Triplet<double>(ii+3, ii+0, JtJ(3, 0));
      triplets[nnz++] = Eigen::Triplet<double>(ii+3, ii+1, JtJ(3, 1));
      triplets[nnz++] = Eigen::Triplet<double>(ii+3, ii+2, JtJ(3, 2));
      triplets[nnz++] = Eigen::Triplet<double>(ii+3, ii+3, JtJ(3, 3));
      // Feedback added on the RHS.
      invVertexDegree *= ei;
      Rknown[ii+0] = invVertexDegree * vertexDescriptors.rotors[i].w();
      Rknown[ii+1] = invVertexDegree * vertexDescriptors.rotors[i].x();
      Rknown[ii+2] = invVertexDegree * vertexDescriptors.rotors[i].y();
      Rknown[ii+3] = invVertexDegree * vertexDescriptors.rotors[i].z();
    }
    else
    {
      for(int v = 0; v < vertexDegree; ++v) {
        Rq[v] = vertexDescriptors.rotors[neighbors[v]];
      }

      Eigen::Quaterniond M = E3GA_Fast4(P, Q, w, alphaA, Rq, vertexDegree);
      Rknown[ii+0] = M.w();
      Rknown[ii+1] = M.x();
      Rknown[ii+2] = M.y();
      Rknown[ii+3] = M.z();
      triplets[nnz++] = Eigen::Triplet<double>(ii+0, ii+0, 1.0);
      triplets[nnz++] = Eigen::Triplet<double>(ii+1, ii+1, 1.0);
      triplets[nnz++] = Eigen::Triplet<double>(ii+2, ii+2, 1.0);
      triplets[nnz++] = Eigen::Triplet<double>(ii+3, ii+3, 1.0);
    }
	}
  RotationMatrix.setFromTriplets(triplets.begin(), triplets.begin() + nnz);
  if(analyzeSystem) {
    solverRotations.analyzePattern(RotationMatrix);
  }
  solverRotations.factorize(RotationMatrix);
	if (solverRotations.info() != Eigen::Success) {
		// TODO: error handling
	}
	Rbest = solverRotations.solve(Rknown);

	for (int i = 0; i < vertexDescriptors.get_size(); ++i) {
    int ii = i*4;
    Eigen::Quaterniond M = Eigen::Quaterniond(Rbest(ii+0), Rbest(ii+1), Rbest(ii+2), Rbest(ii+3));
		vertexDescriptors.rotors[i] = M.normalized();
	}

  std::fill(vertexDescriptors.laplacianCoordinates.begin(), vertexDescriptors.laplacianCoordinates.end(), Eigen::Vector3d(0, 0, 0));

	//concurrency::parallel_for_each(mesh->getVertices().begin(), mesh->getVertices().end(), [&](Vertex& vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;
			double wij = (*A)(i, j);
			Eigen::Quaterniond&Ri = vertexDescriptors.rotors[i];
			Eigen::Quaterniond&Rj = vertexDescriptors.rotors[j];
			Eigen::Vector3d V = mesh->vertexAt(j).p - mesh->vertexAt(i).p;
			vertexDescriptors.laplacianCoordinates[i] += 0.5 * wij * (Ri._transformVector(V) + Rj._transformVector(V));;
		}
	}
}

void transferRotations(Mesh *mesh, std::shared_ptr<SparseMatrix> A, VertexBuffer& vertexDescriptors, VertexBuffer& vertexDescriptorsLow)
{
  LaplacianRotationsInterpolation(vertexDescriptors, vertexDescriptorsLow);

	std::fill(vertexDescriptors.laplacianCoordinates.begin(), vertexDescriptors.laplacianCoordinates.end(), Eigen::Vector3d(0, 0, 0));

	//concurrency::parallel_for_each(mesh->getVertices().begin(), mesh->getVertices().end(), [&](Vertex& vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;
			double wij = (*A)(i, j);
			Eigen::Quaterniond &Ri = vertexDescriptors.rotors[i];
			Eigen::Quaterniond &Rj = vertexDescriptors.rotors[j];
			Eigen::Vector3d V = mesh->vertexAt(j).p - mesh->vertexAt(i).p;
			vertexDescriptors.laplacianCoordinates[i] += 0.5 * wij * (Ri._transformVector(V) + Rj._transformVector(V));;
		}
	}
	//});
}

double ComputeEnergy(Mesh *mesh, std::shared_ptr<SparseMatrix> A, VertexBuffer& vertexDescriptors)
{
	double arapEnergy = 0.0;
	const double alpha = 0.28;
	//concurrency::parallel_for_each(mesh->vertices.begin(), mesh->vertices.end(), [&](Vertex * vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;

		Eigen::Vector3d& pi = vertex.p;
		Eigen::Vector3d &tpi = vertexDescriptors.deformedPositions[i];
		auto& Ri = vertexDescriptors.rotors[i];

		int vertexDegree = 0;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			vertexDegree++;
		}

		double invVertexDegree = alpha * g_meshArea / (double)vertexDegree;

		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;

			Eigen::Vector3d &tpj = vertexDescriptors.deformedPositions[j];
			Eigen::Vector3d &pj = mesh->vertexAt(j).p;

			Eigen::Vector3d eij = (pj - pi);
			auto q_ij = tpj - tpi;
			auto p_ij = eij;
			auto& Rj = vertexDescriptors.rotors[j];

			arapEnergy += (*A)(i, j) * (Ri._transformVector(p_ij) - q_ij).squaredNorm();
			arapEnergy += invVertexDegree * (Rj.coeffs() - Ri.coeffs()).squaredNorm();
		}
	}//);

	return arapEnergy;
}

void display()
{
	/*
	 *	matrices
	 */
	glViewport( 0, 0, g_viewportWidth, g_viewportHeight );
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	pickLoadMatrix();
	GLpick::g_frustumFar = 1000.0;
	GLpick::g_frustumNear = .1;
	gluPerspective( 60.0, (double)g_viewportWidth/(double)g_viewportHeight, GLpick::g_frustumNear, GLpick::g_frustumFar );
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	glShadeModel(GL_SMOOTH);	//gouraud shading
	glClearDepth(1.0f);
	glClearColor( .75f, .75f, .75f, .0f );
	glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );

	/*
	 *	estados
	 */
	glEnable(GL_CULL_FACE);		//face culling
	glCullFace( GL_BACK );
	glFrontFace( GL_CCW );
	glEnable(GL_DEPTH_TEST);	//z-buffer
	glDepthFunc(GL_LEQUAL);

	/*
	 *	iluminacion
	 */
	float		ambient[] = { .3f, .3f, .3f, 1.f };
	float		diffuse[] = { .3f, .3f, .3f, 1.f };
	float		position[] = { .0f, 0.f, -150.f, 1.f };
	float		specular[] = { 1.f, 1.f, 1.f };

	glLightfv( GL_LIGHT0, GL_AMBIENT, ambient );
	glLightfv( GL_LIGHT0, GL_DIFFUSE, diffuse );
	glLightf(GL_LIGHT0, GL_CONSTANT_ATTENUATION, 0);
	glLightf(GL_LIGHT0, GL_LINEAR_ATTENUATION, 0.0125);
	glEnable(  GL_LIGHT0   );
	glEnable(  GL_LIGHTING );
	glMaterialfv( GL_FRONT_AND_BACK, GL_SPECULAR, specular );
	glMaterialf( GL_FRONT_AND_BACK, GL_SHININESS, 50.f );

	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

	glLoadIdentity();

	g_camera.glLookAt();

	glLightfv( GL_LIGHT0, /*GL_POSITION*/GL_SPOT_DIRECTION, position );

	glPushMatrix();

	rotorGLMult(g_modelRotor);

	benchs.Begin();

	if(!g_computeBasis)
	{
		static bool oneTime = true;
		if (oneTime || (g_rotateKeyRotors || g_translateKeyRotors || g_automaticAnimation))
		{
			if(oneTime == true)
			{
				SolvePoissonEquation(vertexDescriptorsLow);
			}

			for(int i = 0 ; i < 3 ; ++i)
			{
        SolveRotationsSystem(&meshLow, A, vertexDescriptorsLow, allconstraints, oneTime == true);
				SolvePoissonEquation(vertexDescriptorsLow);
			}
			transferRotations(&mesh, AHi, vertexDescriptors, vertexDescriptorsLow);
			SolvePoissonEquationHi(vertexDescriptors, vertexDescriptorsLow);

      oneTime = false;
		}
	}
	if(g_iterateManyTimes)
	{
		if (g_convergence)
		{
			clock_t begin;
			clock_t end;
			double avgTime = 0.0;

			auto arapEnergy = ComputeEnergy(&meshLow, A, vertexDescriptorsLow);
			int count = 0;
			for (; count < 2500; ++count)
			{
				begin = clock();

        SolveRotationsSystem(&meshLow, A, vertexDescriptorsLow, allconstraints, false);
				SolvePoissonEquation(vertexDescriptorsLow);

				end = clock();

				double secDeform = (double)(end - begin) / CLOCKS_PER_SEC;

				avgTime += secDeform;

				auto newArapEnergy = ComputeEnergy(&meshLow, A, vertexDescriptorsLow);
				if (fabs(arapEnergy - newArapEnergy) < 1e-4)
				{
					std::cout << "convergence after " << count + 1 << " iterations" << std::endl;
					break;
				}
				arapEnergy = newArapEnergy;
			}
			arapEnergy = ComputeEnergy(&meshLow, A, vertexDescriptorsLow);
			std::cout << arapEnergy << std::endl;
			std::cout << "Average iteration time (sec): " << avgTime / (double)(count + 1) << endl;
			std::cout << "Total accum time (sec): " << avgTime << endl;

			transferRotations(&mesh, AHi, vertexDescriptors, vertexDescriptorsLow);
			SolvePoissonEquationHi(vertexDescriptors, vertexDescriptorsLow);
		}
		else
		{
			for (int i = 0; i < 100; ++i)
			{
        SolveRotationsSystem(&meshLow, A, vertexDescriptorsLow, allconstraints, false);
				SolvePoissonEquation(vertexDescriptorsLow);
			}
			transferRotations(&mesh, AHi, vertexDescriptors, vertexDescriptorsLow);
			SolvePoissonEquationHi(vertexDescriptors, vertexDescriptorsLow);
		}
		g_iterateManyTimes = false;
	}

	benchs.End();

	if (GLpick::g_pickActive) glLoadName((GLuint)-1);

	double alpha = 1.0;

	//glEnable (GL_BLEND);
	//glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	//alpha = 0.5;

	//Mesh-Faces Rendering
	glPolygonMode( GL_FRONT_AND_BACK, GL_FILL /*GL_LINE GL_FILL GL_POINT*/);
	glEnable (GL_POLYGON_OFFSET_FILL);
	glPolygonOffset (1., 1.);
	glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
	glEnable( GL_COLOR_MATERIAL );
	if (GLpick::g_pickActive) glLoadName((GLuint)10);

	glColor4d(1, 1, 1, alpha);
	glEnableClientState(GL_NORMAL_ARRAY);
	glEnableClientState(GL_VERTEX_ARRAY);
	glVertexPointer(3, GL_DOUBLE, 0, &vertexDescriptors.deformedPositions[0]);
	glNormalPointer(GL_DOUBLE, 0, &vertexDescriptors.normals[0]);
	// draw the model
	glDrawElements(GL_TRIANGLES, trianglesHi.get_size(), GL_UNSIGNED_INT, &trianglesHi.faces[0]);
	// deactivate vertex arrays after drawing
	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);

	if (g_showWires)
	{
		if (!GLpick::g_pickActive)
		{
			//Mesh-Edges Rendering (superimposed to faces)
			glPolygonMode(GL_FRONT_AND_BACK, GL_LINE /*GL_LINE GL_FILL GL_POINT*/);
			glColor4d(.5, .5, .5, alpha);
			glDisable(GL_LIGHTING);
			glEnableClientState(GL_VERTEX_ARRAY);
			glVertexPointer(3, GL_DOUBLE, 0, &vertexDescriptors.deformedPositions[0]);
			// draw the model
			glDrawElements(GL_TRIANGLES, trianglesHi.get_size(), GL_UNSIGNED_INT, &trianglesHi.faces[0]);
			// deactivate vertex arrays after drawing
			glDisableClientState(GL_VERTEX_ARRAY);
			glEnable(GL_LIGHTING);
		}
	}

	//for (map<int, int>::iterator iter = mapping.begin(); iter != mapping.end(); iter++){
	//	DrawPoint(c3gaPoint(vertexDescriptors[iter->second]->deformedPosition));
	//}

	glDisable( GL_COLOR_MATERIAL );
	glDisable(GL_POLYGON_OFFSET_FILL);

	//glDisable (GL_BLEND);

	if(g_showSpheres)
	{
		//Handles rendering
		glPolygonMode( GL_FRONT_AND_BACK, GL_FILL /*GL_LINE GL_FILL GL_POINT*/);

		float	turcoise[] = { .0f, .5f, .5f, 0.3f };
		float	red[] = { .5f, .0f, .0f, 0.3f };

		for( int k = 0 ; k < handles.size() ; ++k)
		{
			if (GLpick::g_pickActive) glLoadName((GLuint)k);
			TRversor R = handles[k].GetTRVersor();
			DrawTransparentDualSphere( _dualSphere( R * handles[k].dS * inverse(R) ), turcoise );
		}
	}

	glPopMatrix();

	glutSwapBuffers();
}

void reshape(GLint width, GLint height)
{
	g_viewportWidth = width;
	g_viewportHeight = height;

	// redraw viewport
	glutPostRedisplay();
}

vectorE3GA mousePosToVector(int x, int y) {
	x -= g_viewportWidth / 2;
	y -= g_viewportHeight / 2;
	return _vectorE3GA((float)-x * e1 - (float)y * e2);
}

void MouseButton(int button, int state, int x, int y)
{
	g_rotateModel = false;
	g_rotateKeyRotors = false;
	g_translateKeyRotors = false;

	if (button == GLUT_LEFT_BUTTON)
	{
		g_prevMousePos = mousePosToVector(x, y);

		GLpick::g_pickWinSize = 1;
		g_dragObject = pick(x, g_viewportHeight - y, display, &g_dragDistance);

		if(g_dragObject == -1 || g_dragObject == 10 )
		{
			vectorE3GA mousePos = mousePosToVector(x, y);
			g_rotateModel = true;

			if ((_Float(norm_e(mousePos)) / _Float(norm_e(g_viewportWidth * e1 + g_viewportHeight * e2))) < 0.2)
				g_rotateModelOutOfPlane = true;
			else g_rotateModelOutOfPlane = false;
		}
		else if(g_dragObject >= 0 && g_dragObject < handles.size())
		{
			g_rotateKeyRotors = true;
		}
	}

	if (button == GLUT_RIGHT_BUTTON)
	{
		g_prevMousePos = mousePosToVector(x, y);

		GLpick::g_pickWinSize = 1;
		g_dragObject = pick(x, g_viewportHeight - y, display, &g_dragDistance);

		if(g_dragObject >= 0 && g_dragObject < handles.size())
			g_translateKeyRotors = true;
	}
}

void MouseMotion(int x, int y)
{
	if (g_rotateModel || g_rotateKeyRotors || g_translateKeyRotors )
	{
		// get mouse position, motion
		vectorE3GA mousePos = mousePosToVector(x, y);
		vectorE3GA motion = mousePos - g_prevMousePos;

		if (g_rotateModel)
		{
			// update rotor
			if (g_rotateModelOutOfPlane)
				g_modelRotor = exp(g_camera.rotateVel * (motion ^ e3) ) * g_modelRotor;
			else
				g_modelRotor = exp(0.00001f * (motion ^ mousePos) ) * g_modelRotor;
		}
		if(g_rotateKeyRotors)
		{
			rotor R1 =  _rotor( inverse(g_modelRotor) * exp(-g_camera.rotateVel * (motion ^ e3) ) * g_modelRotor);
			//rotor R1 =  _rotor( exp(-g_camera.rotateVel * (motion ^ e3) ) );
			if(g_dragObject < handles.size())
			{
				rotor R = handles[g_dragObject].R;
				handles[g_dragObject].R = normalize(_TRversor( R1 * R  ) );
			}
		}

		if(g_translateKeyRotors)
		{
			normalizedTranslator T1 = _normalizedTranslator(inverse(g_modelRotor) * exp( _freeVector(-g_camera.translateVel*motion*ni) ) * g_modelRotor);
			if(g_dragObject < handles.size())
			{
				translator T = handles[g_dragObject].T;
				handles[g_dragObject].T = normalize(_TRversor( T1 * T ));
			}
		}

		// remember mouse pos for next motion:
		g_prevMousePos = mousePos;

		// redraw viewport
		glutPostRedisplay();
	}
}

void SpecialFunc(int key, int x, int y)
{
	switch(key) {
		case GLUT_KEY_F1 :
			{
				int mod = glutGetModifiers();
				if(mod == GLUT_ACTIVE_CTRL || mod == GLUT_ACTIVE_SHIFT )
				{
				}
			}
			break;
		case GLUT_KEY_UP:
			{
				if(g_rotateKeyRotors)
				{
					handles[g_dragObject].dS = ChangeDualSphereRadiusSize(handles[g_dragObject].dS, 0.025);

					// redraw viewport
					glutPostRedisplay();
				}
			}
			break;
		case GLUT_KEY_DOWN:
			{
				if(g_rotateKeyRotors)
				{
					handles[g_dragObject].dS = ChangeDualSphereRadiusSize(handles[g_dragObject].dS, -0.025);

					// redraw viewport
					glutPostRedisplay();
				}
			}
			break;
	}
}

void SpecialUpFunc(int key, int x, int y)
{
}

void KeyboardUpFunc(unsigned char key, int x, int y)
{
	if (key == 'c' || key == 'C')
	{
		g_convergence = !g_convergence;
		vectorE3GA motion = _vectorE3GA(-70.0, -60.0, 0);
		normalizedTranslator T1 = _normalizedTranslator(inverse(g_modelRotor) * exp(_freeVector(-g_camera.translateVel*motion*ni)) * g_modelRotor);
		translator T = handles[0].T;
		handles[0].T = normalize(_TRversor(T1 * T));

		glutPostRedisplay();
	}

	if (key == 'a' || key == 'A')
	{
		g_automaticAnimation = !g_automaticAnimation;
		if (g_automaticAnimation)
		{
			benchs.Start();
		}
		else
		{
			benchs.Stop();
		}

		glutPostRedisplay();
	}

	if(key == 'w' || key == 'W')
	{
		g_showWires = !g_showWires;
		glutPostRedisplay();
	}

	if( key == 'h' || key == 'H' )
	{
		g_showSpheres = !g_showSpheres;
		glutPostRedisplay();
	}

	if( key == 'x' || key == 'X' )
	{
		g_iterateManyTimes = true;
		glutPostRedisplay();
	}
}

void Idle()
{
	// redraw viewport
	if (g_automaticAnimation)
	{
		const int minCount = -50;
		const int maxCount = 50;
		static int mycount = 0;
		static int direction = 0;
		vectorE3GA motion = _vectorE3GA(1.0, 0, 0);
		if (mycount == maxCount)
		{
			direction = 1;
		}
		if (mycount == minCount)
		{
			direction = 0;
		}

		if (mycount < maxCount && direction == 0)
		{
			motion = _vectorE3GA(1.0, 0, 0);
			mycount++;
		}
		if (mycount > minCount && direction == 1)
		{
			motion = _vectorE3GA(-1.0, 0, 0);
			mycount--;
		}

		normalizedTranslator T1 = _normalizedTranslator(inverse(g_modelRotor) * exp(_freeVector(-g_camera.translateVel*motion*ni)) * g_modelRotor);
		translator T = handles[0].T;
		handles[0].T = normalize(_TRversor(T1 * T));

		if (!benchs.IsStarted())
		{
			benchs.Start();
		}

		glutPostRedisplay();
	}
}

void DestroyWindow()
{
	ReleaseDrawing();
}


#ifndef __PARTICLE_FILTER_HAND_LIKELIHOOD_HPP
#define __PARTICLE_FILTER_HAND_LIKELIHOOD_HPP

#include <cmath>
#include <ctime>
#include <vector>
#include <algorithm>
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include <padenti/image.hpp>
#include <padenti/mh_rtree.hpp>
#include <padenti/cl_regressor.hpp>

#include "rchord_meanshift.hpp"

#include "particle_filter_hand.hpp"
#include "particle_filter_base.hpp"

// Random Forest parameters
#define RF_FEAT_SIZE (2)
#define RF_VAL_SIZE  (4)
#define RF_N_HYPH    (2)

// Meanshift parameters
#define MS_N_HYPH    (10)
#define MS_SIGMA    (1.6223)
#define MS_STEPSIZE (0.2)
#define MS_STEPTHR  (0.025)
#define MS_SAMECTHR (0.15)
#define MS_NITERTHR (10u)
#define MS_SAMPLED_PIXELS (200u)

// Likelihood parameters
#define EVAL_SIGMA  (MS_SIGMA/3)

// Commodity typedefs to avoid long class type declaration
typedef Image<unsigned short, 1> DepthT;
typedef Image<unsigned char, 1> MaskT;
typedef Image<int, 1> PredictionT;
typedef MHRTree<short int, RF_FEAT_SIZE, RF_VAL_SIZE, RF_N_HYPH> MHRTreeT;
typedef RTreeNode<short int, RF_FEAT_SIZE, RF_VAL_SIZE> RTreeNodeT;
typedef CLRegressor<unsigned short, 1, short, RF_FEAT_SIZE, RF_VAL_SIZE> RegressorT;

class HandLikelihoodModel: public LikelihoodModel<HAND_STATE_SIZE>
{
private:
  RegressorT &regressor;
  Particle<HAND_STATE_SIZE> prevState;
  std::vector<Eigen::Matrix3f> modes;
  std::vector<float> modesW;
  std::vector<int> modesCount;
  std::vector<float> modesVar;
  int nModes;
  float evalSigma;
  float deltaT;
public:

  HandLikelihoodModel(RegressorT &regressor,
		      float evalSigma=EVAL_SIGMA):
    regressor(regressor),
    modes(std::vector<Eigen::Matrix3f>(MS_N_HYPH)),
    modesW(std::vector<float>(MS_N_HYPH)),
    modesCount(std::vector<int>(MS_N_HYPH)),
    modesVar(std::vector<float>(MS_N_HYPH)),
    nModes(0),
    evalSigma(evalSigma)
  {}

  const std::vector<Eigen::Matrix3f> &getModes() const {return modes;}
  const std::vector<float> &getModeWeights() const {return modesW;}
  const std::vector<int> &getModeCount() const {return modesCount;}
  const std::vector<float> &getModeVar() const {return modesVar;}
  int getNModes() const {return nModes;}

  void eval(std::vector<Particle<HAND_STATE_SIZE> > &particles) const
  {
    Eigen::Quaternion<float> prevq(prevState.state[0],
				   prevState.state[1],
				   prevState.state[2],
				   prevState.state[3]);
    Eigen::Vector3d prevV(prevState.state[4],
			  prevState.state[5],
			  prevState.state[6]);

    // Compute the particles weight as the Panzer window estimate on rotation chordal
    // distance values using a Gaussian kernel
    // TODO: per-mode bandwidth as in Herdtweek paper?
    for (int i=0; i<particles.size(); i++)
    {
      Eigen::Quaternion<float> q(particles.at(i).state[0],
				 particles.at(i).state[1],
				 particles.at(i).state[2],
				 particles.at(i).state[3]);
      Eigen::Vector3d currV(particles.at(i).state[4],
			    particles.at(i).state[5],
			    particles.at(i).state[6]);
      Eigen::Matrix3f R(q);

      // Compute the likelihood
      double l = 0;
      for (int j=0; j<nModes; j++)
      {
	Eigen::Matrix3f rotDiff = R-modes.at(j);
	l += modesW.at(j) * exp(-rotDiff.cwiseProduct(rotDiff).sum()/(evalSigma*evalSigma));
	//l += modesCount.at(j)/sqrt(modesVar.at(j)) *
	//  exp(-rotDiff.cwiseProduct(rotDiff).sum()/(modesVar.at(j)));
      }

     // Compute the velocity with respect to the previous best particle rotation
      // TODO: parameterize velocity bound
      Eigen::Quaternion<float> deltaq;
      Eigen::AngleAxisf axang;
      float V;
      deltaq = q * prevq.inverse();
      axang = deltaq;
      V = axang.angle()/deltaT;
      particles.at(i).likelihood = V<10*M_PI ? l : 0.;

      //particles.at(i).likelihood = l;
      //double diffV = (currV-prevV).norm();
      //particles.at(i).likelihood *= exp(-(diffV*diffV)/(M_PI*M_PI));
      //particles.at(i).likelihood *= exp(-(V*V)/(25*M_PI*M_PI));

    }
  }

  void updateModel(const Particle<HAND_STATE_SIZE> &_prevState,
		   const MHRTreeT &rtree, const DepthT &depthmap,
		   float _deltaT=1./30)
  {
    // Perform RF prediction on the current depthmap. The depthmap is supposed to
    // be already segmented
    // - Mask as non-zero pixels
    MaskT mask(depthmap.getWidth(), depthmap.getHeight());
    cv::Mat cvDepth(depthmap.getHeight(), depthmap.getWidth(), CV_16U,
		    reinterpret_cast<unsigned char*>(depthmap.getData()));
    cv::Mat cvMask(depthmap.getHeight(), depthmap.getWidth(), CV_8U,
		   reinterpret_cast<unsigned char*>(mask.getData()));
    cvMask.setTo(0);
    cvMask.setTo(1, cvDepth>0);

    // - perform prediction
    // TODO: handle multiple trees
    PredictionT prediction(depthmap.getWidth(), depthmap.getHeight());
    regressor.predict(0, depthmap, prediction, mask);

    // - sample image pixels, i.e. work on a subset of the per-pixel prediction
    // in order to guarantee real-time execution
    unsigned int *nonNullPixelsBuff =
      new unsigned int[prediction.getWidth()*prediction.getHeight()];
    unsigned int nonNullPixels=0, nSamples=0;
    for (unsigned int i=0; i<prediction.getWidth()*prediction.getHeight(); i++)
    {
      if (mask.getData()[i]) nonNullPixelsBuff[nonNullPixels++]=i;
    }
    nSamples = std::min(MS_SAMPLED_PIXELS, nonNullPixels);

    if (!nSamples)
    {
      // TODO: how to handle the lack of samples?
      return;
    }

    unsigned int *samples = new unsigned int[nSamples];
    if (nSamples<MS_SAMPLED_PIXELS)
    {
      // Not enough samples (i.e. non-null pixels) to perform sampling.
      // Simply copy the whole set of samples
      std::copy(nonNullPixelsBuff, nonNullPixelsBuff+nSamples, samples);
    }
    else
    {
      boost::random::mt19937 gen(time(NULL));
      boost::random::uniform_int_distribution<> U(0, nonNullPixels-1);
      for (int i=0; i<nSamples; i++) samples[i] = nonNullPixelsBuff[U(gen)];
    }

    // Build the list of votes (i.e. Euler angles) and perform meanshift
    std::vector<Eigen::Vector3f> votes;
    std::vector<float> votesW;
    for (int i=0; i<nSamples; i++)
    {
      int nodeID = prediction.getData()[samples[i]];
      unsigned int nVotes = rtree.getNVotes()[nodeID];

      for (int j=0; j<nVotes; j++)
      {
	Eigen::Vector3f vote;
	float *votesPtr =
	  &rtree.getVotes()[nodeID*RF_VAL_SIZE*RF_N_HYPH + j*RF_VAL_SIZE];
	float *weightPtr =
	  &rtree.getVoteWeights()[nodeID*RF_N_HYPH+j];

	vote.z() = votesPtr[0]*M_PI/180.; //static_cast<double>(votesPtr[0])*M_PI/180.;
        vote.y() = votesPtr[1]*M_PI/180.; //static_cast<double>(votesPtr[1])*M_PI/180.;
	vote.x() = votesPtr[2]*M_PI/180.; //static_cast<double>(votesPtr[2])*M_PI/180.;
	votes.push_back(vote);
	votesW.push_back(*weightPtr);
	//votesW.push_back(static_cast<double>(*weightPtr));
      }
    }

    std::vector<Eigen::Vector3f> eulModes(MS_N_HYPH);
    std::vector<int> voteModeID(votes.size());
    nModes = MultiGuessRChordMeanShift(votes, votesW, votes, eulModes, voteModeID,
				       MS_SIGMA, MS_STEPSIZE, MS_STEPTHR,
				       MS_NITERTHR, MS_SAMECTHR);

    // Store modes found as rotation matrices
    for (int i=0; i<nModes; i++)
    {
      modes.at(i) = Eigen::AngleAxisf(eulModes.at(i).x(), Eigen::Vector3f::UnitZ()) *
	            Eigen::AngleAxisf(eulModes.at(i).y(), Eigen::Vector3f::UnitY()) *
	            Eigen::AngleAxisf(eulModes.at(i).z(), Eigen::Vector3f::UnitX());
    }

    // Update the mode weights as the sum of (weights of the) votes that converge to
    // each specific mode
    std::fill(modesW.begin(), modesW.end(), 0.);
    std::fill(modesCount.begin(), modesCount.end(), 0);
    std::fill(modesVar.begin(), modesVar.end(), 0.);
    for (int i=0; i<votes.size(); i++)
    {
      if (voteModeID.at(i)!=-1)
      {
	modesW.at(voteModeID.at(i)) += votesW.at(i);
	modesCount.at(voteModeID.at(i))++;

	Eigen::Matrix3f voteMat(Eigen::AngleAxisf(votes.at(i).x()*M_PI/180,
						  Eigen::Vector3f::UnitZ()) *
				Eigen::AngleAxisf(votes.at(i).y()*M_PI/180,
						  Eigen::Vector3f::UnitY()) *
				Eigen::AngleAxisf(votes.at(i).z()*M_PI/180,
						  Eigen::Vector3f::UnitX()));
	Eigen::Quaternion<float> modeQ(modes.at(voteModeID.at(i)));
	Eigen::Quaternion<float> voteQ(voteMat);

	Eigen::Quaternion<double> tmp(voteQ.inverse() * modeQ);
	float distQ = 2.*acos(fabs(tmp.w()));
	modesVar.at(voteModeID.at(i)) += distQ*distQ;
      }
    }

    for (int i=0; i<nModes; i++) modesVar.at(i)/=modesCount.at(i);

    // Finally, save the previous state and time delta (they will be used to disregard unplausible rotations)
    prevState = _prevState;
    deltaT = _deltaT;

    // Done
    delete []samples;
    delete []nonNullPixelsBuff;
  }
};

#endif // __PARTICLE_FILTER_HAND_LIKELIHOOD_HPP


// Copyright 2010 The Trustees of Indiana University.

// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Jeremiah Willcock
//           Andrew Lumsdaine

#ifndef BOOST_GRAPH_RANDOM_SPANNING_TREE_HPP
#define BOOST_GRAPH_RANDOM_SPANNING_TREE_HPP

#include <vector>
#include <boost/assert.hpp>
#include <boost/graph/loop_erased_random_walk.hpp>
#include <boost/graph/random.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/named_function_params.hpp>

namespace boost {

  namespace detail {
    // Use Wilson's algorithm (based on loop-free random walks) to generate a
    // random spanning tree.  The distribution of edges used is controlled by
    // the next_edge() function, so this version allows either weighted or
    // unweighted selection of trees.
    // Algorithm is from http://en.wikipedia.org/wiki/Uniform_spanning_tree
    template <typename Graph, typename PredMap, typename ColorMap, typename NextEdge>
    void random_spanning_tree_internal(const Graph& g, typename graph_traits<Graph>::vertex_descriptor s, PredMap pred, ColorMap color, NextEdge next_edge) {
      typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
      typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;

      BOOST_ASSERT (num_vertices(g) >= 1); // g must also be undirected (or symmetric) and connected

      typedef color_traits<typename property_traits<ColorMap>::value_type> color_gen;
      BGL_FORALL_VERTICES_T(v, g, Graph) put(color, v, color_gen::white());

      std::vector<vertex_descriptor> path;

      put(color, s, color_gen::black());
      put(pred, s, graph_traits<Graph>::null_vertex());

      BGL_FORALL_VERTICES_T(v, g, Graph) {
        if (get(color, v) != color_gen::white()) continue;
        loop_erased_random_walk(g, v, next_edge, color, path);
        for (typename std::vector<vertex_descriptor>::const_reverse_iterator i = path.rbegin();
             boost::next(i) !=
               (typename std::vector<vertex_descriptor>::const_reverse_iterator)path.rend();
             ++i) {
          typename std::vector<vertex_descriptor>::const_reverse_iterator j = i;
          ++j;
          BOOST_ASSERT (get(color, *j) == color_gen::gray());
          put(color, *j, color_gen::black());
          put(pred, *j, *i);
        }
      }
    }
  }

  // Compute a uniformly-distributed spanning tree on a graph.  Use Wilson's algorithm:
  // @inproceedings{wilson96generating,
  //    author = {Wilson, David Bruce},
  //    title = {Generating random spanning trees more quickly than the cover time},
  //    booktitle = {STOC '96: Proceedings of the twenty-eighth annual ACM symposium on Theory of computing},
  //    year = {1996},
  //    isbn = {0-89791-785-5},
  //    pages = {296--303},
  //    location = {Philadelphia, Pennsylvania, United States},
  //    doi = {http://doi.acm.org/10.1145/237814.237880},
  //    publisher = {ACM},
  //    address = {New York, NY, USA},
  //  }
  //
  template <typename Graph, typename Gen, typename PredMap, typename ColorMap>
  void random_spanning_tree(const Graph& g, Gen& gen, typename graph_traits<Graph>::vertex_descriptor root,
                            PredMap pred, static_property_map<double>, ColorMap color) {
    unweighted_random_out_edge_gen<Graph, Gen> random_oe(gen);
    detail::random_spanning_tree_internal(g, root, pred, color, random_oe);
  }

  // Compute a weight-distributed spanning tree on a graph.
  template <typename Graph, typename Gen, typename PredMap, typename WeightMap, typename ColorMap>
  void random_spanning_tree(const Graph& g, Gen& gen, typename graph_traits<Graph>::vertex_descriptor root,
                            PredMap pred, WeightMap weight, ColorMap color) {
    weighted_random_out_edge_gen<Graph, WeightMap, Gen> random_oe(weight, gen);
    detail::random_spanning_tree_internal(g, root, pred, color, random_oe);
  }

  template <typename Graph, typename Gen, typename P, typename T, typename R>
  void random_spanning_tree(const Graph& g, Gen& gen, const bgl_named_params<P, T, R>& params) {
    using namespace boost::graph::keywords;
    typedef bgl_named_params<P, T, R> params_type;
    BOOST_GRAPH_DECLARE_CONVERTED_PARAMETERS(params_type, params)
    random_spanning_tree(g,
                         gen,
                         arg_pack[_root_vertex | *vertices(g).first],
                         arg_pack[_predecessor_map],
                         arg_pack[_weight_map | static_property_map<double>(1.)],
                         boost::detail::make_color_map_from_arg_pack(g, arg_pack));
  }
}

#include <boost/graph/iteration_macros_undef.hpp>

#endif // BOOST_GRAPH_RANDOM_SPANNING_TREE_HPP


///////////////////////////////////////////////////////////////////////////////
// p_square_cumulative_distribution.hpp
//
//  Copyright 2005 Daniel Egloff, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_P_SQUARE_CUMUL_DIST_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_P_SQUARE_CUMUL_DIST_HPP_DE_01_01_2006

#include <vector>
#include <functional>
#include <boost/parameter/keyword.hpp>
#include <boost/range.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/accumulators_fwd.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>

namespace boost { namespace accumulators
{
///////////////////////////////////////////////////////////////////////////////
// num_cells named parameter
//
BOOST_PARAMETER_NESTED_KEYWORD(tag, p_square_cumulative_distribution_num_cells, num_cells)

BOOST_ACCUMULATORS_IGNORE_GLOBAL(p_square_cumulative_distribution_num_cells)

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // p_square_cumulative_distribution_impl
    //  cumulative_distribution calculation (as histogram)
    /**
        @brief Histogram calculation of the cumulative distribution with the \f$P^2\f$ algorithm

        A histogram of the sample cumulative distribution is computed dynamically without storing samples
        based on the \f$ P^2 \f$ algorithm. The returned histogram has a specifiable amount (num_cells)
        equiprobable (and not equal-sized) cells.

        For further details, see

        R. Jain and I. Chlamtac, The P^2 algorithm for dynamic calculation of quantiles and
        histograms without storing observations, Communications of the ACM,
        Volume 28 (October), Number 10, 1985, p. 1076-1085.

        @param p_square_cumulative_distribution_num_cells.
    */
    template<typename Sample>
    struct p_square_cumulative_distribution_impl
      : accumulator_base
    {
        typedef typename numeric::functional::average<Sample, std::size_t>::result_type float_type;
        typedef std::vector<float_type> array_type;
        typedef std::vector<std::pair<float_type, float_type> > histogram_type;
        // for boost::result_of
        typedef iterator_range<typename histogram_type::iterator> result_type;

        template<typename Args>
        p_square_cumulative_distribution_impl(Args const &args)
          : num_cells(args[p_square_cumulative_distribution_num_cells])
          , heights(num_cells + 1)
          , actual_positions(num_cells + 1)
          , desired_positions(num_cells + 1)
          , positions_increments(num_cells + 1)
          , histogram(num_cells + 1)
          , is_dirty(true)
        {
            std::size_t b = this->num_cells;

            for (std::size_t i = 0; i < b + 1; ++i)
            {
                this->actual_positions[i] = i + 1.;
                this->desired_positions[i] = i + 1.;
                this->positions_increments[i] = numeric::average(i, b);
            }
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            this->is_dirty = true;

            std::size_t cnt = count(args);
            std::size_t sample_cell = 1; // k
            std::size_t b = this->num_cells;

            // accumulate num_cells + 1 first samples
            if (cnt <= b + 1)
            {
                this->heights[cnt - 1] = args[sample];

                // complete the initialization of heights by sorting
                if (cnt == b + 1)
                {
                    std::sort(this->heights.begin(), this->heights.end());
                }
            }
            else
            {
                // find cell k such that heights[k-1] <= args[sample] < heights[k] and adjust extreme values
                if (args[sample] < this->heights[0])
                {
                    this->heights[0] = args[sample];
                    sample_cell = 1;
                }
                else if (this->heights[b] <= args[sample])
                {
                    this->heights[b] = args[sample];
                    sample_cell = b;
                }
                else
                {
                    typename array_type::iterator it;
                    it = std::upper_bound(
                        this->heights.begin()
                      , this->heights.end()
                      , args[sample]
                    );

                    sample_cell = std::distance(this->heights.begin(), it);
                }

                // increment positions of markers above sample_cell
                for (std::size_t i = sample_cell; i < b + 1; ++i)
                {
                    ++this->actual_positions[i];
                }

                // update desired position of markers 2 to num_cells + 1
                // (desired position of first marker is always 1)
                for (std::size_t i = 1; i < b + 1; ++i)
                {
                    this->desired_positions[i] += this->positions_increments[i];
                }

                // adjust heights of markers 2 to num_cells if necessary
                for (std::size_t i = 1; i < b; ++i)
                {
                    // offset to desire position
                    float_type d = this->desired_positions[i] - this->actual_positions[i];

                    // offset to next position
                    float_type dp = this->actual_positions[i + 1] - this->actual_positions[i];

                    // offset to previous position
                    float_type dm = this->actual_positions[i - 1] - this->actual_positions[i];

                    // height ds
                    float_type hp = (this->heights[i + 1] - this->heights[i]) / dp;
                    float_type hm = (this->heights[i - 1] - this->heights[i]) / dm;

                    if ( ( d >= 1. && dp > 1. ) || ( d <= -1. && dm < -1. ) )
                    {
                        short sign_d = static_cast<short>(d / std::abs(d));

                        // try adjusting heights[i] using p-squared formula
                        float_type h = this->heights[i] + sign_d / (dp - dm) * ( (sign_d - dm) * hp + (dp - sign_d) * hm );

                        if ( this->heights[i - 1] < h && h < this->heights[i + 1] )
                        {
                            this->heights[i] = h;
                        }
                        else
                        {
                            // use linear formula
                            if (d>0)
                            {
                                this->heights[i] += hp;
                            }
                            if (d<0)
                            {
                                this->heights[i] -= hm;
                            }
                        }
                        this->actual_positions[i] += sign_d;
                    }
                }
            }
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            if (this->is_dirty)
            {
                this->is_dirty = false;

                // creates a vector of std::pair where each pair i holds
                // the values heights[i] (x-axis of histogram) and
                // actual_positions[i] / cnt (y-axis of histogram)

                std::size_t cnt = count(args);

                for (std::size_t i = 0; i < this->histogram.size(); ++i)
                {
                    this->histogram[i] = std::make_pair(this->heights[i], numeric::average(this->actual_positions[i], cnt));
                }
            }
            //return histogram;
            return make_iterator_range(this->histogram);
        }

    private:
        std::size_t num_cells;            // number of cells b
        array_type  heights;              // q_i
        array_type  actual_positions;     // n_i
        array_type  desired_positions;    // n'_i
        array_type  positions_increments; // dn'_i
        mutable histogram_type histogram; // histogram
        mutable bool is_dirty;
    };

} // namespace detail

///////////////////////////////////////////////////////////////////////////////
// tag::p_square_cumulative_distribution
//
namespace tag
{
    struct p_square_cumulative_distribution
      : depends_on<count>
      , p_square_cumulative_distribution_num_cells
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::p_square_cumulative_distribution_impl<mpl::_1> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::p_square_cumulative_distribution
//
namespace extract
{
    extractor<tag::p_square_cumulative_distribution> const p_square_cumulative_distribution = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(p_square_cumulative_distribution)
}

using extract::p_square_cumulative_distribution;

// So that p_square_cumulative_distribution can be automatically substituted with
// weighted_p_square_cumulative_distribution when the weight parameter is non-void
template<>
struct as_weighted_feature<tag::p_square_cumulative_distribution>
{
    typedef tag::weighted_p_square_cumulative_distribution type;
};

template<>
struct feature_of<tag::weighted_p_square_cumulative_distribution>
  : feature_of<tag::p_square_cumulative_distribution>
{
};

}} // namespace boost::accumulators

#endif


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

Distributed under the Boost Software License, Version 1.0. (See accompanying
file License.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/

#ifndef BOOST_ASTRONOMY_TIME_CONVERSIONS
#define BOOST_ASTRONOMY_TIME_CONVERSIONS

#include <string>
#include <exception>
#include <boost/astronomy/time/parser.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

namespace boost { namespace astronomy { namespace time {

/**
 * Universal time (UT), and therefore the local civil time in any
 * part of the world, is related to the apparent motion of the Sun
 * around the Earth.
 *
 * Sidereal Time (ST) is a time scale that is based on Earth's rate
 * of rotation measured relative to the fixed stars.
 */

/**
 * The Greenwich Sidereal Time(GST) is the sidereal time correct for observations
 * made on the Greenwich meridian, longitude 0◦.
 */

double julian_date(boost::posix_time::ptime t)
{
    //Get date from UT
    boost::gregorian::date dt = t.date();

    //Set y = year, m = month and d = day
    double y = dt.year();
    double m = dt.month();
    double d = dt.day();

    //If m = 1 or 2, set yprime = y − 1 and mprime = m + 12; otherwise yprime = y and mprime = m.
    double yprime;
    double mprime;
    if ( (m == 1) || (m == 2) )
    {
        yprime = y - 1;
        mprime = m + 12;
    }
    else
    {
        yprime = y;
        mprime = m;
    }

    //Calculate B, check if the date is later than 1582 October 15
    double B;
    boost::gregorian::date dt1(1582, boost::gregorian::Oct , 1);
    if ( dt > dt1 )
    {
        double A = floor(yprime / 100);
        B = 2 - A + floor(A / 4);
    }
    else
    {
        B = 0;
    }

    //Calculate C, check if yprime is negative
    double C;
    if (yprime < 0)
    {
        C = floor((365.25 * yprime) - 0.75);
    }
    else
    {
        C = floor(365.25 * yprime);
    }

    //Calculate D
    double D;
    D = floor(30.6001 * (mprime + 1));

    //Finding Julian date
    double JD;
    JD = B + C + D + d + 1720994.5;

    return JD;
}

decimal_hour GST(boost::posix_time::ptime t)
{
    //Get Julian Day Number
    double JD = julian_date(t);

    double S = JD - 2451545.0;

    double T = S/36525.0;

    double T0 = 6.697374558 + (2400.051336 * T) + (0.000025862 * T * T);

    //Reduce the result to the range 0 to 24 by adding or subtracting multiples of 24
    T0 = T0 - 24.0 * floor(T0/24.0);

    //Convert UT to decimal hours
    double UT = ((t.time_of_day().seconds())/60.0 + t.time_of_day().minutes())/60.0 + t.time_of_day().hours();

    //Multiply UT by 1.002737909
    double A = UT * 1.002737909;

    T0 += A;

    //Add this to T0 and reduce to the range 0 to 24 if necessary by subtracting or adding 24. This is the GST.
    T0 = T0 - 24.0 * floor(T0/24.0);

    //Return GST in decimal hours
    return {T0};
}

enum class DIRECTION {WEST, EAST};

//Local Sidereal Time (LST)
decimal_hour LST(double longitude, DIRECTION direction, boost::posix_time::ptime t)
{
    double gst = GST(t).get();

    if(longitude == 0)
      return {gst};

    //Convert longitude to hours
    double long_hours = longitude / 15.0;

    switch(direction)
    {
      case DIRECTION::WEST:
          //Multiply with direction
          long_hours = -1 * long_hours;
            break;
      case DIRECTION::EAST:
          //Multiply with direction
          long_hours = 1 * long_hours;
            break;
    }

    long_hours = long_hours + gst;

    //Bring the result into the range 0 to 24 by adding or subtracting 24 if necessary.
    //This is the local sidereal time (LST).
    long_hours = long_hours - 24.0 * floor(long_hours/24.0);

    return {long_hours};
}

}}} // namespace::astronomy::time
#endif //BOOST_ASTRONOMY_TIME_CONVERSIONS


#include "element.h"
#include <armadillo>

//#define NDEBUG 1
#include <cassert>

using namespace arma;

// TODO let's store material properties in a struct or an instance and only keep a reference to it
// TODO most matrices we can make a fixed, static size. implement this for better performance
// TODO consider a more flyweight approach... how much can we slim down cost of creating an element?
// TODO convert c style arrays to c++ arrays
// TODO separate material data from mesh data
// TODO convert for loops to ranged loops
// TODO delegate integration to a separate "Integration" class

/**
 * Default constructor for a mechanical finite element
 *
 * @return MechElem
 */
MechElem::MechElem() {}

/**
 * Constructor for a mechanical finite element
 *
 * @param [int] E Modulus of elasticity
 * @param [double] v Poissons ratio
 * @param [vec*] b Body forces: x-direction, y-direction
 * @param [vec*] t Traction forces: bottom, right, top, left
 * @param [int*] gnodes Nodal numbers: bot-left, bot-right, top-right, top-left
 * @param [mat*] gcoords Global coordinates corresponding to each node
 * @param [int*] gdof Global degrees of freedom
 * @return MechElem
 */
MechElem::MechElem(long long int E, double v, vec *pb, vec *pt, int *gnodes,
        mat *pgcoords, int *gdofs) : E(E), v(v), pb(pb), pt(pt),
        gnodes(gnodes), pgcoords(pgcoords), gdofs(gdofs) {};

/**
 * Clone of a mechanical finite element
 *
 * @param [int]
 * @return MechElem
 */
MechElem::MechElem(const MechElem& orig)
{
}

/**
 * Deallocation of a mechanical finite element
 *
 * @param void
 * @return void
 */
MechElem::~MechElem(void)
{
}

/**
 * Accessor for global nodal coordinates
 *
 * @return mat *gcoords
 */
mat *MechElem::getGcoords() { return pgcoords; }

/**
 * Accessor for global node numbers
 *
 * @return int *gnodes
 */
int *MechElem::getGnodes() { return gnodes; }

/**
 * Accessor for global degrees of freedom
 *
 * @return int *gdofs
 */
int *MechElem::getGdofs() { return gdofs; }

/*
 * Q4 class constants
 */
double const Q4::gaussPoints[] = { -0.5774, 0.5774 };
int const Q4::weights[] = { 1, 1 };

/**
 * Default constructor for a Q4 finite element
 *
 * @return Q4
 */
Q4::Q4() {}

// TODO: consider separating material data from element object

/**
 * Constructor for a Q4 finite element
 *
 * @param [int] E Modulus of elasticity
 * @param [double] v Poissons ratio
 * @param [double] h Thickness or 'height' of the element
 * @param [vec] b Body forces: x-direction, y-direction
 * @param [vec] t Traction forces: bottom, right, top, left
 * @param [int*] gnodes Nodal numbers: bot-left, bot-right, top-right, top-left
 * @param [mat*] gcoords Global coordinates corresponding to each node
 * @param [int*] gdof Global degrees of freedom
 * @return Q4
 */
Q4::Q4(long long int E, double v, double h, vec *pb, vec *pt, int *gnodes,
    mat *pgcoords, int *gdofs) : MechElem(E, v, pb, pt, gnodes, pgcoords,
    gdofs), h(h) {};

/**
 * Deallocation of a Q4 finite element
 *
 * @param void
 * @return void
 */
Q4::~Q4(void)
{
}

// consider inline functions?
/*
 * Define the Q4 shape functions
 */
#define Q4__N1(xi, eta) (1-xi)*(1-eta)/4
#define Q4__N2(xi, eta) (1+xi)*(1-eta)/4
#define Q4__N3(xi, eta) (1+xi)*(1+eta)/4
#define Q4__N4(xi, eta) (1-xi)*(1+eta)/4

/**
 * Calculates the N matrix
 *
 * @param [double] xi Xi coordinate in parent coordinates
 * @param [double] eta Eta coordinate in parent coordinates
 * @return [mat] N matrix
 */
mat Q4::N(double xi, double eta)
{
    mat::fixed<Q4__DOF_PER_NODE,Q4__DOF_PER_NODE*Q4__NUM_NODES> N;
    N << Q4__N1(xi, eta) << 0 << Q4__N2(xi, eta) << 0 << Q4__N3(xi, eta) << 0
            << Q4__N4(xi, eta) << 0 << endr
      << 0 << Q4__N1(xi, eta) << 0 << Q4__N2(xi, eta) << 0 << Q4__N3(xi, eta)
            << 0 << Q4__N4(xi, eta) << endr;
    return N;
}

/*
 * Define the partials of the Q4 shape functions with respect to xi and eta
 */
#define Q4__dN1_dxi(eta) -(1-eta)/4
#define Q4__dN2_dxi(eta) (1-eta)/4
#define Q4__dN3_dxi(eta) (1+eta)/4
#define Q4__dN4_dxi(eta) -(1+eta)/4
#define Q4__dN1_deta(xi) -(1-xi)/4
#define Q4__dN2_deta(xi) -(1+xi)/4
#define Q4__dN3_deta(xi) (1+xi)/4
#define Q4__dN4_deta(xi) (1-xi)/4

/**
 * Calculates the jacobian
 *
 * @param [double] xi Xi coordinate in parent coordinates
 * @param [double] eta Eta coordinate in parent coordinates
 * @return [mat] Jacobian
 */
mat Q4::J(double xi, double eta)
{
    mat::fixed<Q4__DOF_PER_NODE,Q4__NUM_NODES> dj;
    dj << Q4__dN1_dxi(eta) << Q4__dN2_dxi(eta) << Q4__dN3_dxi(eta)
            << Q4__dN4_dxi(eta) << endr
       << Q4__dN1_deta(xi) << Q4__dN2_deta(xi) << Q4__dN3_deta(xi)
            << Q4__dN4_deta(xi) << endr;
    mat jacobian = dj * *pgcoords;
    return jacobian;
}

// TODO: have Q4 inherit from 'shell element' in order to maximize code reuse

/**
 * Calculate the element body force, mutator
 *
 * @param [vec&] bodyForce Body force vector
 * @return void
 */
void Q4::mbodyForce(vec &bodyForce)
{
    assert(bodyForce.n_rows == Q4__DOF_PER_NODE*Q4__NUM_NODES);
    // TODO make zeros at or not at the beg of function consistent across functions
    bodyForce.zeros();
    unsigned int i, j;
    double xi, eta, weightX, weightY;

    /*
     * calculate element body force using gauss quadrature
     */
    for (i = 0; i < Q4::numPointsX; i++)
    {
        xi = Q4::gaussPoints[i];
        weightX = Q4::weights[i];
        for (j = 0; j < Q4::numPointsY; j++)
        {
            eta = Q4::gaussPoints[j];
            weightY = Q4::weights[j];
            bodyForce += weightX * weightY * trans(N(xi, eta)) * *pb
                    * det(J(xi, eta));
        }
    }
    bodyForce *= h;
}

//TODO: fix this function
/**
 * Calculates the traction for a surface, mutator
 *
 * @param [vec&] traction Traction force vector
 * @return void
 */
void Q4::mtraction(vec &traction)
{
    traction.zeros();
    unsigned int i, j;
    double xi, eta, weightX, weightY;

    /*
     * calculate element body force using gauss quadrature
     */
    for (i = 0; i < Q4::numPointsX; i++)
    {
        xi = Q4::gaussPoints[i];
        weightX = Q4::weights[i];
        for (j = 0; j < Q4::numPointsY; j++)
        {
            eta = Q4::gaussPoints[j];
            weightY = Q4::gaussPoints[j];
            traction += weightX * weightY * trans(N(xi, eta)) * *pt
                    * det(J(xi, eta));
        }
    }
    traction *= h;
}

#define Q4__STRAIN_COMP 3

/**
 * Calculates the stiffness matrix for the element, mutator
 *
 * @param [mat*] stiff Element stiffness matrix
 * @param [PlaneState] pState Plane state control
 * @return void
 */
void Q4::mstiffness(mat &stiff, int pState = PSTRESS)
{
    assert(stiff.n_rows == Q4__DOF_PER_NODE*Q4__NUM_NODES);
    assert(stiff.n_cols == Q4__DOF_PER_NODE*Q4__NUM_NODES);

    stiff.zeros();
    Mat<int>::fixed<3,4> e;
    e << 1 << 0 << 0 << 0 << endr
      << 0 << 0 << 0 << 1 << endr
      << 0 << 1 << 1 << 0 << endr;
    unsigned int i, j, k, m;
    double xi, eta, weightX, weightY, temp;
    mat nStar = zeros<mat>(Q4__DOF_PER_NODE*Q4__DOF_PER_NODE,Q4__DOF_PER_NODE*
                Q4__NUM_NODES);
    mat jacobian(Q4__DOF_PER_NODE*Q4__DOF_PER_NODE,
            Q4__DOF_PER_NODE*Q4__DOF_PER_NODE);
    mat invJac(Q4__DOF_PER_NODE*Q4__DOF_PER_NODE,
            Q4__DOF_PER_NODE*Q4__DOF_PER_NODE);
    mat JE = zeros<mat>(Q4__DOF_PER_NODE*Q4__DOF_PER_NODE,
        Q4__DOF_PER_NODE*Q4__DOF_PER_NODE);
    mat B(Q4__STRAIN_COMP,Q4__DOF_PER_NODE*Q4__NUM_NODES);
    mat C(Q4__STRAIN_COMP,Q4__STRAIN_COMP);

    /* TODO: separate this logic and material properties from element so that
     *          the C matrix will not need calculated a billion times */
    // PROB AN EXPENSIVE OPERATION, DO SOMETHING ABOUT THIS
    switch (pState)
    {
        case PSTRESS:
            C << 1 << v << 0 << endr
              << v << 1 << 0 << endr
              << 0 << 0 << (1-v)/2 << endr;
            C *= E/(1-v*v);
            break;
        case PSTRAIN:
            C << 1-v << v << 0 << endr
              << v << 1-v << 0 << endr
              << 0 << 0 << (1-2*v)/2 << endr;
            C *= E/((1-v)*(1-2*v));
            break;
        default:
            // TODO: IDK, FIX THIS I GUESS
            cout << "Cannot understand plane state" << endl;
            assert(0);
    }

    /*
     * calculate element stiffness matrix using gauss quadrature
     */
    for (i = 0; i < Q4::numPointsX; i++)
    {
        xi = Q4::gaussPoints[i];
        weightX = Q4::weights[i];
        /*
         * create "nStar" matrix
         */
        for (j = 1; j < Q4__DOF_PER_NODE*Q4__DOF_PER_NODE; j+=2)
        {
            nStar(j,j/2) = Q4__dN1_deta(xi);
            nStar(j,j/2+2) = Q4__dN2_deta(xi);
            nStar(j,j/2+4) = Q4__dN3_deta(xi);
            nStar(j,j/2+6) = Q4__dN4_deta(xi);
        }
        /*
         * look up 'eta'
         * look up weight in the y-direction
         * calculate jacobian
         * build JE matrix
         * finish building nStar
         */
        for (j = 0; j < Q4::numPointsY; j++)
        {
            eta = Q4::gaussPoints[j];
            weightY = Q4::weights[j];
            jacobian = J(xi, eta);
            invJac = inv(jacobian);
            for (k = 0; k < Q4__DOF_PER_NODE; k++)
            {
                for (m = 0; m < Q4__DOF_PER_NODE; m++)
                {
                    temp = invJac(k,m);
                    JE(k,m) = temp;
                    JE(k+Q4__DOF_PER_NODE,m+Q4__DOF_PER_NODE) = temp;
                }
            }

            for (k = 0; k < Q4__DOF_PER_NODE*Q4__DOF_PER_NODE; k+=2)
            {
                nStar(k,k/2) = Q4__dN1_dxi(eta);
                nStar(k,k/2+2) = Q4__dN2_dxi(eta);
                nStar(k,k/2+4) = Q4__dN3_dxi(eta);
                nStar(k,k/2+6) = Q4__dN4_dxi(eta);
            }
            // this is derived in class notes (Aquino)
            B = e * JE * nStar;
            stiff += weightX * weightY * trans(B) * C * B * det(jacobian);
        }
    }
    stiff *= h;
}

//TODO implement Q4R element


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#ifndef ROKKO_UTILITY_XYZ_HAMILTONIAN_MPI_HPP
#define ROKKO_UTILITY_XYZ_HAMILTONIAN_MPI_HPP

#include "mpi.h"
#include <vector>
#include <boost/tuple/tuple.hpp>

#include <iostream>

#include <rokko/distributed_matrix.hpp>
#include <rokko/localized_matrix.hpp>

namespace rokko {

namespace xyz_hamiltonian {

void multiply(const MPI_Comm& comm, int L, const std::vector<std::pair<int, int> >& lattice, const std::vector<boost::tuple<double, double, double> >& coupling, const double* v, double* w, double* buffer) {
  int myrank, nproc;
  MPI_Status status;
  int ierr;

  MPI_Comm_size(comm, &nproc);
  MPI_Comm_rank(comm, &myrank);

  int n = nproc;
  int p = -1;
  do {
    n /= 2;
    ++p;
  } while (n > 0);

  if (nproc != (1 << p)) {
    if ( myrank == 0 ) {
      std::cout << "This program can be run only for powers of 2" << std::endl;
    }
    MPI_Abort(comm, 1);
  }
  int N = 1 << (L-p);

  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();

    double diag_plus = jz / 4.0;
    double diag_minus = - jz / 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    if (i < (L-p)) {
      if (j < (L-p)) {
        int m1 = 1 << i;
        int m2 = 1 << j;
        int m3 = m1 + m2;
        for (int k=0; k<N; ++k) {
          if (((k & m3) == m1) || ((k & m3) == m2)) {  // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
            w[k] += diag_minus * v[k] + offdiag_plus * v[k^m3];
          } else {
            w[k] += diag_plus * v[k] + offdiag_minus * v[k^m3];
          }
        }
      } else {
        int m = 1 << (j-(L-p));
        MPI_Sendrecv(const_cast<double*>(&v[0]), N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     &buffer[0], N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     comm, &status);
        int m1 = 1 << i;
        if ((myrank & m) == m) {
          for (int k=0; k<N; ++k) {
            if ((k & m1) == m1) {
              w[k] += diag_plus * v[k] + offdiag_minus * buffer[k^m1];
            } else {
              w[k] += diag_minus * v[k] + offdiag_plus * buffer[k^m1];
            }
          }
        } else {
          for (int k=0; k<N; ++k) {
            if ((k & m1) == m1) {
              w[k] += diag_minus * v[k] + offdiag_plus * buffer[k^m1];
            } else {
              w[k] += diag_plus * v[k] + offdiag_minus * buffer[k^m1];
            }
          }
        }
      }
    } else {
      if (j < (L-p)) {
        int m = 1 << (i-(L-p));
        MPI_Sendrecv(const_cast<double*>(&v[0]), N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     &buffer[0], N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     comm, &status);
        int m1 = 1 << j;
        if ((myrank & m) == m) {
          for (int k=0; k<N; ++k) {
            if ((k & m1) == m1) {
              w[k] += diag_plus * v[k] + offdiag_minus * buffer[k^m1];
            } else {
              w[k] += diag_minus * v[k] + offdiag_plus * buffer[k^m1];
            }
          }
        } else {
          for (int k=0; k<N; ++k) {
            if ((k & m1) == m1) {
              w[k] += diag_minus * v[k] + offdiag_plus * buffer[k^m1];
            } else {
              w[k] += diag_plus * v[k] + offdiag_minus * buffer[k^m1];
            }
          }
        }
      } else {
        int m = (1 << (i-(L-p))) + (1 << (j-(L-p)));
        MPI_Sendrecv(const_cast<double*>(&v[0]), N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     &buffer[0], N, MPI_DOUBLE,
                     myrank ^ m, 0,
                     comm, &status);
        if (((myrank & m) != m) && ((myrank & m) != 0)) {
          for (int k=0; k<N; ++k) {
            w[k] += diag_minus * v[k] + offdiag_plus * buffer[k];
          }
        } else {
          for (int k=0; k<N; ++k) {
            w[k] += diag_plus * v[k] + offdiag_minus * buffer[k];
          }
        }
      }
    }
  }
}

void multiply(const MPI_Comm& comm, int L, const std::vector<std::pair<int, int> >& lattice, const std::vector<boost::tuple<double, double, double> >& coupling, const std::vector<double>& v, std::vector<double>& w, std::vector<double>& buffer) {
  multiply(comm, L, lattice, coupling, &v[0], &w[0], &buffer[0]);
}

void fill_diagonal(const MPI_Comm& comm, int L, const std::vector<std::pair<int, int> >& lattice, const std::vector<boost::tuple<double, double, double> >& coupling, double* w) {
  int myrank, nproc;

  MPI_Comm_size(comm, &nproc);
  MPI_Comm_rank(comm, &myrank);

  int n = nproc;
  int p = -1;
  do {
    n /= 2;
    ++p;
  } while (n > 0);

  if (nproc != (1 << p)) {
    if ( myrank == 0 ) {
      std::cout << "This program can be run only for powers of 2" << std::endl;
    }
    MPI_Abort(comm, 1);
  }

  int N_seq = 1 << L;
  int N = 1 << (L-p);
  int myrank_shift = myrank * N;
  int nproc_shift = (nproc-1) * N;
  int mask = N - 1;

  for (int k=0; k<N; ++k) {
    w[k] = 0;
  }

  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();
    double diag_plus = jz / 4.0;
    double diag_minus = - jz / 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;

    for (int k=0; k<N_seq; ++k) {
      if (myrank_shift == (k & nproc_shift)) {
        if (((k & m3) == m1) || ((k & m3) == m2)) {  // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
          w[k & mask] += diag_minus;
        } else {
          w[k & mask] += diag_plus;
        }
      }
    }  // end for k
  } // end for lattice
}

void fill_diagonal(const MPI_Comm& comm, int L, const std::vector<std::pair<int, int> >& lattice, const std::vector<boost::tuple<double, double, double> >& coupling, std::vector<double>& w) {
  fill_diagonal(comm, L, lattice, coupling, &w[0]);
}

template<typename T, typename MATRIX_MAJOR>
void generate(int L, const std::vector<std::pair<int, int> >& lattice,
  const std::vector<boost::tuple<double, double, double> >& coupling,
  rokko::distributed_matrix<T, MATRIX_MAJOR>& mat) {
  mat.set_zeros();
  int N = 1 << L;
  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();
    double diag_plus = jz / 4.0;
    double diag_minus = - jz/ 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;

    for (int k=0; k<N; ++k) {
      if (mat.is_gindex_mycol(k)) {
        int local_k = mat.translate_g2l_col(k);
        if (((k & m3) == m1) || ((k & m3) == m2)) {  // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
          if (mat.is_gindex_myrow(k^m3)) {
            mat.update_local(mat.translate_g2l_row(k^m3), local_k, offdiag_plus);
          }
          if (mat.is_gindex_myrow(k)) {
            mat.update_local(mat.translate_g2l_row(k), local_k, diag_minus);
          }
        } else {
          if (mat.is_gindex_myrow(k^m3)) {
            mat.update_local(mat.translate_g2l_row(k^m3), local_k, offdiag_minus);
          }
          if (mat.is_gindex_myrow(k)) {
            mat.update_local(mat.translate_g2l_row(k), local_k, diag_plus);
          }
        }
      }
    }
  }
}

// The following routine uses local indices.  It works correctly.
/*
template <typename MATRIX_MAJOR>
void generate(int L, const std::vector<std::pair<int, int> >& lattice, const std::vector<boost::tuple<double, double, double> >& coupling, rokko::distributed_matrix<MATRIX_MAJOR>& mat) {
  mat.set_zeros();
  int N = 1 << L;
  for (int l=0; l<lattice.size(); ++l) {
    int i = lattice[l].first;
    int j = lattice[l].second;
    double jx = coupling[l].get<0>();
    double jy = coupling[l].get<1>();
    double jz = coupling[l].get<2>();
    double diag_plus = jz / 4.0;
    double diag_minus = - jz/ 4.0;
    double offdiag_plus = (jx + jy) / 4.0;
    double offdiag_minus = (jx - jy) / 4.0;

    int m1 = 1 << i;
    int m2 = 1 << j;
    int m3 = m1 + m2;
    for(int local_i = 0; local_i < mat.get_m_local(); ++local_i) {
      int k1 = mat.translate_l2g_row(local_i);
      for(int local_j = 0; local_j < mat.get_n_local(); ++local_j) {
        int k2 = mat.translate_l2g_col(local_j);
        if (((k2 & m3) == m1) || ((k2 & m3) == m2)) {  // when (bit i == 1, bit j == 0) or (bit i == 0, bit j == 1)
          if (k1 == (k2^m3)) {
            mat.update_local(local_i, local_j, offdiag_plus);
          }
          if (k1 == k2) {
            mat.update_local(local_i, local_j, diag_minus);
          }
        } else {
          if (k1 == (k2^m3)) {
            mat.update_local(local_i, local_j, offdiag_minus);
          }
          if (k1 == k2) {
            //std::cout << "k1=" << k1 << " k2=" << k2 << std::endl;
            mat.update_local(local_i, local_j, diag_plus);
          }
        }
      }
    }
  }
}
*/

} // namespace xyz_hamiltonian

} // namespace rokko

#endif // ROKKO_UTILITY_XYZ_HAMILTONIAN_MPI_HPP


#include <cmath>
#include <algorithm>
#include <numeric>
#include <boost/unordered_map.hpp>
#include <emmintrin.h>
#include <immintrin.h>
#include "rchord_meanshift.hpp"

#include <iostream>

static void _meanShiftIter(const std::vector<Eigen::Matrix3f> &votes,
			   const std::vector<float> &weights,
			   const Eigen::Matrix3f &guess, Eigen::Matrix3f &mode,  float b);
static void _RChordMeanShift(const std::vector<Eigen::Matrix3f> &votes,
			     const std::vector<float> &votesWeight,
			     const Eigen::Matrix3f &guess, Eigen::Matrix3f &mode,
			     float b, float stepSize, float stepThr, unsigned int nIterThr);
static inline void _cvtVEuler2Rotm(const std::vector<Eigen::Vector3f> &votesEul,
				   std::vector<Eigen::Matrix3f> &votesRotm);
static inline void _cvtVRotm2Euler(const std::vector<Eigen::Matrix3f> &votesRotm,
				   std::vector<Eigen::Vector3f> &votesEul);
static inline void _cvtEuler2Rotm(const Eigen::Vector3f &voteEul, Eigen::Matrix3f &voteRotm);
static inline void _cvtRotm2Euler(const Eigen::Matrix3f &voteRotm, Eigen::Vector3f &voteEul);
static inline float _rotmDist2(const Eigen::Matrix3f &m1, const Eigen::Matrix3f &m2);

void RChordMeanShift(const std::vector<Eigen::Vector3f> &votes,
		     const std::vector<float> &votesWeight,
		     const Eigen::Vector3f &guess, Eigen::Vector3f &mode,
		     float b, float stepSize, float stepThr, unsigned int nIterThr)
{
  std::vector<Eigen::Matrix3f> votesRotm(votes.size());
  Eigen::Matrix3f guessRotm, modeRotm;

  _cvtVEuler2Rotm(votes, votesRotm);
  _cvtEuler2Rotm(guess, guessRotm);

  _RChordMeanShift(votesRotm, votesWeight, guessRotm, modeRotm,
		   b, stepSize, stepThr, nIterThr);

  _cvtRotm2Euler(modeRotm, mode);
}

static void _RChordMeanShift(const std::vector<Eigen::Matrix3f> &votes,
			     const std::vector<float> &votesWeight,
			     const Eigen::Matrix3f &guess, Eigen::Matrix3f &mode,
			     float b, float stepSize, float stepThr, unsigned int nIterThr)
{
  Eigen::Matrix3f currGuess = guess;

  for (int i=0; i<nIterThr; i++)
  {
    Eigen::Matrix3f m;
    float dist;

    _meanShiftIter(votes, votesWeight, currGuess, m, b);
    dist = _rotmDist2(currGuess, m);

    // Move to the meanshift mode by stepSize. Use quaternion interpolation to
    // rotate towards the target rotation.
    Eigen::Quaternion<float> q1(currGuess);
    Eigen::Quaternion<float> q2(m);

    currGuess = q1.slerp(stepSize, q2);
    if (dist<stepThr*stepThr) break;
  }
  mode = currGuess;
}

struct CmpIdxByWeight
{
  const std::vector<float> *w;
  CmpIdxByWeight(const std::vector<float> &w):w(&w){};
  bool operator() (unsigned int i1, unsigned int i2) const {return w->at(i1)>w->at(i2);}
};

struct CmpIdxByNGuess
{
  const std::vector<int> *nGuess;
  CmpIdxByNGuess(const std::vector<int> &nGuess):nGuess(&nGuess){};
  bool operator() (unsigned int i1, unsigned int i2) const {return nGuess->at(i1)>nGuess->at(i2);}
};

unsigned int  MultiGuessRChordMeanShift(const std::vector<Eigen::Vector3f> &votes,
					const std::vector<float> &votesWeight,
					const std::vector<Eigen::Vector3f> guesses,
					std::vector<Eigen::Vector3f> &modes,
					std::vector<int> &guessModeID,
					float b, float stepSize, float stepThr,
					unsigned int nIterThr, float sameModeThr)
{
  std::vector<Eigen::Matrix3f> vRotm(votes.size());
  std::vector<Eigen::Matrix3f> gRotm(guesses.size());
  std::vector<Eigen::Matrix3f> mRotm(guesses.size());
  std::vector<Eigen::Matrix3f> cRotm;
  std::vector<int> cNGuess;
  unsigned int nCModes=1;

  _cvtVEuler2Rotm(votes, vRotm);
  _cvtVEuler2Rotm(guesses, gRotm);

  #pragma omp parallel for
  for (int i=0; i<gRotm.size(); i++)
  {
    _RChordMeanShift(vRotm, votesWeight,
		     gRotm.at(i), mRotm.at(i),
		     b, stepSize, stepThr, nIterThr);
  }

  // Greedy clustering of modes (count how many guesses reach each mode as weel)
  cRotm.push_back(mRotm.at(0));
  cNGuess.push_back(1);
  guessModeID.at(0) = 0;
  for (int i=1; i<mRotm.size(); i++)
  {
    bool newMode = true;
    for (int j=0; j<nCModes; j++)
    {
      if (_rotmDist2(mRotm.at(i), cRotm.at(j))<sameModeThr*sameModeThr)
      {
	cNGuess.at(j)++;
	guessModeID.at(i) = j;
	newMode = false;
	break;
      }
    }
    if (newMode)
    {
      cRotm.push_back(mRotm.at(i));
      cNGuess.push_back(1);
      guessModeID.at(i) = nCModes;
      nCModes++;
    }
  }

  // Sort the modes by the number of guesses that converged to it
  std::vector<unsigned int> idx;
  for (unsigned int i=0; i<nCModes; i++) idx.push_back(i);
  if (idx.size()>1) std::sort(idx.begin(), idx.end(), CmpIdxByNGuess(cNGuess));

  int outNModes = std::min(nCModes, static_cast<unsigned int>(modes.size()));
  for (int i=0; i<outNModes; i++)
  {
    _cvtRotm2Euler(cRotm.at(idx[i]), modes.at(i));
  }

  // Update the mode id for votes that reached the current node
  // Note: Set the mode id to -1 if the guess reached a discarded mode
  boost::unordered_map<int, int> sortedModeIDMap;
  for (int i=0; i<nCModes; i++) sortedModeIDMap[idx[i]] = (i<outNModes) ? i : -1;
  for (int i=0; i<votes.size(); i++) guessModeID.at(i) = sortedModeIDMap.at(guessModeID.at(i));

  return outNModes;
}

void _meanShiftIter(const std::vector<Eigen::Matrix3f> &votes,
		    const std::vector<float> &weights,
		    const Eigen::Matrix3f &guess, Eigen::Matrix3f &mode, float b)
{
  Eigen::Matrix3f sumR = Eigen::Matrix3f::Zero();

  // Weighted sum of rotations
  for (int i=0; i<votes.size(); i++)
  {
    // Compute the current vote weight (i.e. provided weight multiplied by Guassian kernel
    // on chordal quaternion distance (i.e. Euclidean norm of R3 representation))
    float w = weights.at(i) * exp(-_rotmDist2(guess, votes.at(i))/(b*b));
    /*
    Eigen::Quaternion<float> q1(guess);
    Eigen::Quaternion<float> q2(votes.at(i));
    float dist = q1.angularDistance(q2);
    float w = weights.at(i) * exp(-(dist*dist)/(b*b));
    */
    sumR += w*votes.at(i);
  }

  // Compute the average rotation as in
  // Hartley, Richard, et al. "Rotation averaging." International journal of computer vision 103.3 (2013): 267-305.
  Eigen::JacobiSVD<Eigen::Matrix3f> SVDR(sumR, Eigen::ComputeFullU|Eigen::ComputeFullV);
  mode = SVDR.matrixU()*SVDR.matrixV().transpose();
  if (mode.determinant()<0)
  {
    mode = SVDR.matrixU()*Eigen::Vector3f(1,1,-1).asDiagonal()*SVDR.matrixV().transpose();
  }
}

void _cvtVEuler2Rotm(const std::vector<Eigen::Vector3f> &votesEul,
		    std::vector<Eigen::Matrix3f> &votesRotm)
{
  for (int i=0; i<votesEul.size(); i++)
  {
    const Eigen::Vector3f &currVoteEul = votesEul.at(i);
    Eigen::Matrix3f &currVoteRotm = votesRotm.at(i);
    _cvtEuler2Rotm(currVoteEul, currVoteRotm);
  }
}

void _cvtVRotm2Euler(const std::vector<Eigen::Matrix3f> &votesRotm,
		    std::vector<Eigen::Vector3f> &votesEul)
{
  for (int i=0; i<votesEul.size(); i++)
  {
    const Eigen::Matrix3f &currVoteRotm = votesRotm.at(i);
    Eigen::Vector3f &currVoteEul = votesEul.at(i);
    _cvtRotm2Euler(currVoteRotm, currVoteEul);
  }
}

void _cvtEuler2Rotm(const Eigen::Vector3f &voteEul, Eigen::Matrix3f &voteRotm)
{
  voteRotm = Eigen::AngleAxisf(voteEul.x(), Eigen::Vector3f::UnitZ()) *
             Eigen::AngleAxisf(voteEul.y(), Eigen::Vector3f::UnitY()) *
             Eigen::AngleAxisf(voteEul.z(), Eigen::Vector3f::UnitX());
}

void _cvtRotm2Euler(const Eigen::Matrix3f &voteRotm, Eigen::Vector3f &voteEul)
{
  voteEul = voteRotm.eulerAngles(2,1,0);
}

float _rotmDist2(const Eigen::Matrix3f &m1, const Eigen::Matrix3f &m2)
{
  Eigen::Matrix3f rotDiff = m1-m2;
  return rotDiff.cwiseProduct(rotDiff).sum();
}