/*
Rafael Díaz Medina A01024592
David Benjamin Ruiz A01020825
https://www.boost.org/doc/libs/1_55_0/libs/graph/example/
https://www.boost.org/doc/libs/1_55_0/libs/graph/example/dfs-example.cpp
https://www.boost.org/doc/libs/1_55_0/libs/graph/example/kruskal-example.cpp
https://www.boost.org/doc/libs/1_55_0/libs/graph/example/bfs-example.cpp
https://www.boost.org/doc/libs/1_55_0/libs/graph/example/prim-example.cpp
https://www.boost.org/doc/libs/1_55_0/libs/graph/example/dijkstra-example.cpp

Para poder correr este programa en windows se necesitaron de muchos pasos pero como se
consiguio finalmente fue así:
1- Descargar dev-c++
2- Descargar Boost
3- Configurar las librerías de boost para que el compilador las pudiera leer automaticamente
4- Ejecutar el programa

Tenemos contadores en cada uno de los algoritmos y también en las operaciones
de inserción y delete

Complejidad de crear Vertice O(1)
Complejidad de borrar Vertice O(m+n)
Complejidad de crear Aristas O(1)
Complejidad de borrar Aristas O(1)
Complejidad de DFS O(m+n) Este algoritmo utiliza una tecnica Branch and bound
Complejidad de BFS O(m+n) Este algoritmo utiliza una tecnica Branch and bound
Complejidad de Dijkstra O(n log n) Este algoritmo utiliza una tecnica Avida
Complejidad de Prim O(n log n) Este algoritmo utiliza una tecnica Avida
Complejidad de Kruskal O(n log n) Este algoritmo utiliza una tecnica Avida
Complejidad de Floyd Warshall O(n^3) Este algoritmo utiliza una tecnica de programacion dinamica
*/
#include <iostream>
#include <fstream>
#include <stack>
#include <map>
#include <queue>
#include <vector>
#include <iostream>
#include <iomanip>
#include <chrono>
#include <string.h>
#include <bits/stdc++.h>
#include <ctype.h>
#include <stdio.h>
#include <cstdio>
#include <stdio.h>
#include <stdlib.h>
#include "time.h"
#include <ctime>
#include <string>
#include <algorithm>
#include <tuple>
#include <iterator>
using namespace std;
using namespace boost;
using namespace std::chrono;
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/dijsktra_shortest_paths_no_color_map.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/exterior_property.hpp>

typedef property<vertex_distance_t, int> Vdistance;
typedef property<edge_weight_t, int> EdgeWeight;
typedef adjacency_list <vecS, vecS, bidirectionalS, Vdistance, EdgeWeight> Grafo;
typedef property_map<Grafo, edge_weight_t>::type EdgeWeights;
typedef boost::graph_traits<Grafo>::vertex_descriptor vertex_t;
typedef graph_traits < Grafo >::edge_descriptor Edge;
typedef boost::exterior_vertex_property<Grafo, int> DistanceProperty;
typedef DistanceProperty::matrix_type DistanceMatrix;
typedef DistanceProperty::matrix_map_type DistanceMatrixMap;

class dfsVisitor : public boost::default_dfs_visitor{
public:

	void discover_vertex(vertex_t v, const Grafo& n) const{
		std::cerr << v << " " ;

		return;
	}
};

class bfsVisitor : public boost::default_bfs_visitor{
public:
	void discover_vertex(vertex_t v, const Grafo& n) const{
		std::cerr << v << " ";
		return;
	}
};
	//Complejidad del Algoritmo: O(1)
	Grafo crearVertices(Grafo p,int vertex){
		vertex_t u;
		u= crear_vertice(p);
		name.push_back(	std::to_string(vertex));
		std::cout << "Se puso el vertice "<< vertex << "\n";
		return p;
	}

	//Complejidad del Algoritmo: O(1)
	Grafo crearAristas(Grafo p, int sale, int entra, int peso){
		crear_aristas(sale, entra, peso, p);
		std::cout << "Se puso la arista de " << sale << " a " << entra << "\n";
		return p;
	}

	//Complejidad del Algoritmo: O(1)
	Grafo borrarArista(Grafo p, int sale, int entra){
		borrar_arista(sale, entra, p);
		std::cout << "Se removio la arista de " << sale << " a " << entra << "\n";
		return p;
	}

	//Complejidad del Algoritmo: O(1)
	Grafo borrarVertice(Grafo p, int u){
		clear_vertex(u,p);
		borrar_vertice(u, p);
		std::cout << "Se ha removido el vertice " << u;
		return p;
	}
	//Complejidad del Algoritmo: O(M+N)
	//Este algoritmo utiliza una tecnica Branch and bound
	void DFS(Grafo p){
		dfsVisitor temp;
		std::cout << "DFS: \n";
		depth_first_search(p, visitor(temp));
	}
	//Complejidad del Algoritmo: O(M+N)
	//Este algoritmo utiliza una tecnica Branch and bound
	void BFS(Grafo p, int inicio){
		bfsVisitor temp;
		std::cout << "BFS: \n";
		breadth_first_search(p,inicio, visitor(temp));
	}
	//Complejidad del Algoritmo: O(N^2)
	//Este algoritmo utiliza una tecnica Avida
	void dijsktra(Grafo n){
		std::vector<vertex_t> p(num_vertices(n));
		std::vector<int> d(num_vertices(n));
		vertex_t s = vertex(1, n);
		property_map<Grafo, vertex_index_t>::type indexmap = get(vertex_index, n);
		property_map<Grafo, edge_weight_t>::type weightmap = get(edge_weight, n);
		dijsktra_shortest_paths(n, s, &p[0], &d[0], weightmap, indexmap,
		std::less<int>(), closed_plus<int>(),(std::numeric_limits<int>::max)(), 0,default_dijsktra_visitor());
		graph_traits <Grafo>::vertex_iterator vi, vend;

		for (tie(vi, vend) = vertices(n); vi != vend; ++vi){
			std::cout << "distancia al vertice " << name[*vi] << " = " << d[*vi] << ", ";
			std::cout << " con padre " << name[p[*vi]] << std::
				endl;
		}
	}
	//Complejidad del Algoritmo: O(N Log(M))
	//Este algoritmo utiliza una tecnica Avido
	void prim(Grafo p){
		std::vector<vertex_t> v(num_vertices(p));
		property_map<Grafo, vertex_index_t>::type indexmap = get(vertex_index, p);
		property_map<Grafo, vertex_distance_t>::type distance = get(vertex_distance, p);
		property_map<Grafo, edge_weight_t>::type weightmap = get(edge_weight, p);
		prim_minimum_spanning_tree(p, *vertices(p).first,&v[0],distance,weightmap,indexmap, default_dijsktra_visitor());

		for (std::size_t i = 1; i != v.size(); ++i){

			if (v[i] != i){
			std::cout << "parent[" << i << "] = " << v[i] << std::endl;
			}
			else{
				std::cout << "parent[" << i << "] = no hay padre" << std::endl;
			}
		}
	}
	//Complejidad del Algoritmo: O(N Log(N))
	//Este algoritmo utiliza una tecnica Avido
	void kruskal(Grafo p){
		property_map < Grafo, edge_weight_t >::type weight = get(edge_weight, p);
		std::vector < Edge > spanning_tree;
		kruskal_minimum_spanning_tree(p, std::back_inserter(spanning_tree));
		std::cout << "Kruskal: \n";
		for (std::vector < Edge >::iterator ei = spanning_tree.begin(); ei != spanning_tree.end(); ++ei) {
			std::cout << source(*ei, p) << " <--> " << target(*ei, p)<< " con peso " << weight[*ei] << std::endl;
		}
	}
	//Complejidad del Algoritmo: O(N^3)
	//Este algoritmo utiliza una tecnica de programacion dinamica
	void floydWarshall(Grafo p){
		DistanceMatrix distancias(num_vertices(p));
		DistanceMatrixMap dm(distancias, p);
		property_map < Grafo, edge_weight_t >::type weight = get(edge_weight, p);
		floyd_warshall_all_pairs_shortest_paths(p, dm,boost::weight_map(weight));
		std::cout << "\n Floyd-Warshall AP-SP: " << std::endl;

		for (std::size_t i = 1; i < num_vertices(p); ++i) {

			for (std::size_t j = 1; j < num_vertices(p); ++j) {
				std::cout << "del vertice " << i  << " al " << j  << " : ";

				if (distancias[i][j] == std::numeric_limits<int>::max()){
					std::cout << "X" << std::endl;
				}
				else{
					std::cout << distancias[i][j] << std::endl;
				}
			}
			std::cout << std::endl;
		}
	}
};
class GrafoC{
public:
	std::vector<std::string> name = { "1","2","3","4","5","6","7","8","9","10","11","12","13","14" };
public:

	Grafo crearGrafo(){
		std::vector<int> vertices = { 1,2,3,4,5,6,7,8,9,10,11,12,13,14 };
		Grafo n(vertices.size());
		std::vector<int> pesos = { 8,8,8,7,4,7,9,4,6,2,3,1,2,6,3,3,2,4,10,6,8,6,2,9 };

		crear_aristas(1,3,8,n);
		crear_aristas(1, 4,8,n);
		crear_aristas(3,5,8,n);
		crear_aristas(3,2,7,n);
		crear_aristas(3,10,4,n);
		crear_aristas(2,5,7,n);
		crear_aristas(5,6,9,n);
		crear_aristas(6,13,4,n);
		crear_aristas(13,14,6,n);
		crear_aristas(14,13,2,n);
		crear_aristas(4,7,3,n);
		crear_aristas(4,5,1,n);
		crear_aristas(4,8,2,n);
		crear_aristas(7,4,6,n);
		crear_aristas(8,7,3, n);
		crear_aristas(8,9,3, n);
		crear_aristas(9,10,2, n);
		crear_aristas(9,12,4, n);
		crear_aristas(10,3,10, n);
		crear_aristas(10,6,6, n);
		crear_aristas(12,11,8, n);
		crear_aristas(11,12,6, n);
		crear_aristas(12,9,2, n);
		crear_aristas(12,14,9, n);

		return n;
	}

int main(){
	high_resolution_clock::time_point t1;
   	high_resolution_clock::time_point t2;
   	duration<double> tiempo;

	GrafoC test;
	Grafo n = test.crearGrafo();
	int s;
	while (1){
	printf("1.Crear vertice\n2.Borrar vertice\n3.Crear arista\n4.Borrar arista\n5.DFS\n6.BFS\n7.Prim y Kruskal\n8.dijsktra y Floyd-Warshall\n");
	scanf("%d", &s);
	if(s==1){
		printf("Ingresa el numero\n");
		int num;
		scanf("%d", &num);
		t1=high_resolution_clock::now();
		test.crearVertices(n, num);
		t2=high_resolution_clock::now();
		tiempo = duration_cast<duration<double>>(t2-t1);
        tiempo=tiempo*1000;
        cout << tiempo.count() << "milisegundos" << endl;

	}
	else if(s==2){
		printf("Ingresa el numero\n");
		int num;
		scanf("%d", &num);
		t1=high_resolution_clock::now();
		test.borrarVertice(n, num);
		t2=high_resolution_clock::now();
		tiempo = duration_cast<duration<double>>(t2-t1);
        tiempo=tiempo*1000;
        cout << tiempo.count() << "milisegundos" << endl;
	}
	else if(s==3){
		printf("Los numeros que vamos a ingresar son 14, 15, 2\n");
		t1=high_resolution_clock::now();
		test.crearAristas(n, 14, 15, 2);
		t2=high_resolution_clock::now();
		tiempo = duration_cast<duration<double>>(t2-t1);
        tiempo=tiempo*1000;
        cout << tiempo.count() << "milisegundos" << endl;
	}
	else if(s==4){
		printf("Borraremos 10, 3");
		t1=high_resolution_clock::now();
		test.borrarArista(n, 10, 3);
		t2=high_resolution_clock::now();
		tiempo = duration_cast<duration<double>>(t2-t1);
        tiempo=tiempo*1000;
        cout << tiempo.count() << "milisegundos" << endl;
	}
	else if(s==5){
		printf("Hacemos el corrido de DFS");
		t1=high_resolution_clock::now();
		test.DFS(n);
		t2=high_resolution_clock::now();
		tiempo = duration_cast<duration<double>>(t2-t1);
        tiempo=tiempo*1000;
        cout << tiempo.count() << "milisegundos" << endl;
	}
	else if(s==6){
		printf("Hacemos el corrido de BFS");
		t1=high_resolution_clock::now();
		test.BFS(n,1);
		t2=high_resolution_clock::now();
		tiempo = duration_cast<duration<double>>(t2-t1);
        tiempo=tiempo*1000;
        cout << tiempo.count() << "milisegundos" << endl;
	}
	else if(s==7){
		printf("Arbol de recubrimiento Minimo con Prim y Kruskal");
		t1=high_resolution_clock::now();
		test.prim(n);
		t2=high_resolution_clock::now();
		tiempo = duration_cast<duration<double>>(t2-t1);
        tiempo=tiempo*1000;
        cout << tiempo.count() << "milisegundos" << endl;
		//------------------------------------------------------
		t1=high_resolution_clock::now();
		test.kruskal(n);
		t2=high_resolution_clock::now();
		tiempo = duration_cast<duration<double>>(t2-t1);
        tiempo=tiempo*1000;
        cout << tiempo.count() << "milisegundos" << endl;
	}
	else if(s==8){
		printf("Ruta minima con dijsktra y Floyd-Warshall");
		t1=high_resolution_clock::now();
		test.dijsktra(n);
		t2=high_resolution_clock::now();
		tiempo = duration_cast<duration<double>>(t2-t1);
        tiempo=tiempo*1000;
        cout << tiempo.count() << "milisegundos" << endl;
		//-------------------------------------------------
		t1=high_resolution_clock::now();
		test.FloydWarshall(n);
		t2=high_resolution_clock::now();
		tiempo = duration_cast<duration<double>>(t2-t1);
        tiempo=tiempo*1000;
        cout << tiempo.count() << "milisegundos" << endl;

	}
	else
	break;
	}
	return 0;
}


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

  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_FUNCTION_ERFCX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ERFCX_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-euler
    Function object implementing erfcx capabilities

