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

#include <map>
#include <vector>
#include <string>
#include <queue>
#include <limits>

#include <boost/format.hpp>

#include "luxrays/core/exttrianglemesh.h"
#include "slg/shapes/simplify.h"
#include "slg/scene/scene.h"
#include "slg/utils/harlequincolors.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
//
// The following code is based on Sven Forstmann's quadric mesh simplification
// code (https://github.com/sp4cerat/Fast-Quadric-Mesh-Simplification)
// and heavily modified for LuxCoreRender
//
// Papers at https://mgarland.org/research/quadrics.html

/////////////////////////////////////////////
//
// Mesh Simplification Tutorial
//
// (C) by Sven Forstmann in 2014
//
// License : MIT
// http://opensource.org/licenses/MIT
//
// https://github.com/sp4cerat/Fast-Quadric-Mesh-Simplification
//
// 5/2016: Chris Rorden created minimal version for OSX/Linux/Windows compile

class SymetricMatrix {
public:
	// Constructor
	SymetricMatrix(const float c = 0.f) {
		for (u_int i = 0; i < 10; ++i)
			m[i] = c;
	}

	SymetricMatrix(
			const float m11, const float m12, const float m13, const float m14,
			const float m22, const float m23, const float m24,
			const float m33, const float m34,
			const float m44) {
		m[0] = m11;
		m[1] = m12;
		m[2] = m13;
		m[3] = m14;
		m[4] = m22;
		m[5] = m23;
		m[6] = m24;
		m[7] = m33;
		m[8] = m34;
		m[9] = m44;
	}

	// Make plane
	SymetricMatrix(const float a, const float b, const float c, const float d) {
		m[0] = a * a;
		m[1] = a * b;
		m[2] = a * c;
		m[3] = a * d;
		m[4] = b * b;
		m[5] = b * c;
		m[6] = b * d;
		m[7] = c * c;
		m[8] = c * d;
		m[9] = d * d;
	}

	float operator[](int c) const {
		return m[c];
	}

	// Determinant
	float det(
			const u_int a11, const u_int a12, const u_int a13,
			const u_int a21, const u_int a22, const u_int a23,
			const u_int a31, const u_int a32, const u_int a33) const {
		const float det = m[a11] * m[a22] * m[a33] + m[a13] * m[a21] * m[a32] + m[a12] * m[a23] * m[a31]
				- m[a13] * m[a22] * m[a31] - m[a11] * m[a23] * m[a32] - m[a12] * m[a21] * m[a33];
		return det;
	}

	const SymetricMatrix operator+(const SymetricMatrix &n) const {
		return SymetricMatrix(
				m[0] + n[0], m[1] + n[1], m[2] + n[2], m[3] + n[3],
				m[4] + n[4], m[5] + n[5], m[6] + n[6],
				m[7] + n[7], m[8] + n[8],
				m[9] + n[9]);
	}

	SymetricMatrix& operator+=(const SymetricMatrix& n) {
		m[0] += n[0];
		m[1] += n[1];
		m[2] += n[2];
		m[3] += n[3];
		m[4] += n[4];
		m[5] += n[5];
		m[6] += n[6];
		m[7] += n[7];
		m[8] += n[8];
		m[9] += n[9];

		return *this;
	}

	float m[10];
};

class Simplify {
public:
	Simplify(const ExtTriangleMesh &srcMesh) {
		const u_int vertCount = srcMesh.GetTotalVertexCount();
		const u_int triCount = srcMesh.GetTotalTriangleCount();
		const Point *verts = srcMesh.GetVertices();
		const Triangle *tris = srcMesh.GetTriangles();

		vertices.resize(vertCount);
		for (u_int i = 0; i < vertCount; ++i)
			vertices[i].p = verts[i];

		if (srcMesh.HasNormals()) {
			const Normal *norms = srcMesh.GetNormals();
			for (u_int i = 0; i < vertCount; ++i)
				vertices[i].norm = norms[i];

			hasNormals = true;
		} else
			hasNormals = false;

		if (srcMesh.HasUVs(0)) {
			const UV *uvs = srcMesh.GetUVs(0);
			for (u_int i = 0; i < vertCount; ++i)
				vertices[i].uv = uvs[i];

			hasUVs = true;
		} else
			hasUVs = false;

		if (srcMesh.HasColors(0)) {
			const Spectrum *cols = srcMesh.GetColors(0);
			for (u_int i = 0; i < vertCount; ++i)
				vertices[i].col = cols[i];

			hasColors = true;
		} else
			hasColors = false;

		if (srcMesh.HasAlphas(0)) {
			const float *alphas = srcMesh.GetAlphas(0);
			for (u_int i = 0; i < vertCount; ++i)
				vertices[i].alpha = alphas[i];

			hasAlphas = true;
		} else
			hasAlphas = false;

		triangles.resize(triCount);
		for (u_int i = 0; i < triCount; ++i) {
			triangles[i].v[0] = tris[i].v[0];
			triangles[i].v[1] = tris[i].v[1];
			triangles[i].v[2] = tris[i].v[2];
		}
	}

	~Simplify() {
	}

	ExtTriangleMesh *GetExtMesh() const {
		const u_int vertCount = vertices.size();
		const u_int triCount = triangles.size();

		Point *newVertices = ExtTriangleMesh::AllocVerticesBuffer(vertCount);
		for (u_int i = 0; i < vertCount; ++i)
			newVertices[i] = vertices[i].p;

		Normal *newNorms = nullptr;
		if (hasNormals) {
			newNorms = new Normal[vertCount];
			for (u_int i = 0; i < vertCount; ++i)
				newNorms[i] = vertices[i].norm;
		}

		UV *newUVs = nullptr;
		if (hasUVs) {
			newUVs = new UV[vertCount];
			for (u_int i = 0; i < vertCount; ++i)
				newUVs[i] = vertices[i].uv;
		}

		Spectrum *newCols = nullptr;
		if (hasColors) {
			newCols = new Spectrum[vertCount];
			for (u_int i = 0; i < vertCount; ++i)
				newCols[i] = vertices[i].col;
		}

		float *newAlphas = nullptr;
		if (hasAlphas) {
			newAlphas = new float[vertCount];
			for (u_int i = 0; i < vertCount; ++i)
				newAlphas[i] = vertices[i].alpha;
		}

		Triangle *newTris = ExtTriangleMesh::AllocTrianglesBuffer(triCount);
		for (u_int i = 0; i < triCount; ++i) {
			assert (triangles[i].v[0] < vertCount);
			newTris[i].v[0] = triangles[i].v[0];

			assert (triangles[i].v[1] < vertCount);
			newTris[i].v[1] = triangles[i].v[1];

			assert (triangles[i].v[2] < vertCount);
			newTris[i].v[2] = triangles[i].v[2];
		}

		return new ExtTriangleMesh(vertCount, triCount, newVertices, newTris, newNorms,
				newUVs, newCols, newAlphas);
	}

	void Decimate(const float targetTriangleCount, const Camera *scnCamera,
			const float screenSize, const bool border) {
		preserveBorder = border;
		camera = scnCamera;
		edgeScreenSize = screenSize;

		// Work on 10% of all triangles for each iteration
		maxCandidateQueueSize = Max(64u, Floor2UInt(triangles.size() * .1f));

		// Init
		for (u_int i = 0; i < triangles.size(); ++i)
			triangles[i].deleted = false;

		// Main iteration loop
		const u_int startTriangleCount = triangles.size();
		deletedTriangles = 0;
		vector<bool> deleted0, deleted1;
		for (u_int iteration = 0; iteration < 64; ++iteration) {
			if (startTriangleCount - deletedTriangles <= targetTriangleCount)
				break;

			const u_int initialdeletedTriangles = deletedTriangles;

			// Update mesh constantly
			UpdateMesh(iteration);

			// Remove vertices & mark deleted triangles
			for (u_int i = 0; i < candidateList.size(); ++i)
				CollapseEdge(candidateList[i].tid, candidateList[i].tvertex, deleted0, deleted1);

			const u_int iterationDeletedTriangles = deletedTriangles - initialdeletedTriangles;
			SDL_LOG("Simplify iteration " << iteration << " (" << candidateList.size() << " edge candidates, deleted " << iterationDeletedTriangles << "/" << deletedTriangles << " of " << startTriangleCount << " triangles)");
			if (iterationDeletedTriangles == 0)
				break;
		}

		// Clean up mesh
		CompactMesh();
	}

private:
	struct SimplifyTriangle {
		u_int v[3];
		Normal geometryN;
		float err[3];
		bool deleted, dirty;
	};

	struct SimplifyVertex {
		Point p;
		Normal norm;
		UV uv;
		Spectrum col;
		float alpha;

		u_int tstart, tcount;
		SymetricMatrix q;

		bool border;
	};

	struct SimplifyRef {
		u_int tid, tvertex;
	};

	class SimplifyRefErrCompare {
	public:
		SimplifyRefErrCompare(const Simplify &s) : simplify(s) { }

		bool operator()(const SimplifyRef &sr1, const SimplifyRef &sr2) const {
			return simplify.triangles[sr1.tid].err[sr1.tvertex] < simplify.triangles[sr2.tid].err[sr2.tvertex];
		}

	private:
		const Simplify &simplify;
	};

	vector<SimplifyTriangle> triangles;
	vector<SimplifyVertex> vertices;
	vector<SimplifyRef> refs;

	const Camera *camera;
	float edgeScreenSize;

	u_int maxCandidateQueueSize;
	vector<SimplifyRef> candidateList;

	u_int deletedTriangles;
	bool hasNormals, hasUVs, hasColors, hasAlphas, preserveBorder;

	bool CollapseEdge(const u_int trinagleIndex, const u_int startVertexIndex,
			vector<bool> &deleted0, vector<bool> &deleted1) {
		SimplifyTriangle &t = triangles[trinagleIndex];

		if (t.deleted)
			return false;
		if (t.dirty)
			return false;

		const u_int i0 = t.v[startVertexIndex];
		SimplifyVertex &v0 = vertices[i0];

		const u_int i1 = t.v[(startVertexIndex + 1) % 3];
		SimplifyVertex &v1 = vertices[i1];

		// Border check
		if (v0.border != v1.border)
			return false;

		// Compute vertex to collapse to
		Point p;
		CalculateCollapseError(i0, i1, &p);

		// true/false if the triangles referencing the vertex are deleted
		deleted0.resize(v0.tcount);
		deleted1.resize(v1.tcount);

		// Don't remove if flipped
		if (Flipped(p, i0, i1, &deleted0))
			return false;
		if (Flipped(p, i1, i0, &deleted1))
			return false;

		// Save original vertex information
		const Point triPoint0 = vertices[t.v[0]].p;
		const Point triPoint1 = vertices[t.v[1]].p;
		const Point triPoint2 = vertices[t.v[2]].p;

		const Normal triNorm0 = vertices[t.v[0]].norm;
		const Normal triNorm1 = vertices[t.v[1]].norm;
		const Normal triNorm2 = vertices[t.v[2]].norm;

		const UV triUV0 = vertices[t.v[0]].uv;
		const UV triUV1 = vertices[t.v[1]].uv;
		const UV triUV2 = vertices[t.v[2]].uv;

		const Spectrum triCol0 = vertices[t.v[0]].col;
		const Spectrum triCol1 = vertices[t.v[1]].col;
		const Spectrum triCol2 = vertices[t.v[2]].col;

		const float triAlpha0 = vertices[t.v[0]].alpha;
		const float triAlpha1 = vertices[t.v[1]].alpha;
		const float triAlpha2 = vertices[t.v[2]].alpha;

		// Not flipped, so remove edge
		v0.p = p;
		v0.q = v1.q + v0.q;

		// Interpolate other vertex attributes
		float b1, b2;
		if (Triangle::GetBaryCoords(
				triPoint0,
				triPoint1,
				triPoint2,
				p, &b1, &b2)) {
			const float b0 = 1.f - b1 - b2;

			if (hasNormals)
				v0.norm = Normalize(b0 * triNorm0 + b1 * triNorm1 + b2 * triNorm2);
			if (hasUVs)
				v0.uv = b0 * triUV0 + b1 * triUV1 + b2 * triUV2;
			if (hasColors)
				v0.col = b0 * triCol0 + b1 * triCol1 + b2 * triCol2;
			if (hasAlphas)
				v0.alpha = b0 * triAlpha0 + b1 * triAlpha1 + b2 * triAlpha2;
		} else {
			// Must be a malformed triangle
			if (hasNormals)
				v0.norm = triNorm0;
			if (hasUVs)
				v0.uv = triUV0;
			if (hasColors)
				v0.col = triCol0;
			if (hasAlphas)
				v0.alpha = triAlpha0;
		}

		const u_int tstart = refs.size();

		UpdateTriangles(i0, v0, deleted0);
		UpdateTriangles(i0, v1, deleted1);

		const u_int tcount = refs.size() - tstart;

		if (tcount <= v0.tcount) {
			// Save ram
			if (tcount)
				copy(&refs[tstart], &refs[tstart] + tcount, &refs[v0.tstart]);
		} else
			// Append
			v0.tstart = tstart;

		v0.tcount = tcount;

		return true;
	}

