//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include "Geomlib_TriMeshSubdivide.h"

#include <iostream>
#include <vector>

#include <Urho3D/Core/Variant.h>

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

#pragma warning(push, 0)
#include <igl/edges.h>
#include <igl/edge_flaps.h>
#include <igl/unique_edge_map.h>
#pragma warning(pop)

#include "ConversionUtilities.h"
#include "TriMesh.h"

#pragma warning(disable : 4244)

// collection of helper functions quarantined in anonymous namespace
namespace {

	// reviewed: CHECK
	int look_up_edge_index(const Eigen::MatrixXi& E, int s, int t)
	{
		int edge_index = -1;
		for (unsigned e = 0; e < E.rows(); ++e) {
			if ((E(e, 0) == s && E(e, 1) == t) || (E(e, 0) == t && E(e, 1) == s)) {
				edge_index = e;
				break;
			}
		}
		return edge_index;
	}

	void face_edge_map(
		const Eigen::MatrixXi& F,
		const Eigen::MatrixXi& E,
		Eigen::MatrixXi& FE)
	{
		FE.setZero(F.rows(), 3);

		Eigen::VectorXi EMAP;
		Eigen::MatrixXi NE, EF, EI;
		igl::edge_flaps(F, NE, EMAP, EF, EI);

		for (int f = 0; f < F.rows(); ++f) {
			for (int i = 0; i < 3; ++i) {
				int j = (i + 1) % 3;
				int k = (i + 2) % 3;
				int r, s, t;
				r = F(f, i);
				s = F(f, j);
				t = F(f, k);
				int e = look_up_edge_index(E, s, t);
				FE(f, i) = e;
			}
		}
		return;
	}

	// If a row of F has entries
	//   a, b, c
	// Then the corresponding row for FM has entries
	//   i, j, k
	// where
	//   i is the index (into edge-subdivided vertex matrix) of the midpoint of the edge joining (b,c)
	//   j is the index (into edge-subdivided vertex matrix) of the midpoint of the edge joining (c,a)
	//   k is the index (into edge-subdivided vertex matrix) of the midpoint of the edge joining (a,b)
	void face_edge_midpoint_map(
		int numVertices,
		const Eigen::MatrixXi& F,
		const Eigen::MatrixXi& E,
		Eigen::MatrixXi& FM)
	{
		FM.setZero(F.rows(), 3);

		for (int f = 0; f < F.rows(); ++f) {
			for (int i = 0; i < 3; ++i) {
				int j = (i + 1) % 3;
				int k = (i + 2) % 3;
				int r, s, t;
				r = F(f, i);
				s = F(f, j);
				t = F(f, k);
				int e = look_up_edge_index(E, s, t);
				FM(f, i) = e + numVertices;
			}
		}
		return;
	}

	void subdivide_mesh(
		const Eigen::MatrixXd& OV,
		const Eigen::MatrixXi& OF,
		Eigen::MatrixXd& NV,
		Eigen::MatrixXi& NF)
	{
		Eigen::MatrixXi E;
		Eigen::MatrixXi uE;
		Eigen::VectorXi EMAP;
		std::vector<std::vector<int> > uE2E;
		igl::unique_edge_map(OF, E, uE, EMAP, uE2E);

		// add midpoints to edge-subdivided vertex matrix NV
		NV.setZero(OV.rows() + uE.rows(), 3);
		NV.block(0, 0, OV.rows(), 3) = OV;
		for (int e = 0; e < uE.rows(); ++e) {
			int v, w;
			v = uE(e, 0);
			w = uE(e, 1);
			NV.row(OV.rows() + e) = 0.5 * (OV.row(v) + OV.row(w));
		}

		// compute subdivided face data into NF
		NF.setZero(4 * OF.rows(), 3);
		for (int f = 0; f < OF.rows(); ++f) {
			int v0, v1, v2;
			v0 = OV.rows() + EMAP(f);
			v1 = OV.rows() + EMAP(OF.rows() + f);
			v2 = OV.rows() + EMAP(2 * OF.rows() + f);
			//
			NF.row(4 * f) = Eigen::RowVector3i(v0, v1, v2);
			NF.row(4 * f + 1) = Eigen::RowVector3i(OF(f, 0), v2, v1);
			NF.row(4 * f + 2) = Eigen::RowVector3i(v2, OF(f, 1), v0);
			NF.row(4 * f + 3) = Eigen::RowVector3i(v1, v0, OF(f, 2));
		}
		return;
	}

	void subdivide_mesh(
		const Eigen::MatrixXd& OV,
		const Eigen::MatrixXi& OF,
		const Eigen::VectorXi& KEEP,
		Eigen::MatrixXd& NV,
		Eigen::MatrixXi& NF,
		Eigen::VectorXi& NKEEP)
	{
		Eigen::MatrixXi E;
		Eigen::MatrixXi uE;
		Eigen::VectorXi EMAP;
		std::vector<std::vector<int> > uE2E;
		igl::unique_edge_map(OF, E, uE, EMAP, uE2E);

		NKEEP.setZero(4 * KEEP.rows());

		// add midpoints to edge-subdivided vertex matrix NV
		NV.setZero(OV.rows() + uE.rows(), 3);
		NV.block(0, 0, OV.rows(), 3) = OV;
		for (int e = 0; e < uE.rows(); ++e) {
			int v, w;
			v = uE(e, 0);
			w = uE(e, 1);
			NV.row(OV.rows() + e) = 0.5 * (OV.row(v) + OV.row(w));
		}

		// compute subdivided face data into NF
		NF.setZero(4 * OF.rows(), 3);
		for (int f = 0; f < OF.rows(); ++f) {
			int v0, v1, v2;
			v0 = OV.rows() + EMAP(f);
			v1 = OV.rows() + EMAP(OF.rows() + f);
			v2 = OV.rows() + EMAP(2 * OF.rows() + f);
			//
			NF.row(4 * f) = Eigen::RowVector3i(v0, v1, v2);
			NF.row(4 * f + 1) = Eigen::RowVector3i(OF(f, 0), v2, v1);
			NF.row(4 * f + 2) = Eigen::RowVector3i(v2, OF(f, 1), v0);
			NF.row(4 * f + 3) = Eigen::RowVector3i(v1, v0, OF(f, 2));
			int val = KEEP(f);
			NKEEP(4 * f) = val;
			NKEEP(4 * f + 1) = val;
			NKEEP(4 * f + 2) = val;
			NKEEP(4 * f + 3) = val;
		}
		return;
	}

	void double_subdivide_mesh(
		const Eigen::MatrixXd& OV,
		const Eigen::MatrixXi& OF,
		Eigen::MatrixXd& NV,
		Eigen::MatrixXi& NF)
	{
		Eigen::MatrixXd V;
		Eigen::MatrixXi F;
		subdivide_mesh(OV, OF, V, F);
		subdivide_mesh(V, F, NV, NF);

		return;
	}

	void subdivide_mesh(
		const Eigen::MatrixXd& OV,
		const Eigen::MatrixXi& OF,
		const Eigen::VectorXi& KEEP,
		Eigen::MatrixXd& NV,
		Eigen::MatrixXi& NF,
		Eigen::VectorXi& NKEEP,
		int steps)
	{
		Eigen::MatrixXd V = OV;
		Eigen::MatrixXi F = OF;
		Eigen::VectorXi nKEEP = KEEP;
		for (int i = 0; i < steps; ++i) {
			subdivide_mesh(V, F, nKEEP, NV, NF, NKEEP);
			V.setZero(NV.rows(), 3);
			F.setZero(NF.rows(), 3);
			nKEEP.setZero(NKEEP.rows());
			V = NV;
			F = NF;
			nKEEP = NKEEP;
		}
		return;
	}

	///////////////////////////////////////////////////////////////////////////////////////////////