   Computes the  underflow-compensating (scaled) complementary  error function:
   \f$\displaystyle e^{x^2}\frac{2}{\sqrt\pi}\int_{x}^{\infty} e^{-t^2}\mbox{d}t\f$

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = erfcx(x);
    @endcode

    is similar to:

    @code
    T r = exp(sqr(x))*erfc(x);
    @endcode

    But avoid underflow as much as possible.

    @see erfc, erf

  **/
  Value erfcx(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/erfcx.hpp>
#include <boost/simd/function/simd/erfcx.hpp>

#endif


#include <tiny_math_types.h>
#include <tiny_vector_functions.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(tiny_axis_angle);

BOOST_AUTO_TEST_CASE(simple_test)
{

  typedef tiny::MathTypes<double>       math_types;

  typedef math_types::real_type        T;
  typedef math_types::vector3_type     V;
  typedef math_types::quaternion_type  Q;

  // Small positive angle
  {
    Q q;
    T const phi = 0.01;
    V const m = tiny::unit( V::make(1.0, 1.0, 1.0) );
    q = Q::Ru( phi, m);

    T theta;
    V n;
    tiny::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( m(2), n(2), 0.01 );
  }
  // See what happens if axis is flipped
  {
    Q q;
    T const phi = 0.01;
    V const m = -tiny::unit( V::make(1.0, 1.0, 1.0) );
    q = Q::Ru( phi, m);

    T theta;
    V n;
    tiny::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( m(2), n(2), 0.01 );
  }
  // Pick larger angle
  {
    Q q;
    T const phi = 3.0;
    V const m = tiny::unit( V::make(1.0, 1.0, 1.0) );
    q = Q::Ru( phi, m);

    T theta;
    V n;
    tiny::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( m(2), n(2), 0.01 );
  }
  // Pick larger negative angle, the positive angle version representation should be returned!
  {
    Q q;
    T const phi = -3.0;
    V const m = tiny::unit( V::make(1.0, 1.0, 1.0) );
    q = Q::Ru( phi, m);

    T theta;
    V n;
    tiny::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( -phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( -m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( -m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( -m(2), n(2), 0.01 );
  }
  // Flip the axis
  {
    Q q;
    T const phi = -3.0;
    V const m = -tiny::unit( V::make(1.0, 1.0, 1.0) );
    q = Q::Ru( phi, m);

    T theta;
    V n;
    tiny::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( -phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( -m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( -m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( -m(2), n(2), 0.01 );
  }
  // Large positive angle
  {
    Q q;
    T const phi = 6.0;
    V const m = tiny::unit( V::make(1.0, 1.0, 1.0) );
    q = Q::Ru( phi, m);

    T theta;
    V n;
    tiny::get_axis_angle( q, n, theta );

    BOOST_CHECK_CLOSE( phi, theta, 0.01 );
    BOOST_CHECK_CLOSE( m(0), n(0), 0.01 );
    BOOST_CHECK_CLOSE( m(1), n(1), 0.01 );
    BOOST_CHECK_CLOSE( m(2), n(2), 0.01 );
  }

}

BOOST_AUTO_TEST_SUITE_END();


#include <cmath>
#include <vector>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "drake/solvers/fbstab/fbstab_mpc.h"
#include "drake/solvers/fbstab/test/ocp_generator.h"

namespace drake {
namespace solvers {
namespace fbstab {
namespace test {

using VectorXd = Eigen::VectorXd;

GTEST_TEST(FBstabMpc, DoubleIntegrator) {
  // Get the problem data.
  OcpGenerator ocp;
  ocp.DoubleIntegrator(2);  // horizon length of 2
  FBstabMpc::QPData data = ocp.GetFBstabInput();

  // Set up the initial guess.
  VectorXd z = VectorXd::Zero(ocp.nz());
  VectorXd l = VectorXd::Zero(ocp.nl());
  VectorXd v = VectorXd::Zero(ocp.nv());
  VectorXd y = VectorXd::Zero(ocp.nv());
  FBstabMpc::QPVariable x = {&z, &l, &v, &y};

  // Call the solver.
  Eigen::Vector4d size = ocp.ProblemSize();
  FBstabMpc solver(size(0), size(1), size(2), size(3));

  solver.UpdateOption("abs_tol", 1e-6);
  solver.SetDisplayLevel(FBstabAlgoMpc::Display::ITER);
  SolverOut out = solver.Solve(data, &x);

  ASSERT_EQ(out.eflag, ExitFlag::SUCCESS);
  ASSERT_LE(out.residual, 1e-6);

  VectorXd zopt(ocp.nz());
  VectorXd lopt(ocp.nl());
  VectorXd vopt(ocp.nv());

  // These numbers were computed using MATLAB's quadprog command.
  zopt << -5.31028204670497e-14, 5.02854354118183e-13, 0.311688311338095,
      5.35637944798588e-13, 0.311688311339015, -0.0779220779990502,
      0.311688311339667, 0.233766233340057, -0.103896103779874;

  lopt << -5.24675324688535, -4.49350649223710, -3.55844155822323,
      -0.935064934014372, -1.48051948022526, 0.233766233996585;

  vopt << 1.06213597221667e-13, -1.41190425869539e-21, 0, 0, 0, 0,
      -1.50393600622818e-21, -8.75144622575045e-10, 0, 0, 0, 0,
      -8.75144611157041e-10, -6.56358459377444e-10, 0, 0, 0, 0;

  for (int i = 0; i < ocp.nz(); i++) {
    EXPECT_NEAR(z(i), zopt(i), 1e-8);
  }

  for (int i = 0; i < ocp.nl(); i++) {
    EXPECT_NEAR(l(i), lopt(i), 1e-8);
  }

  for (int i = 0; i < ocp.nv(); i++) {
    EXPECT_NEAR(v(i), vopt(i), 1e-8);
  }
}

GTEST_TEST(FBstabMpc, DoubleIntegratorLongHorizon) {
  // Get the problem data.
  OcpGenerator ocp;
  ocp.DoubleIntegrator(20);  // horizon length of 20
  FBstabMpc::QPData data = ocp.GetFBstabInput();

  // Set up the initial guess.
  VectorXd z = VectorXd::Zero(ocp.nz());
  VectorXd l = VectorXd::Zero(ocp.nl());
  VectorXd v = VectorXd::Zero(ocp.nv());
  VectorXd y = VectorXd::Zero(ocp.nv());
  FBstabMpc::QPVariable x = {&z, &l, &v, &y};

  // Call the solver.
  Eigen::Vector4d size = ocp.ProblemSize();
  FBstabMpc solver(size(0), size(1), size(2), size(3));

  solver.UpdateOption("abs_tol", 1e-6);
  solver.SetDisplayLevel(FBstabAlgoMpc::Display::ITER);
  SolverOut out = solver.Solve(data, &x);

  ASSERT_EQ(out.eflag, ExitFlag::SUCCESS);
  ASSERT_LE(out.residual, 1e-6);
}

GTEST_TEST(FBstabMpc, ServoMotor) {
  // Get the problem data.
  OcpGenerator ocp;
  ocp.ServoMotor(25);  // horizon length of 25
  FBstabMpc::QPData data = ocp.GetFBstabInput();

  // Set up the initial guess.
  VectorXd z = VectorXd::Zero(ocp.nz());
  VectorXd l = VectorXd::Zero(ocp.nl());
  VectorXd v = VectorXd::Zero(ocp.nv());
  VectorXd y = VectorXd::Zero(ocp.nv());
  FBstabMpc::QPVariable x = {&z, &l, &v, &y};

  // Call the solver.
  Eigen::Vector4d size = ocp.ProblemSize();
  FBstabMpc solver(size(0), size(1), size(2), size(3));

  solver.UpdateOption("abs_tol", 1e-6);
  solver.SetDisplayLevel(FBstabAlgoMpc::Display::ITER);
  SolverOut out = solver.Solve(data, &x);

  ASSERT_EQ(out.eflag, ExitFlag::SUCCESS);
  ASSERT_LE(out.residual, 1e-6);
}

GTEST_TEST(FBstabMpc, SpacecraftRelativeMotion) {
  // Get the problem data.
  OcpGenerator ocp;
  ocp.SpacecraftRelativeMotion(40);  // horizon length of 40
  FBstabMpc::QPData data = ocp.GetFBstabInput();

  // Set up the initial guess.
  VectorXd z = VectorXd::Zero(ocp.nz());
  VectorXd l = VectorXd::Zero(ocp.nl());
  VectorXd v = VectorXd::Zero(ocp.nv());
  VectorXd y = VectorXd::Zero(ocp.nv());
  FBstabMpc::QPVariable x = {&z, &l, &v, &y};

  // Call the solver.
  Eigen::Vector4d size = ocp.ProblemSize();
  FBstabMpc solver(size(0), size(1), size(2), size(3));

  solver.UpdateOption("abs_tol", 1e-6);
  solver.SetDisplayLevel(FBstabAlgoMpc::Display::ITER);
  SolverOut out = solver.Solve(data, &x);

  ASSERT_EQ(out.eflag, ExitFlag::SUCCESS);
  ASSERT_LE(out.residual, 1e-6);
}

GTEST_TEST(FBstabMpc, CopolymerizationReactor) {
  // Get the problem data.
  OcpGenerator ocp;
  ocp.CopolymerizationReactor(80);  // horizon length of 80
  FBstabMpc::QPData data = ocp.GetFBstabInput();

  // Set up the initial guess.
  VectorXd z = VectorXd::Zero(ocp.nz());
  VectorXd l = VectorXd::Zero(ocp.nl());
  VectorXd v = VectorXd::Zero(ocp.nv());
  VectorXd y = VectorXd::Zero(ocp.nv());
  FBstabMpc::QPVariable x = {&z, &l, &v, &y};

  // Call the solver.
  Eigen::Vector4d size = ocp.ProblemSize();
  FBstabMpc solver(size(0), size(1), size(2), size(3));

  solver.UpdateOption("abs_tol", 1e-6);
  solver.SetDisplayLevel(FBstabAlgoMpc::Display::ITER);
  SolverOut out = solver.Solve(data, &x);

  ASSERT_EQ(out.eflag, ExitFlag::SUCCESS);
  ASSERT_LE(out.residual, 1e-6);
}

}  // namespace test
}  // namespace fbstab
}  // namespace solvers
}  // namespace drake


/** \file functors.hpp */

#pragma once

// std c++ headers
#include <cmath>
#include <complex>
#include <type_traits>

// boost headers
#include <boost/math/special_functions/cbrt.hpp>
#include <boost/math/special_functions/pow.hpp>

// AMDiS headers
#include "operations/functor_generator.hpp"
#include "operations/meta.hpp"
#include "traits/basic.hpp"
#include "traits/scalar_types.hpp"

namespace AMDiS
{
  namespace functors
  {
    /// identity(v) == v
    AMDIS_MAKE_UNARY_FUNCTOR( identity , d0 , v )

    /// constant(v) == val
    template <class T>
    struct constant : FunctorBase
    {
      constant(T val_) : val(val_) {}

      template <class V>
      T operator()(V&&) const
      {
        return val;
      }

    private:
      T val;
    };

    /// ct_constant(v) == val
    template <class T, long val_>
    struct ct_constant : FunctorBase
    {
      static constexpr T val = val_;

      template <class V> static constexpr T eval(V&&)
      {
        return val;
      }
      template <class V> static constexpr T apply(V&&)
      {
        return val;
      }
      template <class V> constexpr T operator()(V&&) const
      {
        return val;
      }
    };

    /// abs(v) == |v|
    template <class T>
    struct abs : FunctorBase
    {
      static constexpr int getDegree(int d0)
      {
        return d0;
      }
      static constexpr auto eval(const T& v) RETURNS( math::abs(v) )
      constexpr auto operator()(const T& v) const RETURNS( eval(v) )
    };

    // specialization of abs for complex values
    template <class T>
    struct abs<std::complex<T>> : FunctorBase
    {
      static constexpr int getDegree(int d0)
      {
        return d0;
      }
      static constexpr auto eval(const T& v) RETURNS( std::norm(v) )
      constexpr auto operator()(const T& v) const RETURNS( eval(v) )
    };

    /// negate(v) == -v
    AMDIS_MAKE_UNARY_FUNCTOR( negate, d0, -v  )

    AMDIS_MAKE_BINARY_FUNCTOR( plus,  math::max(d0, d1), v0 + v1  )
    AMDIS_MAKE_BINARY_FUNCTOR( minus, math::max(d0, d1), v0 - v1  )
//     AMDIS_MAKE_BINARY_FUNCTOR( multiplies, d0 + d1,      v0 * v1  )
    AMDIS_MAKE_BINARY_FUNCTOR( divides,    d0 + d1,      v0 / v1  )

    struct multiplies : FunctorBase
    {
      int getDegree(int d0, int d1) const
      {
        return d0 + d1;
      }

      template <class T0, class T1 = T0>
      static constexpr auto eval(T0&& v0, T1&& v1) RETURNS( std::forward<T0>(v0) * std::forward<T1>(v1) )

      template <class T0, class T1 = T0>
      auto operator() (T0&& v0, T1&& v1) const RETURNS( std::forward<T0>(v0) * std::forward<T1>(v1) )
    };

    // _____ logical functors _________________________________________________

    AMDIS_MAKE_BINARY_FUNCTOR( equal,   0, v0 == v1  )
    AMDIS_MAKE_BINARY_FUNCTOR( unequal, 0, v0 != v1  )
    AMDIS_MAKE_BINARY_FUNCTOR( less,    0, v0 < v1  )
    AMDIS_MAKE_BINARY_FUNCTOR( greater, 0, v0 > v1  )

    AMDIS_MAKE_BINARY_FUNCTOR( logical_and, 0, v0 && v1  )
    AMDIS_MAKE_BINARY_FUNCTOR( logical_or,  0, v0 || v1  )

    AMDIS_MAKE_BINARY_FUNCTOR( max, math::max(d0, d1), math::max(v0, v1)  )
    AMDIS_MAKE_BINARY_FUNCTOR( min, math::max(d0, d1), math::min(v0, v1)  )

    /// max(|a|,|b|)
    AMDIS_MAKE_BINARY_FUNCTOR( abs_max, math::max(d0, d1), math::max(math::abs(v0), math::abs(v1))  )

    /// min(|a|,|b|)
    AMDIS_MAKE_BINARY_FUNCTOR( abs_min, math::max(d0, d1), math::min(math::abs(v0), math::abs(v1))  )

    /// conditional(a,b,c) = a ? b : c
    template <class T1, class T2>
    struct conditional : FunctorBase
    {
      constexpr int getDegree(int d0, int d1, int d2) const
      {
        return math::max(d1, d2);
      }
      static constexpr typename std::common_type<T1, T2>::type
      eval(bool cond, T1 const& v1, T2 const& v2)
      {
        return cond ? v1 : v2;
      }
      constexpr auto operator()(bool cond, T1 const& v1, T2 const& v2) const RETURNS
      (
        eval(cond, v1, v2)
      )
    };

    /// cross(v1, v2) = v1 x v2, TODO: find better name
    template <class T1, class T2>
    struct MyCross : FunctorBase
    {
      using value_type = decltype( std::declval<T1>() * std::declval<T2>() );
      constexpr int getDegree(int /*d*/, int d0, int d1) const
      {
        return d0+d1;
      }

      template <class Vec1, class Vec2>
      static value_type eval(size_t i, const Vec1& v1, const Vec2& v2)
      {
        using size_type = Size_t<traits::category<Vec1>>;
        value_type result;

        TEST_EXIT_DBG( size(v1) == 3 && size(v1) == size(v2), "cross: inkompatible sizes!\n");

        size_type k = (i+1) % 3, l = (i+2) % 3;
        result = v1(k) * v2(l) - v1(l) * v2(k);
        return result;
      }

      template <class Vec1, class Vec2>
      auto operator()(size_t i, const Vec1& v1, const Vec2& v2) const RETURNS
      (
        eval(i, v1, v2)
      )
    };

    /// apply a functor N times
    template <class Functor, int N>
    struct apply
    {
      apply(Functor const& f_) : f(f_), inner(f_) {}

      int getDegree(int d0) const
      {
        return f.getDegree(inner.getDegree(d0));
      }

      template <class V>
      static auto eval(const V& v) RETURNS
      (
        Functor::eval(apply<Functor, N-1>::eval(v))
      )

      template <class V>
      auto operator()(const V& v) const RETURNS( f(inner(v)) )

    private:
      Functor f;
      apply<Functor, N-1> inner;
    };

    template <class Functor>
    struct apply<Functor, 0>
    {
      apply(Functor const& f_) : f(f_) {}
      int getDegree(int d0) const
      {
        return d0;
      }

      template <class V>
      static auto eval(const V& v) RETURNS( v )
      template <class V>
      auto operator()(const V& v) const RETURNS( v )

    private:
      Functor f;
    };

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

    template <class F, int arg, class G>
    struct compose;

    template <class F, class G>
    struct compose<F, 1, G>
    {
      template <class T>
      auto operator()(T const& v, T const& v0) RETURNS( f(g(v), v0) )

    private:
      F f;
      G g;
    };

    template <class F, class G>
    struct compose<F, 2, G>
    {
      template <class T>
      auto operator()(T const& v, T const& v0) RETURNS( f(v, g(v0)) )

    private:
      F f;
      G g;
    };

    /// pow<p>(v) == v^p
    template <int p, class T>
    struct pow : FunctorBase
    {
      constexpr int getDegree(int d0) const
      {
        return p*d0;
      }

      static constexpr T eval(const T& v)
      {
        return boost::math::pow<p>(v);
      }
      constexpr T operator()(const T& v) const
      {
        return eval(v);
      }
    };

    /// root<p>(v) == p-th-root(v)
    template <int p, class T, class = void>
    struct root_dispatch;

    template <int p, class T>
    struct root : FunctorBase
    {
      constexpr int getDegree(int d0) const
      {
        return p*d0;    // optimal polynomial approximation degree ?
      }

      static constexpr T eval(const T& v)
      {
        return root_dispatch<p,T>::eval(v);
      }
      constexpr T operator()(const T& v) const
      {
        return eval(v);
      }
    };

    template <int p, class T, class>
    struct root_dispatch
    {
      static constexpr T eval(const T& v)
      {
        return std::pow(v, 1.0/p);
      }
    };

    template <int p, class T>
    struct root_dispatch<p, T,
      Requires_t<meta::is_power_of<p, 3>> >
    {
      static constexpr T eval(const T& v)
      {
        return apply<root<3, T>, meta::log<p, 3>::value>::eval(v);
      }
    };

    template <int p, class T>
    struct root_dispatch<p, T,
      Requires_t<meta::is_power_of<p, 2>> >
    {
      static constexpr T eval(const T& v)
      {
        return apply<root<2, T>, meta::log<p, 2>::value>::eval(v);
      }
    };

    template <class T>
    struct root_dispatch<3, T>
    {
      static constexpr T eval(const T& v)
      {
        return boost::math::cbrt(v);
      }
    };

    template <class T>
    struct root_dispatch<2, T>
    {
      static constexpr T eval(const T& v)
      {
        return std::sqrt(v);
      }
    };

    template <class T>
    struct root_dispatch<1, T>
    {
      static constexpr T eval(const T& v)
      {
        return v;
      }
    };

    template <class T>
    struct root_dispatch<0, T>
    {
      static constexpr T eval(const T& /*v*/)
      {
        return 1.0;
      }
    };

  } // end namespace functors

} // end namespace AMDiS


// Copyright (C) 2016-2018 T. Zachary Laine
//
// 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)
#include <boost/yap/expression.hpp>

#include <boost/test/minimal.hpp>

template<typename T>
using term = boost::yap::terminal<boost::yap::expression, T>;

template<typename T>
using ref = boost::yap::expression_ref<boost::yap::expression, T>;

namespace yap = boost::yap;
namespace bh = boost::hana;

namespace user {

    struct number
    {
        double value;
    };

    struct eval_xform
    {
        auto
        operator()(yap::expr_tag<yap::expr_kind::terminal>, number const & n)
        {
            return n;
        }

        template<typename Expr>
        decltype(auto) operator()(
            yap::expression<yap::expr_kind::negate, bh::tuple<Expr>> const &
                expr)
        {
            number const n = transform(yap::value(expr), *this);
            return number{-n.value};
        }

        template<typename Expr1, typename Expr2>
        decltype(auto) operator()(yap::expression<
                                  yap::expr_kind::plus,
                                  bh::tuple<Expr1, Expr2>> const & expr)
        {
            number const lhs = transform(yap::left(expr), *this);
            number const rhs = transform(yap::right(expr), *this);
            return number{lhs.value + rhs.value};
        }
    };
}

template<typename Expr>
auto make_ref(Expr && expr)
{
    using type = yap::detail::operand_type_t<yap::expression, Expr>;
    return yap::detail::make_operand<type>{}(static_cast<Expr &&>(expr));
}

int test_main(int, char * [])
{
{
    {
        term<user::number> a{{1.0}};

        {
            user::number result = transform(a, user::eval_xform{});
            BOOST_CHECK(result.value == 1);
        }

        {
            user::number result = transform(make_ref(a), user::eval_xform{});
            BOOST_CHECK(result.value == 1);
        }

        {
            user::number result = transform(-a, user::eval_xform{});
            BOOST_CHECK(result.value == -1);
        }

        {
            auto expr = make_ref(a);
            user::number result = transform(-expr, user::eval_xform{});
            BOOST_CHECK(result.value == -1);
        }

        {
            auto expr = -a;
            user::number result = transform(expr, user::eval_xform{});
            BOOST_CHECK(result.value == -1);
        }

        {
            auto expr1 = make_ref(a);
            auto expr2 = make_ref(expr1);
            user::number result = transform(expr2, user::eval_xform{});
            BOOST_CHECK(result.value == 1);
        }

        {
            auto expr1 = -a;
            auto expr2 = make_ref(expr1);
            user::number result = transform(expr2, user::eval_xform{});
            BOOST_CHECK(result.value == -1);
        }

        {
            auto expr1 = make_ref(a);
            auto expr2 = -expr1;
            user::number result = transform(expr2, user::eval_xform{});
            BOOST_CHECK(result.value == -1);
        }

        {
            auto expr1 = a;
            auto expr2 = make_ref(expr1);
            auto expr3 = make_ref(expr2);
            user::number result = transform(expr3, user::eval_xform{});
            BOOST_CHECK(result.value == 1);
        }

        {
            auto expr1 = -a;
            auto expr2 = make_ref(expr1);
            auto expr3 = make_ref(expr2);
            user::number result = transform(expr3, user::eval_xform{});
            BOOST_CHECK(result.value == -1);
        }

        {
            auto expr1 = make_ref(a);
            auto expr2 = -expr1;
            auto expr3 = make_ref(expr2);
            user::number result = transform(expr3, user::eval_xform{});
            BOOST_CHECK(result.value == -1);
        }

        {
            auto expr1 = make_ref(a);
            auto expr2 = make_ref(expr1);
            auto expr3 = -expr2;
            user::number result = transform(expr3, user::eval_xform{});
            BOOST_CHECK(result.value == -1);
        }
    }

    {
        user::number result =
            transform(-term<user::number>{{1.0}}, user::eval_xform{});
        BOOST_CHECK(result.value == -1);
    }
}

{
    term<user::number> a{{1.0}};
    term<user::number> x{{41.0}};

    {
        user::number result = transform(a + x, user::eval_xform{});
        BOOST_CHECK(result.value == 42);
    }

    {
        user::number result =
            transform(make_ref(a) + make_ref(x), user::eval_xform{});
        BOOST_CHECK(result.value == 42);
    }

    {
        user::number result = transform(make_ref(a) + x, user::eval_xform{});
        BOOST_CHECK(result.value == 42);
    }

    {
        user::number result = transform(a + make_ref(x), user::eval_xform{});
        BOOST_CHECK(result.value == 42);
    }

    {
        user::number result = transform(a + x, user::eval_xform{});
        BOOST_CHECK(result.value == 42);
    }

    {
        user::number result =
            transform(-make_ref(a) + make_ref(x), user::eval_xform{});
        BOOST_CHECK(result.value == 40);
    }

    {
        user::number result = transform(-make_ref(a) + x, user::eval_xform{});
        BOOST_CHECK(result.value == 40);
    }

    {
        user::number result = transform(-a + make_ref(x), user::eval_xform{});
        BOOST_CHECK(result.value == 40);
    }

    {
        user::number result = transform(-a + x, user::eval_xform{});
        BOOST_CHECK(result.value == 40);
    }

    {
        user::number result =
            transform(make_ref(a) + -make_ref(x), user::eval_xform{});
        BOOST_CHECK(result.value == -40);
    }

    {
        user::number result = transform(make_ref(a) + -x, user::eval_xform{});
        BOOST_CHECK(result.value == -40);
    }

    {
        user::number result = transform(a + -make_ref(x), user::eval_xform{});
        BOOST_CHECK(result.value == -40);
    }

    {
        user::number result = transform(a + -x, user::eval_xform{});
        BOOST_CHECK(result.value == -40);
    }

    {
        user::number result =
            transform(-make_ref(a) + -make_ref(x), user::eval_xform{});
        BOOST_CHECK(result.value == -42);
    }

    {
        user::number result = transform(-make_ref(a) + -x, user::eval_xform{});
        BOOST_CHECK(result.value == -42);
    }

    {
        user::number result = transform(-a + -make_ref(x), user::eval_xform{});
        BOOST_CHECK(result.value == -42);
    }

    {
        user::number result = transform(-a + -x, user::eval_xform{});
        BOOST_CHECK(result.value == -42);
    }
}

return 0;
}


// Boost.Geometry Index
//
// Quickbook Examples
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[rtree_value_index

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>

#include <boost/geometry/index/rtree.hpp>

#include <cmath>
#include <vector>
#include <iostream>
#include <boost/foreach.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

template <typename Container>
class my_indexable
{
    typedef typename Container::size_type size_t;
    typedef typename Container::const_reference cref;
    Container const& container;

public:
    typedef cref result_type;
    explicit my_indexable(Container const& c) : container(c) {}
    result_type operator()(size_t i) const { return container[i]; }
};

int main()
{
    typedef bg::model::point<float, 2, bg::cs::cartesian> point;
    typedef bg::model::box<point> box;
    typedef std::vector<box>::size_type value;
    typedef bgi::rstar<16, 4> parameters;
    typedef my_indexable< std::vector<box> > indexable_getter;

    // boxes
    std::vector<box> boxes;

    // create some boxes
    for ( unsigned i = 0 ; i < 10 ; ++i )
    {
        // add a box
        boxes.push_back(box(point(i+0.0f, i+0.0f), point(i+0.5f, i+0.5f)));
    }

    // display boxes
    std::cout << "generated boxes:" << std::endl;
    BOOST_FOREACH(box const& b, boxes)
        std::cout << bg::wkt<box>(b) << std::endl;

    // create the rtree
    parameters params;
    indexable_getter ind(boxes);
    bgi::rtree<value, parameters, indexable_getter> rtree(params, ind);

    // fill the spatial index
    for ( size_t i = 0 ; i < boxes.size() ; ++i )
        rtree.insert(i);

    // find values intersecting some area defined by a box
    box query_box(point(0, 0), point(5, 5));
    std::vector<value> result_s;
    rtree.query(bgi::intersects(query_box), std::back_inserter(result_s));

    // find 5 nearest values to a point
    std::vector<value> result_n;
    rtree.query(bgi::nearest(point(0, 0), 5), std::back_inserter(result_n));

    // note: in Boost.Geometry the WKT representation of a box is polygon

    // display results
    std::cout << "spatial query box:" << std::endl;
    std::cout << bg::wkt<box>(query_box) << std::endl;
    std::cout << "spatial query result:" << std::endl;
    BOOST_FOREACH(value i, result_s)
        std::cout << bg::wkt<box>(boxes[i]) << std::endl;

    std::cout << "knn query point:" << std::endl;
    std::cout << bg::wkt<point>(point(0, 0)) << std::endl;
    std::cout << "knn query result:" << std::endl;
    BOOST_FOREACH(value i, result_n)
        std::cout << bg::wkt<box>(boxes[i]) << std::endl;

    return 0;
}

//]


//
//  EigenEx1DenseMatVec.cpp
//
//
//  Created by Zac Schulwolf on 12/26/16.
//
// Compile by g++ -I "$(brew --prefix eigen)/include/eigen3" EigenEx1DenseMatVec.cpp -o EigenEx1DenseMatVec
//
// From https://eigen.tuxfamily.org/dox/GettingStarted.html
// From https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html
// Installing Libraries http://stackoverflow.com/questions/34340578/installing-c-libraries-on-os-x

#include <iostream>
#include <Eigen/Dense>
//using Eigen::MatrixXd; used for 1
using namespace Eigen;
using namespace std;

int main()
{
    //1
    MatrixXd m1(2,2);
    m1(0,0) = 3;
    m1(1,0) = 2.5;
    m1(0,1) = -1;
    m1(1,1) = m1(1,0) + m1(0,1);
    cout << "Here is the matrix m1:\n" << m1 << endl;

    VectorXd v1(2);
    v1(0) = 4;
    v1(1) = v1(0) - 1;
    cout << "Here is the vector v1:\n" << v1 << endl;
    cout << endl << endl;

    //2A
    Matrix3d m2a= Matrix3d::Random();
    m2a= (m2a+ Matrix3d::Constant(1.2)) * 50;
    cout << "m2a=" << endl << m2a<< endl;
    Vector3d v2a(1,2,3);
    cout << "m2a* v2a =" << endl << m2a* v2a << endl;
    cout << endl << endl;

    //2B
    MatrixXd m2b= MatrixXd::Random(3,3);
    m2b= (m2b+ MatrixXd::Constant(3,3,1.2)) * 50;
    cout << "m2b=" << endl << m2b<< endl;
    VectorXd v2b(3);
    v2b << 1, 2, 3;
    cout << "m2b* v2b =" << endl << m2b* v2b << endl;
    cout << endl << endl;

    //3
    //initialization
    Matrix3d m3a; //3 by 3 matrix of uninitialized coefficients
    MatrixXd m3b; //dynamic size matrix, size currently 0 by 0, coefficients not allocated
    MatrixXf m3c(10,15); //10 by 15 dynamic sized matrix, allocated and uninitialized coeffients
    VectorXf v3a(30); //size 30 dynamic sized vector, allocated and uninitialized coeffients
    Vector2d v3b(5.0, 6.0);
    Vector3d v3c(5.0, 6.0, 7.0);
    Vector4d v3d(5.0, 6.0, 7.0, 8.0); //can initialize up to 4 coeffients with this method
    Matrix3f m3d;
    m3d << 1, 2, 3, 4, 5, 6, 7, 8, 9; //Comma-initialization for matrix
    cout << m3d << endl;

    Vector3f v3e; //column vector <float,3,1> Vector
    RowVector3f v3f; //row vector <float,3,2> RowVector
    cout << endl << endl;

    //4
    //rows(), cols(), size(), resize()
    //resize() changes coeffients, use conservativeResize() to keep coeffients
    MatrixXd m4(2,5); //Note that this is Xd so its dynamic sized
    m4.resize(4,3); //Needs to be dynamic, now 4 by 3
    cout << "The matrix m4 is of size " << m4.rows() << "x" << m4.cols() << endl;
    cout << "It has " << m4.size() << " coefficients" << endl;

    VectorXd v4(2);
    v4.resize(5);
    cout << "The vector v4 is of size " << v4.size() << endl;
    cout << "As a matrix, v4 is of size " << v4.rows() << "x" << v4.cols() << endl;
    cout << endl << endl;

    //5
    //Assignment and resizing
    MatrixXf m5a(2,2);
    cout << "m5a is of size " << m5a.rows() << "x" << m5a.cols() << endl;
    MatrixXf m5b(3,3);
    m5a = m5b;
    cout << "m5a is now of size " << m5a.rows() << "x" << m5a.cols() << endl;
    cout << "m5a is now of size " << m5a.rows() << "x" << m5a.cols() << endl;

    //6
    //Formula
    /*
     cout << "MatrixNt; ex: MatrixXi (Matrix<int, Dynamic, Dynamic>)
     VectorNt; ex: Vector2f (Matrix<float, 2, 1>)
     RowVectonNt; ex: Vector3d (Matrix<double, 1, 3>)

     N can be 2,3,4, or x (dynamic)
     t can be i (int), f (float), d (double), cf (complex<float>), or cd(complex<double>)" << endl;
    */

}


#include "SparseCMPC/SparseCMPC.h"
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

void SparseCMPC::c2d(u32 trajIdx, u32 bBlockStartIdx, u32 block_count) {

  Eigen::Matrix<double, 24, 24> AB, expmm;
  AB.setZero();
  AB.block(0,0,12,12) = _aMat[trajIdx];

  for(u32 i = bBlockStartIdx; i < bBlockStartIdx + block_count; i++) {
    BblockID id = _bBlockIds[i];
    if(id.timestep != trajIdx) throw std::runtime_error("c2d timestep error");
    AB.block(0,12 + 3 * id.foot, 12, 3) = _bBlocks[i];
  }

  AB *= _dtTrajectory[trajIdx];

  expmm = AB.exp();
  _aMat[trajIdx] = expmm.block(0,0,12,12);

  for(u32 i = bBlockStartIdx; i < bBlockStartIdx + block_count; i++) {
    //BblockID id = _bBlockIds[i];
    //_bBlocks[i] = expmm.block(0,12 + 3 * id.foot, 12, 3);
    _bBlocks[i] *= _dtTrajectory[trajIdx];
  }
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file   testGaussianFactorGraphB.cpp
 *  @brief  Unit tests for Linear Factor Graph
 *  @author Christian Potthast
 **/

#include <tests/smallExample.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;

#include <string.h>
#include <iostream>

using namespace std;
using namespace gtsam;
using namespace example;

double tol=1e-5;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
TEST( GaussianFactorGraph, equals ) {

  Ordering ordering; ordering += X(1),X(2),L(1);
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
  GaussianFactorGraph fg2 = createGaussianFactorGraph(ordering);
  EXPECT(fg.equals(fg2));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, error ) {
  Ordering ordering; ordering += X(1),X(2),L(1);
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
  VectorValues cfg = createZeroDelta(ordering);

  // note the error is the same as in testNonlinearFactorGraph as a
  // zero delta config in the linear graph is equivalent to noisy in
  // non-linear, which is really linear under the hood
  double actual = fg.error(cfg);
  DOUBLES_EQUAL( 5.625, actual, 1e-9 );
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x1 )
{
  Ordering ordering; ordering += X(1),L(1),X(2);
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);

  GaussianConditional::shared_ptr conditional;
  GaussianFactorGraph remaining;
  boost::tie(conditional,remaining) = fg.eliminateOne(0, EliminateQR);

  // create expected Conditional Gaussian
  Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
  Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
  GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);

  EXPECT(assert_equal(expected,*conditional,tol));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x2 )
{
  Ordering ordering; ordering += X(2),L(1),X(1);
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
  GaussianConditional::shared_ptr actual = fg.eliminateOne(0, EliminateQR).first;

  // create expected Conditional Gaussian
  double sig = 0.0894427;
  Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
  Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
  GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma);

  EXPECT(assert_equal(expected,*actual,tol));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_l1 )
{
  Ordering ordering; ordering += L(1),X(1),X(2);
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
  GaussianConditional::shared_ptr actual = fg.eliminateOne(0, EliminateQR).first;

  // create expected Conditional Gaussian
  double sig = sqrt(2.0)/10.;
  Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
  Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
  GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);

  EXPECT(assert_equal(expected,*actual,tol));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x1_fast )
{
  Ordering ordering; ordering += X(1),L(1),X(2);
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
  GaussianConditional::shared_ptr conditional;
  GaussianFactorGraph remaining;
  boost::tie(conditional,remaining) = fg.eliminateOne(ordering[X(1)], EliminateQR);

  // create expected Conditional Gaussian
  Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
  Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
  GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);

  // Create expected remaining new factor
  JacobianFactor expectedFactor(1, Matrix_(4,2,
             4.714045207910318,                   0.,
                             0.,   4.714045207910318,
                             0.,                   0.,
                             0.,                   0.),
     2, Matrix_(4,2,
           -2.357022603955159,                   0.,
                            0.,  -2.357022603955159,
            7.071067811865475,                   0.,
                            0.,   7.071067811865475),
     Vector_(4, -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), noiseModel::Unit::Create(4));

  EXPECT(assert_equal(expected,*conditional,tol));
  EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_x2_fast )
{
  Ordering ordering; ordering += X(1),L(1),X(2);
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
  GaussianConditional::shared_ptr actual = fg.eliminateOne(ordering[X(2)], EliminateQR).first;

  // create expected Conditional Gaussian
  double sig = 0.0894427;
  Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
  Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
  GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma);

  EXPECT(assert_equal(expected,*actual,tol));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateOne_l1_fast )
{
  Ordering ordering; ordering += X(1),L(1),X(2);
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
  GaussianConditional::shared_ptr actual = fg.eliminateOne(ordering[L(1)], EliminateQR).first;

  // create expected Conditional Gaussian
  double sig = sqrt(2.0)/10.;
  Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
  Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
  GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);

  EXPECT(assert_equal(expected,*actual,tol));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, eliminateAll )
{
  // create expected Chordal bayes Net
  Matrix I = eye(2);

  Ordering ordering;
  ordering += X(2),L(1),X(1);

  Vector d1 = Vector_(2, -0.1,-0.1);
  GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1);

  double sig1 = 0.149071;
  Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2);
  push_front(expected,ordering[L(1)],d2, I/sig1,ordering[X(1)], (-1)*I/sig1,sigma2);

  double sig2 = 0.0894427;
  Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2);
  push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3);

  // Check one ordering
  GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering);
  GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate();
  EXPECT(assert_equal(expected,actual,tol));

  GaussianBayesNet actualQR = *GaussianSequentialSolver(fg1, true).eliminate();
  EXPECT(assert_equal(expected,actualQR,tol));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, copying )
{
  // Create a graph
  Ordering ordering; ordering += X(2),L(1),X(1);
  GaussianFactorGraph actual = createGaussianFactorGraph(ordering);

  // Copy the graph !
  GaussianFactorGraph copy = actual;

  // now eliminate the copy
  GaussianBayesNet actual1 = *GaussianSequentialSolver(copy).eliminate();

  // Create the same graph, but not by copying
  GaussianFactorGraph expected = createGaussianFactorGraph(ordering);

  // and check that original is still the same graph
  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
{
  Ordering ord;
  ord += X(2),L(1),X(1);
  GaussianFactorGraph fg = createGaussianFactorGraph(ord);

  // render with a given ordering
  GaussianBayesNet CBN = *GaussianSequentialSolver(fg).eliminate();

  // True GaussianFactorGraph
  GaussianFactorGraph fg2(CBN);
  GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate();
  EXPECT(assert_equal(CBN,CBN2));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, getOrdering)
{
  Ordering original; original += L(1),X(1),X(2);
  FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
  Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic)));
  Ordering actual = original; actual.permuteInPlace(perm);
  Ordering expected; expected += L(1),X(2),X(1);
  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, optimize_Cholesky )
{
  // create an ordering
  Ordering ord; ord += X(2),L(1),X(1);

  // create a graph
  GaussianFactorGraph fg = createGaussianFactorGraph(ord);

  // optimize the graph
  VectorValues actual = *GaussianSequentialSolver(fg, false).optimize();

  // verify
  VectorValues expected = createCorrectDelta(ord);

  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, optimize_QR )
{
  // create an ordering
  Ordering ord; ord += X(2),L(1),X(1);

  // create a graph
  GaussianFactorGraph fg = createGaussianFactorGraph(ord);

  // optimize the graph
  VectorValues actual = *GaussianSequentialSolver(fg, true).optimize();

  // verify
  VectorValues expected = createCorrectDelta(ord);

  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, combine)
{
  // create an ordering
  Ordering ord; ord += X(2),L(1),X(1);

  // create a test graph
  GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);

  // create another factor graph
  GaussianFactorGraph fg2 = createGaussianFactorGraph(ord);

  // get sizes
  size_t size1 = fg1.size();
  size_t size2 = fg2.size();

  // combine them
  fg1.combine(fg2);

  EXPECT(size1+size2 == fg1.size());
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, combine2)
{
  // create an ordering
  Ordering ord; ord += X(2),L(1),X(1);

  // create a test graph
  GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);

  // create another factor graph
  GaussianFactorGraph fg2 = createGaussianFactorGraph(ord);

  // get sizes
  size_t size1 = fg1.size();
  size_t size2 = fg2.size();

  // combine them
  GaussianFactorGraph fg3 = GaussianFactorGraph::combine2(fg1, fg2);

  EXPECT(size1+size2 == fg3.size());
}