	// Check if a triangle flips when this edge is removed
	bool Flipped(const Point &p, const u_int i0, const u_int i1,
			vector<bool> *deleted = nullptr) const {
		const SimplifyVertex &v0 = vertices[i0];

		for (u_int k = 0; k < v0.tcount; ++k) {
			const SimplifyTriangle &t = triangles[refs[v0.tstart + k].tid];

			if (t.deleted)
				continue;

			const u_int s = refs[v0.tstart + k].tvertex;
			const u_int id1 = t.v[(s + 1) % 3];
			const u_int id2 = t.v[(s + 2) % 3];

			// Delete ?
			if (id1 == i1 || id2 == i1) {
				if (deleted)
					(*deleted)[k] = true;
				continue;
			}

			// Check if the triangle is too narrow
			const Vector d1 = Normalize(vertices[id1].p - p);
			const Vector d2 = Normalize(vertices[id2].p - p);
			if (AbsDot(d1, d2) > .999f)
				return true;

			// Check if the Normal is changing side
			const Normal geometryN(Normalize(Cross(d1, d2)));
			if (Dot(geometryN, t.geometryN) < .2f)
				return true;

			if (deleted)
				(*deleted)[k] = false;
		}

		return false;
	}

	// Update triangle connections and edge error after a edge is collapsed
	void UpdateTriangles(const u_int i0, const SimplifyVertex &v,
			const  vector<bool> &deleted) {
		for (u_int k = 0; k < v.tcount; ++k) {
			const SimplifyRef &r = refs[v.tstart + k];
			SimplifyTriangle &t = triangles[r.tid];

			if (t.deleted)
				continue;

			if (deleted[k]) {
				t.deleted = true;
				deletedTriangles++;
				continue;
			}

			t.v[r.tvertex] = i0;
			t.dirty = true;
			UpdateTriangleError(t);

			refs.push_back(r);
		}
	}

	// Compact triangles, compute edge error and build reference list
	void UpdateMesh(const u_int iteration) {
		if (iteration > 0) {
			// Compact triangles
			int dst = 0;
			for (u_int i = 0; i < triangles.size(); ++i)
				if (!triangles[i].deleted)
					triangles[dst++] = triangles[i];

			triangles.resize(dst);
		}

		// Init Quadrics by Plane & Edge Errors
		//
		// Required at the beginning (iteration == 0)
		//
		if (iteration == 0) {
			for (u_int i = 0; i < vertices.size(); ++i)
				vertices[i].q = SymetricMatrix(0.0);

			for (u_int i = 0; i < triangles.size(); ++i) {
				SimplifyTriangle &t = triangles[i];

				SimplifyVertex &v0 = vertices[t.v[0]];
				SimplifyVertex &v1 = vertices[t.v[1]];
				SimplifyVertex &v2 = vertices[t.v[2]];

				const Normal geometryN(Normalize(Cross(v1.p - v0.p, v2.p - v0.p)));
				t.geometryN = geometryN;

				// It doesn't matter what vertex I use here because the triangle
				// plane will pass for all 3
				const SymetricMatrix sm(geometryN.x, geometryN.y, geometryN.z,
						-Dot(Vector(geometryN), Vector(v0.p)));
				v0.q += sm;
				v1.q += sm;
				v2.q += sm;
			}

			for (u_int i = 0; i < triangles.size(); ++i) {
				// Calc Edge Error
				SimplifyTriangle &t = triangles[i];

				UpdateTriangleError(t);
			}
		}

		// Init Reference ID list
		for (u_int i = 0; i < vertices.size(); ++i) {
			vertices[i].tstart = 0;
			vertices[i].tcount = 0;
		}

		for (u_int i = 0; i < triangles.size(); ++i) {
			SimplifyTriangle &t = triangles[i];

			vertices[t.v[0]].tcount++;
			vertices[t.v[1]].tcount++;
			vertices[t.v[2]].tcount++;
		}

		u_int tstart = 0;
		for (u_int i = 0; i < vertices.size(); ++i) {
			SimplifyVertex &v = vertices[i];

			v.tstart = tstart;
			tstart += v.tcount;
			v.tcount = 0;
		}

		// Write References
		refs.resize(triangles.size() * 3);
		for (u_int i = 0; i < triangles.size(); ++i) {
			SimplifyTriangle &t = triangles[i];

			for (u_int j = 0; j < 3; ++j) {
				SimplifyVertex &v = vertices[t.v[j]];

				refs[v.tstart + v.tcount].tid = i;
				refs[v.tstart + v.tcount].tvertex = j;

				v.tcount++;
			}
		}

		// Identify boundary : vertices[].border=0,1
		//
		// Required at the beginning (iteration == 0)
		if (iteration == 0) {
			for (u_int i = 0; i < vertices.size(); ++i)
				vertices[i].border = false;

			vector<u_int> vcount, vids;
			for (u_int i = 0; i < vertices.size(); ++i) {
				SimplifyVertex &v = vertices[i];
				vcount.clear();
				vids.clear();

				for (u_int j = 0; j < v.tcount; ++j) {
					int k = refs[v.tstart + j].tid;
					SimplifyTriangle &t = triangles[k];

					for (u_int k = 0; k < 3; ++k) {
						u_int ofs = 0;
						u_int id = t.v[k];

						while (ofs < vcount.size()) {
							if (vids[ofs] == id)
								break;

							ofs++;
						}

						if (ofs == vcount.size()) {
							vcount.push_back(1);
							vids.push_back(id);
						} else
							vcount[ofs]++;
					}
				}

				for (u_int j = 0; j < vcount.size(); ++j) {
					if (vcount[j] == 1)
						vertices[vids[j]].border = true;
				}
			}
		}

		// Build the edge candidate queue
		priority_queue<SimplifyRef, vector<SimplifyRef>, SimplifyRefErrCompare>
			candidateQueue{ SimplifyRefErrCompare(*this) };
		for (u_int i = 0; i < triangles.size(); ++i) {
			const SimplifyTriangle &t = triangles[i];

			// Look for the (valid) triangle vertex with the minimum error
			u_int minErrorIndex = NULL_INDEX;
			float minError = numeric_limits<float>::infinity();
			for (u_int j = 0; j < 3; ++j) {
				const u_int i0 = t.v[j];
				SimplifyVertex &v0 = vertices[i0];

				const u_int i1 = t.v[(j + 1) % 3];
				SimplifyVertex &v1 = vertices[i1];

				// Border check
				if (preserveBorder) {
					if (v0.border && v1.border)
						continue;
				} else {
					if (v0.border != v1.border)
						continue;
				}

				// Compute vertex to collapse to
				Point p;
				CalculateCollapseError(i0, i1, &p);

				// Don't remove if flipped
				if (Flipped(p, i0, i1))
					continue;
				if (Flipped(p, i1, i0))
					continue;

				if (t.err[j] < minError) {
					minErrorIndex = j;
					minError = t.err[j];
				}
			}

			if (minErrorIndex == NULL_INDEX)
				continue;

			if (candidateQueue.size() < maxCandidateQueueSize) {
				candidateQueue.push(SimplifyRef{i, minErrorIndex});
				continue;
			}

			const SimplifyRef &top = candidateQueue.top();
			if (t.err[minErrorIndex] < triangles[top.tid].err[top.tvertex]) {
				candidateQueue.pop();
				candidateQueue.push(SimplifyRef{i, minErrorIndex});
			}
		}

		if (candidateQueue.size() > 0) {
			candidateList.resize(candidateQueue.size());
			for (u_int i = candidateList.size() - 1;;) {
				candidateList[i] = candidateQueue.top();
				candidateQueue.pop();

				if (i == 0)
					break;
				--i;
			}

			/*for (u_int i = 0; i < Min<u_int>(candidateList.size(), 10u); ++i) {
				const SimplifyTriangle &t = triangles[candidateList[i].tid];

				SDL_LOG("#" << i << " Min. error: " << fixed << setprecision(10) << t.err[candidateList[i].tvertex] << " (triangle " << candidateList[i].tid << ")");

				const u_int i0 = t.v[candidateList[i].tvertex];
				SimplifyVertex &v0 = vertices[i0];

				const u_int i1 = t.v[(candidateList[i].tvertex + 1) % 3];
				SimplifyVertex &v1 = vertices[i1];

				SDL_LOG("#" << i << " Collapse screen error scale: " << fixed << setprecision(10) <<  CalculateCollapseScreenErrorScale(v0.p, v1.p));
				SDL_LOG("#" << i << " Triangle " << candidateList[i].tid << " border: " <<
						vertices[t.v[candidateList[i].tvertex]].border << " " <<
						vertices[t.v[(candidateList[i].tvertex + 1) % 3]].border);
			}*/

			/*ExtTriangleMeshBuilder meshBuilder;
			for (u_int i = 0; i < candidateList.size(); ++i) {
				const SimplifyTriangle &t = triangles[candidateList[i].tid];

				const u_int index = meshBuilder.vertices.size();
				meshBuilder.AddVertex(vertices[t.v[candidateList[i].tvertex]].p);
				meshBuilder.AddVertex(vertices[t.v[(candidateList[i].tvertex + 1) % 3]].p);
				meshBuilder.AddVertex(vertices[t.v[(candidateList[i].tvertex + 2) % 3]].p);

				meshBuilder.AddTriangle(Triangle(index, index + 1, index +2));
			}
			ExtTriangleMesh *debugMesh = meshBuilder.GetExtTriangleMesh();
			debugMesh->Save("debug-candidates.ply");
			delete debugMesh;*/
		}

		// Clear dirty flag
		for (u_int i = 0; i < triangles.size(); ++i)
			triangles[i].dirty = false;
	}

	// Finally compact mesh before exiting
	void CompactMesh() {
		u_int dst = 0;

		for (u_int i = 0; i < vertices.size(); ++i)
			vertices[i].tcount = 0;

		for (u_int i = 0; i < triangles.size(); ++i) {
			if (!triangles[i].deleted) {
				const SimplifyTriangle &t = triangles[i];
				triangles[dst++] = t;

				vertices[t.v[0]].tcount = 1;
				vertices[t.v[1]].tcount = 1;
				vertices[t.v[2]].tcount = 1;
			}
		}
		triangles.resize(dst);

		dst = 0;
		for (u_int i = 0; i < vertices.size(); ++i) {
			if (vertices[i].tcount) {
				vertices[i].tstart = dst;
				vertices[dst].p = vertices[i].p;

				vertices[dst].norm = vertices[i].norm;
				vertices[dst].uv = vertices[i].uv;
				vertices[dst].col = vertices[i].col;
				vertices[dst].alpha = vertices[i].alpha;

				dst++;
			}
		}

		for (u_int i = 0; i < triangles.size(); ++i) {
			SimplifyTriangle &t = triangles[i];

			t.v[0] = vertices[t.v[0]].tstart;
			t.v[1] = vertices[t.v[1]].tstart;
			t.v[2] = vertices[t.v[2]].tstart;
		}
		vertices.resize(dst);
	}

	// Error between vertex and Quadric
	float VertexError(const SymetricMatrix &q, const float x, const float y, const float z) const {
		return q[0] * x * x + 2.f * q[1] * x * y + 2.f * q[2] * x * z + 2.f * q[3] * x +
				q[4] * y * y + 2.f * q[5] * y * z + 2.f * q[6] * y +
				q[7] * z * z + 2.f * q[8] * z +
				q[9];
	}

	// Error for one edge
	float CalculateCollapseError(const u_int v1Index, const u_int v2Index,
			Point *pResult = nullptr) const {
		const SymetricMatrix q = vertices[v1Index].q + vertices[v2Index].q;

		// Compute interpolated vertex
		const Point &p1 = vertices[v1Index].p;
		const Point &p2 = vertices[v2Index].p;
		const Point p3 = (p1 + p2) / 2;

		// Error can be negative, I add 1 to have screenErrorScale can than
		// work as expected
		const float error1 = VertexError(q, p1.x, p1.y, p1.z) + 1.f;
		const float error2 = VertexError(q, p2.x, p2.y, p2.z) + 1.f;
		const float error3 = VertexError(q, p3.x, p3.y, p3.z) + 1.f;

		float error;
		if (preserveBorder && vertices[v1Index].border) {
			error = error1;
			if (pResult)
				*pResult = p1;
		} else if (preserveBorder && vertices[v2Index].border) {
			error = error2;
			if (pResult)
				*pResult = p2;
		} else {
			error = Min(error1, Min(error2, error3));

			if (pResult) {
				if (error1 == error)
					*pResult = p1;
				if (error2 == error)
					*pResult = p2;
				if (error3 == error)
					*pResult = p3;
			}
		}

		// Adding 1.0 because error have negative values
		return Max(error + 1.f, 0.f);
	}