	// Inputs:
	//     V: #V by 3 list of vertices
	//     F: #F by 3 list of faces into V
	//     KEEP: #F by 1 list taking values -1 (in shadow), 0 (on shadow-light boundary), 1 (in sunlight)
	// Outputs:
	//     NV: #NV by 3 list of vertices in the subdivided mesh
	//     NF: #NF by 3 list of faces (into NV) of the subdivided mesh
	//     NKEEP: #NF by 1 list taking values -1, 0, 1
	void subdivide_step(
		const Eigen::MatrixXd& V,
		const Eigen::MatrixXi& F,
		const Eigen::VectorXi& KEEP,
		Eigen::MatrixXd& NV,
		Eigen::MatrixXi& NF,
		Eigen::VectorXi& NKEEP)
	{
		Eigen::VectorXi nKEEP = KEEP;

		// compute adjacency data
		Eigen::MatrixXi E;
		Eigen::MatrixXi uE;
		Eigen::VectorXi EMAP;
		std::vector<std::vector<int> > uE2E;
		igl::unique_edge_map(F, E, uE, EMAP, uE2E);

		// set up the new vertex matrix
		// make it as large as it could possibly need to be, i.e., in case every face gets subdivided
		NV.setZero(V.rows() + uE.rows(), 3);
		NV.block(0, 0, V.rows(), 3) = V;
		// set up the new face matrix
		// make it as large as it could possibly need to be, i.e., in case every face gets subdivided
		NF.resize(4 * F.rows(), 3);
		NF = Eigen::MatrixXi::Constant(4 * F.rows(), 3, -1);
		NF.block(0, 0, F.rows(), 3) = F;
		NKEEP.setZero(NF.rows());

		// FIRST PASS of FACE SUBDIVISION

		// Rows of SPLIT correspond to rows of uE (unordered edges).
		// If row e of SPLIT has value -1 then row e (index into uE) has not yet been split.
		// If row e of SPLIT has non-negative value i then edge e has been split,
		// in the sense that its midpoint has been added to the updated vertex matrix
		// and its index there is i.
		Eigen::VectorXi SPLIT = Eigen::VectorXi::Constant(uE.rows(), -1);

		int facesDivided = 0;
		int edgesSplit = 0;
		std::vector<int> undivided_faces;
		for (int f = 0; f < F.rows(); ++f) {
			// indices into V (and therefore into NV) of vertices of face f
			int i, j, k;
			i = F(f, 0);
			j = F(f, 1);
			k = F(f, 2);
			// indices into uE of edges of face f
			int e0, e1, e2;
			e0 = EMAP(f);
			e1 = EMAP(F.rows() + f);
			e2 = EMAP(2 * F.rows() + f);
			// indices into NV of midpoints of edges e0, e1, e2
			int m0, m1, m2;

			if (nKEEP(f) == 0) {
				// split edges with indices e0, e1, e2 if they have not already been split

				// split e0 if necessary
				int v = SPLIT(e0);
				m0 = v;
				if (v == -1) {
					v = V.rows() + edgesSplit;
					SPLIT(e0) = v;
					m0 = v;
					// do the split
					NV.row(v) = 0.5 * (V.row(uE(e0, 0)) + V.row(uE(e0, 1)));
					edgesSplit++;
				}
				// split e1 if necessary
				v = SPLIT(e1);
				m1 = v;
				if (v == -1) {
					v = V.rows() + edgesSplit;
					SPLIT(e1) = v;
					m1 = v;
					// do the split
					NV.row(v) = 0.5 * (V.row(uE(e1, 0)) + V.row(uE(e1, 1)));
					edgesSplit++;
				}
				// split e2 if necessary
				v = SPLIT(e2);
				m2 = v;
				if (v == -1) {
					v = V.rows() + edgesSplit;
					SPLIT(e2) = v;
					m2 = v;
					// do the split
					NV.row(v) = 0.5 * (V.row(uE(e2, 0)) + V.row(uE(e2, 1)));
					edgesSplit++;
				}

				// Now all the edges are split (whether we did it just now or it was split at a previous face).
				// face f has vertices with indices i,j,k into NV
				// midpoints of edges have indices m0,m1,m2 into NV
				// and the four faces should go into NF with indices
				//   f (inner face overwrites big original face)
				//   f0, f1, f2
				int f0, f1, f2;
				f0 = F.rows() + 3 * facesDivided;
				f1 = f0 + 1;
				f2 = f0 + 2;
				facesDivided++;
				NF.row(f) = Eigen::RowVector3i(m0, m1, m2);
				NF.row(f0) = Eigen::RowVector3i(i, m2, m1);
				NF.row(f1) = Eigen::RowVector3i(m2, j, m0);
				NF.row(f2) = Eigen::RowVector3i(m1, m0, k);
			}
			else {
				undivided_faces.push_back(f);
			}
		}

		// clean up new vertex matrix NV
		NV.conservativeResize(V.rows() + edgesSplit, 3);

		// SECOND PASS of FACE SUBDIVISION, i.e., clean up

		for (int i = 0; i < undivided_faces.size(); ++i) {
			int f = undivided_faces[i];
			int e0, e1, e2;
			e0 = EMAP(f);
			e1 = EMAP(f + F.rows());
			e2 = EMAP(f + 2 * F.rows());
			int m0, m1, m2;
			m0 = SPLIT(e0);
			m1 = SPLIT(e1);
			m2 = SPLIT(e2);
			int v0, v1, v2;
			v0 = F(f, 0);
			v1 = F(f, 1);
			v2 = F(f, 2);
			int f0, f1, f2;
			f0 = F.rows() + 3 * facesDivided;
			f1 = f0 + 1;
			f2 = f0 + 2;
			facesDivided++;

			int l = nKEEP(f);
			NKEEP(f) = l;
			NKEEP(f0) = l;
			NKEEP(f1) = l;
			NKEEP(f2) = l;

			if (m0 == m1 && m1 == m2) {
				// face has had NO edges split
			}
			else if (m0 == m1) {
				// face has had only edge e2 split
				NF.row(f) = Eigen::RowVector3i(v0, m2, v2);
				NF.row(f0) = Eigen::RowVector3i(m2, v1, v2);
			}
			else if (m1 == m2) { // -1, -1, m0
								 // face has had only edge e0 split
				NF.row(f) = Eigen::RowVector3i(v1, m0, v0);
				NF.row(f0) = Eigen::RowVector3i(m0, v2, v0);
			}
			else if (m2 == m0) { // -1, -1, m1
								 // face has had only edge e1 split
				NF.row(f) = Eigen::RowVector3i(v2, m1, v1);
				NF.row(f0) = Eigen::RowVector3i(m1, v0, v1);
			}
			else {
				if (m0 == -1) {
					// face has had edges e1, e2 split
					NF.row(f) = Eigen::RowVector3i(v1, v2, m2);
					NF.row(f0) = Eigen::RowVector3i(m2, v2, m1);
					NF.row(f1) = Eigen::RowVector3i(m2, m1, v0);
				}
				else if (m1 == -1) {
					// face has had edges e0, e2 split
					NF.row(f) = Eigen::RowVector3i(v2, v0, m0);
					NF.row(f0) = Eigen::RowVector3i(m0, v0, m2);
					NF.row(f1) = Eigen::RowVector3i(m0, m2, v1);
				}
				else if (m2 == -1) {
					// face has had edges e0, e1 split
					NF.row(f) = Eigen::RowVector3i(v0, v1, m1);
					NF.row(f0) = Eigen::RowVector3i(m1, v1, m0);
					NF.row(f1) = Eigen::RowVector3i(m1, m0, v2);
				}
				else {
					// all three edges e0, e1, e2 have been split
					// face f has vertices with indices v0, v1, v2 into NV
					// midpoints of edges have indices m0, m1, m2 into NV
					// and the four faces should go into NF with indices
					//   f (inner face overwrites big original face)
					//   f0, f1, f2
					NF.row(f) = Eigen::RowVector3i(m0, m1, m2);
					NF.row(f0) = Eigen::RowVector3i(v0, m2, m1);
					NF.row(f1) = Eigen::RowVector3i(m2, v1, m0);
					NF.row(f2) = Eigen::RowVector3i(m1, m0, v2);
				}
			}

		}

		// clean up new face matrix NF and new kept faces vector NKEEP
		std::vector<int> faces;
		std::vector<int> keep;
		for (int f = 0; f < NF.rows(); ++f) {
			if (NF(f, 0) != -1) {
				faces.push_back(NF(f, 0));
				faces.push_back(NF(f, 1));
				faces.push_back(NF(f, 2));
				keep.push_back(NKEEP(f));
			}
		}
		int numFaces = faces.size() / 3;
		NF.resize(numFaces, 3);
		NKEEP.resize(numFaces);
		for (int f = 0; f < NF.rows(); ++f) {
			NF(f, 0) = faces[3 * f + 0];
			NF(f, 1) = faces[3 * f + 1];
			NF(f, 2) = faces[3 * f + 2];
			NKEEP(f) = keep[f];
		}

		return;
	}

	void smart_subdivide_mesh(
		const Eigen::MatrixXd& OV,
		const Eigen::MatrixXi& OF,
		Eigen::MatrixXd& NV,
		Eigen::MatrixXi& NF,
		Eigen::VectorXi& NKEEP,
		int steps)
	{

		Eigen::MatrixXd V;
		Eigen::MatrixXi F;
		Eigen::VectorXi nKEEP;

		Eigen::VectorXi KEEP = Eigen::VectorXi::Constant(OF.rows(), 0);
		subdivide_step(OV, OF, KEEP, NV, NF, NKEEP);

		if (steps > 1) {
			V.resize(NV.rows(), 3);
			V = NV;
			F.resize(NF.rows(), 3);
			F = NF;
			nKEEP.resize(NKEEP.rows());
			nKEEP = NKEEP;
		}

		for (int i = 1; i < steps; ++i) {
			subdivide_step(V, F, nKEEP, NV, NF, NKEEP);
			// update intermediate matrices
			V.resize(NV.rows(), 3);
			V = NV;
			F.resize(NF.rows(), 3);
			F = NF;
			nKEEP.resize(NKEEP.rows());
			nKEEP = NKEEP;
		}
		return;
	}

	void subdivide_mesh(
		const Eigen::MatrixXd& OV,
		const Eigen::MatrixXi& OF,
		Eigen::MatrixXd& NV,
		Eigen::MatrixXi& NF,
		int steps)
	{
		Eigen::MatrixXd V = OV;
		Eigen::MatrixXi F = OF;
		for (int i = 0; i < steps; ++i) {
			subdivide_mesh(V, F, NV, NF);
			V.setZero(NV.rows(), 3);
			F.setZero(NF.rows(), 3);
			V = NV;
			F = NF;
		}
		return;
	}
}

bool Geomlib::TriMeshSubdivide(
	const Urho3D::Variant& meshIn,
	int steps,
	Urho3D::Variant& meshOut
)
{
	if (!TriMesh_Verify(meshIn)) {
		meshOut = Urho3D::Variant();
		return false;
	}

	Eigen::MatrixXf V;
	Eigen::MatrixXi F;
	IglMeshToMatrices(meshIn, V, F);

	Eigen::MatrixXd Vd = IglFloatToDouble(V);
	Eigen::MatrixXd NVd;
	Eigen::MatrixXi NF;

	subdivide_mesh(Vd, F, NVd, NF, steps);

	Eigen::MatrixXf NV = IglDoubleToFloat(NVd);

	meshOut = TriMesh_Make(NV, NF);
	return true;
}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2007.

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

#ifndef BOOST_STATS_FIND_SCALE_HPP
#define BOOST_STATS_FIND_SCALE_HPP

#include <boost/math/distributions/fwd.hpp> // for all distribution signatures.
#include <boost/math/distributions/complement.hpp>
#include <boost/math/policies/policy.hpp>
// using boost::math::policies::policy;
#include <boost/math/tools/traits.hpp>
#include <boost/static_assert.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/policies/error_handling.hpp>
// using boost::math::complement; // will be needed by users who want complement,
// but NOT placed here to avoid putting it in global scope.