/* ************************************************************************* */
// print a vector of ints if needed for debugging
void print(vector<int> v) {
  for (size_t k = 0; k < v.size(); k++)
    cout << v[k] << " ";
  cout << endl;
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, createSmoother)
{
  GaussianFactorGraph fg1 = createSmoother(2).first;
  LONGS_EQUAL(3,fg1.size());
  GaussianFactorGraph fg2 = createSmoother(3).first;
  LONGS_EQUAL(5,fg2.size());
}

/* ************************************************************************* */
double error(const VectorValues& x) {
  // create an ordering
  Ordering ord; ord += X(2),L(1),X(1);

  GaussianFactorGraph fg = createGaussianFactorGraph(ord);
  return fg.error(x);
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, multiplication )
{
  // create an ordering
  Ordering ord; ord += X(2),L(1),X(1);

  GaussianFactorGraph A = createGaussianFactorGraph(ord);
  VectorValues x = createCorrectDelta(ord);
  Errors actual = A * x;
  Errors expected;
  expected += Vector_(2,-1.0,-1.0);
  expected += Vector_(2, 2.0,-1.0);
  expected += Vector_(2, 0.0, 1.0);
  expected += Vector_(2,-1.0, 1.5);
  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
// Extra test on elimination prompted by Michael's email to Frank 1/4/2010
TEST( GaussianFactorGraph, elimination )
{
  Ordering ord;
  ord += X(1), X(2);
  // Create Gaussian Factor Graph
  GaussianFactorGraph fg;
  Matrix Ap = eye(1), An = eye(1) * -1;
  Vector b = Vector_(1, 0.0);
  SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0);
  fg.add(ord[X(1)], An, ord[X(2)], Ap, b, sigma);
  fg.add(ord[X(1)], Ap, b, sigma);
  fg.add(ord[X(2)], Ap, b, sigma);

  // Eliminate
  GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate();

  // Check sigma
  EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[X(2)]]->get_sigmas()(0),1e-5);

  // Check matrix
  Matrix R;Vector d;
  boost::tie(R,d) = matrix(bayesNet);
  Matrix expected = Matrix_(2,2,
      0.707107,  -0.353553,
      0.0,   0.612372);
  Matrix expected2 = Matrix_(2,2,
      0.707107,  -0.353553,
      0.0,   -0.612372);
  EXPECT(equal_with_abs_tol(expected, R, 1e-6) || equal_with_abs_tol(expected2, R, 1e-6));
}

 /* ************************************************************************* */