	float CalculateCollapseScreenErrorScale(const Point &v0, const Point &v1) const {
		if (edgeScreenSize > 0.f) {
			const float notVisibleScale = .5f;

			float v0x, v0y;
			if (!camera->GetSamplePosition(v0, &v0x, &v0y) ||
					!IsValid(v0x) || !IsValid(v0y))
				return notVisibleScale;

			// Normalize
			v0x /= camera->filmWidth;
			v0y /= camera->filmHeight;

			float v1x, v1y;
			if (!camera->GetSamplePosition(v1, &v1x, &v1y) ||
					!IsValid(v1x) || !IsValid(v1y))
				return notVisibleScale;

			// Normalize
			v1x /= camera->filmWidth;
			v1y /= camera->filmHeight;

			const float edge = sqrtf(Sqr(v0x - v1x) + Sqr(v0y - v1y));
			if (edge == 0.f)
				return notVisibleScale;

			return Max(edge / edgeScreenSize, notVisibleScale);
		} else
			return 1.f;
	}

	void UpdateTriangleError(SimplifyTriangle &t) const {
		t.err[0] = CalculateCollapseError(t.v[0], t.v[1]) *
				CalculateCollapseScreenErrorScale(vertices[t.v[0]].p, vertices[t.v[1]].p);

		t.err[1] = CalculateCollapseError(t.v[1], t.v[2]) *
				CalculateCollapseScreenErrorScale(vertices[t.v[1]].p, vertices[t.v[2]].p);

		t.err[2] = CalculateCollapseError(t.v[2], t.v[0]) *
				CalculateCollapseScreenErrorScale(vertices[t.v[2]].p, vertices[t.v[0]].p);
	}
};

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

SimplifyShape::SimplifyShape(const Camera *camera, ExtTriangleMesh *srcMesh,
		const float target, const float edgeScreenSize, const bool preserveBorder) {
	SDL_LOG("Simplify shape " << srcMesh->GetName() << " with target " << target);

	if ((edgeScreenSize > 0.f) && !camera)
		throw runtime_error("The scene camera must be defined in order to enable simplify edgescreensize option");

	const float startTime = WallClockTime();

	const u_int targetCount = Max(1u, Floor2UInt(srcMesh->GetTotalTriangleCount() * target));

	/*srcMesh->Save("debug-start.ply");
	ExtTriangleMesh *debugMeshStart = ScreenProjection(*camera, *srcMesh);
	debugMeshStart->Save("debug-start-proj.ply");
	delete debugMeshStart;*/

	Simplify simplify(*srcMesh);
	simplify.Decimate(targetCount, camera, edgeScreenSize, preserveBorder);
	mesh = simplify.GetExtMesh();

	/*srcMesh->Save("debug-end.ply");
	ExtTriangleMesh *debugMeshEnd = ScreenProjection(*camera, *mesh);
	debugMeshEnd->Save("debug-end-proj.ply");
	delete debugMeshEnd;*/

	SDL_LOG("Subdivided shape from " << srcMesh->GetTotalTriangleCount() << " to " << mesh->GetTotalTriangleCount() << " faces");

	// For some debugging
	//mesh->Save("debug.ply");

	const float endTime = WallClockTime();
	SDL_LOG("Simplify time: " << (boost::format("%.3f") % (endTime - startTime)) << "secs");
}

SimplifyShape::~SimplifyShape() {
	if (!refined)
		delete mesh;
}

ExtTriangleMesh *SimplifyShape::RefineImpl(const Scene *scene) {
	return mesh;
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_FUNCTION_GENERIC_COSH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_GENERIC_COSH_HPP_INCLUDED

#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/maxlog.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/average.hpp>
#include <boost/simd/function/simd/exp.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_greater.hpp>
#include <boost/simd/function/simd/rec.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( cosh_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_<bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) according x < Threshold e =  exp(x) or exp(x/2) is
      // respectively computed
      // *  in the first case cosh (e+rec(e))/2
      // *  in the second     cosh is (e/2)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2 defined in Maxshlog
      //////////////////////////////////////////////////////////////////////////////
      A0 x = bs::abs(a0);
      auto test1 = (x > Maxlog<A0>()-Log_2<A0>());
      A0 fac = if_else(test1, Half<A0>(), One<A0>());
      A0 tmp = exp(x*fac);
      A0 tmp1 = Half<A0>()*tmp;
      return if_else(test1, tmp1*tmp, bs::average(tmp, rec(tmp)));
    }
  };
} } }

#endif


/**
 * This is a simple unoptimized code to evaluate some low-order dual diagrams in FK model
 * input :
 *  --order : order of the diagram
 *  --beta : order of the diagram
 *  --kpts : order of the diagram
 *  --U : order of the diagram
 * output:
 *  - dual self-energy "sigma_wk.dat"
 *  - cut of dual self-energy at first Matsubara freq
 *  - k-dependence of dual bubbles (summer over Matsubara freqs).
 */

#include <boost/program_options.hpp>
#include <gftools.hpp>
#include <Eigen/Core>

namespace po = boost::program_options;
using namespace gftools;

int main(int argc, char *argv[])
{
    // parse command line options - define beta, kpts and U
    po::options_description desc("FK DF 2d");
    desc.add_options()
        ("order,n", po::value<int>()->default_value(2), "order of diagrams")
        ("beta", po::value<double>()->default_value(1), "inverse temperature")
        ("kpts", po::value<int>()->default_value(16), "number of points in one dimension of k-space")
        ("U", po::value<double>()->default_value(8), "interaction strength")
        ("help", "produce help message");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    //po::notify(vm);
    if (vm.count("help")) { std::cout << desc << std::endl; exit(0); }
    int diagram_order = vm["order"].as<int>();
    int kpts = vm["kpts"].as<int>();
    // we will use diagram_order as amount of iterations in vertices. 1 iteration = 2nd order, etc.
    diagram_order-=(diagram_order>0);
    double beta = vm["beta"].as<double>();
    double U = vm["U"].as<double>();
    static constexpr int NDim = 2; // work in 2 dimensions

    // Calc starts here
    std::cout << "Atomic limit of FK model in 2 dimensions using DF approximation" << std::endl;
    double T = 1.0/beta;
    std::cout << "beta = " << beta << std::endl;
    std::cout << "T = " << T << std::endl;
    int totalkpts = std::pow(kpts, NDim);
    std::cout << "kmesh : " << kpts << " points " << std::endl;
    std::cout << "total pts in BZ = " << totalkpts << std::endl;

    // typedef for F(w, w') type objects
    typedef grid_object<std::complex<double>, fmatsubara_grid, fmatsubara_grid> vertex_type;
    // G(w)
    typedef grid_object<std::complex<double>, fmatsubara_grid> gw_type;
    // G(w, kx, ky)
    typedef grid_object<std::complex<double>, fmatsubara_grid, kmesh, kmesh> gk_type;
    // e(kx, ky)
    typedef grid_object<std::complex<double>, kmesh, kmesh> disp_type;
    // typedef for matrices
    typedef Eigen::MatrixXcd matrix_type;

    // Prepare evaluation grids
    fmatsubara_grid fgrid(-20,20,beta);
    // Hybridization function (atomic limit)
    gw_type delta(fgrid);
    delta.fill([&](std::complex<double> w){return double(2*NDim) / w; });
    // Local gf (atomic limit)
    gw_type gw(fgrid);
    gw.fill([&](std::complex<double> w){return 0.5/(w - U/2.0) + 0.5/(w + U/2.0);});
    // Local vertex (atomic limit)
    vertex_type gamma4(std::make_tuple(fgrid, fgrid));
    typename gw_type::function_type Lambda = [U](std::complex<double> w){return 1. - U*U/4./w/w;};
    gamma4.fill([&](std::complex<double> w1, std::complex<double> w2){
            return beta * U * U / 4.0 * double(1 - tools::is_float_equal(w1, w2) ) * (1. - U*U/4./w1/w1) * (1. - U*U/4./w2/w2);
        });

    // k-dependent input
    // define a mesh in Brilloin zone (BZ)
    kmesh kgrid(kpts);
    // lattice dispersion
    disp_type eps(std::forward_as_tuple(kgrid, kgrid));
    eps.fill([&](double kx, double ky){return -2*(cos(kx) + cos(ky));});
    // lattice gf in dmft
    gk_type glat_dmft(std::forward_as_tuple(fgrid, kgrid, kgrid));
    glat_dmft.fill([&](fmatsubara_grid::point w, kmesh::point kx, kmesh::point ky){return 1.0 / (1.0 / gw(w) + delta(w) - eps(kx, ky));});
    // bare dual df
    gk_type gd0(glat_dmft.grids());
    gd0.fill([&](fmatsubara_grid::point w, kmesh::point kx, kmesh::point ky){return glat_dmft(w, kx, ky) - gw(w);});

    double prec = 1e-5;
    // check - local part of gd0 should be zero (check is approximate, as atomic limit is used)
    for (auto w : fgrid.points()) {
        double diff0 = std::abs(gd0[w].sum()) / double(totalkpts);
        if (diff0 > prec) std::cerr << "Problem encountered - w = " << w << ": dual gd0 has a non-zero local part : " <<  diff0 << " > " << prec << std::endl;
        }

    // define full vertex (will be evaluated below)
    vertex_type full_vertex(gamma4);
    // make a matrix from the vertex
    matrix_type gamma4_matrix = gamma4.data().as_matrix();
    matrix_type full_vertex_matrix(gamma4_matrix);

    // check that we are consistent - 2 frequency grids are the same
    if (gd0.template grid<0>() != fgrid) throw std::logic_error("matsubara grid mismatch");

    std::cout << "kmesh : " << kgrid << std::endl;
    // Initalize dual self-energy on the same grids as gd0
    gk_type sigma_dual(gd0.grids());
    sigma_dual = 0.0;
    disp_type bare_bubbles(kgrid,kgrid);

    // now loop through the BZ (no irreducible part optimization)
    for (kmesh::point q1 : kgrid.points()) {
        for (kmesh::point q2 : kgrid.points()) {
            std::cout << "[" << q1.index()*kpts + q2.index()+1 << "/" << totalkpts <<  "]; q = {" <<  q1.value() << " " << q2.value() << "}" << std::endl;
            // Evaluate -T \sum_k G_{w,k} G_{w,k+q}
            // Shift dual g in k-space and make no frequency shift.
            // note : this operation is typically optimized via an fft.
            gk_type gd0_shift = gd0.shift(std::make_tuple(0.0,q1,q2));
            // obtain a bubble
            gk_type bubble_wk = -T * gd0 * gd0_shift;
            // perform sum over k
            gw_type dual_bubble(fgrid);
            for (auto w : fgrid.points()) { dual_bubble[w] = bubble_wk[w].sum() / double(totalkpts); }
            // save the bubble for output
            bare_bubbles(q1, q2) = dual_bubble.sum();
            // construct a diagonal matrix (in frequency space from the bubble)
            matrix_type dual_bubble_matrix = dual_bubble.data().as_diagonal_matrix();
            // refresh vertex
            full_vertex_matrix = gamma4_matrix;
            // IMPORTANT : Evaluate dual diagram
            for (int n=0; n<diagram_order; n++)
                full_vertex_matrix= gamma4_matrix * dual_bubble_matrix * full_vertex_matrix;
            // update self-energy
            for (auto w : fgrid.points())
                sigma_dual[w.index()] += T* full_vertex_matrix(w.index(), w.index()) * gd0_shift[w.index()] / double(totalkpts);
        }
    }
    // output
    // save bare bubbles
    bare_bubbles.savetxt("db0.dat");
    // save sigma
    sigma_dual.savetxt("sigma_wk.dat");
    // save sigma at first matsubara
    auto w0 = fgrid.find_nearest(I*PI/beta);
    disp_type sigma_w0(std::forward_as_tuple(kgrid,kgrid),sigma_dual[w0]);
    sigma_w0.savetxt("sigma_w0.dat");

    // save lattice self-energy
    gk_type sigma_lat(sigma_dual.grids());
    for (fmatsubara_grid::point w : fgrid.points()) {
        sigma_lat[w] = sigma_dual[w] / (sigma_dual[w] * gw[w] + 1.0);
        }
    sigma_lat.savetxt("sigma_lattice.dat");
    sigma_w0.data() = sigma_lat[w0];
    sigma_w0.savetxt("sigma_lattice_w0.dat");
}