namespace boost
{
  namespace math
  {
    // Function to find location of random variable z
    // to give probability p (given scale)
    // Applies to normal, lognormal, extreme value, Cauchy, (and symmetrical triangular),
    // distributions that have scale.
    // BOOST_STATIC_ASSERTs, see below, are used to enforce this.

    template <class Dist, class Policy>
    inline
      typename Dist::value_type find_scale( // For example, normal mean.
      typename Dist::value_type z, // location of random variable z to give probability, P(X > z) == p.
      // For example, a nominal minimum acceptable weight z, so that p * 100 % are > z
      typename Dist::value_type p, // probability value desired at x, say 0.95 for 95% > z.
      typename Dist::value_type location, // location parameter, for example, normal distribution mean.
      const Policy& pol
      )
    {
#if !defined(BOOST_NO_SFINAE) && !BOOST_WORKAROUND(__SUNPRO_CC, BOOST_TESTED_AT(0x590))
      BOOST_STATIC_ASSERT(::boost::math::tools::is_distribution<Dist>::value);
      BOOST_STATIC_ASSERT(::boost::math::tools::is_scaled_distribution<Dist>::value);
#endif
      static const char* function = "boost::math::find_scale<Dist, Policy>(%1%, %1%, %1%, Policy)";

      if(!(boost::math::isfinite)(p) || (p < 0) || (p > 1))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "Probability parameter was %1%, but must be >= 0 and <= 1!", p, pol);
      }
      if(!(boost::math::isfinite)(z))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale z parameter was %1%, but must be finite!", z, pol);
      }
      if(!(boost::math::isfinite)(location))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale location parameter was %1%, but must be finite!", location, pol);
      }

      //cout << "z " << z << ", p " << p << ",  quantile(Dist(), p) "
      //<< quantile(Dist(), p) << ", z - mean " << z - location
      //<<", sd " << (z - location)  / quantile(Dist(), p) << endl;

      //quantile(N01, 0.001) -3.09023
      //quantile(N01, 0.01) -2.32635
      //quantile(N01, 0.05) -1.64485
      //quantile(N01, 0.333333) -0.430728
      //quantile(N01, 0.5) 0
      //quantile(N01, 0.666667) 0.430728
      //quantile(N01, 0.9) 1.28155
      //quantile(N01, 0.95) 1.64485
      //quantile(N01, 0.99) 2.32635
      //quantile(N01, 0.999) 3.09023

      typename Dist::value_type result =
        (z - location)  // difference between desired x and current location.
        / quantile(Dist(), p); // standard distribution.

      if (result <= 0)
      { // If policy isn't to throw, return the scale <= 0.
        policies::raise_evaluation_error<typename Dist::value_type>(function,
          "Computed scale (%1%) is <= 0!" " Was the complement intended?",
          result, Policy());
      }
      return result;
    } // template <class Dist, class Policy> find_scale

    template <class Dist>
    inline // with default policy.
      typename Dist::value_type find_scale( // For example, normal mean.
      typename Dist::value_type z, // location of random variable z to give probability, P(X > z) == p.
      // For example, a nominal minimum acceptable z, so that p * 100 % are > z
      typename Dist::value_type p, // probability value desired at x, say 0.95 for 95% > z.
      typename Dist::value_type location) // location parameter, for example, mean.
    { // Forward to find_scale using the default policy.
      return (find_scale<Dist>(z, p, location, policies::policy<>()));
    } // find_scale

    template <class Dist, class Real1, class Real2, class Real3, class Policy>
    inline typename Dist::value_type find_scale(
      complemented4_type<Real1, Real2, Real3, Policy> const& c)
    {
      //cout << "cparam1 q " << c.param1 // q
      //  << ", c.dist z " << c.dist // z
      //  << ", c.param2 l " << c.param2 // l
      //  << ", quantile (Dist(), c.param1 = q) "
      //  << quantile(Dist(), c.param1) //q
      //  << endl;

#if !defined(BOOST_NO_SFINAE) && !BOOST_WORKAROUND(__SUNPRO_CC, BOOST_TESTED_AT(0x590))
      BOOST_STATIC_ASSERT(::boost::math::tools::is_distribution<Dist>::value);
      BOOST_STATIC_ASSERT(::boost::math::tools::is_scaled_distribution<Dist>::value);
#endif
      static const char* function = "boost::math::find_scale<Dist, Policy>(complement(%1%, %1%, %1%, Policy))";

      // Checks on arguments, as not complemented version,
      // Explicit policy.
      typename Dist::value_type q = c.param1;
      if(!(boost::math::isfinite)(q) || (q < 0) || (q > 1))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "Probability parameter was %1%, but must be >= 0 and <= 1!", q, c.param3);
      }
      typename Dist::value_type z = c.dist;
      if(!(boost::math::isfinite)(z))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale z parameter was %1%, but must be finite!", z, c.param3);
      }
      typename Dist::value_type location = c.param2;
      if(!(boost::math::isfinite)(location))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale location parameter was %1%, but must be finite!", location, c.param3);
      }

      typename Dist::value_type result =
        (c.dist - c.param2)  // difference between desired x and current location.
        / quantile(complement(Dist(), c.param1));
      //     (  z    - location) / (quantile(complement(Dist(),  q))
      if (result <= 0)
      { // If policy isn't to throw, return the scale <= 0.
        policies::raise_evaluation_error<typename Dist::value_type>(function,
          "Computed scale (%1%) is <= 0!" " Was the complement intended?",
          result, Policy());
      }
      return result;
    } // template <class Dist, class Policy, class Real1, class Real2, class Real3> typename Dist::value_type find_scale

    // So the user can start from the complement q = (1 - p) of the probability p,
    // for example, s = find_scale<normal>(complement(z, q, l));

    template <class Dist, class Real1, class Real2, class Real3>
    inline typename Dist::value_type find_scale(
      complemented3_type<Real1, Real2, Real3> const& c)
    {
      //cout << "cparam1 q " << c.param1 // q
      //  << ", c.dist z " << c.dist // z
      //  << ", c.param2 l " << c.param2 // l
      //  << ", quantile (Dist(), c.param1 = q) "
      //  << quantile(Dist(), c.param1) //q
      //  << endl;

#if !defined(BOOST_NO_SFINAE) && !BOOST_WORKAROUND(__SUNPRO_CC, BOOST_TESTED_AT(0x590))
      BOOST_STATIC_ASSERT(::boost::math::tools::is_distribution<Dist>::value);
      BOOST_STATIC_ASSERT(::boost::math::tools::is_scaled_distribution<Dist>::value);
#endif
      static const char* function = "boost::math::find_scale<Dist, Policy>(complement(%1%, %1%, %1%, Policy))";

      // Checks on arguments, as not complemented version,
      // default policy policies::policy<>().
      typename Dist::value_type q = c.param1;
      if(!(boost::math::isfinite)(q) || (q < 0) || (q > 1))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "Probability parameter was %1%, but must be >= 0 and <= 1!", q, policies::policy<>());
      }
      typename Dist::value_type z = c.dist;
      if(!(boost::math::isfinite)(z))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale z parameter was %1%, but must be finite!", z, policies::policy<>());
      }
      typename Dist::value_type location = c.param2;
      if(!(boost::math::isfinite)(location))
      {
        return policies::raise_domain_error<typename Dist::value_type>(
          function, "find_scale location parameter was %1%, but must be finite!", location, policies::policy<>());
      }

      typename Dist::value_type result =
        (z - location)  // difference between desired x and current location.
        / quantile(complement(Dist(), q));
      //     (  z    - location) / (quantile(complement(Dist(),  q))
      if (result <= 0)
      { // If policy isn't to throw, return the scale <= 0.
        policies::raise_evaluation_error<typename Dist::value_type>(function,
          "Computed scale (%1%) is <= 0!" " Was the complement intended?",
          result, policies::policy<>()); // This is only the default policy - also Want a version with Policy here.
      }
      return result;
    } // template <class Dist, class Real1, class Real2, class Real3> typename Dist::value_type find_scale

  } // namespace boost
} // namespace math

#endif // BOOST_STATS_FIND_SCALE_HPP


/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Panel splitting
// Split a panel and compute AIC on each sub-panel
//
// Inputs:
// - idP: influencing panel index
// - idF: target field cell index
// - x0: x - coordinate of first corner point of panel in local axes
// - x1: x - coordinate of second corner point of panel in local axes
// - x2: x - coordinate of third corner point of panel in local axes
// - x3: x - coordinate of fourth corner point of panel in local axes
// - y0: y - coordinate of first corner point of panel in local axes
// - y1: y - coordinate of second corner point of panel in local axes
// - y2: y - coordinate of third corner point of panel in local axes
// - y3: y - coordinate of fourth corner point of panel in local axes
// - bPan: body panels (structure)
// - fPan: field panels (structure)
// - sp: sub-panels (structure)

#include <iostream>
#include <Eigen/Dense>
#include <array>
#include "split_panel.h"
#include "infcB.h"

#define NDIM 3
#define NSING 2

using namespace std;
using namespace Eigen;