// Tests ported from ConstrainedGaussianFactorGraph
/* ************************************************************************* */
TEST( GaussianFactorGraph, constrained_simple )
{
  // get a graph with a constraint in it
  GaussianFactorGraph fg = createSimpleConstraintGraph();
  EXPECT(hasConstraints(fg));

  // eliminate and solve
  VectorValues actual = *GaussianSequentialSolver(fg).optimize();

  // verify
  VectorValues expected = createSimpleConstraintValues();
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, constrained_single )
{
  // get a graph with a constraint in it
  GaussianFactorGraph fg = createSingleConstraintGraph();
  EXPECT(hasConstraints(fg));

  // eliminate and solve
  VectorValues actual = *GaussianSequentialSolver(fg).optimize();

  // verify
  VectorValues expected = createSingleConstraintValues();
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, constrained_multi1 )
{
  // get a graph with a constraint in it
  GaussianFactorGraph fg = createMultiConstraintGraph();
  EXPECT(hasConstraints(fg));

  // eliminate and solve
  VectorValues actual = *GaussianSequentialSolver(fg).optimize();

  // verify
  VectorValues expected = createMultiConstraintValues();
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */

static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);

/* ************************************************************************* */
TEST(GaussianFactorGraph, replace)
{
  Ordering ord; ord += X(1),X(2),X(3),X(4),X(5),X(6);
  SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0));

  GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
      ord[X(1)], eye(3,3), ord[X(2)], eye(3,3), zero(3), noise));
  GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
      ord[X(2)], eye(3,3), ord[X(3)], eye(3,3), zero(3), noise));
  GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
      ord[X(3)], eye(3,3), ord[X(4)], eye(3,3), zero(3), noise));
  GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
      ord[X(5)], eye(3,3), ord[X(6)], eye(3,3), zero(3), noise));

  GaussianFactorGraph actual;
  actual.push_back(f1);
  actual.push_back(f2);
  actual.push_back(f3);
  actual.replace(0, f4);

  GaussianFactorGraph expected;
  expected.push_back(f4);
  expected.push_back(f2);
  expected.push_back(f3);

  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, createSmoother2)
{
  using namespace example;
  GaussianFactorGraph fg2;
  Ordering ordering;
  boost::tie(fg2,ordering) = createSmoother(3);
  LONGS_EQUAL(5,fg2.size());

  // eliminate
  vector<Index> x3var; x3var.push_back(ordering[X(3)]);
  vector<Index> x1var; x1var.push_back(ordering[X(1)]);
  GaussianBayesNet p_x3 = *GaussianSequentialSolver(
      *GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate();
  GaussianBayesNet p_x1 = *GaussianSequentialSolver(
      *GaussianSequentialSolver(fg2).jointFactorGraph(x1var)).eliminate();
  CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, hasConstraints)
{
  FactorGraph<GaussianFactor> fgc1 = createMultiConstraintGraph();
  EXPECT(hasConstraints(fgc1));

  FactorGraph<GaussianFactor> fgc2 = createSimpleConstraintGraph() ;
  EXPECT(hasConstraints(fgc2));

  Ordering ordering; ordering += X(1), X(2), L(1);
  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
  EXPECT(!hasConstraints(fg));
}

#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>

/* ************************************************************************* */
TEST( GaussianFactorGraph, conditional_sigma_failure) {
  // This system derives from a failure case in DDF in which a Bayes Tree
  // has non-unit sigmas for conditionals in the Bayes Tree, which
  // should never happen by construction

  // Reason for the failure: using Vector_() is dangerous as having a non-float gets set to zero, resulting in constraints
  gtsam::Key xC1 = 0, l32 = 1, l41 = 2;

  // noisemodels at nonlinear level
  gtsam::SharedNoiseModel priorModel = noiseModel::Diagonal::Sigmas(Vector_(6, 0.05, 0.05, 3.0, 0.2, 0.2, 0.2));
  gtsam::SharedNoiseModel measModel = noiseModel::Unit::Create(2);
  gtsam::SharedNoiseModel elevationModel = noiseModel::Isotropic::Sigma(1, 3.0);

  double fov = 60; // degrees
  double imgW = 640; // pixels
  double imgH = 480; // pixels
  gtsam::Cal3_S2::shared_ptr K(new gtsam::Cal3_S2(fov, imgW, imgH));

  typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;

  double relElevation = 6;

  Values initValues;
  initValues.insert(xC1,
      Pose3(Rot3(
          -1.,           0.0,  1.2246468e-16,
          0.0,             1.,           0.0,
          -1.2246468e-16,           0.0,            -1.),
          Point3(0.511832102, 8.42819594, 5.76841725)));
  initValues.insert(l32,  Point3(0.364081507, 6.89766221, -0.231582751) );
  initValues.insert(l41,  Point3(1.61051523, 6.7373052, -0.231582751)   );

  NonlinearFactorGraph factors;
  factors.add(PriorFactor<Pose3>(xC1,
      Pose3(Rot3(
          -1.,           0.0,  1.2246468e-16,
          0.0,             1.,           0.0,
          -1.2246468e-16,           0.0,            -1),
          Point3(0.511832102, 8.42819594, 5.76841725)), priorModel));
  factors.add(ProjectionFactor(Point2(333.648615, 98.61535), measModel, xC1, l32, K));
  factors.add(ProjectionFactor(Point2(218.508, 83.8022039), measModel, xC1, l41, K));
  factors.add(RangeFactor<Pose3,Point3>(xC1, l32, relElevation, elevationModel));
  factors.add(RangeFactor<Pose3,Point3>(xC1, l41, relElevation, elevationModel));

  Ordering orderingC; orderingC += xC1, l32, l41;

  // Check that sigmas are correct (i.e., unit)
  GaussianFactorGraph lfg = *factors.linearize(initValues, orderingC);

  GaussianMultifrontalSolver solver(lfg, false);
  GaussianBayesTree actBT = *solver.eliminate();

  // Check that all sigmas in an unconstrained bayes tree are set to one
  BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes()) {
    GaussianConditional::shared_ptr conditional = clique->conditional();
    size_t dim = conditional->dim();
    EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol));
  }
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