/**
 * Created by beck on 24/4/2018.
 * Extended Kalman Filter for 16 states, with quaternion for the orientation
 * Fused A3 flight controller IMU with UWB x and y position
 * With IMU raw reading comes in 400Hz and UWB raw reading in 50Hz
 */
#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <uwb_msgs/uwb.h>
#include <Eigen/Eigen>
#include <queue>

// #define INIT_Q_R_BY_MEASUREMENT

using namespace std;
using namespace Eigen;
ros::Publisher odom_pub;
string imu_topic, uwb_topic, publisher_topic;
double uwb_weight, acc_angle_weight, mag_weight;
double acc_weight, gyro_weight, acc_bias_weight, gyro_bias_weight;

/**
 * Define states:
 *      x = [rotation_quaternion, position, velocity, bias_accel, bias_gyro]
 * Define inputs:
 *      u = [omg, accel], all imu measurement
 * Define noises:
 *      n = [n_gyro, n_acc, n_bias_acc, n_bias_gyro]
 */
VectorXd x(16);                             // state
MatrixXd P = MatrixXd::Zero(15, 15);        // covariance
MatrixXd Q = MatrixXd::Identity(12, 12);    // prediction noise covariance
MatrixXd R = MatrixXd::Identity(6, 6);      // observation noise covariance

// buffers to save imu and uwb reading for time synchronization
queue <sensor_msgs::Imu::ConstPtr> imu_buf;
queue <nav_msgs::Odometry::ConstPtr> odom_buf;
queue <Matrix<double, 16, 1>> x_history;
queue <Matrix<double, 15, 15>> P_history;

// previous propagated time
double t_prev;

// Initialization and covariance estimation
// UWB wait until imu is initialized
const int IMU_INIT_COUNT = 40;
int imu_count = 0;
Vector3d g_init = Vector3d::Zero();
Vector3d G = Vector3d::Zero();
Vector3d accl_init_buf[IMU_INIT_COUNT];
Vector3d gyro_init_buf[IMU_INIT_COUNT];
queue<double> uwb_init_buf_w;
queue<double> uwb_init_buf_x;
queue<double> uwb_init_buf_y;
double theta_bias = 0;
bool imu_initialized = false;
bool odom_initialized = false;

// TODO: Calibrate sensor position
// imu frame to world frame rotation matrix, {[0, 0, -1], pi/2}
//Matrix3d imu_R_world = Quaterniond(sqrt(1/2), 0, 0, -sqrt(1/2)).toRotationMatrix();
Matrix3d imu_R_world;

// Matrix3d init_robot_pose = Quaterniond(sqrt(1/2), 0, 0, -sqrt(1/2)).toRotationMatrix();
// Matrix3d init_robot_pose = Quaterniond(0, 0, 0, 1).toRotationMatrix();

void pub_odom_ekf(std_msgs::Header header) {
    nav_msgs::Odometry odom;
    odom.header.stamp = header.stamp;
    odom.header.frame_id = "world";
    odom.pose.pose.orientation.w = x(0);
    odom.pose.pose.orientation.x = x(1);
    odom.pose.pose.orientation.y = x(2);
    odom.pose.pose.orientation.z = x(3);
    odom.pose.pose.position.x = x(4);
    odom.pose.pose.position.y = x(5);
    odom.pose.pose.position.z = x(6);
    odom.twist.twist.linear.x = x(7);
    odom.twist.twist.linear.y = x(8);
    odom.twist.twist.linear.z = x(9);

    odom_pub.publish(odom);
}

// imu propagate in the world frame
void propagate(const sensor_msgs::Imu::ConstPtr &imu_msg) {
    double cur_t = imu_msg->header.stamp.toSec();
    double dt = cur_t - t_prev;
    Vector3d w_raw;
    Vector3d a_raw;
    a_raw(0) = imu_msg->linear_acceleration.x;
    a_raw(1) = imu_msg->linear_acceleration.y;
    a_raw(2) = imu_msg->linear_acceleration.z;
    w_raw(0) = imu_msg->angular_velocity.x;
    w_raw(1) = imu_msg->angular_velocity.y;
    w_raw(2) = imu_msg->angular_velocity.z;

    Vector3d a = a_raw - x.segment<3>(10);
    Vector3d omg = w_raw - x.segment<3>(13);
    Vector3d domg = 0.5 * dt * omg;

    // propagate the state with quaternion calculus
    Quaterniond dR(sqrt(1 - domg.squaredNorm()), domg(0), domg(1), domg(2));
    Quaterniond Rt(x(0), x(1), x(2), x(3));
    Quaterniond R_t = (Rt * dR).normalized();

    x.segment<4>(0) << R_t.w(), R_t.x(), R_t.y(), R_t.z();
    x.segment<3>(4) += x.segment<3>(7) * dt + (Rt * (a - G)) * 0.5 * dt * dt;
    x.segment<3>(7) += (Rt * (a - G)) * dt;

    // propagate the covariance with skew-symmetric matrix
    MatrixXd I = MatrixXd::Identity(3, 3);
    Matrix3d R_omg, R_a;
    R_omg << 0, -omg(2), omg(1),
            omg(2), 0, -omg(0),
            -omg(1), omg(0), 0;
    R_a << 0, -a(2), a(1),
            a(2), 0, -a(0),
            -a(1), a(0), 0;

    MatrixXd A = MatrixXd::Zero(15, 15);
    A.block<3, 3>(0, 0) = -R_omg;
    A.block<3, 3>(0, 12)= -1 * I;
    A.block<3, 3>(3, 6) = I;
    A.block<3, 3>(6, 0) = (-1 * Rt.toRotationMatrix()) * R_a;
    A.block<3, 3>(6, 9) = (-1 * Rt.toRotationMatrix());
    // cout << "DEBUG:: propagate A" << endl << A << endl;

    MatrixXd U = MatrixXd::Zero(15, 12);
    U.block<3, 3>(0, 0) = -1 * I;
    U.block<3, 3>(6, 3) = -1 * Rt.toRotationMatrix();
    U.block<3, 3>(9, 6) = I;
    U.block<3, 3>(12, 9)= I;
    // cout << "DEBUG:: propagate U" << endl << U << endl;

    MatrixXd F, V;
    F = MatrixXd::Identity(15, 15) + dt * A;
    V = dt * U;

    P = F * P * F.transpose() + V * Q * V.transpose();
    // cout << "DEBUG:: P after propagate" << endl << P << endl;

    t_prev = cur_t;
}

// Loosely coupled update in world frame, fusing the global position of UWB and angle
void update_loosely(const uwb_msgs::uwb &msg,
                    const sensor_msgs::Imu::ConstPtr &imu_msg) {
    Quaterniond q_g(1, 0, 0, 0);
/*
    if (imu_msg != nullptr && &imu_msg != NULL) {
        Vector3d a_raw(imu_msg->linear_acceleration.x,
                       imu_msg->linear_acceleration.y,
                       imu_msg->linear_acceleration.z);
        Vector3d a_norm = a_raw.normalized();
        // cout << "DEBUG:: raw accel" << endl << a_norm << endl;
        Vector3d q_vec(a_norm(1), -a_norm(0), 0);
        Vector3d q_norm = q_vec.normalized();
        q_g.w() = acos(a_norm(2));
        q_g.x() = q_norm(0);
        q_g.y() = q_norm(1);
        q_g.z() = q_norm(2);
    }
*/

    double theta_z = msg.pos_theta - theta_bias;
    VectorXd T(3);
    T(0) = msg.pos_x;
    T(1) = msg.pos_y;
    T(2) = 0;

    MatrixXd C = MatrixXd::Zero(6, 15);
    C.block<3, 3>(0, 0) = Matrix3d::Identity();
//    C(2, 2) = 1;
    C.block<3, 3>(3, 3) = Matrix3d::Identity();

    MatrixXd K(15, 6);
    K = P * C.transpose() * (C * P * C.transpose() + R).inverse();
//    cout << "DEBUG:: update K" << endl << K << endl;

    // Matrix3d uwb_R_world = AngleAxisd(theta_z, Vector3d::UnitZ()) * imu_R_world * q_g.toRotationMatrix();
	Matrix3d uwb_R_world = AngleAxisd(theta_z, Vector3d::UnitZ()) * q_g.toRotationMatrix();
//    cout << "DEBUG:: measured angle" << endl << uwb_R_world << endl;

    VectorXd r(6);
    Quaterniond qm(uwb_R_world);
    Quaterniond q = Quaterniond(x(0), x(1), x(2), x(3));
    Quaterniond dq = q.conjugate() * qm; // Hamilton style
    r.head(3) = 2 * dq.vec();
    r.tail(3) = T - x.segment<3>(4);
    VectorXd _r = K * r;
    Vector3d dw(0.5 * _r(0), 0.5 * _r(1), 0.5 * _r(2));
    Quaterniond _dq = Quaterniond(1, dw(0), dw(1), dw(2)).normalized();
    q = q * _dq;

    x(0) = q.w();
    x(1) = q.x();
    x(2) = q.y();
    x(3) = q.z();
    x.segment<12>(4) += _r.segment<12>(3);
    P = P - K * C * P;
//    cout << "DEBUG:: P after update" << endl << P << endl;
}

/**
 * Initialize, handle and save imu messages
 * @param imu_msg
 */
void imu_callback(const sensor_msgs::Imu::ConstPtr &imu_msg) {
    if (!imu_initialized && imu_count < IMU_INIT_COUNT) {
        accl_init_buf[imu_count](0) = imu_msg->linear_acceleration.x;
        accl_init_buf[imu_count](1) = imu_msg->linear_acceleration.y;
        accl_init_buf[imu_count](2) = imu_msg->linear_acceleration.z;
        gyro_init_buf[imu_count](0) = imu_msg->angular_velocity.x;
        gyro_init_buf[imu_count](1) = imu_msg->angular_velocity.y;
        gyro_init_buf[imu_count](2) = imu_msg->angular_velocity.z;
        g_init(0) += accl_init_buf[imu_count](0);
        g_init(1) += accl_init_buf[imu_count](1);
        g_init(2) += accl_init_buf[imu_count](2);
        imu_count++;
    } else if (!imu_initialized && imu_count == IMU_INIT_COUNT) {
#ifdef INIT_Q_R_BY_MEASUREMENT
        double accl_mean[3] = {0};
        double gyro_mean[3] = {0};
        double accl_cova[3] = {0};
        double gyro_cova[3] = {0};
        for (int i = 0; i < IMU_INIT_COUNT; ++i) {
            for (int j = 0; j < 3; ++j) {
                accl_mean[j] += accl_init_buf[i](j);
                gyro_mean[j] += gyro_init_buf[i](j);
                accl_cova[j] += pow(accl_init_buf[i](j), 2);
                gyro_cova[j] += pow(gyro_init_buf[i](j), 2);
            }
        }
        for (int j = 0; j < 3; ++j) {
            accl_mean[j] /= IMU_INIT_COUNT;
            gyro_mean[j] /= IMU_INIT_COUNT;
            accl_cova[j] /= IMU_INIT_COUNT;
            gyro_cova[j] /= IMU_INIT_COUNT;

            // Q omg first, Q acceleration second
            Q(j, j)         = gyro_cova[j] - pow(gyro_mean[j], 2);
            Q(j + 3, j + 3) = accl_cova[j] - pow(accl_mean[j], 2);
        }
        // Add the ratio
        Q.block<3, 3>(0, 0) = gyro_weight * Q.block<3, 3>(0, 0);
        Q.block<3, 3>(3, 3) = acc_weight  * Q.block<3, 3>(3, 3);

        // Hardcoded initial IMU bias_a, bias_g
        Q.block<3, 3>(6, 6) = acc_bias_weight  * Q.block<3, 3>(6, 6);     // IMU bias_a, bias_g
        Q.block<3, 3>(9, 9) = gyro_bias_weight * Q.block<3, 3>(9, 9);     // IMU bias_a, bias_g
        cout << "DEBUG: measured Q" << endl << Q << endl;
#else
        // Initialize the covariance
        Q.block<3, 3>(0, 0) = gyro_weight * Q.block<3, 3>(0, 0);     // IMU omg, accel
        Q.block<3, 3>(3, 3) = acc_weight  * Q.block<3, 3>(3, 3);     // IMU omg, accel
        Q.block<3, 3>(6, 6) = acc_bias_weight  * Q.block<3, 3>(6, 6);     // IMU bias_a, bias_g
        Q.block<3, 3>(9, 9) = gyro_bias_weight * Q.block<3, 3>(9, 9);     // IMU bias_a, bias_g
        cout << "DEBUG: hardcoded Q" << endl << Q << endl;
#endif
        g_init /= IMU_INIT_COUNT;

        // Initialize the state and the gravity
        x.setZero();
        // Quaterniond init_pose(init_robot_pose);
        Quaterniond init_pose(imu_R_world);
        x(0) = init_pose.w();
        x(1) = init_pose.x();
        x(2) = init_pose.y();
        x(3) = init_pose.z();

        G = imu_R_world * g_init;
        cout << "DEBUG: init state x" << x(0) << x(1) << x(2) << x(3) << endl;
		cout << "DEBUG: measured G" << endl << G << endl;
        t_prev = imu_msg->header.stamp.toSec();
        imu_initialized = true;
    } else if (imu_initialized && odom_initialized) {
        propagate(imu_msg);
        pub_odom_ekf(imu_msg->header);

        imu_buf.push(imu_msg);
        x_history.push(x);
        P_history.push(P);
    }
}