array<RowVectorXd,NSING> split_panel(int idP, int idF,
                                  double x0, double x1, double x2, double x3, double y0, double y1, double y2, double y3,
                                  Network &bPan, Field &fPan, Subpanel &sp) {

    //// Initialization
    int idx; // counter
    double a0, a1 ,b0, b1; // interpolation coefficients
    VectorXd xV0, xV1, xV2, xV3, yV0, yV1, yV2, yV3; // sub-panel vertices
    xV0.resize(sp.NS);
    xV1.resize(sp.NS);
    xV2.resize(sp.NS);
    xV3.resize(sp.NS);
    yV0.resize(sp.NS);
    yV1.resize(sp.NS);
    yV2.resize(sp.NS);
    yV3.resize(sp.NS);
    Vector3d tgt; // target points
    tgt.resize(NDIM);

    array <RowVectorXd, NSING> coeff; // returned and temporary coefficients
    array <double, NSING> coeffT;
    for (int i = 0; i < NSING; i++)
        coeff[i].resize(sp.NS);

    //// Change coordinates
    // Cell center
    tgt(0) = bPan.l(idP,0)*fPan.CG(idF,0) + bPan.l(idP,1)*fPan.CG(idF,1) + bPan.l(idP,2)*fPan.CG(idF,2);
    tgt(1) = bPan.p(idP,0)*fPan.CG(idF,0) + bPan.p(idP,1)*fPan.CG(idF,1) + bPan.p(idP,2)*fPan.CG(idF,2);
    tgt(2) = bPan.n(idP,0)*(fPan.CG(idF,0) - bPan.CG(idP,0)) + bPan.n(idP,1)*(fPan.CG(idF,1) - bPan.CG(idP,1))
               + bPan.n(idP,2)*(fPan.CG(idF,2) - bPan.CG(idP,2));

    //// Split panel
    // Compute sub-panels vertices
    idx = 0;
    for (int j = 0; j < sp.NSs; j++) {
        for (int i = 0; i < sp.NSs; i++) {
            // Compute weight factors
            a0 = (double) i/sp.NSs;
            a1 = (double) (i+1)/sp.NSs;
            b0 = (double) j/sp.NSs;
            b1 = (double) (j+1)/sp.NSs;
            // Compute new vertices
            xV0(idx) = (1-b0)*((1-a0)*x0 + a0*x1) + b0*(a0*x2 +(1-a0)*x3);
            xV1(idx) = (1-b0)*((1-a1)*x0 + a1*x1) + b0*(a1*x2 +(1-a1)*x3);
            xV2(idx) = (1-b1)*((1-a1)*x0 + a1*x1) + b1*(a1*x2 +(1-a1)*x3);
            xV3(idx) = (1-b1)*((1-a0)*x0 + a0*x1) + b1*(a0*x2 +(1-a0)*x3);
            yV0(idx) = (1-b0)*((1-a0)*y0 + a0*y1) + b0*(a0*y2 +(1-a0)*y3);
            yV1(idx) = (1-b0)*((1-a1)*y0 + a1*y1) + b0*(a1*y2 +(1-a1)*y3);
            yV2(idx) = (1-b1)*((1-a1)*y0 + a1*y1) + b1*(a1*y2 +(1-a1)*y3);
            yV3(idx) = (1-b1)*((1-a0)*y0 + a0*y1) + b1*(a0*y2 +(1-a0)*y3);
            idx ++;
        }
    }

    //// Compute AIC
    for (int j = 0; j < sp.NS; j++) {
        // call infcBB
        coeffT = infcB(0, 0, 1, tgt(0), tgt(1), tgt(2), xV0(j), yV0(j), xV1(j), yV1(j), xV2(j), yV2(j), xV3(j), yV3(j));
        // Store to return
        coeff[0](j) = coeffT[0];
        coeff[1](j) = coeffT[1];
    }
    return coeff;
}


#ifndef AQ1_MAIN_HPP
#define AQ1_MAIN_HPP

#include <iostream>
#include <memory>
#include <optional>
#include <string>
#include <variant>
#include <vector>

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

namespace mp = boost::multiprecision;
using MPInt = mp::cpp_int;
using MPRational = boost::rational<MPInt>;
using MPFloat = mp::cpp_dec_float_100;

enum class TOK {
    NUMLIT,  // Numeric literal
    OWARI,   // End of file
    PLUS,
    MINUS,
    STAR,
    SLASH,
    PERCENT,
    NEWLINE,
    LPAREN,
    RPAREN,
    IDENT,
    COMMA,
    FMTLIT,
};

struct FormatLiteral {
    bool is_signed, is_zero_padded;
    int field_width, precision;
};

struct Token {
    TOK kind;
    std::variant<std::monostate, MPRational, std::string, FormatLiteral> data;

    static const Token &owari()
    {
        static const Token tok{TOK::OWARI, std::monostate{}};
        return tok;
    }
};

class Lex {
private:
    std::istream &is_;
    std::optional<Token> pending_;
    std::vector<char> history_;

    int getch();
    void putback(int ch);
    Token next_token();

public:
    Lex(std::istream &is) : is_(is), pending_(std::nullopt)
    {
    }

    // Get the next non-NEWLINE token.
    Token get();
    // Expect the next non-NEWLINE token.
    Token expect(TOK kind);
    // Return if the next non-NEWLINE token's kind is `kind`.
    bool match(TOK kind);

    // Peek the next token. This function will NOT skip NEWLINE.
    Token peek_next();

    std::string clear_history();
};

class ASTNode {
public:
    ASTNode() = default;

    // Thanks to:
    // https://github.com/isocpp/CppCoreGuidelines/blob/master/CppCoreGuidelines.md#Rc-copy-virtual
    // Thanks to:
    // https://github.com/isocpp/CppCoreGuidelines/blob/master/CppCoreGuidelines.md#Rc-five
    virtual ~ASTNode() = default;
    ASTNode(const ASTNode &) = delete;
    ASTNode &operator=(const ASTNode &) = delete;
    ASTNode(ASTNode &&) = delete;
    ASTNode &operator=(ASTNode &&) = delete;

    virtual MPRational eval() const = 0;
};

using ASTNodePtr = std::shared_ptr<ASTNode>;

enum class UNARYOP {
    PLUS,
    MINUS,
};

class UnaryOp : public ASTNode {
private:
    UNARYOP kind_;
    ASTNodePtr src_;

public:
    UnaryOp(UNARYOP kind, ASTNodePtr src) : kind_(kind), src_(std::move(src))
    {
    }

    MPRational eval() const override;
};

enum class BINOP {
    ADD,
    SUB,
    MUL,
    DIV,
};

class BinOp : public ASTNode {
private:
    BINOP kind_;
    ASTNodePtr lhs_, rhs_;

public:
    BinOp(BINOP kind, ASTNodePtr lhs, ASTNodePtr rhs)
        : kind_(kind), lhs_(std::move(lhs)), rhs_(std::move(rhs))
    {
    }

    MPRational eval() const override;
};

class NumImm : public ASTNode {
private:
    MPRational val_;

public:
    NumImm(MPRational val) : val_(std::move(val))
    {
    }

    MPRational eval() const override
    {
        return val_;
    }
};

class FuncCall : public ASTNode {
private:
    std::string name_;
    std::vector<ASTNodePtr> args_;

public:
    FuncCall(std::string name, std::vector<ASTNodePtr> args)
        : name_(std::move(name)), args_(std::move(args))
    {
    }

    MPRational eval() const override;
};

class FormatPrint : public ASTNode {
private:
    ASTNodePtr src_;
    int field_width_, precision_;

public:
    FormatPrint(ASTNodePtr src, int field_width, int precision)
        : src_(std::move(src)), field_width_(field_width), precision_(precision)
    {
    }

    MPRational eval() const override;
};

class Parser {
private:
    Lex &lex_;

    ASTNodePtr parse_primary();
    ASTNodePtr parse_unary();
    ASTNodePtr parse_multiplicative();
    ASTNodePtr parse_additive();
    ASTNodePtr parse_expr();

public:
    Parser(Lex &lex) : lex_(lex)
    {
    }

    ASTNodePtr parse();
};

#endif


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <string>
#include <list>
#include <vector>

using namespace std::string_literals;

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "vlasovpp/field.h"
#include "vlasovpp/complex_field.h"
#include "vlasovpp/weno.h"
#include "vlasovpp/fft.h"
#include "vlasovpp/array_view.h"
#include "vlasovpp/poisson.h"
#include "vlasovpp/rk.h"
#include "vlasovpp/config.h"
#include "vlasovpp/signal_handler.h"
#include "vlasovpp/iteration.h"
#include "vlasovpp/splitting.h"

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Zi(i) (i*f.step.dz+f.range.z_min)
#define Vkx(k) (k*f.step.dvx+f.range.vx_min)
#define Vky(k) (k*f.step.dvy+f.range.vy_min)
#define Vkz(k) (k*f.step.dvz+f.range.vz_min)

auto
maxwellian ( double rho , std::vector<double> u , double T ) {
  return [=](double z,double vx,double vy,double vz) {
    return rho/( std::pow(2.*math::pi<double>()*T,1.5) )*std::exp( -0.5*(SQ(vx-u[0])+SQ(vy-u[1])+SQ(vz-u[2]))/T );
  };
}