#include "Discretization.hpp"
#include "constants.hpp"

#include <boost/numeric/odeint/external/eigen/eigen_algebra.hpp>
#include <boost/numeric/odeint.hpp>

class DiscretizationODE {
private:
    Model::ControlVector u_t, u_t1;
    double sigma, dt;
    Model& model;

public:

    static constexpr size_t n_V_states = 3 + Model::n_states + 2 * Model::n_inputs;
    using state_type = Eigen::Matrix<double, Model::n_states, n_V_states>;

    DiscretizationODE(
        const Model::ControlVector &u_t,
        const Model::ControlVector &u_t1,
        const double &sigma,
        double dt,
        Model& model
    )
    :u_t(u_t)
    ,u_t1(u_t1)
    ,sigma(sigma)
    ,dt(dt)
    ,model(model) {}

    void operator()(const state_type &V, state_type &dVdt, const double t){

        const Model::StateVector &x = V.col(0);
        const Model::ControlVector u = u_t + t / dt * (u_t1 - u_t);

        const double alpha = t / dt;
        const double beta = 1. - alpha;

        const Model::StateMatrix   A_bar  = sigma * model.state_jacobian(x, u);
        const Model::ControlMatrix B_bar  = sigma * model.control_jacobian(x, u);
        const Model::StateVector   f      =         model.ode(x, u);

        Model::StateMatrix Phi_A_xi = V.block<Model::n_states, Model::n_states>(0, 1);
        Model::StateMatrix Phi_A_xi_inverse = Phi_A_xi.inverse();

        size_t cols = 0;

        dVdt.block<Model::n_states,               1>(0, cols) = sigma * f;                                   cols += 1;
        dVdt.block<Model::n_states, Model::n_states>(0, cols) = A_bar * Phi_A_xi;                            cols += Model::n_states;
        dVdt.block<Model::n_states, Model::n_inputs>(0, cols) = Phi_A_xi_inverse * B_bar * alpha;            cols += Model::n_inputs;
        dVdt.block<Model::n_states, Model::n_inputs>(0, cols) = Phi_A_xi_inverse * B_bar * beta;             cols += Model::n_inputs;
        dVdt.block<Model::n_states,               1>(0, cols) = Phi_A_xi_inverse * f;                        cols += 1;
        dVdt.block<Model::n_states,               1>(0, cols) = Phi_A_xi_inverse * (-A_bar * x - B_bar * u);
    }
};