/**
 * initialize, handle and save uwb messages
 * Also doing time synchronization
 * @param uwb msg
 */
void odom_callback(const uwb_msgs::uwb &msg) {
    if (!imu_initialized && !odom_initialized) {
        uwb_init_buf_x.push(msg.pos_x);
        uwb_init_buf_y.push(msg.pos_y);
        uwb_init_buf_w.push(msg.pos_theta);
    } else if (imu_initialized && !odom_initialized) {
#ifdef INIT_Q_R_BY_MEASUREMENT
        double uwb_mean[3] = {0, 0, 0};
        double uwb_cova[3] = {0, 0, 0};
        int uwb_count = (int) uwb_init_buf_x.size();
        for (int i = 0; i < uwb_count; ++i) {
            uwb_mean[0] += uwb_init_buf_x.front();
            uwb_mean[1] += uwb_init_buf_y.front();
            uwb_mean[2] += uwb_init_buf_w.front();
            uwb_cova[0] += pow(uwb_init_buf_x.front(), 2);
            uwb_cova[1] += pow(uwb_init_buf_y.front(), 2);
            uwb_cova[2] += pow(uwb_init_buf_w.front(), 2);
            uwb_init_buf_x.pop();
            uwb_init_buf_y.pop();
            uwb_init_buf_w.pop();
        }
        uwb_mean[0] /= uwb_count;
        uwb_mean[1] /= uwb_count;
        uwb_mean[2] /= uwb_count;
        uwb_cova[0] /= uwb_count;
        uwb_cova[1] /= uwb_count;
        uwb_cova[2] /= uwb_count;

        // Initialize the position and bias
        x(4) = uwb_mean[0];
        x(5) = uwb_mean[1];
        x(6) = 0;
        theta_bias = uwb_mean[2];

        // Initialize the covariance R
        R.topLeftCorner(2, 2) = acc_angle_weight * R.topLeftCorner(2, 2);
        R(2, 2) = (uwb_cova[2] - pow(uwb_mean[2], 2)) * mag_weight;
        R(3, 3) = (uwb_cova[0] - pow(uwb_mean[0], 2)) * uwb_weight;
        R(4, 4) = (uwb_cova[1] - pow(uwb_mean[1], 2)) * uwb_weight;
        R(5, 5) = 0.01 * R(5, 5) * uwb_weight;
        cout << "DEBUG: measured R" << endl << R << endl;
#else
        x(4) = msg.pos_x;
        x(5) = msg.pos_y;
        x(6) = 0;

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

        theta_bias = msg.pos_theta;
        R.topLeftCorner(2, 2) = acc_angle_weight * R.topLeftCorner(2, 2);
        R(2, 2) = mag_weight * R(2, 2);
        R.bottomRightCorner(3, 3) = uwb_weight * R.bottomRightCorner(3, 3); // Measure x, y
        cout << "DEBUG: hardcoded R" << endl << R << endl;
#endif
        t_prev = msg.header.stamp.toSec();
        odom_initialized = true;
    } else {
        // throw the state and covariance history before the uwb time
        while (!imu_buf.empty() && imu_buf.front()->header.stamp < msg.header.stamp) {
            // trace the time backwards to imu time
            t_prev = imu_buf.front()->header.stamp.toSec();
//            ROS_INFO("throw state with time: %f", t_prev);
            imu_buf.pop();
            x_history.pop();
            P_history.pop();
        }
        // If x_history is empty then the uwb reading is the same as the last imu reading
        // And the current estimated x could be used.
        // If not, use the oldest time in the x_history
        if (!x_history.empty()) {
            x = x_history.front();
            P = P_history.front();
            t_prev = imu_buf.front()->header.stamp.toSec();
            imu_buf.pop();
            x_history.pop();
            P_history.pop();
        }

//        ROS_INFO("update state with time: %f", msg.header.stamp.toSec());
        if (imu_buf.empty()){
            update_loosely(msg, NULL);
        } else {
            update_loosely(msg, imu_buf.front());
        }

        // clean the x and P history since the new update corrects the previous propagate
        while (!x_history.empty()) x_history.pop();
        while (!P_history.empty()) P_history.pop();

        queue <sensor_msgs::Imu::ConstPtr> temp_imu_buf;
        while (!imu_buf.empty()) {
//            ROS_INFO("propagate state with time: %f", imu_buf.front()->header.stamp.toSec());
            propagate(imu_buf.front());
            temp_imu_buf.push(imu_buf.front());
            x_history.push(x);
            P_history.push(P);
            imu_buf.pop();
        }
        std::swap(imu_buf, temp_imu_buf);
    }
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "ekf_quaternion_16states");
    ros::NodeHandle n("~");

    // sleep for 10 seconds in order to launch both sensors
    ros::Duration(10).sleep();

    n.param("imu_topic", imu_topic, string("/dji_sdk/imu"));
    n.param("uwb_topic", uwb_topic, string("/uwb_driver/info"));
    n.param("publisher_topic", publisher_topic, string("/ekf_odom"));
    n.param("uwb_weight", uwb_weight, 0.01);
    n.param("acc_angle_weight", acc_angle_weight, 0.01);
    n.param("magnetometer_weight" , mag_weight, 0.01);
    n.param("accelerometer_weight", acc_weight, 0.01);
    n.param("gyroscope_weight", gyro_weight, 0.01);
    n.param("acc_bias_weight" , acc_bias_weight, 0.01);
    n.param("gyro_bias_weight", gyro_bias_weight, 0.01);

    ros::Subscriber s1 = n.subscribe(imu_topic, 100, imu_callback);
    ros::Subscriber s2 = n.subscribe(uwb_topic, 10, odom_callback);
    odom_pub = n.advertise<nav_msgs::Odometry>(publisher_topic, 100);

    // Running the odometry in 400Hz as the IMU update
    ros::Rate r(400);

//    Rimu = Quaterniond(0.7071, 0, 0, -0.7071).toRotationMatrix();
    imu_R_world << 1, 0, 0,
            0, 1,  0,
            0, 0,  1;

    cout << "imu_R_world" << endl << imu_R_world << endl;

    ros::spin();
}

/**
 *  0   q_w     body frame --> world frame
 *  1   q_x
 *  2   q_y
 *  3   q_z
 *  4   p_x     world frame
 *  5   p_y
 *  6   p_z
 *  7   v_x     world frame
 *  8   v_y
 *  9   v_z
 *  10  ba_x    accel_bias      body frame
 *  11  ba_y
 *  12  ba_z
 *  13  bw_x    gyro_bias       body frame
 *  14  bw_y
 *  15  bw_z
 */


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

#ifndef NETKET_ADAMAX_HPP
#define NETKET_ADAMAX_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <complex>
#include <iostream>
#include "abstract_optimizer.hpp"

namespace netket {

class AdaMax : public AbstractOptimizer {
  int npar_;

  double alpha_;
  double beta1_;
  double beta2_;

  Eigen::VectorXd ut_;
  Eigen::VectorXd mt_;

  double niter_;
  double niter_reset_;

  double epscut_;

 public:
  explicit AdaMax(double alpha = 0.001, double beta1 = 0.9,
                  double beta2 = 0.999, double epscut = 1.0e-7)
      : alpha_(alpha), beta1_(beta1), beta2_(beta2), epscut_(epscut) {
    npar_ = -1;
    niter_ = 0;
    niter_reset_ = -1;

    PrintParameters();
  }

  void PrintParameters() {
    InfoMessage() << "Adamax optimizer initialized with these parameters :"
                  << std::endl;
    InfoMessage() << "Alpha = " << alpha_ << std::endl;
    InfoMessage() << "Beta1 = " << beta1_ << std::endl;
    InfoMessage() << "Beta2 = " << beta2_ << std::endl;
    InfoMessage() << "Epscut = " << epscut_ << std::endl;
  }

  void Init(int npar) override {
    npar_ = npar;
    ut_.setZero(npar_);
    mt_.setZero(npar_);

    niter_ = 0;
  }

  void Update(const Eigen::VectorXd &grad,
              Eigen::Ref<Eigen::VectorXd> pars) override {
    assert(npar_ > 0);

    mt_ = beta1_ * mt_ + (1. - beta1_) * grad;

    for (int i = 0; i < npar_; i++) {
      ut_(i) = std::max(std::max(std::abs(grad(i)), beta2_ * ut_(i)), epscut_);
    }
    niter_ += 1.;
    if (niter_reset_ > 0) {
      if (niter_ > niter_reset_) {
        niter_ = 1;
      }
    }

    double eta = alpha_ / (1. - std::pow(beta1_, niter_));
    for (int i = 0; i < npar_; i++) {
      pars(i) -= eta * mt_(i) / ut_(i);
    }
  }

  void Reset() override {
    ut_ = Eigen::VectorXd::Zero(npar_);
    mt_ = Eigen::VectorXd::Zero(npar_);
    niter_ = 0;
  }

  void SetResetEvery(double niter_reset) { niter_reset_ = niter_reset; }
};

}  // namespace netket

#endif


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/bundle_adjustment/optimize_relative_position_with_known_rotation.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <glog/logging.h>
#include <algorithm>
#include <vector>

#include "theia/math/util.h"
#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/triangulation/triangulation.h"