int
main ( int argc , char const * argv[] )
{
  std::string p("config.init");
  if ( argc > 1 )
    { p = argv[1]; }
  auto c = config(p);
  c.name = "vmls";

  c.create_output_directory();
  {
    std::ofstream ofconfig( c.output_dir / "config.init" );
    ofconfig << c << "\n";
    ofconfig.close();
  }

/* ------------------------------------------------------------------------- */
  field3d<double> f(boost::extents[c.Nvx][c.Nvy][c.Nvz][c.Nz]);
  complex_field<double,3> hf(boost::extents[c.Nvx][c.Nvy][c.Nvz][c.Nz]);

  const double Kx = 0.5;
  f.range.vx_min = -5.; f.range.vx_max = 5.;
  f.range.vy_min = -5.; f.range.vy_max = 5.;
  f.range.vz_min = -5.; f.range.vz_max = 5.;
  f.range.z_min =  0.;  f.range.z_max = 2./Kx*math::pi<double>();
  f.compute_steps();

  double dt = 0.1;

  ublas::vector<double> vx(c.Nv,0.),vy(c.Nv,0.),vz(c.Nv,0.);
  std::generate( vx.begin() , vx.end() , [&,k=0]() mutable {return (k++)*f.step.dvx+f.range.vx_min;} );
  std::generate( vy.begin() , vy.end() , [&,k=0]() mutable {return (k++)*f.step.dvy+f.range.vy_min;} );
  std::generate( vz.begin() , vz.end() , [&,k=0]() mutable {return (k++)*f.step.dvz+f.range.vz_min;} );

  ublas::vector<double> kx(c.Nx); // beware, Nx need to be odd
  {
    double l = f.range.len_z();
    for ( auto i=0u ; i<c.Nx/2 ; ++i ) { kx[i]      = 2.*math::pi<double>()*i/l; }
    for ( int i=-c.Nx/2 ; i<0 ; ++i ) { kx[c.Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  auto M1 = maxwellian(1.,{0.,0.,0.},1.);
  for (std::size_t k_x=0u ; k_x<f.size(0) ; ++k_x ) {
    for (std::size_t k_y=0u ; k_y<f.size(1) ; ++k_y ) {
      for (std::size_t k_z=0u ; k_z<f.size(2) ; ++k_z ) {
        for (std::size_t i=0u ; i<f.size_x() ; ++i ) {
          f[k_x][k_y][k_z][i] = M1( Zi(i),Vkx(k_x),Vky(k_y),Vkz(k_z) ) * (1. + 0.5*std::cos(Kx*Zi(i)));
        }
        fft::fft(f[k_x][k_y][k_z].begin(),f[k_x][k_y][k_z].end(),hf[k_x][k_y][k_z].begin());
      }
    }
  }

  const double B0 = 1.;
  ublas::vector<double> Ex(c.Nz,0.),Ey(c.Nz,0.);
  ublas::vector<double> jcx(c.Nz,0.),jcy(c.Nz,0.);

  std::vector<double> electric_energy;  electric_energy.reserve(100);
  std::vector<double> electric_energy_x; electric_energy_x.reserve(100);
  std::vector<double> electric_energy_y; electric_energy_y.reserve(100);
  std::vector<double> times; times.reserve(100);

  hybird1dx3dv_b0<double> Lie( f , f.range.len_z() , B0 );
  double current_t = 0.;
  times.push_back(0.);

  auto compute_electric_energy = [&]( const ublas::vector<double> & E ) {
    double electric_energy = 0.;
    for ( const auto & ei : E ) { electric_energy += ei*ei*f.step.dz; }
      return electric_energy;
  };
  double eex=0.,eey=0.;
  eex = compute_electric_energy(Ex); eey = compute_electric_energy(Ey);
  electric_energy_x.push_back(std::sqrt(eex));
  electric_energy_y.push_back(std::sqrt(eey));
  electric_energy.push_back(std::sqrt(eex+eey));

  while ( current_t<c.Tf ) {
    std::cout << "\r" << current_t << " / " << c.Tf << std::flush;

/*
    Lie.H_E_tilde(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }
    Lie.H_jc(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }
    Lie.H_f_tilde(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }
*/

    Lie.H_E(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }

    Lie.H_jc(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }

    Lie.H_f(dt,jcx,jcy,Ex,Ey,hf);
    //for ( int i = 0 ; i<c.Nz ; ++i ) { jcx[i] = 0.; jcy[i] = 0.; }

    eex = compute_electric_energy(Ex); eey = compute_electric_energy(Ey);
    electric_energy.push_back( std::sqrt(eex+eey) );
    electric_energy_x.push_back(std::sqrt(eex));
    electric_energy_y.push_back(std::sqrt(eey));
    current_t += dt;
    times.push_back(current_t);
  }

  auto dt_y = [&,count=0] (auto const& y) mutable {
    std::stringstream ss; ss<<times[count++]<<" "<<y;
    return ss.str();
  };
  c << monitoring::data( "ee_tilde.dat"  , electric_energy   , dt_y );
  c << monitoring::data( "eex_tilde.dat" , electric_energy_x , dt_y );
  c << monitoring::data( "eey_tilde.dat" , electric_energy_y , dt_y );

  return 0;
}


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file iterative_rounding_example.cpp
 * @brief Iterative rounding example
 * This is an example implementation of an algorithm within the Iterative
 * Rounding framework.
 * The implemented algorithm is a vertex cover 2-approximation.
 * @author Piotr Godlewski
 * @version 1.0
 * @date 2014-03-24
 */

//! [Iterative Rounding Problem Example]
#include "paal/iterative_rounding/iterative_rounding.hpp"
#include "paal/utils/floating.hpp"
#include "paal/utils/functors.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/range/iterator_range.hpp>

#include <iostream>
#include <unordered_map>

namespace ir = paal::ir;

template <typename Graph, typename CostMap, typename OutputIter>
class vertex_cover {
public:
    vertex_cover(const Graph & g, CostMap cost_map, OutputIter cover) :
        m_g(g), m_cost_map(cost_map), m_cover(cover) {}

    using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
    using VertexMap = std::unordered_map<Vertex, paal::lp::col_id>;

    const Graph &get_graph() const { return m_g; }

    auto get_cost(Vertex v)->decltype(std::declval<CostMap>()(v)) {
        return m_cost_map(v);
    }

    void bind_col_to_vertex(paal::lp::col_id col, Vertex v) {
        m_vertex_map.insert(typename VertexMap::value_type(v, col));
    }

    paal::lp::col_id vertex_to_column(Vertex v) { return m_vertex_map[v]; }

    void add_to_cover(Vertex v) {
        *m_cover = v;
        ++m_cover;
    }

  private:
    const Graph &m_g;
    CostMap m_cost_map;
    OutputIter m_cover;
    VertexMap m_vertex_map;
};
//! [Iterative Rounding Problem Example]

//! [Iterative Rounding Components Example]
struct vertex_cover_init {
    template <typename Problem, typename LP>
    void operator()(Problem &problem, LP &lp) {
        lp.set_optimization_type(paal::lp::MINIMIZE);

        // variables for vertices
        for (auto v :
             boost::make_iterator_range(vertices(problem.get_graph()))) {
            problem.bind_col_to_vertex(lp.add_column(problem.get_cost(v)), v);
        }

        // x_u + x_v >= 1 for each edge e=(u,v)
        for (auto e : boost::make_iterator_range(edges(problem.get_graph()))) {
            auto x_u = problem.vertex_to_column(source(e, problem.get_graph()));
            auto x_v = problem.vertex_to_column(target(e, problem.get_graph()));
            lp.add_row(x_u + x_v >= 1);
        }
    }
};

struct vertex_cover_set_solution {
    template <typename Problem, typename GetSolution>
    void operator()(Problem &problem, const GetSolution &solution) {
        // add vertices with column value equal 1 to cover
        for (auto v :
             boost::make_iterator_range(vertices(problem.get_graph()))) {
            if (m_compare.e(solution(problem.vertex_to_column(v)), 1)) {
                problem.add_to_cover(v);
            }
        }
    }

private:
    const paal::utils::compare<double> m_compare;
};

using vertex_cover_ir_components =
    ir::IRcomponents<vertex_cover_init, ir::round_condition_greater_than_half,
                     paal::utils::always_false, vertex_cover_set_solution>;
//! [Iterative Rounding Components Example]

//! [Iterative Rounding Example]
int main() {
    using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
        boost::no_property, boost::no_property>;
    using Vertex = boost::graph_traits<Graph>::vertex_descriptor;

    // sample problem
    std::vector<std::pair<int, int>> edges{ { 0, 1 }, { 0, 2 }, { 1, 2 },
                                            { 1, 3 }, { 1, 4 }, { 1, 5 },
                                            { 5, 0 }, { 3, 4 } };
    std::vector<int> costs{ 1, 2, 1, 2, 1, 5 };

    Graph g(edges.begin(), edges.end(), 6);
    auto vertex_costs = paal::utils::make_array_to_functor(costs);
    std::vector<Vertex> result_cover;
    auto insert_iter = std::back_inserter(result_cover);

    vertex_cover<Graph, decltype(vertex_costs), decltype(insert_iter)> problem(
        g, vertex_costs, insert_iter);

    // solve it
    auto result =
        ir::solve_iterative_rounding(problem, vertex_cover_ir_components());

    // print result
    if (result.first == paal::lp::OPTIMAL) {
        std::cout << "Vertices in the cover:" << std::endl;
        for (auto v : result_cover) {
            std::cout << "Vertex " << v << std::endl;
        }
        std::cout << "Cost of the solution: " << *(result.second) << std::endl;
    } else {
        std::cout << "The instance is infeasible" << std::endl;
    }
    paal::lp::glp::free_env();
    return 0;
}
//! [Iterative Rounding Example]


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/xfeatures2d/nonfree.hpp>
#include <opencv2/opencv.hpp>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <Eigen/Core>

#include <chrono>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
using namespace std;
using namespace cv;

void writeResults( const string& filename, const vector<string>& timestamps, const vector<Mat>& Rt )
{
    CV_Assert( timestamps.size() == Rt.size() );

    ofstream file( filename.c_str() );
    if( !file.is_open() )
        return;

    cout.precision(4);
    for( size_t i = 0; i < Rt.size(); i++ )
    {
        const Mat& Rt_curr = Rt[i];
        if( Rt_curr.empty() )
            continue;

        CV_Assert( Rt_curr.type() == CV_64FC1 );

        Mat R = Rt_curr(Rect(0,0,3,3)), rvec;
        Rodrigues(R, rvec);
        double alpha = norm( rvec );
        if(alpha > DBL_MIN)
            rvec = rvec / alpha;

        double cos_alpha2 = std::cos(0.5 * alpha);
        double sin_alpha2 = std::sin(0.5 * alpha);

        rvec *= sin_alpha2;

        CV_Assert( rvec.type() == CV_64FC1 );
        // timestamp tx ty tz qx qy qz qw
        file << timestamps[i] << " " << fixed
             << Rt_curr.at<double>(0,3) << " " << Rt_curr.at<double>(1,3) << " " << Rt_curr.at<double>(2,3) << " "
             << rvec.at<double>(0) << " " << rvec.at<double>(1) << " " << rvec.at<double>(2) << " " << cos_alpha2 << endl;

    }
    file.close();
}

void find_feature_matches(
 const Mat &img_1, const Mat &img_2, std::vector<KeyPoint> &keypoints_1,vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches, const Mat &img_3);
 void find_feature_matches_another(
 const Mat &img_1, const Mat &img_2, std::vector<KeyPoint> &keypoints_1,vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches, const Mat &img_3);

// // 像素坐标转相机归一化坐标
 Point2d pixel2cam(const Point2d &p, const Mat &K);

// BA by g2o
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

Mat bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  const string& filename,
  const vector<string>& timestamps
);

int main(int argc, char **argv) {
	if(argc != 4){
        cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP or FastICP]" << endl;
        return -1;
   }

   vector<string> timestamps;
   vector<Mat> Rts, Rts_ba;

   const string filename = argv[1];
   ifstream file( filename.c_str() );
   if( !file.is_open() )
      return -1;
   char dlmrt = '/';
   size_t pos = filename.rfind(dlmrt);
   string dirname = pos == string::npos ? "" : filename.substr(0, pos) + dlmrt;

   const int timestampLength = 17;
   const int rgbPathLehgth = 17+8;
   const int depthPathLehgth = 17+10;

   float fx = 517.3f, // default
         fy = 516.5f,
         cx = 318.6f,
         cy = 255.3f;
   string datas[793];
   string str1;
   std::getline(file, str1);
    datas[0] = str1;

    for(int i = 1; !file.eof(); i++){
        string str;
        std::getline(file, str);
        datas[i] = str;
        if(str.empty()) break;
        if(str.at(0) == '#') continue; /* comment */
        cout << " previous image: " << datas[i-1] << "\n" << " current image "<< str << endl;
        Mat image, depth, image1, depth1, image2, depth2;
        // if(i > 2) {
        //     string rgbFilename2 = datas[i-2].substr(timestampLength + 1, rgbPathLehgth );
        //     string timestap2 = datas[i-2].substr(0, timestampLength);
        //     string depthFilename2 = datas[i-2].substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );

        //     image2 = imread(dirname + rgbFilename2);
        //     depth2 = imread(dirname + depthFilename2, -1);
        // }

        string rgbFilename1 = datas[i-1].substr(timestampLength + 1, rgbPathLehgth );
        string timestap1 = datas[i-1].substr(0, timestampLength);
        string depthFilename1 = datas[i-1].substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
        image1 = imread(dirname + rgbFilename1);
        depth1 = imread(dirname + depthFilename1, -1);

        string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth );
        string timestap = str.substr(0, timestampLength);
        string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
        image = imread(dirname + rgbFilename);
        depth = imread(dirname + depthFilename, -1);

        // cout << "prev prev " << datas[i-2] << " previous image: " << datas[i-1] << " current image "<< str << endl;
        // CV_Assert(!image.empty());
        // CV_Assert(!depth.empty());
        // CV_Assert(!image1.empty());
        // CV_Assert(!depth1.empty());
        // CV_Assert(depth.type() == CV_16UC1);
        // CV_Assert(depth1.type() == CV_16UC1);

        // if(i > 2){
        //     CV_Assert(!image2.empty());
        //     CV_Assert(!depth2.empty());
        //     CV_Assert(depth2.type() == CV_16UC1);
        // }

        std::vector<KeyPoint> keypoints_1, keypoints_2, key1, key2;
        vector<DMatch> matches;
        Ptr<FeatureDetector> detector = ORB::create();
        detector->detect(image1, key1);
        detector->detect(image, key2);
        if(key1.size() == 0 || key2.size() == 0){
            find_feature_matches_another(image1, image, keypoints_1, keypoints_2, matches, image2);
            cout << "第二個: " <<  "一共找到了" << matches.size() << "组匹配点" << endl;
        }
        else{
            find_feature_matches(image1, image, keypoints_1, keypoints_2, matches, image2);
            cout << "第一個: " <<"一共找到了" << matches.size() << "组匹配点" << endl;
        }

        // 建立3D点
        //Mat d1 = imread(depth1, IMREAD_UNCHANGED);       // 深度图为16位无符号数，单通道图像
        Mat K = (Mat_<double>(3, 3) << 517.3f, 0, 318.6f, 0, 516.5f, 255.3f, 0, 0, 1);
        vector<Point2f> pts_2d_old;
        vector<Point2f> pts_2d;
        int index = 1;
        for (DMatch m:matches) {
                ushort d = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
                // cout << "depth " << d << endl;
                if (d == 0){   // bad depth
                    continue;
                // d = 1;
                }
                // cout << "this is matches: " <<  index << endl;
                float dd = d / 5000.0;
                // cout << "keypoints_1[m.queryIdx].pt " << keypoints_1[m.queryIdx].pt << endl;
                Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
                // cout << "p1.x " << p1.x << " p1.y " << p1.y << endl;
                // cout << "keypoints_2[m.trainIdx].pt " << keypoints_2[m.trainIdx].pt << endl;
                pts_2d_old.push_back(Point2f(p1.x * dd, p1.y * dd));
                pts_2d.push_back(keypoints_2[m.trainIdx].pt);
                index += 1;
            // }
        }
        bool b = false;
        // if(i ==373) {b = true;}
        cout << "2d-2d pairs: " << pts_2d_old.size() << " " << pts_2d.size() << " "  << i <<  endl;
        vector<Scalar> colors;
        cv::RNG rng;
        for(int i = 0; i < 100; i++){
            int r = rng.uniform(0, 256);
            int g = rng.uniform(0, 256);
            int b = rng.uniform(0, 256);
            colors.push_back(Scalar(r,g,b));
        }
        // vector<Point2f> p_old, p_new;
        // cv::goodFeaturesToTrack(image1, p_old, 100, 0.3, 7, Mat(), 7, false, 0.04);
        // // Calculate optical flow
        vector<uchar> status;
        vector<float> err;
        Mat mask = Mat::zeros(image1.size(), image1.type());
        cv::TermCriteria criteria = TermCriteria((TermCriteria::COUNT) + (TermCriteria::EPS), 10, 0.03);
        cv::calcOpticalFlowPyrLK(image, image1, pts_2d_old, pts_2d, status, err, Size(15,15), 2, criteria);
        vector<Point2f> good_new;
        // Visualization part
        for(uint i = 0; i < pts_2d_old.size(); i++){
            // Select good points
            if(status[i] == 1) {
                good_new.push_back(pts_2d[i]);
                // Draw the tracks
                line(mask,pts_2d[i], pts_2d_old[i], colors[i], 2);
                circle(image, pts_2d[i], 5, colors[i], -1);
            }
        }
        // Display the demo
        Mat img;
        cv::add(image, mask, img);
        // if (save) {
        //     string save_path = "./optical_flow_frames/frame_" + to_string(counter) + ".jpg";
        //     imwrite(save_path, img);
        // }
        cv::imshow("flow", img);
        // int keyboard = cv::waitKey(25);
        // if (keyboard == 'q' || keyboard == 27)
        //     break;
        // Update the previous frame and previous points
        image1 = image.clone();
        pts_2d_old = good_new;

//       chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
//       Mat r, t, inliers;
//       solvePnPRansac(pts_3d, pts_2d, K, Mat(), r, t, b, 300, 0.05, 0.95, inliers);
//       // cout << inliers << inliers.size()  << inliers.at<int>(1,0)  << inliers.at<int>(3,0)<< endl;
//       for (int i=0; i<inliers.rows; i++){
//          cout << "inliers -> keypoints_1: " << keypoints_1[inliers.at<int>(i,0)].pt << "inliers -> keypoints_2: " << keypoints_2[inliers.at<int>(i,0)].pt << endl;
//       }
//       // cout << "inliers "<< inliers << endl;
//       cout<<"pnp OK = "<<b<<", inliers point num = "<<inliers.rows<<endl;
//       Mat R;
//       cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
//       chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
//       chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
//       cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;

//       cout << "R=" << endl << R << endl;
//       cout << "t=" << endl << t << endl;
//       timestamps.push_back( timestap );
//       Mat output;
//       hconcat(R, t, output);

//       Mat Rt = Mat::eye(4,4,CV_64FC1);
//       Rt.at<double>(0,0) = R.at<double>(0,0);
//       Rt.at<double>(0,1) = R.at<double>(0,1);
//       Rt.at<double>(0,2) = R.at<double>(0,2);
//       Rt.at<double>(1,0) = R.at<double>(1,0);
//       Rt.at<double>(1,1) = R.at<double>(1,1);
//       Rt.at<double>(1,2) = R.at<double>(1,2);
//       Rt.at<double>(2,0) = R.at<double>(2,0);
//       Rt.at<double>(2,1) = R.at<double>(2,1);
//       Rt.at<double>(2,2) = R.at<double>(2,2);
//       Rt.at<double>(0,3) = t.at<double>(0,0);
//       Rt.at<double>(1,3) = t.at<double>(0,1);
//       Rt.at<double>(2,3) = t.at<double>(0,2);
//       cout << "Rt " << Rt << endl;
//       Rts.push_back(Rt);

//       Mat Rt_ba;

//       VecVector3d pts_3d_eigen;
//       VecVector2d pts_2d_eigen;
//       for (size_t i = 0; i < pts_3d.size(); ++i) {
//          // cout << "vector3d " << Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z) << "vector2d " << Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y) << endl;
//          pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
//          pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
//       }
//       // for (size_t i = 0; i < pts_3d.size(); ++i) {
//       //    for (size_t j = 0; j < 3; ++j) {
//       //       cout << " eigen3d " << pts_3d_eigen[i][j] << cout << " eigen2d "  <<*pts_2d_eigen[i][j];
//       //    }
//       // }
//       cout << "calling bundle adjustment by g2o" << endl;
//       Sophus::SE3d pose_g2o;
//       t1 = chrono::steady_clock::now();
//       Rt_ba = bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o, argv[2], timestamps);
//       Rts_ba.push_back(Rt_ba);
//       t2 = chrono::steady_clock::now();
//       time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
//       cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;
   }
//    writeResults(argv[2], timestamps, Rts);
//    //writeResults(argv[2], timestamps, Rts_ba);

  return 0;
}
void find_feature_matches_another(const Mat &img_1, const Mat &img_2,
                           std::vector<KeyPoint> &keypoints_1,
			                  std::vector<KeyPoint> &keypoints_2,
                           std::vector<DMatch> &matches,
                           const Mat &img_3) {
   Mat descriptors_1, descriptors_2, descriptors_3, descriptors_4;
   Ptr<FeatureDetector> detector = AgastFeatureDetector::create();
   Ptr<DescriptorExtractor> descriptor = AgastFeatureDetector::create();
   Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
   detector->detect(img_1, keypoints_1);
   detector->detect(img_2, keypoints_2);
   cout << "keypoints_1.size() "<< keypoints_1.size() << " keypoints_2.size() " << keypoints_2.size() << endl;
   // if(keypoints_1.size() != 0 && keypoints_2.size() != 0){
      for (int i=0; i< keypoints_1.size(); i++){
         // cout << "keypoint1 " << keypoints_1[i].pt << "keypoint2 " << keypoints_2[i].pt << endl;
      }
      descriptor->compute(img_1, keypoints_1, descriptors_1);
      descriptor->compute(img_2, keypoints_2, descriptors_2);
   // }
   int eee = descriptors_1.empty();
   int ddd = descriptors_2.empty();

   vector<DMatch> match;
   matcher->match(descriptors_1, descriptors_2, match);
   double min_dist = 10000, max_dist = 0;
     for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
     if (dist < min_dist) min_dist = dist;
     if (dist > max_dist) max_dist = dist;
   }
   printf("-- Max dist : %f \n", max_dist);
   printf("-- Min dist : %f \n", min_dist);
   for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 10.0)) {
       matches.push_back(match[i]);
     }
   }
 }
 void find_feature_matches(const Mat &img_1, const Mat &img_2,
                           std::vector<KeyPoint> &keypoints_1,
			                  std::vector<KeyPoint> &keypoints_2,
                           std::vector<DMatch> &matches,
                           const Mat &img_3) {

   Mat descriptors_1, descriptors_2, descriptors_3, descriptors_4;
   Ptr<FeatureDetector> detector = ORB::create();
   Ptr<DescriptorExtractor> descriptor = ORB::create();
   Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
   detector->detect(img_1, keypoints_1);
   detector->detect(img_2, keypoints_2);
   cout << "keypoints_1.size() "<< keypoints_1.size() << " keypoints_2.size() " << keypoints_2.size() << endl;
      for (int i=0; i< keypoints_1.size(); i++){
         // cout << "keypoint1 " << keypoints_1[i].pt << "keypoint2 " << keypoints_2[i].pt << endl;
      }
      descriptor->compute(img_1, keypoints_1, descriptors_1);
      descriptor->compute(img_2, keypoints_2, descriptors_2);
   int eee = descriptors_1.empty();
   int ddd = descriptors_2.empty();
   vector<DMatch> match;
   matcher->match(descriptors_1, descriptors_2, match);
   double min_dist = 10000, max_dist = 0;
     for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
     if (dist < min_dist) min_dist = dist;
     if (dist > max_dist) max_dist = dist;
   }

   printf("-- Max dist : %f \n", max_dist);
   printf("-- Min dist : %f \n", min_dist);
   for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 10.0)) {
       matches.push_back(match[i]);
     }
   }
 }

 Point2d pixel2cam(const Point2d &p, const Mat &K) {
   return Point2d
     (
       (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
       (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
     );
 }

/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
   public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

   virtual void setToOriginImpl() override {
      _estimate = Sophus::SE3d();
   }

   /// left multiplication on SE3
   virtual void oplusImpl(const double *update) override {
      Eigen::Matrix<double, 6, 1> update_eigen;
      update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
      _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
   }

   virtual bool read(istream &in) override {}

   virtual bool write(ostream &out) const override {}
};

class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
   public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

      EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}

   virtual void computeError() override {
      const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
      Sophus::SE3d T = v->estimate();
      Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
      pos_pixel /= pos_pixel[2];
      _error = _measurement - pos_pixel.head<2>();
   }

   virtual void linearizeOplus() override {
      const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
      Sophus::SE3d T = v->estimate();
      Eigen::Vector3d pos_cam = T * _pos3d;
      double fx = _K(0, 0);
      double fy = _K(1, 1);
      double cx = _K(0, 2);
      double cy = _K(1, 2);
      double X = pos_cam[0];
      double Y = pos_cam[1];
      double Z = pos_cam[2];
      double Z2 = Z * Z;
      _jacobianOplusXi
         << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
         0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
   }

   virtual bool read(istream &in) override {}

   virtual bool write(ostream &out) const override {}

   private:
      Eigen::Vector3d _pos3d;
      Eigen::Matrix3d _K;
};