void calculate_discretization (
    Model &model,
    double &sigma,
    Eigen::Matrix<double, Model::n_states, K> &X,
    Eigen::Matrix<double, Model::n_inputs, K> &U,
    array<Model::StateMatrix,   (K-1)> &A_bar,
    array<Model::ControlMatrix, (K-1)> &B_bar,
    array<Model::ControlMatrix, (K-1)> &C_bar,
    array<Model::StateVector,   (K-1)> &Sigma_bar,
    array<Model::StateVector,   (K-1)> &z_bar
) {

    const double dt = 1 / double(K-1);
    using namespace boost::numeric::odeint;
    runge_kutta4<DiscretizationODE::state_type, double, DiscretizationODE::state_type, double, vector_space_algebra> stepper;

    for (size_t k = 0; k < K-1; k++) {
        DiscretizationODE::state_type V;
        V.setZero();
        V.col(0) = X.col(k);
        V.block<Model::n_states,Model::n_states>(0, 1).setIdentity();

        DiscretizationODE discretizationODE(U.col(k), U.col(k+1), sigma, dt, model);
        integrate_n_steps( stepper , discretizationODE , V , 0. , dt/10.0 , 10 );

        size_t cols = 1;
        A_bar[k]      =            V.block<Model::n_states,Model::n_states>(0, cols);   cols += Model::n_states;
        B_bar[k]      = A_bar[k] * V.block<Model::n_states,Model::n_inputs>(0, cols);   cols += Model::n_inputs;
        C_bar[k]      = A_bar[k] * V.block<Model::n_states,Model::n_inputs>(0, cols);   cols += Model::n_inputs;
        Sigma_bar[k]  = A_bar[k] * V.block<Model::n_states,1>(0, cols);                 cols += 1;
        z_bar[k]      = A_bar[k] * V.block<Model::n_states,1>(0, cols);
    }
}


/*!
 * Title ---- intercom-party/LocationManager.hpp
 * Author --- Giacomo Trudu aka `Wicker25` - wicker25[at]gmail[dot]com
 *
 * Copyright (C) 2017 by Giacomo Trudu.
 * All rights reserved.
 */

#ifndef __INTERCOM_PARTY_LOCATION_MANAGER_HPP__
#define __INTERCOM_PARTY_LOCATION_MANAGER_HPP__

#include <intercom-party.hpp>
#include <intercom-party/Exception.hpp>
#include <intercom-party/Location.hpp>

#include <cmath>

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

using namespace boost;

namespace intercom { // Begin main namespace

/*!
 * The location manager.
 */
class LocationManager
{

public:

    /*!
     * The Earth radius in km.
     */
    static constexpr double EarthRadius = 6'371.0;

    /*!
     * Converts degrees in radians.
     *
     * @return The distance (km).
     */
    static double getRadians(double degrees);

    /*!
     * Returns the distance between two locations.
     *
     * @return The distance (km).
     */
    static double getDistance(const Location &first, const Location &second);
};

} // End of main namespace

#endif /* __INTERCOM_PARTY_LOCATION_MANAGER_HPP__ */

// Include inline methods
#include <intercom-party/LocationManager-inl.hpp>


/*
 * spectral_impl.hpp
 *
 *  Created on: Jan 29, 2012
 *      Author: david
 */

#ifndef GRAPHSEG_SPECTRAL_SPECTRALIMPL_HPP_
#define GRAPHSEG_SPECTRAL_SPECTRALIMPL_HPP_

#include "../Common.hpp"
#include "../as_range.hpp"
#include <boost/graph/adjacency_list.hpp>
#include <functional>
#include <iostream>
#include <fstream>
#include <vector>
#ifdef SEGS_DBG_PRINT
#include <boost/format.hpp>
#include <fstream>
#endif

namespace graphseg { namespace detail {

constexpr float c_D_min = 0.001f;

template<typename K>
struct DenseGev
{
	Eigen::Matrix<K,-1,-1> L;
	Eigen::Matrix<K,-1,1> D;
};

template<typename K, typename Graph, typename EdgeWeightMap>
DenseGev<K> dense_graph_to_gev(const Graph& graph, EdgeWeightMap edge_weights)
{
	typedef Eigen::Matrix<K,-1,-1> matrix_t;
	typedef Eigen::Matrix<K,-1,1> vector_t;
	const unsigned int dim = boost::num_vertices(graph);
	// creating matrices
	matrix_t W = matrix_t::Zero(dim,dim);
	vector_t D = vector_t::Zero(dim);

#ifdef SPECTRAL_VERBOSE
	std::cout << "DEBUG: Number of vertices = " << boost::num_vertices(graph) << std::endl;
	std::cout << "DEBUG: Number of edges = " << boost::num_edges(graph) << std::endl;
#endif

	for(auto eid : as_range(boost::edges(graph))) {
		unsigned int ea = boost::source(eid, graph);
		unsigned int eb = boost::target(eid, graph);
		K ew = edge_weights[eid];
		if(std::isnan(ew)) {
			std::cerr << "ERROR: Weight for edge (" << ea << "," << eb << ") is nan!" << std::endl;
			continue;
		}
		if(ew < 0) {
			std::cerr << "ERROR: Weight for edge (" << ea << "," << eb << ") is negative!" << std::endl;
			continue;
		}
		W(ea, eb) = ew;
		W(eb, ea) = ew;
		D[ea] += ew;
		D[eb] += ew;
	}
	// connect disconnected segments to everything
	// FIXME why is this necessary?
#ifdef SPECTRAL_VERBOSE
	std::vector<int> nodes_with_no_connection;
#endif
	for(unsigned int i=0; i<dim; i++) {
		K& di = D[i];
		if(di < c_D_min) {
#ifdef SPECTRAL_VERBOSE
			nodes_with_no_connection.push_back(i);
#endif
			// connect the disconnected cluster to all other clusters with a very small weight
			di = static_cast<K>(1);
			K q = di / static_cast<K>(dim-1);
			for(unsigned int j=0; j<dim; j++) {
				if(j == i) continue;
				W(i,j) = q;
				W(j,i) = q;
			}
		}
	}
#ifdef SPECTRAL_VERBOSE
	if(!nodes_with_no_connection.empty()) {
		std::cout << "DEBUG: Nodes without connections (#=" << nodes_with_no_connection.size() << "): ";
		for(int i : nodes_with_no_connection) {
			std::cout << i << ", ";
		}
		std::cout << std::endl;
	}
#endif
	// compute matrix L = D - W
	matrix_t L = -W;
	for(unsigned int i=0; i<dim; i++) {
		L(i,i) += D[i];
	}
	// ready
	return { L, D };
}

template<typename K>
struct DenseGevTransformed
{
	Eigen::Matrix<K,-1,-1> A;
	Eigen::Matrix<K,-1,1> D_inv_sqrt;
};

template<typename K>
DenseGevTransformed<K> dense_gev_to_ev(const DenseGev<K>& gev)
{
	typedef Eigen::Matrix<K,-1,-1> matrix_t;
	typedef Eigen::Matrix<K,-1,1> vector_t;
	// The general eigenvalue problem
	//     (D - W) x = \lambda D x
	// can be transformed as follows:
	// <=> D^{-1/2} (D - W) x = \lambda D^{1/2} x
	// using z := D^{1/2} x i.e. x = D^{-1/2} z
	// <=> D^{-1/2} (D - W) D^{-1/2} z = \lambda z
	// Thus we have a "normal" eigenvalue problem A z = \lambda z with
	//     A := D^{-1/2} (D - W) D^{-1/2}
	// Using L := D - W this gives for coefficients:
	//		a_ij = l_ij / \sqrt(d_i * d_j)
	// where d_i := D_ii
	const matrix_t& L = gev.L;
	const vector_t& D = gev.D;
	assert(L.cols() == L.rows());
	assert(D.rows() == L.rows());
	const unsigned int N = L.rows();
	vector_t D_inv_sqrt = D.array().sqrt().inverse().matrix();
	matrix_t A(N,N);
	for(unsigned int i=0; i<N; i++) {
		A.col(i) = D_inv_sqrt[i] * L.col(i).cwiseProduct(D_inv_sqrt);
	}
	return { A, D_inv_sqrt };
}

template<typename K>
struct SparseGEVT
{
	SparseMatrix A;
	Eigen::VectorXf D_inv_sqrt;
};

template<typename K, typename Graph, typename EdgeWeightMap>
SparseGEVT<K> sparse_graph_entries(const Graph& graph, EdgeWeightMap edge_weights)
{
	// We want to solve the EV problem: (D - W) x = \lamda D x.
	// Each edge of the graph defines two entries into the symmetric matrix W.
	// The diagonal matrix D is defined via d_i = sum_j{w_ij}.

	// As D is a diagonal matrix the the general problem can be easily transformed
	// into a normal eigenvalue problem by decomposing D = L L^t, which yields L = sqrt(D).
	// Thus the EV problem is: L^{-1} (D - W) L^{-T} y = \lambda y.
	// Eigenvectors can be transformed using x = L^{-T} y.

	// The dimension of the problem
	const int n = boost::num_vertices(graph);

	// Each edge defines two entries (one in the upper and one in the lower).
	// In addition all diagonal entries are non-zero.
	// Thus the number of non-zero entries in the lower triangle is equal to
	// the number of edges plus the number of nodes.
	// This is not entirely true as some connections are possibly rejected.
	// Additionally some connections may be added to assure global connectivity.
	const int nnz_guess = boost::num_edges(graph) + n;

	// collect all non-zero elements
	SparseGEVT<K> sgevt;
	sgevt.A.dim = n;

	std::vector<SparseEntry>& entries = sgevt.A.entries;
	entries.reserve(nnz_guess);

	// also collect diagonal entries
	Eigen::VectorXf& diag = sgevt.D_inv_sqrt;
	diag = Eigen::VectorXf(n);

	// no collect entries
	for(auto eid : as_range(boost::edges(graph))) {
		int ea = static_cast<int>(boost::source(eid, graph));
		int eb = static_cast<int>(boost::target(eid, graph));
		K ew = edge_weights[eid];
		// assure correct edge weight
		if(std::isnan(ew)) {
			std::cerr << "ERROR: Weight for edge (" << ea << "," << eb << ") is nan!" << std::endl;
			continue;
		}
		if(ew < 0) {
			std::cerr << "ERROR: Weight for edge (" << ea << "," << eb << ") is negative!" << std::endl;
			continue;
		}
		// assure that no vertices is connected to self
		if(ea == eb) {
			std::cerr << "ERROR: Vertex " << ea << " is connected to self!" << std::endl;
			continue;
		}
		// In the lower triangle the row index i is bigger or equal than the column index j.
		// The next statement fullfills this requirement.
		if(ea < eb) {
			std::swap(ea, eb);
		}
		entries.push_back(SparseEntry{ea, eb, ew});
		diag[ea] += ew;
		diag[eb] += ew;
	}

	// do the conversion to a normal ev problem
	// assure global connectivity
	for(unsigned int i=0; i<diag.size(); i++) {
		K& v = diag[i];
		if(v == 0) {
			// connect the disconnected cluster to all other clusters with a very small weight
			v = static_cast<K>(1);
			K q = static_cast<K>(1) / static_cast<K>(n-1);
			for(unsigned int j=0; j<i; j++) {
				auto it = std::find_if(entries.begin(), entries.end(), [i, j](const SparseEntry& e) { return e.i == i && e.j == j; });
				if(it == entries.end()) {
					entries.push_back(SparseEntry{i, j, q});
				}
			}
			for(unsigned int j=i+1; j<n; j++) {
				auto it = std::find_if(entries.begin(), entries.end(), [j, i](const SparseEntry& e) { return e.i == j && e.j == i; });
				if(it == entries.end()) {
					entries.push_back(SparseEntry{j, i, q});
				}
			}
			std::cerr << "ERROR: Diagonal is 0! (i=" << i << ")" << std::endl;
		}
		else {
			v = static_cast<K>(1) / std::sqrt(v);
		}
	}

	// a_ij for the transformed "normal" EV problem
	//		A x = \lambda x
	// is computed as follow from the diagonal matrix D and the weight
	// matrix W of the general EV problem
	//		(D - W) x = \lambda D x
	// as follows:
	//		a_ij = - w_ij / sqrt(d_i * d_j) if i != j
	//		a_ii = 1
	for(SparseEntry& e : entries) {
		e.weight = - e.weight * diag[e.i] * diag[e.j];
	}
	for(unsigned int i=0; i<n; i++) {
		entries.push_back(SparseEntry{i, i, static_cast<K>(1)});
	}

	// sort entries to form a lower triangle matrix
	std::sort(entries.begin(), entries.end(), [](const SparseEntry& a, const SparseEntry& b) {
		return (a.j != b.j) ? (a.j < b.j) : (a.i < b.i);
	});

	return sgevt;
}

template<typename K>
void transform_gev_solution(const Eigen::Matrix<K,-1,1>& D_inv_sqrt, std::vector<EigenComponent>& ec)
{
	// We have x = D^{-1/2} z (see dense_gev_to_ev)
	// thus x_i = z_i / \sqrt(d_i)
	const unsigned int dim = D_inv_sqrt.rows();
	for(std::size_t i=0; i<ec.size(); i++) {
		ec[i].eigenvector = ec[i].eigenvector.cwiseProduct(D_inv_sqrt);
	}
}

/** Assembles edge weights from eigenvalues and eigenvectors */
template<typename Graph>
Eigen::VectorXf ev_to_graph_weights(const Graph& graph, const std::vector<EigenComponent>& solution)
{
	typedef Eigen::Matrix<float,-1,-1> matrix_t;
	typedef Eigen::Matrix<float,-1,1> vector_t;
	vector_t edge_weight = vector_t::Zero(boost::num_edges(graph));
//	// later we weight by eigenvalues
//	// find a positive eigenvalue (need to do this because of ugly instabilities ...
//	Real ew_pos = -1.0f;
//	for(unsigned int i=0; ; i++) {
//		if(solver.eigenvalues()[i] > 0) {
//			// F IXME magic to get a not too small eigenvalue
////			unsigned int x = (n_used_ew + i)/2;
//			unsigned int x = i + 5;
//			ew_pos = solver.eigenvalues()[x];
//			break;
//		}
//	}
//	// compute normalized weights from eigenvalues
//	Vec weights = Vec::Zero(n_used_ew);
//	for(unsigned int k=0; k<n_used_ew; k++) {
//		Real ew = solver.eigenvalues()[k + 1];
//		if(ew <= ew_pos) {
//			ew = ew_pos;
//		}
//		weights[k] = 1.0f / std::sqrt(ew);
//	}
//	std::cout << "Weights = " << weights.transpose() << std::endl;
	// look into first eigenvectors
	// skip first component
	for(unsigned int k=0; k<solution.size(); k++) {
		const EigenComponent& eigen = solution[k];
		// omit if eigenvalue is not positive
		float ew = eigen.eigenvalue;
		// FIXME this is due to numerical instabilities
		if(ew <= 0.0001f) {
			continue;
		}
		// weight by eigenvalue
		float w = 1.0f / std::sqrt(ew);
		// get eigenvector and normalize
		vector_t ev = eigen.eigenvector;
		ev = (ev - ev.minCoeff() * vector_t::Ones(ev.rows())) / (ev.maxCoeff() - ev.minCoeff());
		// for each edge compute difference of eigenvector values
		vector_t e_k = vector_t::Zero(edge_weight.rows());
		// FIXME proper edge indexing
		unsigned int eid_index = 0;
		for(auto eid : as_range(boost::edges(graph))) {
			e_k[eid_index] = std::abs(ev[boost::source(eid, graph)] - ev[boost::target(eid, graph)]);
			eid_index++;
		}
#ifdef SPECTRAL_VERBOSE
		std::cout << "DEBUG w=" << w << " e_k.maxCoeff()=" << e_k.maxCoeff() << std::endl;
#endif
//		e_k /= e_k.maxCoeff();
//		for(unsigned int i=0; i<e_k.rows(); i++) {
//			e_k[i] = std::exp(-e_k[i]);
//		}
		e_k *= w;

#ifdef SEGS_DBG_PRINT
		{
			std::ofstream ofs((boost::format("/tmp/edge_weights_%03d.txt") % k).str());
			for(unsigned int i=0; i<e_k.rows(); i++) {
				ofs << e_k[i] << std::endl;
			}
		}
#endif
		//
		edge_weight += e_k;
	}
	return edge_weight;
}

template<typename K, int ROWS, int COLS>
inline void print_matrix(std::ostream& os, const Eigen::Matrix<K,ROWS,COLS>& m)
{
	for(unsigned int j=0; j<m.rows(); j++) {
		for(unsigned int i=0; i<m.cols(); i++) {
			os << m(j,i);
			if(i+1 == m.cols()) {
				os << "\n";
			}
			else {
				os << "\t";
			}
		}
	}
}

template<typename Graph, typename EdgeWeightMap>
std::vector<EigenComponent> solve_dense(const Graph& graph, EdgeWeightMap edge_weights,
	const std::function<std::vector<EigenComponent>(const Eigen::MatrixXf&)>& solver)
{
	typedef float K;

	DenseGev<K> gev = dense_graph_to_gev<K>(graph, edge_weights);
#ifdef SEGS_DBG_PRINT
		{	std::ofstream ofs("/tmp/L.tsv"); print_matrix(ofs, gev.L); }
		{	std::ofstream ofs("/tmp/D.tsv"); print_matrix(ofs, gev.D); }
#endif

	DenseGevTransformed<K> gevt = dense_gev_to_ev(gev);
#ifdef SEGS_DBG_PRINT
		{	std::ofstream ofs("/tmp/A.tsv"); print_matrix(ofs, gevt.A); }
		{	std::ofstream ofs("/tmp/D_inv_sqrt.tsv"); print_matrix(ofs, gevt.D_inv_sqrt); }
#endif

	std::vector<EigenComponent> v_ec = solver(gevt.A);

	transform_gev_solution(gevt.D_inv_sqrt, v_ec);

	return v_ec;
}

template<typename Graph, typename EdgeWeightMap>
std::vector<EigenComponent> solve_sparse(const Graph& graph, EdgeWeightMap edge_weights,
	const std::function<std::vector<EigenComponent>(const SparseMatrix&)>& solver)
{
	typedef float K;
	SparseGEVT<K> sgevt = sparse_graph_entries<K>(graph, edge_weights);
	std::vector<EigenComponent> v_ec = solver(sgevt.A);
	transform_gev_solution(sgevt.D_inv_sqrt, v_ec);
	return v_ec;
}

}}