namespace theia {
namespace {

// Creates the constraint matrix such that ||A * t|| is minimized, where A is
// R_i * f_i x R_j * f_j. Given known rotations, we can solve for the
// relative translation from this constraint matrix.
void CreateConstraintMatrix(
    const std::vector<FeatureCorrespondence>& correspondences,
    const Eigen::Vector3d& rotation1,
    const Eigen::Vector3d& rotation2,
    Eigen::MatrixXd* constraint_matrix) {
  constraint_matrix->resize(3, correspondences.size());

  Eigen::Matrix3d rotation_matrix1;
  ceres::AngleAxisToRotationMatrix(
      rotation1.data(), ceres::ColumnMajorAdapter3x3(rotation_matrix1.data()));
  Eigen::Matrix3d rotation_matrix2;
  ceres::AngleAxisToRotationMatrix(
      rotation2.data(), ceres::ColumnMajorAdapter3x3(rotation_matrix2.data()));

  for (int i = 0; i < correspondences.size(); i++) {
    const Eigen::Vector3d rotated_feature1 =
        rotation_matrix1.transpose() *
        correspondences[i].feature1.point_.homogeneous();
    const Eigen::Vector3d rotated_feature2 =
        rotation_matrix2.transpose() *
        correspondences[i].feature2.point_.homogeneous();

    constraint_matrix->col(i) =
        rotated_feature2.cross(rotated_feature1).transpose() *
        rotation_matrix1.transpose();
  }
}

// Determines if the majority of the points are in front of the cameras. This is
// useful for determining the sign of the relative position. Returns true if
// more than 50% of correspondences are in front of both cameras and false
// otherwise.
bool MajorityOfPointsInFrontOfCameras(
    const std::vector<FeatureCorrespondence>& correspondences,
    const Eigen::Vector3d& rotation1,
    const Eigen::Vector3d& rotation2,
    const Eigen::Vector3d& relative_position) {
  // Compose the relative rotation.
  Eigen::Matrix3d rotation_matrix1, rotation_matrix2;
  ceres::AngleAxisToRotationMatrix(
      rotation1.data(), ceres::ColumnMajorAdapter3x3(rotation_matrix1.data()));
  ceres::AngleAxisToRotationMatrix(
      rotation2.data(), ceres::ColumnMajorAdapter3x3(rotation_matrix2.data()));
  const Eigen::Matrix3d relative_rotation_matrix =
      rotation_matrix2 * rotation_matrix1.transpose();

  // Tests all points for cheirality.
  int num_points_in_front_of_cameras = 0;
  for (const FeatureCorrespondence& match : correspondences) {
    if (IsTriangulatedPointInFrontOfCameras(match,
                                            relative_rotation_matrix,
                                            relative_position)) {
      ++num_points_in_front_of_cameras;
    }
  }

  return num_points_in_front_of_cameras > (correspondences.size() / 2);
}

}  // namespace

// Given known camera rotations and feature correspondences, this method solves
// for the relative translation that optimizes the epipolar error
// f_i * E * f_j^t = 0.
bool OptimizeRelativePositionWithKnownRotation(
    const std::vector<FeatureCorrespondence>& correspondences,
    const Eigen::Vector3d& rotation1,
    const Eigen::Vector3d& rotation2,
    Eigen::Vector3d* relative_position) {
  CHECK_NOTNULL(relative_position);

  // Set the initial relative position to random. This helps avoid a bad local
  // minima that is achieved from poor initialization.
  relative_position->setRandom();

  // Constants used for the IRLS solving.
  const double eps = 1e-5;
  const int kMaxIterations = 100;
  const int kMaxInnerIterations = 10;
  const double kMinWeight = 1e-7;

  // Create the constraint matrix from the known correspondences and rotations.
  Eigen::MatrixXd constraint_matrix;
  CreateConstraintMatrix(correspondences,
                         rotation1,
                         rotation2,
                         &constraint_matrix);

  // Initialize the weighting terms for each correspondence.
  Eigen::VectorXd weights(correspondences.size());
  weights.setConstant(1.0);

  // Solve for the relative positions using a robust IRLS.
  double cost = 0;
  int num_inner_iterations = 0;
  for (int i = 0;
       i < kMaxIterations && num_inner_iterations < kMaxInnerIterations;
       i++) {
    // Limit the minimum weight at kMinWeight.
    weights = (weights.array() < kMinWeight).select(kMinWeight, weights);

    // Apply the weights to the constraint matrix.
    const Eigen::Matrix3d lhs = constraint_matrix *
                                weights.asDiagonal().inverse() *
                                constraint_matrix.transpose();

    // Solve for the relative position which is the null vector of the weighted
    // constraints.
    const Eigen::Vector3d new_relative_position =
        lhs.jacobiSvd(Eigen::ComputeFullU).matrixU().rightCols<1>();

    // Update the weights based on the current errors.
    weights =
        (new_relative_position.transpose() * constraint_matrix).array().abs();

    // Compute the new cost.
    const double new_cost = weights.sum();

    // Check for convergence.
    const double delta = std::max(std::abs(cost - new_cost),
                                  1 - new_relative_position.squaredNorm());

    // If we have good convergence, attempt an inner iteration.
    if (delta <= eps) {
      ++num_inner_iterations;
    } else {
      num_inner_iterations = 0;
    }

    cost = new_cost;
    *relative_position = new_relative_position;
  }

  // The position solver above does not consider the sign of the relative
  // position. We can determine the sign by choosing the sign that puts the most
  // points in front of the camera.
  if (!MajorityOfPointsInFrontOfCameras(correspondences,
                                        rotation1,
                                        rotation2,
                                        *relative_position)) {
    *relative_position *= -1.0;
  }

  return true;
}

}  // namespace theia


//
// Created by philipp on 13.01.20.
//

#ifndef FUNNELS_CPP_EX1_HEU_HH
#define FUNNELS_CPP_EX1_HEU_HH

#include <Eigen/Core>
#include <cmath>
#include <sstream>
#include <algorithm>

#include <funnels/utils.hh>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795d
#endif

// Attention, in example 1, velocity coincides with the control
// signal
// Attention, assumes constant velocity/input

const double max_r_step = 0.1; // different funnel sizes in a family
const double min_r = 0.001; // Minimal radius
const double max_r = 0.5; // Minimal radius
const double max_alpha_step = max_r_step*max_r_step; // Minimal step for alpha
const double min_alpha = min_r*min_r; // Minimal alpha for ellipsoids
const double max_alpha = max_r*max_r; // Minimal alpha for ellipsoids
const double max_vel = 1.; // Norm of velocity
const double min_vel_diff = 0.1; // Norm difference in velocity
const double min_ang_diff = 20.*M_PI/180.;
const double gamma_conv = 3.; // exponential convergence rate
const double max_traj_length = 0.2; // Length of the new segment
const double dt_step = 0.05; //time step between two verification points
const double max_time = 10.;
const double min_englobe_fac = 1.1;

Eigen::Matrix2d getR(double alpha);

template<class DERIVED>
double get_smallest_englobing(double r_src,
    const Eigen::MatrixBase<DERIVED> &delta_vel){
  return min_englobe_fac*(r_src + delta_vel.norm());
}

double get_smallest_englobing(double r_src, double d_v_norm);

template <class FUN_PTR_T>
FUN_PTR_T get_this_funnel(const FUN_PTR_T &src, double radius,
    const Eigen::Vector4d &x0, const Eigen::Vector2d u){

  using fun_t = typename FUN_PTR_T::element_type;

  // Get maximal time before the maximal length is attained
  // or the playground is left
  double n_u = u.norm()+1e-200;
  double t_max = std::min(max_time, max_traj_length/n_u);
  // distance to x/y-border
  for (size_t i=0; i<2; i++){
    if(x0(i)>=0. && u(i)>=0.){
      t_max = std::min(t_max, (1.-x0(i))/(u(i)+1e-200));
    }else if (x0(i)<=0. && u(i)<=0.){
      t_max = std::min(t_max, (1.+x0(i))/(u(i)+1e-200));
    }
  }
  if (t_max < 3.*dt_step){
    // trajectory is too short
    // initial is on the boundary and we are pointing outwards
    return nullptr;
  }

  // Convert to number of points
  size_t this_n_verif = t_max/dt_step + 3;

  // todo check if this funnel already exists in the
  // funnel system
  // get the new name
  std::stringstream sstream;
  sstream.setf(std::ios::fixed);
  sstream.precision(2);

  sstream << "fun_";
  for(size_t i=0; i<4; i++){
    sstream << (double) x0(i) << "_";
  }
  sstream << radius;
  std::string name = sstream.str();
  std::replace( name.begin(), name.end(), '.', 'p');
  std::replace( name.begin(), name.end(), '-', 'm');

  // If exactly this funnel exists, the name will exist
  if (utils_ext::loc_map.find(name)){
    return nullptr;
  }

  FUN_PTR_T new_fun = src->make_copy(name, this_n_verif);

  // set the new radius
  new_fun->set_P(src->get_P()*(src->get_alpha()/(radius*radius)));
  // and convergence
  new_fun->set_gamma(gamma_conv);

  new_fun->compute(x0, 0., t_max, u);

  // He starts a family
  new_fun->start_family(new_fun);

  return new_fun;
}

template <class FUN_PTR_T>
std::vector<FUN_PTR_T> get_children(FUN_PTR_T &parent){

  using fun_t = typename FUN_PTR_T::element_type;
  using matrix_t = typename fun_t::matrix_t;

  FUN_PTR_T this_child_fun;
  FUN_PTR_T this_parent_fun = parent;

  std::stringstream sstream;
  std::string name;

  // Original unscaled matrix
  matrix_t P_p = parent->get_P()*parent->get_alpha();

  sstream.setf(std::ios::fixed);
  sstream.precision(2);

  std::vector<FUN_PTR_T> fun_vec_child;

  const double alpha_step = max_alpha_step;
  const double r_step = max_r_step;

  // Radius of parent
  double alpha = parent->get_alpha();

  if(std::sqrt(alpha) < min_r + r_step){
    return fun_vec_child;
  }

  alpha = std::max(min_alpha, (std::sqrt(alpha)-r_step)*(std::sqrt(alpha)-r_step));

  while(alpha>=min_alpha){
    // Get the name
    sstream.str(std::string());
    sstream << "fun_";
    for(size_t i=0; i<4; i++){
      sstream << (double) parent->x0()(i) << "_";
    }
    sstream << std::sqrt(alpha);
    name = sstream.str();
    std::replace( name.begin(), name.end(), '.', 'p');
    std::replace( name.begin(), name.end(), '-', 'm');
    if (utils_ext::loc_map.find(name)){
      // Already exists
      return fun_vec_child;
    }

    this_child_fun = this_parent_fun->make_copy(name, this_parent_fun->size());
    // set the new radius
    this_child_fun->set_P(P_p/alpha);
    this_child_fun->set_cyclic(this_parent_fun->get_cyclic());
    // And convergence
    this_child_fun->set_gamma(gamma_conv);

    fun_t::set_parent(this_parent_fun, this_child_fun);
    fun_vec_child.emplace_back(this_child_fun);
    this_parent_fun = this_child_fun;
    this_child_fun = nullptr;
    // Update alpha
    alpha = (std::sqrt(alpha)-r_step); // New alpha as r
    if (alpha <= min_r){
      break; // Necessary as negative radii become positive in next step
    }
    alpha *= alpha; // Convert to alpha
  }
  return fun_vec_child;
}

template<class FUN_PTR_T, class FUN_SYS>
void add_new_funnels(const FUN_PTR_T& src,
    double t_src, FUN_SYS &fun_sys){

  using fun_t = typename FUN_PTR_T::element_type;
  using vector_t = typename fun_t::vector_t;
  using matrix_t = typename fun_t::matrix_t;

  // Return val
  FUN_PTR_T fun_parent;
  std::vector<FUN_PTR_T> fun_vec_child;

  // Radius of source
  double r_src = std::sqrt(src->get_alpha());
  if(r_src>=0.99*max_r){
    return; // Quick exit
  }

  Eigen::Vector2d v_src = src->x().block(2,0,2,1);
  double n_v_src = v_src.norm();
  Eigen::Vector2d v_src_n;
  if (n_v_src<1e-10){
    // The funnel is static -> any direction will do
    n_v_src = 0.;
    v_src_n(0) = 1.;
    v_src_n(1) = 0.;
  }else{
    v_src_n = v_src/(n_v_src+1e-200); // Normalized
  }

  //Initial position
  size_t idx_t = (src->t().array()-t_src).array().abs().minCoeff();
  // Branching point of new funnels
  Eigen::Vector2d pos_t = src->x().block(0,idx_t,2,1);

  // Create all possible successors with min_ang_diff
  Eigen::Vector2d new_vel_n, new_vel;
  Eigen::Vector4d x0;
  double n_new_vel, tgt_radius;
  double d_alpha = -M_PI;
  while (d_alpha <= M_PI-min_ang_diff/2){

    // Desired new velocity direction
    new_vel_n = getR(d_alpha)*v_src_n;

    // Loop over all allowed velocity norms
    for (double delta_new_vel = -2.*max_vel; delta_new_vel<=2.*max_vel;
        delta_new_vel+=min_vel_diff){
      n_new_vel = n_v_src + delta_new_vel;
      if(n_new_vel<-max_vel || n_new_vel > max_vel){
        // New velocity norm is forbidden
        continue;
      }
      if ((std::abs(n_new_vel)<1e-10) && (d_alpha != -M_PI/2) ){
        // No need to add many static funnels with "different"
        // directions
        continue;
      }
      tgt_radius = get_smallest_englobing(r_src, std::abs(delta_new_vel));
      if (tgt_radius>max_r){
        // Minimal funnel size needed to englobe is larger then
        // the maximal size
        continue;
      }
      // Now we have a new parent funnel candidate
      x0.block(0,0,2,1) = pos_t;
      // todo refactor norm vs normalized
      x0.block(2,0,2,1) = n_new_vel*new_vel_n;
      // Parent
      fun_parent = get_this_funnel(src, tgt_radius, x0, n_new_vel*new_vel_n);
      if (fun_parent == nullptr){
        continue;
      }else{
        // add
        fun_sys.add_funnel(fun_parent, true);
        // all_children
        fun_vec_child = get_children(fun_parent);
        for (auto a_f : fun_vec_child){
          fun_sys.add_funnel(a_f);
        }
      }
    } // Done velocity
    d_alpha += min_ang_diff; // Update angles
  }// Done angles
  // All calculated
  return;
}

#endif //FUNNELS_CPP_EX1_HEU_HH


#include <NTL/vec_ZZ.h>

#include <NTL/new.h>

NTL_START_IMPL

NTL_vector_impl(ZZ,vec_ZZ)

NTL_eq_vector_impl(ZZ,vec_ZZ)

NTL_io_vector_impl(ZZ,vec_ZZ)

void InnerProduct(ZZ& xx, const vec_ZZ& a, const vec_ZZ& b)
{
   ZZ t1, x;

   long n = min(a.length(), b.length());
   long i;

   clear(x);
   for (i = 1; i <= n; i++) {
      mul(t1, a(i), b(i));
      add(x, x, t1);
   }

   xx = x;
}