Mat bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  const string& filename,
  const vector<string>& timestamps) {

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
  g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // vertex
  VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
  vertex_pose->setId(0);
  vertex_pose->setEstimate(Sophus::SE3d());
  optimizer.addVertex(vertex_pose);

  // K
  Eigen::Matrix3d K_eigen;
  K_eigen <<
          K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
    K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
    K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

  // edges
  int index = 1;
  for (size_t i = 0; i < points_2d.size(); ++i) {
    auto p2d = points_2d[i];
    auto p3d = points_3d[i];
    EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
    edge->setId(index);
    edge->setVertex(0, vertex_pose);
    edge->setMeasurement(p2d);
    edge->setInformation(Eigen::Matrix2d::Identity());
    optimizer.addEdge(edge);
    index++;
  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.setVerbose(true);
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
  cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
//   cout << "pose matrix "  << vertex_pose->estimate().matrix()(0) << vertex_pose->estimate().matrix()(1) << vertex_pose->estimate().matrix()(4) << endl;

  Mat Rt = Mat::eye(4,4,CV_64FC1);
  Rt.at<double>(0,0) = vertex_pose->estimate().matrix()(0);
  Rt.at<double>(1,0) = vertex_pose->estimate().matrix()(1);
  Rt.at<double>(2,0) = vertex_pose->estimate().matrix()(2);
  Rt.at<double>(3,0) = vertex_pose->estimate().matrix()(3);
  Rt.at<double>(0,1) = vertex_pose->estimate().matrix()(4);
  Rt.at<double>(1,1) = vertex_pose->estimate().matrix()(5);
  Rt.at<double>(2,1) = vertex_pose->estimate().matrix()(6);
  Rt.at<double>(3,1) = vertex_pose->estimate().matrix()(7);
  Rt.at<double>(0,2) = vertex_pose->estimate().matrix()(8);
  Rt.at<double>(1,2) = vertex_pose->estimate().matrix()(9);
  Rt.at<double>(2,2) = vertex_pose->estimate().matrix()(10);
  Rt.at<double>(3,2) = vertex_pose->estimate().matrix()(11);
  Rt.at<double>(0,3) = vertex_pose->estimate().matrix()(12);
  Rt.at<double>(1,3) = vertex_pose->estimate().matrix()(13);
  Rt.at<double>(2,3) = vertex_pose->estimate().matrix()(14);
  Rt.at<double>(3,3) = vertex_pose->estimate().matrix()(15);

//   writeResults(filename, timestamps, vertex_pose->estimate().matrix());
  pose = vertex_pose->estimate();
  return Rt;
}


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "../include/structures.h"
#include "../include/transformations.h"
#include "../../point_to_point_source_to_target_tait_bryan_wc_jacobian.h"
#include "../../point_to_point_source_to_target_rodrigues_wc_jacobian.h"
#include "../../point_to_point_source_to_target_quaternion_wc_jacobian.h"
#include "../../quaternion_constraint_jacobian.h"
#include "../include/cauchy.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -100.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

Eigen::Affine3d pose_source;
std::vector<Eigen::Vector3d> points_target_global;
std::vector<Eigen::Vector3d> points_source_local;

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

	for(size_t i = 0 ; i < 100; i++){
		Eigen::Vector3d p;
		p.x() = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		p.y() = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		p.z() = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		points_target_global.push_back(p);
	}

	TaitBryanPose pose;
	pose.px = -4;
	pose.py = 0.4;
	pose.pz = -0.2;
	pose.om = 0.1;
	pose.fi = 0.2;
	pose.ka = 0.3;
	pose_source = affine_matrix_from_pose_tait_bryan(pose);

	Eigen::Affine3d m_inv = pose_source.inverse();
	for(size_t j = 0 ; j < points_target_global.size(); j++){
		Eigen::Vector3d vt = m_inv * points_target_global[j];
		points_source_local.push_back(vt);
	}

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("point to point source to target");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(1.0f, 0.0f, 0.0f);

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 1.0f, 0.0f);

	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 1.0f);
	glEnd();

	Eigen::Affine3d &m = pose_source;

	glBegin(GL_LINES);
		glColor3f(1.0f, 0.0f, 0.0f);
		glVertex3f(m(0,3), m(1,3), m(2,3));
		glVertex3f(m(0,3) + m(0,0), m(1,3) + m(1,0), m(2,3) + m(2,0));

		glColor3f(0.0f, 1.0f, 0.0f);
		glVertex3f(m(0,3), m(1,3), m(2,3));
		glVertex3f(m(0,3) + m(0,1), m(1,3) + m(1,1), m(2,3) + m(2,1));

		glColor3f(0.0f, 0.0f, 1.0f);
		glVertex3f(m(0,3), m(1,3), m(2,3));
		glVertex3f(m(0,3) + m(0,2), m(1,3) + m(1,2), m(2,3) + m(2,2));
	glEnd();

	glColor3f(1,0,0);
	glPointSize(3);
	glBegin(GL_POINTS);
	for(auto &p:points_source_local){
		Eigen::Vector3d vt;
		vt = m * p;
		glVertex3f(vt.x(), vt.y(), vt.z());
	}
	glEnd();
	glPointSize(1);

	glColor3f(0,0,1);
	glPointSize(3);
	glBegin(GL_POINTS);

	for(auto &p:points_target_global){
		glVertex3f(p.x(), p.y(), p.z());
	}
	glEnd();
	glPointSize(1);

	glColor3f(0,1,0);
	glBegin(GL_LINES);
	for(int i = 0; i < points_source_local.size(); i++){
		Eigen::Vector3d v(points_source_local[i].x(), points_source_local[i].y(), points_source_local[i].z());
		Eigen::Vector3d vt;
		vt = m * v;
		glVertex3f(vt.x(), vt.y(), vt.z());
		glVertex3f(points_target_global[i].x(), points_target_global[i].y(), points_target_global[i].z());
	}
	glEnd();

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'n':{
				TaitBryanPose pose;
				pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;

				pose_source = pose_source * affine_matrix_from_pose_tait_bryan(pose);

			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			TaitBryanPose pose_s = pose_tait_bryan_from_affine_matrix(pose_source);

for(size_t i = 0 ; i < points_source_local.size(); i++){
	Eigen::Vector3d &p_t = points_target_global[i];
	Eigen::Vector3d &p_s = points_source_local[i];
	double delta_x;
	double delta_y;
	double delta_z;
	point_to_point_source_to_target_tait_bryan_wc(delta_x, delta_y, delta_z, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z());

	Eigen::Matrix<double, 3, 6, Eigen::RowMajor> jacobian;
	point_to_point_source_to_target_tait_bryan_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.om, pose_s.fi, pose_s.ka, p_s.x(), p_s.y(), p_s.z());

	int ir = tripletListB.size();

	for(int row = 0; row < 3; row++){
		for(int col = 0; col < 6; col++){
			if(jacobian(row,col)!=0.0){
			tripletListA.emplace_back(ir+row,col,-jacobian(row,col));
			}
		}
	}

	tripletListP.emplace_back(ir    , ir    , 1);
	tripletListP.emplace_back(ir + 1, ir + 1, 1);
	tripletListP.emplace_back(ir + 2, ir + 2, 1);

	tripletListB.emplace_back(ir    , 0,  delta_x);
	tripletListB.emplace_back(ir + 1, 0,  delta_y);
	tripletListB.emplace_back(ir + 2, 0,  delta_z);
}