#endif


#include <frovedis.hpp>
#include <frovedis/matrix/blas_wrapper.hpp>
#include <frovedis/matrix/lapack_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // creating a colmajor matrix local from file
    colmajor_matrix_local<float> A (
           make_rowmajor_matrix_local_load<float>("./sample_2x2"));

    colmajor_matrix_local<float> B (
           make_rowmajor_matrix_local_load<float>("./sample_2x1"));

    std::vector<int> ipiv;   // empty ipiv array
    getrf<float> (A,ipiv);   // A will be factorized and ipiv will contain pivoting info
    getrs<float> (A,B,ipiv); // solving AX=B, B will be overwritten with result matrix X

    // checking whether the above operations successfully taken place
    B.to_rowmajor().save("./out_2x1");
    BOOST_CHECK (system("diff ./out_2x1 ./ref_2x1") == 0);
    system("rm -f ./out_2x1");
}


//////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::students_t::log_unnormalized_pdf.hpp    //
//                                                                              //
//  (C) Copyright 2009 Erwann Rogard                                            //
//  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_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_STUDENTS_T_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_STUDENTS_T_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/numeric/conversion/converter.hpp>
// #include <boost/math/policies/policy.hpp> // TODO

namespace boost{
namespace math{

    template<typename T,typename Policy>
    T
    log_unnormalized_pdf(
        const boost::math::students_t_distribution<T,Policy>& d,
        const T& x
    ){

        typedef boost::numeric::converter<T,int> int2R_t;

        T r1 = int2R_t::convert(1);
        T r2 = int2R_t::convert(2);

        T nu = d.degrees_of_freedom();
        T m = ( nu + r1 ) / r2;
        T y = ( x * x ) / nu;
        return (- m ) * math::log1p(y);
    }

}// math
}// boost

#endif


/* based upon http://www.boost.org/doc/libs/1_55_0/libs/multiprecision/doc/html/boost_multiprecision/tut/floats/cpp_dec_float.html
 * 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)
 * Copyright ???? 20??. */

#include <iostream>
#include <utility>

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

using boost::multiprecision::cpp_dec_float;
typedef boost::multiprecision::number<cpp_dec_float<64> > mp_type;

int main(void)
{
    // Operations at fixed precision and full numeric_limits support:
    mp_type b = 2;
    std::cout << std::numeric_limits<mp_type>::digits << std::endl;
    // Note that digits10 is the same as digits, since we're base 10! :
    std::cout << std::numeric_limits<mp_type>::digits10 << std::endl;
    // We can use any C++ std lib function, lets print all the digits as well:
    std::cout << std::setprecision(std::numeric_limits<mp_type>::max_digits10) << log(b) << std::endl; // print log(2)
    // We can also use any function from Boost.Math:
    std::cout << boost::math::tgamma(b) << std::endl;
    // These even work when the argument is an expression template:
    std::cout << boost::math::tgamma(b * b) << std::endl;
    // And since we have an extended exponent range we can generate some really large numbers here (4.0238726007709377354370243e+2564):
    std::cout << boost::math::tgamma(mp_type(1000)) << std::endl;

    return 0;
}


#pragma once

#include <Eigen/Dense>
#include <cmath>
#include <iomanip>
#include <random>
#include <vector>

namespace Robotics {
    template <int N>
    using ColumnVector = Eigen::Matrix<double, N, 1>;

    template <int N>
    using SquareMatrix = Eigen::Matrix<double, N, N>;

    template <int N, int M>
    using Matrix = Eigen::Matrix<double, N, M>;

    double deg2rad(double deg)
    {
        return deg * 3.14 / 180.0;  // TODO: replace 3.14 with M_PI (cross-platform)
    }

    class NormalDistributionRandomGenerator {
      public:
        template <int Size>
        ColumnVector<Size> GetColumnVector()
        {
            ColumnVector<Size> rand_vector;

            for (auto i = 0; i < rand_vector.size(); i++) rand_vector(i) = distribution(generator);

            return rand_vector;
        }

      private:
        std::default_random_engine generator;
        std::normal_distribution<double> distribution{0.0, 1.0};
    };

}  // namespace Robotics


#include <iostream>
#include "EigenMatrix.h"
#include <Eigen/Dense>
#include <Eigen/LU>

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> ColumnVector;
typedef Eigen::Matrix<double, 1, Eigen::Dynamic> RowVector;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;

ColumnVector* CastToColumnVector(void* ptr);

ColumnVector* NewColumnVector();

ColumnVector* NewColumnVector(const ColumnVector& v);

RowVector* CastToRowVector(void* ptr);

RowVector* NewRowVector();

RowVector* NewRowVector(const RowVector& v);

void* EigenMatrix_Create(int rows, int columns)
{
	auto m = new Matrix(rows, columns);
	for (int y = 0; y < columns; y++)
		for (int x = 0; x < rows; x++)
			(*m)(x, y) = 0;

	return m;
}

void* EigenMatrix_CreateIdentity(int rows, int columns)
{
	auto m = new Matrix(rows, columns);
	for (int y = 0; y < columns; y++)
	{
		for (int x = 0; x < rows; x++)
		{
			if (x == y)
				(*m)(x,y) = 1;
			else
				(*m)(x, y) = 0;
		}
	}

	return m;
}

void EigenMatrix_Release(void* ptr)
{
	auto obj = static_cast<Matrix*>(ptr);
	if (obj != nullptr)
	{
		delete obj;
		obj = nullptr;
	}
}

Matrix* CastToMatrix(void* ptr)
{
	return static_cast<Matrix*>(ptr);
}

Matrix* NewMatrix(const Matrix& m)
{
	return new Matrix(m.rows(), m.cols());
}

Matrix* NewMatrix()
{
	return new Matrix();
}

int EigenMatrix_Rows(void* ptr)
{
	auto m = CastToMatrix(ptr);
	return (int)m->rows();
}