void mul(vec_ZZ& x, const vec_ZZ& a, const ZZ& b_in)
{
   ZZ b = b_in;
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      mul(x[i], a[i], b);
}

void mul(vec_ZZ& x, const vec_ZZ& a, long b)
{
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      mul(x[i], a[i], b);
}

void add(vec_ZZ& x, const vec_ZZ& a, const vec_ZZ& b)
{
   long n = a.length();
   if (b.length() != n) Error("vector add: dimension mismatch");

   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      add(x[i], a[i], b[i]);
}

void sub(vec_ZZ& x, const vec_ZZ& a, const vec_ZZ& b)
{
   long n = a.length();
   if (b.length() != n) Error("vector sub: dimension mismatch");
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      sub(x[i], a[i], b[i]);
}

void clear(vec_ZZ& x)
{
   long n = x.length();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

void negate(vec_ZZ& x, const vec_ZZ& a)
{
   long n = a.length();
   x.SetLength(n);
   long i;
   for (i = 0; i < n; i++)
      negate(x[i], a[i]);
}

long IsZero(const vec_ZZ& a)
{
   long n = a.length();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

vec_ZZ operator+(const vec_ZZ& a, const vec_ZZ& b)
{
   vec_ZZ res;
   add(res, a, b);
   NTL_OPT_RETURN(vec_ZZ, res);
}

vec_ZZ operator-(const vec_ZZ& a, const vec_ZZ& b)
{
   vec_ZZ res;
   sub(res, a, b);
   NTL_OPT_RETURN(vec_ZZ, res);
}

vec_ZZ operator-(const vec_ZZ& a)
{
   vec_ZZ res;
   negate(res, a);
   NTL_OPT_RETURN(vec_ZZ, res);
}

ZZ operator*(const vec_ZZ& a, const vec_ZZ& b)
{
   ZZ res;
   InnerProduct(res, a, b);
   NTL_OPT_RETURN(ZZ, res);
}

void VectorCopy(vec_ZZ& x, const vec_ZZ& a, long n)
{
   if (n < 0) Error("VectorCopy: negative length");
   if (NTL_OVERFLOW(n, 1, 0)) Error("overflow in VectorCopy");

   long m = min(n, a.length());

   x.SetLength(n);

   long i;

   for (i = 0; i < m; i++)
      x[i] = a[i];

   for (i = m; i < n; i++)
      clear(x[i]);
}

NTL_END_IMPL


#pragma once

#include <boost/bimap.hpp>
#include <boost/graph/adjacency_list.hpp>
// #include <boost/graph/find_flow_cost.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>

namespace libMultiRobotPlanning {

/*!
  \example assignment.cpp example that takes cost mappings from a file
*/

/*! \brief Find optimal (lowest total cost) assignment

This class can find the lowest sum-of-cost assignment
for given agents and tasks. The costs must be integers, the agents and
tasks can be of any user-specified type.

This method is based on maximum flow formulation.

\tparam Agent Type of the agent. Needs to be copy'able and comparable
\tparam Task Type of task. Needs to be copy'able and comparable
*/
template <typename Agent, typename Task>
class Assignment {
 public:
  Assignment()
      : m_agents(), m_tasks(), m_graph(), m_sourceVertex(), m_sinkVertex() {
    m_sourceVertex = boost::add_vertex(m_graph);
    m_sinkVertex = boost::add_vertex(m_graph);
  }

  void clear() {
    // std::cout << "Asg: clear" << std::endl;
    std::set<edge_t> edgesToRemove;
    for (const auto& agent : m_agents) {
      auto es = boost::out_edges(agent.right, m_graph);
      for (auto eit = es.first; eit != es.second; ++eit) {
        if (!m_graph[*eit].isReverseEdge) {
          edgesToRemove.insert(*eit);
          edgesToRemove.insert(m_graph[*eit].reverseEdge);
        }
      }
    }

    for (const auto& e : edgesToRemove) {
      boost::remove_edge(e, m_graph);
    }
  }

  void setCost(const Agent& agent, const Task& task, long cost) {
    // std::cout << "setCost: " << agent << "->" << task << " cost: " << cost <<
    // std::endl;
    // Lazily create vertex for agent
    auto agentIter = m_agents.left.find(agent);
    vertex_t agentVertex;
    if (agentIter == m_agents.left.end()) {
      agentVertex = boost::add_vertex(m_graph);
      addOrUpdateEdge(m_sourceVertex, agentVertex, 0);
      m_agents.insert(agentsMapEntry_t(agent, agentVertex));
    } else {
      agentVertex = agentIter->second;
    }

    // Lazily create vertex for task
    auto taskIter = m_tasks.left.find(task);
    vertex_t taskVertex;
    if (taskIter == m_tasks.left.end()) {
      taskVertex = boost::add_vertex(m_graph);
      addOrUpdateEdge(taskVertex, m_sinkVertex, 0);
      m_tasks.insert(tasksMapEntry_t(task, taskVertex));
    } else {
      taskVertex = taskIter->second;
    }

    addOrUpdateEdge(agentVertex, taskVertex, cost);
  }

  // find first (optimal) solution with minimal cost
  long solve(std::map<Agent, Task>& solution) {
    using namespace boost;

    successive_shortest_path_nonnegative_weights(
        m_graph, m_sourceVertex, m_sinkVertex,
        boost::capacity_map(get(&Edge::capacity, m_graph))
            .residual_capacity_map(get(&Edge::residualCapacity, m_graph))
            .weight_map(get(&Edge::cost, m_graph))
            .reverse_edge_map(get(&Edge::reverseEdge, m_graph)));

    // long cost = find_flow_cost(
    //   m_graph,
    //   boost::capacity_map(get(&Edge::capacity, m_graph))
    //   .residual_capacity_map(get(&Edge::residualCapacity, m_graph))
    //   .weight_map(get(&Edge::cost, m_graph)));
    long cost = 0;

    // find solution
    solution.clear();
    auto es = out_edges(m_sourceVertex, m_graph);
    for (auto eit = es.first; eit != es.second; ++eit) {
      vertex_t agentVertex = target(*eit, m_graph);
      auto es2 = out_edges(agentVertex, m_graph);
      for (auto eit2 = es2.first; eit2 != es2.second; ++eit2) {
        if (!m_graph[*eit2].isReverseEdge) {
          vertex_t taskVertex = target(*eit2, m_graph);
          if (m_graph[*eit2].residualCapacity == 0) {
            solution[m_agents.right.at(agentVertex)] =
                m_tasks.right.at(taskVertex);
            cost += m_graph[edge(agentVertex, taskVertex, m_graph).first].cost;
            break;
          }
        }
      }
    }

    return cost;
  }

 protected:
  typedef boost::adjacency_list_traits<boost::vecS, boost::vecS,
                                       boost::bidirectionalS>
      graphTraits_t;
  typedef graphTraits_t::vertex_descriptor vertex_t;
  typedef graphTraits_t::edge_descriptor edge_t;

  struct Vertex {
    // boost::default_color_type color;
    // edge_t predecessor;
  };

  struct Edge {
    Edge()
        : cost(0),
          capacity(0),
          residualCapacity(0),
          reverseEdge(),
          isReverseEdge(false) {}

    long cost;
    long capacity;
    long residualCapacity;
    edge_t reverseEdge;
    bool isReverseEdge;
  };

  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::bidirectionalS,
                                Vertex, Edge>
      graph_t;

 protected:
  void addOrUpdateEdge(vertex_t from, vertex_t to, long cost) {
    auto e = boost::edge(from, to, m_graph);
    if (e.second) {
      m_graph[e.first].cost = cost;
      m_graph[m_graph[e.first].reverseEdge].cost = -cost;
    } else {
      auto e1 = boost::add_edge(from, to, m_graph);
      m_graph[e1.first].cost = cost;
      m_graph[e1.first].capacity = 1;
      auto e2 = boost::add_edge(to, from, m_graph);
      m_graph[e2.first].isReverseEdge = true;
      m_graph[e2.first].cost = -cost;
      m_graph[e2.first].capacity = 0;
      m_graph[e1.first].reverseEdge = e2.first;
      m_graph[e2.first].reverseEdge = e1.first;
    }
  }

 private:
  typedef boost::bimap<Agent, vertex_t> agentsMap_t;
  typedef typename agentsMap_t::value_type agentsMapEntry_t;
  typedef boost::bimap<Task, vertex_t> tasksMap_t;
  typedef typename tasksMap_t::value_type tasksMapEntry_t;

  agentsMap_t m_agents;
  tasksMap_t m_tasks;

  graph_t m_graph;
  vertex_t m_sourceVertex;
  vertex_t m_sinkVertex;
};

}  // namespace libMultiRobotPlanning


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

#include <iostream>

#include <boost/program_options.hpp>

#include <dpMM/dirNaiveBayes.hpp>
#include <dpMM/niwBaseMeasure.hpp>
#include <dpMM/typedef.h>
#include <dpMM/timer.hpp>

namespace po = boost::program_options;

int main(int argc, char **argv){

  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
	("K,K", po::value<int>(), "number of initial clusters ")
	("T,T", po::value<int>(), "iterations")
	("v,v", po::value<bool>(), "verbose output")
    ("input,i", po::value<string>(),
      "path to input dataset .csv file (rows: dimensions; cols: different "
      "datapoints)")
    ("output,o", po::value<string>(),
      "path to output labels .csv file (rows: time; cols: different "
      "datapoints)")
    ;

    po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	uint K=2;
	uint T=100;
	uint N=100;
	uint D=2;
	uint M=2;
	uint NumObs = 1;
	bool verbose = false;
	vector<uint> Mword;
	if (vm.count("K"))
		K = vm["K"].as<int>();
	if (vm.count("T"))
		T = vm["T"].as<int>();
	if (vm.count("v"))
		verbose = vm["v"].as<bool>();

	string pathIn ="";
	string pathOut ="";
	if(vm.count("input"))
		pathIn = vm["input"].as<string>();
	if(vm.count("output"))
		pathOut= vm["output"].as<string>();

	vector< Matrix<double, Dynamic, Dynamic> > x;
	x.reserve(N);
	if (!pathIn.compare(""))
	{
		cout<<"making some data up " <<endl;
		uint Ndoc=M;
		uint Nword=int(N/M);
		for(uint i=0; i<Ndoc; ++i) {
			MatrixXd  xdoc(D,Nword);
			for(uint w=0; w<Nword; ++w) {
				if(i<Ndoc/2)
					xdoc.col(w) <<  VectorXd::Zero(D);
				else
					xdoc.col(w) <<  2.0*VectorXd::Ones(D);
			}

			x.push_back(xdoc);
		}
	}else{

		cout<<"loading data from "<<pathIn<<endl;
		std::ifstream fin(pathIn.data(),std::ifstream::in);

		//read parameters from file (tired of passing them in)
		fin>>NumObs;
		fin>>N;
		fin>>M;
		fin>>D;

		MatrixXd data(D,N);
		VectorXu words(M);

		for (uint j=0; j<M; ++j)
			fin>>words(j);

		for (uint j=1; j<(D+1); ++j)
			for (uint i=0; i<N; ++i)
				fin>>data(j-1,i);

		uint count = 0;
		for (uint j=0; j<M; ++j)
		{
			x.push_back(data.middleCols(count,words[j]));
			count+=words[j];
		}
		fin.close();
	}

	double nu = D+1;
	double kappa = D+1;
	MatrixXd Delta = 0.1*MatrixXd::Identity(D,D);
	Delta *= nu;
	VectorXd theta = VectorXd::Zero(D);
	VectorXd alpha = 10.0*VectorXd::Ones(K);

	boost::mt19937 rndGen(9191);
	NIW<double> niw(Delta,theta,nu,kappa,&rndGen);

	Dir<Catd,double> dir(alpha,&rndGen);

	//cout<<"------ marginalized ---- NIW "<<endl;
	//DirNaiveBayes<double> naive_marg(dir,niwMargBase);
	//naive_marg.initialize(x);
	//cout<<naive_marg.labels().transpose()<<endl;
	//for(uint t=0; t<30; ++t)
	//{
	//naive_marg.sampleLabels();
	//naive_marg.sampleParameters();
	//cout<<naive_marg.labels().transpose()
		//<<" logJoint="<<naive_marg.logJoint()<<endl;
	//}
	Timer tlocal;
	tlocal.tic();

	boost::shared_ptr<NiwSampled<double> > niwSampled( new NiwSampled<double>(niw));
	DirNaiveBayes<double> naive_samp(dir,niwSampled);

	cout << "naiveBayesian Clustering:" << endl;
	cout << "Ndocs=" << M << endl;
	cout << "Ndata=" << N << endl;
	cout << "dim=" << D << endl;
	cout << "Num Cluster = " << K << ", (" << T << " iterations)." << endl;

	naive_samp.initialize( (const vector< Matrix<double, Dynamic, Dynamic> >) x );
	naive_samp.inferAll(T,verbose);

	if (pathOut.compare(""))
	{
		std::ofstream fout(pathOut.data(),std::ofstream::out);

		std::streambuf *coutbuf = std::cout.rdbuf(); //save old cout buffer
		cout.rdbuf(fout.rdbuf()); //redirect std::cout to fout1 buffer

			naive_samp.dump(fout,fout);

		std::cout.rdbuf(coutbuf); //reset to standard output again

		fout.close();
	}

	tlocal.displayElapsedTimeAuto();
	return(0);

};


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

/**
 * \file
 * \brief Implementation of \link maptk::rotation_ rotation_<T> \endlink
 *        for \c T = { \c float, \c double }
 */

#include "rotation.h"
#include "eigen_io.h"

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

namespace maptk
{

/// Constructor - from a Rodrigues vector
template <typename T>
rotation_<T>
::rotation_(const Eigen::Matrix<T,3,1>& rvec)
{
  T mag = rvec.norm();
  if (mag == T(0))
  {
    // identity rotation is a special case
    q_.setIdentity();
  }
  else
  {
    q_ = Eigen::Quaternion<T>(Eigen::AngleAxis<T>(mag, rvec/mag));
  }
}

/// Constructor - from rotation angle and axis
template <typename T>
rotation_<T>
::rotation_(T angle, const Eigen::Matrix<T,3,1>& axis)
  : q_(Eigen::Quaternion<T>(Eigen::AngleAxis<T>(angle, axis.normalized())))
{
}

/// Constructor - from yaw, pitch, and roll
template <typename T>
rotation_<T>
::rotation_(const T& yaw, const T& pitch, const T& roll)
{
  using std::sin;
  using std::cos;
  // compute the rotation from North-East-Down (NED) coordinates to
  // East-North-Up coordinates (ENU). It is a 180 degree rotation about
  // the axis [1/sqrt(2), 1/sqrt(2), 0]
  const double root_two = boost::math::constants::root_two<double>();
  const T inv_root_two = static_cast<T>(1.0/root_two);
  const rotation_<T> Rned2enu(Eigen::Quaternion<T>(0, inv_root_two, inv_root_two, 0));
  const double half_x = 0.5 * static_cast<double>(-roll);
  const double half_y = 0.5 * static_cast<double>(-pitch);
  const double half_z = 0.5 * static_cast<double>(-yaw);
  rotation_<T> Rx(Eigen::Quaternion<T>(T(cos(half_x)), T(sin(half_x)), 0, 0));
  rotation_<T> Ry(Eigen::Quaternion<T>(T(cos(half_y)), 0, T(sin(half_y)), 0));
  rotation_<T> Rz(Eigen::Quaternion<T>(T(cos(half_z)), 0, 0, T(sin(half_z))));
  *this = Rx * Ry * Rz * Rned2enu;
}

/// Constructor - from a matrix
/**
 * requires orthonormal matrix with +1 determinant
 */
template <typename T>
rotation_<T>
::rotation_(const Eigen::Matrix<T,3,3>& rot)
{
  q_ = Eigen::Quaternion<T>(rot);
}

/// Convert to a 3x3 matrix
template <typename T>
rotation_<T>
::operator Eigen::Matrix<T,3,3>() const
{
  return q_.toRotationMatrix();
}

/// Returns the axis of rotation
template <typename T>
Eigen::Matrix<T,3,1>
rotation_<T>
::axis() const
{
  Eigen::Matrix<T,3,1> dir(q_.x(), q_.y(), q_.z());
  T mag = dir.norm();
  if (mag == T(0))
  {
    return Eigen::Matrix<T,3,1>(0,0,1);
  }
  return dir / mag;
}

/// Returns the angle of the rotation in radians about the axis
template <typename T>
T
rotation_<T>
::angle() const
{
  const double i = Eigen::Matrix<T,3,1>(q_.x(), q_.y(), q_.z()).norm();
  const double r = q_.w();
  T a = static_cast<T>(2.0 * std::atan2(i, r));
  const T pi = boost::math::constants::pi<T>();
  const T two_pi = static_cast<T>(2) * boost::math::constants::pi<T>();
  // make sure computed angle lies within a sensible range,
  // i.e. -pi/2 < a < pi/2
  if (a >= pi)
  {
    a -= two_pi;
  }
  if (a <= -pi)
  {
    a += two_pi;
  }
  return a;
}

/// Return the rotation as a Rodrigues vector
template <typename T>
Eigen::Matrix<T,3,1>
rotation_<T>
::rodrigues() const
{
  T angle = this->angle();
  if (angle == 0.0)
  {
    return Eigen::Matrix<T,3,1>(0,0,0);
  }
  return this->axis() * angle;
}

/// Convert to yaw, pitch, and roll
template <typename T>
void
rotation_<T>
::get_yaw_pitch_roll(T& yaw, T& pitch, T& roll) const
{
  Eigen::Matrix<T,3,3> rotM(*this);
  T cos_p = T(std::sqrt(double(rotM(1,2)*rotM(1,2)) + rotM(2,2)*rotM(2,2)));
  yaw   = T(std::atan2(double(rotM(0,0)),double(rotM(0,1))));
  pitch = T(std::atan2(double(rotM(0,2)),double(cos_p)));
  roll  = T(std::atan2(double(-rotM(1,2)),double(-rotM(2,2))));
}

/// Compose two rotations
template <typename T>
rotation_<T>
rotation_<T>
::operator*(const rotation_<T>& rhs) const
{
  return q_ * rhs.q_;
}

/// Rotate a vector
/**
 * \note for a large number of vectors, it is more efficient to
 * create a rotation matrix and use matrix multiplcation
 */
template <typename T>
Eigen::Matrix<T,3,1>
rotation_<T>
::operator*(const Eigen::Matrix<T,3,1>& rhs) const
{
  return q_ * rhs;
}

/// output stream operator for a rotation
template <typename T>
std::ostream&  operator<<(std::ostream& s, const rotation_<T>& r)
{
  s << r.quaternion().coeffs();
  return s;
}

/// input stream operator for a rotation
template <typename T>
std::istream&  operator>>(std::istream& s, rotation_<T>& r)
{
  Eigen::Matrix<T,4,1> q;
  s >> q;
  r = rotation_<T>(q);
  return s;
}

/// Generate a rotation vector that, when applied to A N times, produces B.
template <typename T>
rotation_<T>
interpolate_rotation(rotation_<T> const& A, rotation_<T> const& B, T f)
{
  // rotation from A -> B
  rotation_<T> C = A.inverse() * B;
  // Reduce the angle of rotation by the fraction provided
  return A * rotation_<T>(C.angle() * f, C.axis());
}

/// Generate N evenly interpolated rotations inbetween \c A and \c B.
template <typename T>
void
interpolated_rotations(rotation_<T> const& A, rotation_<T> const& B, size_t n, std::vector< rotation_<T> > & interp_rots)
{
  interp_rots.reserve(interp_rots.capacity() + n);
  size_t denom = n + 1;
  for (size_t i=1; i<denom; ++i)
  {
    interp_rots.push_back(interpolate_rotation<T>(A, B, static_cast<T>(i) / denom));
  }
}

/// \cond DoxygenSuppress
#define INSTANTIATE_ROTATION(T) \
template class MAPTK_LIB_EXPORT rotation_<T>; \
template MAPTK_LIB_EXPORT std::ostream&  operator<<(std::ostream& s, const rotation_<T>& r); \
template MAPTK_LIB_EXPORT std::istream&  operator>>(std::istream& s, rotation_<T>& r); \
template MAPTK_LIB_EXPORT rotation_<T> interpolate_rotation(rotation_<T> const& A, rotation_<T> const& B, T f); \
template MAPTK_LIB_EXPORT void \
interpolated_rotations(rotation_<T> const& A, rotation_<T> const& B, size_t n, std::vector< rotation_<T> > & interp_rots)

INSTANTIATE_ROTATION(double);
INSTANTIATE_ROTATION(float);

#undef INSTANTIATE_ROTATION
/// \endcond

} // end namespace maptk


#pragma once
#include <Eigen/Sparse>

namespace edp
{
namespace internal
{
    inline uint32_t ipow(uint32_t base, uint32_t exp)
    {
        uint32_t result = 1U;
        while(exp != 0)
        {
            if((exp & 1U) != 0)
            {
                result *= base;
            }
            exp >>= 1U;
            base *= base;
        }

        return result;
    }
}

template<typename T> struct TwoSiteTerm
{
    std::pair<uint32_t, uint32_t> sites;
    Eigen::SparseMatrix<T> m;

    TwoSiteTerm(std::pair<uint32_t, uint32_t> p1, const Eigen::SparseMatrix<T>& p2)
        : sites(std::move(p1)), m(p2)
    { }
    TwoSiteTerm(std::pair<uint32_t, uint32_t>&& p1, Eigen::SparseMatrix<T>&& p2) : sites(p1), m(p2)
    { }
};

template<typename T> struct OneSiteTerm
{
    uint32_t site;
    Eigen::SparseMatrix<T> m;

    OneSiteTerm(uint32_t p1, const Eigen::SparseMatrix<T>& p2) : site(p1), m(p2) { }

    OneSiteTerm(uint32_t p1, Eigen::SparseMatrix<T>&& p2) : site(p1), m(p2) { }
};

template<typename T> class LocalHamiltonian
{
private:
    uint32_t numSites_;
    uint64_t d_; // local Hibert dimension

    std::vector<TwoSiteTerm<T>> twoSiteTerms_;
    std::vector<OneSiteTerm<T>> oneSiteTerms_;

    [[nodiscard]] uint32_t swapBaseD(uint32_t idx, uint32_t pos, uint32_t val) const
    {
        uint32_t b = internal::ipow(d_, pos);
        uint32_t upper = (idx / (b * d_)) * d_ + val;
        return upper * b + (idx % b);
    }

public:
    LocalHamiltonian(uint32_t numSites, uint32_t d) : numSites_{numSites}, d_{d} { }

    void clearTerms()
    {
        std::vector<TwoSiteTerm<T>>().swap(twoSiteTerms_);
        std::vector<OneSiteTerm<T>>().swap(oneSiteTerms_);
    }
    [[nodiscard]] uint32_t getNumSites() const { return numSites_; }

    [[nodiscard]] std::map<uint32_t, T> getCol(uint32_t n) const;
    [[nodiscard]] inline std::map<uint32_t, T> operator()(uint32_t n) const { return getCol(n); }

    void addTwoSiteTerm(const std::pair<int, int>& site, Eigen::SparseMatrix<T> m)
    {
        m.makeCompressed();
        twoSiteTerms_.emplace_back(site, std::move(m));
    }
    void addOneSiteTerm(uint32_t site, Eigen::SparseMatrix<T> m)
    {
        m.makeCompressed();
        oneSiteTerms_.emplace_back(site, std::move(m));
    }
};
} // namespace edp

template<typename T> std::map<uint32_t, T> edp::LocalHamiltonian<T>::getCol(uint32_t n) const
{
    using Eigen::SparseMatrix;
    using internal::ipow;

    std::map<uint32_t, T> m;

    for(auto& twoSiteTerm : twoSiteTerms_)
    {
        auto a = (n / ipow(d_, twoSiteTerm.sites.first)) % d_;
        auto b = (n / ipow(d_, twoSiteTerm.sites.second)) % d_;
        // auto col = twoSiteTerm.m.col(a*d_ + b);
        auto col = b * d_ + a;
        for(typename SparseMatrix<T>::InnerIterator it(twoSiteTerm.m, col); it; ++it)
        {
            uint32_t r = it.row();
            uint32_t t = n;
            t = swapBaseD(t, twoSiteTerm.sites.first, r % d_);
            t = swapBaseD(t, twoSiteTerm.sites.second, r / d_);
            m[t] += it.value();
        }
    }
    for(auto& oneSiteTerm : oneSiteTerms_)
    {
        uint32_t a = (n / ipow(d_, oneSiteTerm.site)) % d_;
        // auto col = oneSiteTerm.m.col(a);
        auto col = a;

        for(typename SparseMatrix<T>::InnerIterator it(oneSiteTerm.m, col); it; ++it)
        {
            uint32_t r = it.row();
            uint32_t t = n;
            t = swapBaseD(t, oneSiteTerm.site, r);
            m[t] += it.value();
        }
    }
    return m;
}