Eigen::SparseMatrix<double> matA(tripletListB.size(), 6);
Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

Eigen::SparseMatrix<double> AtPA(6, 6);
Eigen::SparseMatrix<double> AtPB(6, 1);

Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
AtPA = AtP * matA;
AtPB = AtP * matB;

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);
Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(pose_source);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.om += h_x[counter++];
				pose.fi += h_x[counter++];
				pose.ka += h_x[counter++];

				pose_source = affine_matrix_from_pose_tait_bryan(pose);

			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{

			TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(pose_source);
			posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
			posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
			posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
			pose_source = affine_matrix_from_pose_tait_bryan(posetb);

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			RodriguesPose pose_s = pose_rodrigues_from_affine_matrix(pose_source);

			for(size_t i = 0 ; i < points_source_local.size(); i++){
				Eigen::Vector3d &p_t = points_target_global[i];
				Eigen::Vector3d &p_s = points_source_local[i];
				double delta_x;
				double delta_y;
				double delta_z;
				point_to_point_source_to_target_rodrigues_wc(delta_x, delta_y, delta_z, pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z());

				Eigen::Matrix<double, 3, 6, Eigen::RowMajor> jacobian;
				point_to_point_source_to_target_rodrigues_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.sx, pose_s.sy, pose_s.sz, p_s.x(), p_s.y(), p_s.z());

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir     , 0, -jacobian(0,0));
				tripletListA.emplace_back(ir     , 1, -jacobian(0,1));
				tripletListA.emplace_back(ir     , 2, -jacobian(0,2));
				tripletListA.emplace_back(ir     , 3, -jacobian(0,3));
				tripletListA.emplace_back(ir     , 4, -jacobian(0,4));
				tripletListA.emplace_back(ir     , 5, -jacobian(0,5));

				tripletListA.emplace_back(ir + 1 , 0, -jacobian(1,0));
				tripletListA.emplace_back(ir + 1 , 1, -jacobian(1,1));
				tripletListA.emplace_back(ir + 1 , 2, -jacobian(1,2));
				tripletListA.emplace_back(ir + 1 , 3, -jacobian(1,3));
				tripletListA.emplace_back(ir + 1 , 4, -jacobian(1,4));
				tripletListA.emplace_back(ir + 1 , 5, -jacobian(1,5));

				tripletListA.emplace_back(ir + 2 , 0, -jacobian(2,0));
				tripletListA.emplace_back(ir + 2 , 1, -jacobian(2,1));
				tripletListA.emplace_back(ir + 2 , 2, -jacobian(2,2));
				tripletListA.emplace_back(ir + 2 , 3, -jacobian(2,3));
				tripletListA.emplace_back(ir + 2 , 4, -jacobian(2,4));
				tripletListA.emplace_back(ir + 2 , 5, -jacobian(2,5));

				tripletListP.emplace_back(ir    , ir    ,  cauchy(delta_x, 1));
				tripletListP.emplace_back(ir + 1, ir + 1,  cauchy(delta_y, 1));
				tripletListP.emplace_back(ir + 2, ir + 2,  cauchy(delta_z, 1));

				tripletListB.emplace_back(ir    , 0,  delta_x);
				tripletListB.emplace_back(ir + 1, 0,  delta_y);
				tripletListB.emplace_back(ir + 2, 0,  delta_z);
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(6, 6);
			Eigen::SparseMatrix<double> AtPB(6, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				RodriguesPose pose = pose_rodrigues_from_affine_matrix(pose_source);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.sx += h_x[counter++];
				pose.sy += h_x[counter++];
				pose.sz += h_x[counter++];

				pose_source = affine_matrix_from_pose_rodrigues(pose);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			QuaternionPose pose_s = pose_quaternion_from_affine_matrix(pose_source);

			for(size_t i = 0 ; i < points_source_local.size(); i++){
				Eigen::Vector3d &p_t = points_target_global[i];
				Eigen::Vector3d &p_s = points_source_local[i];
				double delta_x;
				double delta_y;
				double delta_z;
				point_to_point_source_to_target_quaternion_wc(delta_x, delta_y, delta_z, pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, p_s.x(), p_s.y(), p_s.z(), p_t.x(), p_t.y(), p_t.z());

				Eigen::Matrix<double, 3, 7, Eigen::RowMajor> jacobian;
				point_to_point_source_to_target_quaternion_wc_jacobian(jacobian, pose_s.px, pose_s.py, pose_s.pz, pose_s.q0, pose_s.q1, pose_s.q2, pose_s.q3, p_s.x(), p_s.y(), p_s.z());

				int ir = tripletListB.size();

				tripletListA.emplace_back(ir     , 0, -jacobian(0,0));
				tripletListA.emplace_back(ir     , 1, -jacobian(0,1));
				tripletListA.emplace_back(ir     , 2, -jacobian(0,2));
				tripletListA.emplace_back(ir     , 3, -jacobian(0,3));
				tripletListA.emplace_back(ir     , 4, -jacobian(0,4));
				tripletListA.emplace_back(ir     , 5, -jacobian(0,5));
				tripletListA.emplace_back(ir     , 6, -jacobian(0,6));

				tripletListA.emplace_back(ir + 1 , 0, -jacobian(1,0));
				tripletListA.emplace_back(ir + 1 , 1, -jacobian(1,1));
				tripletListA.emplace_back(ir + 1 , 2, -jacobian(1,2));
				tripletListA.emplace_back(ir + 1 , 3, -jacobian(1,3));
				tripletListA.emplace_back(ir + 1 , 4, -jacobian(1,4));
				tripletListA.emplace_back(ir + 1 , 5, -jacobian(1,5));
				tripletListA.emplace_back(ir + 1 , 6, -jacobian(1,6));

				tripletListA.emplace_back(ir + 2 , 0, -jacobian(2,0));
				tripletListA.emplace_back(ir + 2 , 1, -jacobian(2,1));
				tripletListA.emplace_back(ir + 2 , 2, -jacobian(2,2));
				tripletListA.emplace_back(ir + 2 , 3, -jacobian(2,3));
				tripletListA.emplace_back(ir + 2 , 4, -jacobian(2,4));
				tripletListA.emplace_back(ir + 2 , 5, -jacobian(2,5));
				tripletListA.emplace_back(ir + 2 , 6, -jacobian(2,6));

				tripletListP.emplace_back(ir    , ir    ,  cauchy(delta_x, 1));
				tripletListP.emplace_back(ir + 1, ir + 1,  cauchy(delta_y, 1));
				tripletListP.emplace_back(ir + 2, ir + 2,  cauchy(delta_z, 1));

				tripletListB.emplace_back(ir    , 0,  delta_x);
				tripletListB.emplace_back(ir + 1, 0,  delta_y);
				tripletListB.emplace_back(ir + 2, 0,  delta_z);
			}

			int ir = tripletListB.size();
			QuaternionPose pose = pose_quaternion_from_affine_matrix(pose_source);

			double delta;
			quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3);

			Eigen::Matrix<double, 1, 4> jacobian;
			quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3);

			tripletListA.emplace_back(ir, 3 , -jacobian(0,0));
			tripletListA.emplace_back(ir, 4 , -jacobian(0,1));
			tripletListA.emplace_back(ir, 5 , -jacobian(0,2));
			tripletListA.emplace_back(ir, 6 , -jacobian(0,3));

			tripletListP.emplace_back(ir, ir, 1000000.0);

			tripletListB.emplace_back(ir, 0, delta);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 7);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(7, 7);
			Eigen::SparseMatrix<double> AtPB(7, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == 7){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				QuaternionPose pose = pose_quaternion_from_affine_matrix(pose_source);
				pose.px += h_x[counter++];
				pose.py += h_x[counter++];
				pose.pz += h_x[counter++];
				pose.q0 += h_x[counter++];
				pose.q1 += h_x[counter++];
				pose.q2 += h_x[counter++];
				pose.q3 += h_x[counter++];

				pose_source = affine_matrix_from_pose_quaternion(pose);
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: add noise to pose source" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize (Rodrigues)" << std::endl;
	std::cout << "q: optimize (Quaternion)" << std::endl;
}