int EigenMatrix_Columns(void* ptr)
{
	auto m = CastToMatrix(ptr);
	return (int)m->cols();
}

double EigenMatrix_GetXY(void* ptr, int x, int y)
{
	auto m = CastToMatrix(ptr);
	return (*m)(x, y);
}

void EigenMatrix_SetXY(void* ptr, int x, int y, double value)
{
	auto m = CastToMatrix(ptr);
	(*m)(x, y) = value;
}

double EigenMatrix_GetX(void* ptr, int x)
{
	auto m = CastToMatrix(ptr);
	return (*m)(x);
}

void EigenMatrix_SetX(void* ptr, int x, double value)
{
	auto m = CastToMatrix(ptr);
	(*m)(x) = value;
}

void* EigenMatrix_Transpose(void* ptr)
{
	auto m = CastToMatrix(ptr);
	auto m2 = NewMatrix();
	(*m2) = m->transpose();
	return m2;
}

void* EigenMatrix_Conjugate(void* ptr)
{
	auto m = CastToMatrix(ptr);
	auto m2 = NewMatrix();
	(*m2) = m->conjugate();
	return m2;
}

void* EigenMatrix_Adjoint(void* ptr)
{
	auto m = CastToMatrix(ptr);
	auto m2 = NewMatrix();
	(*m2) = m->adjoint();
	return m2;
}

void* EigenMatrix_Inverse(void* ptr)
{
	auto m = CastToMatrix(ptr);
	auto m2 = NewMatrix();
	(*m2) = m->inverse();
	return m2;
}

BOOL EigenMatrix_IsInvertible(void* ptr)
{
	auto m = CastToMatrix(ptr);
	Eigen::FullPivLU<Matrix> lu(*m);

	return lu.isInvertible();
}

void* EigenMatrix_TryInverse(void* ptr)
{
	auto m = CastToMatrix(ptr);
	Eigen::FullPivLU<Matrix> lu(*m);

	if (lu.isInvertible())
	{
		auto inv = NewMatrix();
		(*inv) = lu.inverse();
		return inv;
	}
	else
	{
		return nullptr;
	}
}

double EigenMatrix_Determinant(void* ptr)
{
	auto m = CastToMatrix(ptr);
	return m->determinant();
}

double EigenMatrix_Trace(void* ptr)
{
	auto m = CastToMatrix(ptr);
	return m->trace();
}

BOOL EigenMatrix_IsIdentity(void* ptr)
{
	auto m = CastToMatrix(ptr);
	return m->isIdentity();
}

BOOL EigenMatrix_IsDiagonal(void* ptr)
{
	auto m = CastToMatrix(ptr);
	return m->isDiagonal();
}

BOOL EigenMatrix_IsUpperTriangular(void* ptr)
{
	auto m = CastToMatrix(ptr);
	return m->isUpperTriangular();
}

BOOL EigenMatrix_IsLowerTriangular(void* ptr)
{
	auto m = CastToMatrix(ptr);
	return m->isLowerTriangular();
}

void* EigenMatrix_MulScalar(void* ptr1, double s)
{
	auto m1 = CastToMatrix(ptr1);
	auto m = NewMatrix(*m1);
	(*m) = (*m1) * s;
	return m;
}

void* EigenMatrix_DivideScalar(void* ptr1, double s)
{
	auto m1 = CastToMatrix(ptr1);
	auto m = NewMatrix(*m1);
	(*m) = (*m1) / s;
	return m;
}

void* EigenMatrix_MulMatrix(void* ptr1, void* ptr2)
{
	auto m1 = CastToMatrix(ptr1);
	auto m2 = CastToMatrix(ptr2);
	auto m = NewMatrix(*m1);
	(*m) = (*m1) * (*m2);
	return m;
}

void* EigenMatrix_AddMatrix(void* ptr1, void* ptr2)
{
	auto m1 = CastToMatrix(ptr1);
	auto m2 = CastToMatrix(ptr2);
	auto m = NewMatrix(*m1);
	(*m) = (*m1) + (*m2);
	return m;
}

void* EigenMatrix_SubMatrix(void* ptr1, void* ptr2)
{
	auto m1 = CastToMatrix(ptr1);
	auto m2 = CastToMatrix(ptr2);
	auto m = NewMatrix(*m1);
	(*m) = (*m1) - (*m2);
	return m;
}

void* EigenMatrix_MulColumnVector(void* ptr1, void* ptr2)
{
	auto m1 = CastToMatrix(ptr1);
	auto v2 = CastToColumnVector(ptr2);
	auto v = NewColumnVector(*v2);
	(*v) = (*m1) * (*v2);
	return v;
}

void* EigenMatrix_MulRowVector(void* ptr1, void* ptr2)
{
	auto m1 = CastToMatrix(ptr1);
	auto v2 = CastToRowVector(ptr2);
	auto v = NewRowVector(*v2);
	(*v) = (*m1) * (*v2);
	return v;
}

void* EigenMatrix_Block(void* ptr, int startRox, int startCol, int blockRows, int blockCols)
{
	auto m = CastToMatrix(ptr);
	auto m2 = NewMatrix();
	(*m2) = m->block(startRox, startCol, blockRows, blockCols);
	return m2;
}

void* EigenMatrix_Reshaped(void* ptr, int rows, int cols)
{
	auto m = CastToMatrix(ptr);
	auto m2 = NewMatrix();
	(*m2) = m->reshaped(rows, cols);
	return m2;
}

void* EigenMatrix_ColPivHouseholderQr_Vec(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToColumnVector(ptr2);
	auto x = NewColumnVector();

	(*x) = m->colPivHouseholderQr().solve(*v);
	return x;
}

void* EigenMatrix_ColPivHouseholderQr_Mat(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToMatrix(ptr2);
	auto x = NewMatrix();

	(*x) = m->colPivHouseholderQr().solve(*v);
	return x;
}

void* EigenMatrix_PartialPivLu_Vec(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToColumnVector(ptr2);
	auto x = NewColumnVector();

	(*x) = m->partialPivLu().solve(*v);
	return x;
}

void* EigenMatrix_PartialPivLu_Mat(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToMatrix(ptr2);
	auto x = NewMatrix();

	(*x) = m->partialPivLu().solve(*v);
	return x;
}

void* EigenMatrix_FullPivLu_Vec(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToColumnVector(ptr2);
	auto x = NewColumnVector();

	(*x) = m->fullPivLu().solve(*v);
	return x;
}

void* EigenMatrix_FullPivLu_Mat(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToMatrix(ptr2);
	auto x = NewMatrix();

	(*x) = m->fullPivLu().solve(*v);
	return x;
}

void* EigenMatrix_HouseholderQr_Vec(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToColumnVector(ptr2);
	auto x = NewColumnVector();

	(*x) = m->householderQr().solve(*v);
	return x;
}

void* EigenMatrix_HouseholderQr_Mat(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToMatrix(ptr2);
	auto x = NewMatrix();

	(*x) = m->householderQr().solve(*v);
	return x;
}

void* EigenMatrix_LLT_Vec(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToColumnVector(ptr2);
	auto x = NewColumnVector();

	(*x) = m->llt().solve(*v);
	return x;
}

void* EigenMatrix_LLT_Mat(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToMatrix(ptr2);
	auto x = NewMatrix();

	(*x) = m->llt().solve(*v);
	return x;
}

void* EigenMatrix_LDLT_Vec(void* ptr1, void* ptr2)
{
	auto m1 = CastToMatrix(ptr1);
	auto v = CastToColumnVector(ptr2);
	auto x = NewColumnVector();

	(*x) = m1->ldlt().solve(*v);
	return x;
}

void* EigenMatrix_LDLT_Mat(void* ptr1, void* ptr2)
{
	auto m1 = CastToMatrix(ptr1);
	auto v = CastToMatrix(ptr2);
	auto x = NewMatrix();

	(*x) = m1->ldlt().solve(*v);
	return x;
}

void* EigenMatrix_BdcSvd_Vec(void* ptr1, void* ptr2, int options)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToColumnVector(ptr2);
	auto x = NewColumnVector();

	(*x) = m->bdcSvd(options).solve(*v);
	return x;
}

void* EigenMatrix_BdcSvd_Mat(void* ptr1, void* ptr2, int options)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToMatrix(ptr2);
	auto x = NewMatrix();

	(*x) = m->bdcSvd(options).solve(*v);
	return x;
}

void* EigenMatrix_JacobiSvd_Vec(void* ptr1, void* ptr2, int options)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToColumnVector(ptr2);
	auto x = NewColumnVector();

	(*x) = m->jacobiSvd(options).solve(*v);
	return x;
}

void* EigenMatrix_JacobiSvd_Mat(void* ptr1, void* ptr2, int options)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToMatrix(ptr2);
	auto x = NewMatrix();

	(*x) = m->jacobiSvd(options).solve(*v);
	return x;
}

void* EigenMatrix_FullPivHouseholderQr_Vec(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToColumnVector(ptr2);
	auto x = NewColumnVector();

	(*x) = m->fullPivHouseholderQr().solve(*v);
	return x;
}

void* EigenMatrix_FullPivHouseholderQr_Mat(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToMatrix(ptr2);
	auto x = NewMatrix();

	(*x) = m->fullPivHouseholderQr().solve(*v);
	return x;
}

void* EigenMatrix_CompleteOrthogonalDecomposition_Vec(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToColumnVector(ptr2);
	auto x = NewColumnVector();

	(*x) = m->completeOrthogonalDecomposition().solve(*v);
	return x;
}

void* EigenMatrix_CompleteOrthogonalDecomposition_Mat(void* ptr1, void* ptr2)
{
	auto m = CastToMatrix(ptr1);
	auto v = CastToMatrix(ptr2);
	auto x = NewMatrix();

	(*x) = m->completeOrthogonalDecomposition().solve(*v);
	return x;
}

double EigenMatrix_RelativeError_Vec(void* ptr1, void* ptr2, void* ptr3)
{
	auto A = CastToMatrix(ptr1);
	auto b = CastToColumnVector(ptr2);
	auto x = CastToColumnVector(ptr3);

	return ((*A) * (*x) - (*b)).norm() / b->norm();
}

double EigenMatrix_RelativeError_Mat(void* ptr1, void* ptr2, void* ptr3)
{
	auto A = CastToMatrix(ptr1);
	auto b = CastToMatrix(ptr2);
	auto x = CastToMatrix(ptr3);

	return ((*A) * (*x) - (*b)).norm() / b->norm();
}

void* EigenMatrix_Eigenvalues(void* ptr)
{
	auto m = CastToMatrix(ptr);

	Eigen::SelfAdjointEigenSolver<Matrix> solver(*m);

	if (solver.info() != Eigen::Success)
		return nullptr;
	else
	{
		auto x = NewColumnVector();
		(*x) = solver.eigenvalues();

		return x;
	}
}

void* EigenMatrix_Eigenvectors(void* ptr)
{
	auto m = CastToMatrix(ptr);

	Eigen::SelfAdjointEigenSolver<Matrix> solver(*m);

	if (solver.info() != Eigen::Success)
		return nullptr;
	else
	{
		auto x = NewMatrix();
		(*x) = solver.eigenvectors();

		return x;
	}
}

BOOL EigenMatrix_EigenValuesVectors(void* ptr, void** values, void** vectors)
{
	auto m = CastToMatrix(ptr);

	Eigen::SelfAdjointEigenSolver<Matrix> solver(*m);
	*values = nullptr;
	*vectors = nullptr;

	if (solver.info() != Eigen::Success)
	{
		return FALSE;
	}
	else
	{
		auto _values = solver.eigenvalues();
		auto _vectors = solver.eigenvectors();

		auto v = NewColumnVector(_values);
		for (auto i = 0; i < _values.size(); i++)
			(*v)[i] = _values[i];

		auto m = NewMatrix(_vectors);
		for (auto i = 0; i < _vectors.size(); i++)
			(*m)(i) = _vectors(i);

		(*values) = v;
		(*vectors) = m;

		return TRUE;
	}
}


#include <Eigen/Dense>
#include <iostream>

#include "ransac.h"

using namespace Eigen;
using namespace std;

int main(){
    Matrix3d sigma;
    sigma << 0.2, 0, 0, 0, 0.3, 0, 0, 0, 0.5;
    Vector3d X = Vector3d::Random(3,1);
    Vector3d A = Vector3d::Random(3,1);
    Vector3d B = Vector3d::Random(3,1);

    cout << "================" << endl;

    Ransac myRansac(100, 1e-4);
    double dist = myRansac.mahalanobis_distance(X, sigma, A, B);
    Matrix3Xd temp = MatrixXd::Random(3,100);
    vector<Matrix3d> cov;
    for(int i=0; i< 100; ++i){
        cov.push_back(sigma);
    }
    Eigen::MatrixXd temp2 = myRansac.removeOutlierPoints(temp, cov);

    return 0;
}