#include <iostream>
#include <omp.h>
#include <X11/Xlib.h>
#include <boost/graph/adjacency_list.hpp>
#include <boost/config.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/depth_first_search.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/floyd_warshall_shortest.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <vector>
#include <algorithm>

using namespace std;
using namespace boost;

struct vertex_info {
    std::string name;
    int value1;
    int value2;
};

typedef property<edge_weight_t, int> EdgeWeightProperty;
typedef adjacency_list<vecS, vecS, directedS, vertex_info, EdgeWeightProperty> graph_t;
typedef graph_traits<graph_t>::vertex_iterator vertex_iter;
typedef graph_traits<graph_t>::vertex_descriptor Vertice;
typedef graph_traits<graph_t>::edge_iterator edge_iter;
typedef graph_traits <graph_t >::edge_descriptor Arista;

#include <fstream>
#include <stdio.h>
#include <graphics.h>
#include <stdlib.h>
#define M 300

//LGRAPH

void generarGrafo(graph_t& graph, char* filename)
{
        int numeros[3];
        int i;
        std::ifstream  data(filename);

        std::string line;
        while(std::getline(data,line))
        {
            i = 0;
            std::stringstream  lineStream(line);
            std::string        cell;
            while(std::getline(lineStream,cell,','))
            {
                std::cout << cell << " ";
                if(isdigit(cell[0]))
                {
                    numeros[i] = atoi(cell.c_str());
                    i++;
                }
                if (i == 3)
                    add_edge(numeros[0],numeros[1],numeros[2], graph);
            }

            std::cout << std::endl;
        }
}
void printEdgeWeights(const graph_t graph) {

  std::pair<edge_iter, edge_iter> ei;

  typedef boost::property_map<graph_t, boost::edge_weight_t>::const_type WeightMap;
  // The following line will not compile:
  WeightMap weights = boost::get(boost::edge_weight_t(), graph);

 // EdgeIterator edge;

  std::ofstream out("test.csv");
  out << "Estacion Origen,Estacion Destino,Tiempo\n";
  for (ei = edges(graph); ei.first != ei.second; ++ei.first) {
    //std::cout <<*ei.first << "," << boost::get(weights, *ei.first) << std::endl;
   /* std::stringstream temp;
    temp << *edge;
    std::string tempstring = temp.str();

    tempstring = tempstring.substr(1,tempstring.length()-2);
    */
    out << source(*ei.first,graph) << ","  << target(*ei.first,graph) << "," << boost::get(weights, *ei.first) << std::endl;
    //out << (std::string)(*edge).substr(1,(std::string)(*edge).length() - 1) << "," << boost::get(weights, *edge) << std::endl;
  }
  out.close();
}

void printNodos(const graph_t& graph)
{
    std::ofstream outNodos("Nodos.csv");

    outNodos << "Nombre,CoordenadaX,CoordenadaY\n";

    for (int i =0; i < num_vertices(graph); i++)
    {
        /*std::cout << "nombre " << graph[i].name << "    valor x " << graph[i].value1 << "     valor y " << graph[i].value2 << std::endl;
*/
        outNodos<<graph[i].name<< "," << graph[i].value1 << "," << graph[i].value2<<std::endl;
    }
}

void generarNodos(graph_t& graph, char* filename)
{
        int i = -1;
        std::ifstream  data(filename);

        std::string line;

        while(std::getline(data,line))
        {
            std::stringstream  lineStream(line);
            std::string        cell;
            int n = 0;
            if(i >= 0)
            {
                while(std::getline(lineStream,cell,','))
                {
                    std::cout << cell << " ";

                    if(n == 0)
                    {
                        graph[i].name = cell.c_str();
                    }

                    if (n == 1)
                    {
                        graph[i].value1 = atoi(cell.c_str());
                    }
                    if (n == 2)
                        graph[i].value2 = atoi(cell.c_str());
                    n++;
                }
            }

          i++;
          std::cout << std::endl;
        }
}

void swap(int* aa,int* bb)
{
	int tt=*aa;
	*aa=*bb;
	*bb=tt;
}

void midpointline(int x1,int y1,int x2,int y2)
{
	int dx,dy,dd,incry,incre,incrne,slopegt1=0;
	dx=abs(x1-x2);dy=abs(y1-y2);
	if(dy>dx)
	{
		swap(&x1,&y1);
		swap(&x2,&y2);
		swap(&dx,&dy);
		slopegt1=1;
	}
	if(x1>x2)
	{
		swap(&x1,&x2);
		swap(&y1,&y2);
	}
	if(y1>y2)
		incry=-1;
	else
		incry=1;
	dd=2*dy-dx;
	incre=2*dy;
	incrne=2*(dy-dx);
	while(x1<x2)
	{
		if(dd<=0)
			dd+=incre;
		else
		{
			dd+=incrne;
			y1+=incry;
		}
		x1++;
		if(slopegt1)
			putpixel(y1,x1,RED);
		else
			putpixel(x1,y1,RED);
	}
}//Cierre de MidPoint

class DFSVertexVisitor : public default_dfs_visitor
{
public:
  void discover_vertex(graph_t::vertex_descriptor v, graph_t g)
  {
    std::cout << "DFS: Nodo #: " <<  v << " con nombre: " << g[v].name << " y coordenadas: " << g[v].value1 << "," << g[v].value2 << std::endl;
  }
};//Cierre clase DFS

class BFSVertexVisitor : public default_bfs_visitor
{
public:
  void discover_vertex(graph_t::vertex_descriptor v, graph_t g)
  {
    std::cout << "BFS: Nodo #: "<< v << " con nombre: " << g[v].name << " y coordenadas: " << g[v].value1 << "," << g[v].value2 << std::endl;;
  }
};//Cierre clase BFS

//Función para buscar un dato
int buscarNombre(graph_t g, std::string busqueda)
{
    vector<string> nombres;
    for (int i =0; i < num_vertices(g); i++)
    {
        nombres.push_back(g[i].name);

    }//Cierre de for

    vector<string>::iterator vec_it;
    vec_it = std::find (nombres.begin(), nombres.end(), busqueda);
    if(vec_it!=nombres.end())
        //cout << "Se encuentra en la posicion: " << vec_it - nombres.begin();
        return vec_it - nombres.begin();
    else
        return -1;

    nombres.clear();

}//Cierre función buscar

//Función del tiempo
double diffclock(clock_t clock1,clock_t clock2)
{
double diffms = ((long double )(clock1 - clock2) / CLOCKS_PER_SEC)* 1000.0;
    return diffms;
}

int main()
{

XInitThreads();

clock_t start; clock_t end;
 graph_t g;

/*
    add_edge(0,1, 2, g);
    add_edge(1,4, 8, g);
    add_edge(1,3, 8, g);
    add_edge(3,2, 7, g);
    add_edge(4,7, 3, g);
    add_edge(7,4, 6, g);
    add_edge(4,5, 1, g);
    add_edge(5,6, 0, g);
    add_edge(2,5, 7, g);

g[0].name = "Reforma";
g[0].value1 = -100;
g[0].value2 = 0;
g[1].name = "Popotla";
g[1].value1 = -100;
g[1].value2 = 50;
g[2].name = "Tacubaya";
g[2].value1 = 100;
g[2].value2 = 50;
g[3].name = "Polanco";
g[3].value1 = 150;
g[3].value2 = 0;
g[4].name ="Auditorio";
g[4].value1= 50;
g[4].value2= -50;
g[5].name ="Camarones";
g[5].value1=0;
g[5].value2=-50;
g[6].name="Hidalgo";
g[6].value2=0;
g[6].value1=-100;
g[7].name="Taxqueña";
g[7].value1=50;
g[7].value2=-100;

*/
//MENÚ

 int n; int temp1; int temp2; string a; string b; int w; int ejex; int ejey; int n2;
 	//variables de GEOMETRÍA
	int x1,x2,y1,y2;
	int gd=DETECT,gm;

while (1) {
n=0;
		cout<<endl;
cout << endl <<"¿Qué quieres hacer?  1.Configurar Grafo, 2.Ejecutar un algoritmo 3. Importar Grafo 4.Exportar Grafo 5.Salir del programa " <<endl;
cin>>n2;
if (n2==1)
{
while (1) {
		cout<<endl;
		std::ofstream dotfile("test.dot");
write_graphviz(dotfile,g);
      cout <<"Este es el grafo" << endl;
   	std::pair<edge_iter, edge_iter> ei;
		for(ei = edges(g); ei.first != ei.second; ++ei.first)
{
	cout << g[source(*ei.first,g)].name <<" a " << g[target(*ei.first,g)].name<< endl;
}
cout << endl <<"¿Qué quieres hacer?  1.Insertar un nuevo enlace, 2.Eliminar un enlace, 3.Eliminar un nodo 4.Imprimir la gráfica 5.Regresar al menú anterior " <<endl;
cin>>n;
   if (n==1)
{

cout<<"Ingresa el nodo inicial"<< endl;
cin>> a;
temp1 = buscarNombre(g, a);
if(temp1==-1)
{
add_vertex(g);
g[num_vertices(g)-1].name=a;

cout<<"La estación " << a <<" se acaba de agregar" << endl;
cout<<"Ingrese el eje x: ";
cin>> ejex;
g[num_vertices(g)-1].value1=ejex;
cout<<"Ingrese el eje y: ";
cin>> ejey;
g[num_vertices(g)-1].value2=ejey;
temp1=num_vertices(g)-1;
}
cout<<endl <<"Ingresa el nodo final"<< endl;
cin>> b;
temp2 = buscarNombre(g, b);
if(temp2==-1)
{
add_vertex(g);
g[num_vertices(g)-1].name=b;

cout<<"La estación " << b <<" se acaba de agregar" << endl;
cout<<"Ingrese el eje x: ";
cin>> ejex;
g[num_vertices(g)-1].value1=ejex;
cout<<"Ingrese el eje y: ";
cin>> ejey;
g[num_vertices(g)-1].value2=ejey;
temp2=num_vertices(g)-1;
}
cout<<"Ingrese el tiempo: ";
cin >> w;

   add_edge(temp1, temp2, w , g );
}//Cierre de 1
else if(n==2)
{
cout<<"Ingresa el nodo inicial"<< endl;
cin>> a;
temp1 = buscarNombre(g, a);
cout<<"Ingresa el nodo final"<< endl;
cin>> b;
temp2 = buscarNombre(g, b);

if(temp1==-1||temp2==-1)
	cout<<"Ese enlace no existe"<<endl;
else {
cout<<"Se eliminó el enlace" <<endl;
remove_edge(temp1, temp2, g );
}

}//Cierre de 2
else if (n==3)
{
cout<<"¿Qué nodo quieres eliminar: ";
cin >>a;
temp1 = buscarNombre(g,a);

if (temp1!=-1)
{
    clear_vertex(temp1,g);
    remove_vertex(temp1,g);
	cout<<"El nodo "<< a <<" se eliminó correctamente" << endl;
}
else
cout<<"El nodo no existe";
}
else if(n==4)
{

initgraph(&gd,&gm,"..\\bgi");

for(ei = edges(g); ei.first != ei.second; ++ei.first)
{
string str=g[source(*ei.first,g)].name;
char* writable=new char[str.size()+1];
copy(str.begin(), str.end(), writable);
writable[str.size()]='\0';
 str=g[target(*ei.first,g)].name;
char* writable2=new char[str.size()+1];
copy(str.begin(), str.end(), writable2);
writable2[str.size()]='\0';
	x1= g[source(*ei.first,g)].value1 +320;
	y1=240 - g[source(*ei.first,g)].value2 ;
	x2= g[target(*ei.first,g)].value1 +320;
	y2=240 - g[target(*ei.first,g)].value2 ;

	outtextxy(x1-2,y1-3, writable);
	outtextxy(x2-2,y2-3, writable2);
	midpointline(x1,y1,x2,y2);
}
	getch();
	closegraph();

}//Cierre de 3

else if (n==5)
	break;
else cout<<endl <<"Esa opción no es válida";
}//Cierre de while 2
}//Cierre de if 1 menú general

else if (n2==2)
{
/*Menú opción 2 del general*/
while(1)
{
if(num_vertices(g)==0)
{
cout<<"Genera o importa un grafo antes de ejecutar los algoritmos"<<endl;
break;
}
else
{
cout << endl <<"¿Qué algoritmo quieres ejecutar?  1.DFS, 2.BFS, 3.KRUSKAL 4.PRIM 5.DIJKSTRA 6.FLOYD WARSHALL  7.Ejecutar Todos los algoritmos 8.Regresar al menú anterior" <<endl;
cin>> n;
if(n==7)
{
	int nhilos = 6;
	double tiempos[6];
	omp_set_num_threads(nhilos);
#pragma omp parallel private (start,end)
{

	int id = omp_get_thread_num();  //obtener el id de cada hilo
	// cout<<"Prueba " << id;
if (id==0)
{
 DFSVertexVisitor dfsVisitor;
    start = clock();
    depth_first_search(g, visitor(dfsVisitor).root_vertex(1));
    end = clock();
tiempos[0]=diffclock(end,start);
    //std::cout << diffclock(end,start) << "MILISEGUNDOS Terminó DFS"<<endl;
}//Cierre de 0
if(id==1)
{
    BFSVertexVisitor bfsVisitor;
    start = clock();
    breadth_first_search(g, vertex(1, g), visitor(bfsVisitor));
    end = clock();
tiempos[1]=diffclock(end,start);
    //std::cout << diffclock(end,start) << "MILISEGUNDOS Terminó BFS"<<endl;

}//Cierre de 1
if(id==2)
{
property_map<graph_t, edge_weight_t>::type EdgeWeightMap = get(edge_weight_t(), g);
//Kruskal
    std::vector < Arista > spanning_tree;
    start = clock();
    kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));
    end = clock();
   tiempos[2]=diffclock(end,start);
    //std::cout << diffclock(end,start) << "MILISEGUNDOS Terminó Kruskal"<<endl;
}//Cierre de 2

if(id==3)
{
std::vector < Vertice > p(num_vertices(g));

    start = clock();
    prim_minimum_spanning_tree(g, &p[0]);
    end = clock();
    tiempos[3]=diffclock(end,start);
    //std::cout << diffclock(end,start) << "MILISEGUNDOS Terminó Prim"<<endl;

}//Cierre de 3

if(id == 4)
{
std::vector < Vertice > p(num_vertices(g));
    start = clock();
    std::vector<int> d(num_vertices(g));

    Vertice s = vertex(1, g);
    dijkstra_shortest_paths(g, s, predecessor_map(make_iterator_property_map(p.begin(),
                           get(vertex_index, g))).distance_map(make_iterator_property_map(d.begin(),
                           get(vertex_index, g))));

    end = clock();
    tiempos[4]=diffclock(end,start);
  // std::cout << diffclock(end,start) << "MILISEGUNDOS Terminó Dijkstra"<<endl;

}//Cierre 4

if(id == 5)
{
property_map<graph_t, edge_weight_t>::type EdgeWeightMap = get(edge_weight_t(), g);
//FLOYD WARSHALL
    start = clock();
    std::map<Vertice, std::map<Vertice, int> > matrix;
    floyd_warshall_all_pairs_shortest_paths(g,matrix,weight_map(EdgeWeightMap));
    end = clock();
    tiempos[5]=diffclock(end,start);
   // std::cout << diffclock(end,start) << "MILISEGUNDOS Terminó Floyd Warshal"<<endl;
}//Cierre 5
	//if(id== nhilos - 1) { cout<<"Prueba " << id; }

}//Cierre de pragma
for (int i=0; i<6;++i)
{
	cout<< tiempos[i] <<endl;
}//Cierre de for
w=0;
cout<<"¿Quieres ver la comparación del tiempo de ejecución?  si/1 no/0: ";
cin>>w;
if(w==1)
{
//cout << *max_element(tiempos,tiempos+6) << endl;
initgraph(&gd, &gm, "C:\\TC\\BGI");
char str[16];// = malloc(16);

snprintf(str,16,"%d", (int)(*max_element(tiempos,tiempos+6)));
outtextxy(0,0,str);
outtextxy(0,450, "0ms");
for( int i = 0; i < 6; i++)
{
	snprintf(str,16,"%d", (int)(i*(*max_element(tiempos,tiempos+6)/5)));
	outtextxy(0, 450 -  i*(450/5), str);
snprintf(str,16,"%d", i + 1);
outtextxy(100 + (i*75), 450, str);
}

for(int i = 0; i < 6; i++)
{
   bar(100 + i*75, 450 - (int)(tiempos[i]*(450/(*max_element(tiempos,tiempos+6)))) + 1, 150 + i*75, 450);
   //bar(175,200,225,450);
}
line(0,450,600,450);
line(50,0,50,450);
   getch();
   closegraph();
}//Cierre de if imprimir tablas

}//Cierre de 7

 else if (n==1)
{
 std::cout << std::endl << "Búsqueda DFS" << std::endl;
    DFSVertexVisitor dfsVisitor;
    start = clock();
    depth_first_search(g, visitor(dfsVisitor).root_vertex(1));
    end = clock();
    std::cout << diffclock(end,start) << "MILISEGUNDOS";
}//Cierre de 1
else if(n==2)
{

std::cout << std::endl << "Búsqueda BFS" << std::endl;
    BFSVertexVisitor bfsVisitor;
    start = clock();
    breadth_first_search(g, vertex(1, g), visitor(bfsVisitor));
    end = clock();
    std::cout << diffclock(end,start) << "MILISEGUNDOS";

}//Cierre de 2
else if(n==3)
{
property_map<graph_t, edge_weight_t>::type EdgeWeightMap = get(edge_weight_t(), g);
//Kruskal
    std::vector < Arista > spanning_tree;
    start = clock();
    kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));
    end = clock();
    std::cout << diffclock(end,start) << "MILISEGUNDOS";

    std::cout << std::endl << "Imprimiendo nodos en MST (kruskal):" << std::endl;
    for (std::vector <Arista>::iterator aristait = spanning_tree.begin();
    aristait != spanning_tree.end(); ++aristait)
    {
        std::cout << g[source(*aristait, g)].name << " <--> " << g[target(*aristait, g)].name
        << " con tiempo " << EdgeWeightMap[*aristait] << " minutos" << std::endl;
    }

}//Cierre de 3

else if(n==4)
{

//PRIM

    std::vector < Vertice > p(num_vertices(g));

    start = clock();
    prim_minimum_spanning_tree(g, &p[0]);
    end = clock();
    std::cout << diffclock(end,start) << "MILISEGUNDOS";

    std::cout << std::endl << "Prim..." << std::endl;

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  property_map<graph_t, vertex_distance_t>::type distance = get(vertex_distance, g);
  property_map<graph_t, vertex_index_t>::type indexmap = get(vertex_index, g);
  prim_minimum_spanning_tree
    (g, *vertices(g).first, &p[0], distance, weightmap, indexmap,
     default_dijkstra_visitor());
#else
  prim_minimum_spanning_tree(g, &p[0]);
#endif

 for (std::size_t i = 0; i != p.size(); ++i)
    if (p[i] != i)
      std::cout << "parent[" << g[i].name << "] = " << g[p[i]].name << std::endl;
    else
 std::cout << "parent[" << g[i].name << "] = no parent" << std::endl;

}//Cierre de 4

else if(n==5)
{
//Dijkstra
std::vector < Vertice > p(num_vertices(g));
    start = clock();
    std::vector<int> d(num_vertices(g));
cout<<"¿De qué estación quieres partir?: ";
string op;
cin>>op;
temp1 = buscarNombre(g, op);
if(temp1!=-1)
{
    Vertice s = vertex(temp1, g);
    dijkstra_shortest_paths(g, s, predecessor_map(make_iterator_property_map(p.begin(),
                           get(vertex_index, g))).distance_map(make_iterator_property_map(d.begin(),
                           get(vertex_index, g))));

    end = clock();
   std::cout << diffclock(end,start) << "MILISEGUNDOS";

    std::cout << std::endl << "Dijkstra partiendo de la estación: " << op << std::endl;
    graph_traits <graph_t >::vertex_iterator vi, vend;
    for (tie(vi, vend) = vertices(g); vi != vend; ++vi)
       {
           if(d[*vi] != std::numeric_limits<int>::max())
{
               std::cout << "distancia a " << g[*vi].name << " = " << d[*vi] << ", ";
 std::cout << "proviene del nodo " << g[p[*vi]].name << std::endl;
}//Cierre de if
           else
               std::cout << "distancia a " << g[*vi].name << " = No se puede " << endl;

       }
}
else cout <<"EL nodo no se encuentra";

}//Cierre de 5

else if(n==6)
{

property_map<graph_t, edge_weight_t>::type EdgeWeightMap = get(edge_weight_t(), g);
//FLOYD WARSHALL
    std::cout << std::endl << "Floyd Warshall...." << std::endl;

    start = clock();
    std::map<Vertice, std::map<Vertice, int> > matrix;
    floyd_warshall_all_pairs_shortest_paths(g,matrix,weight_map(EdgeWeightMap));
    end = clock();
    std::cout << diffclock(end,start) << "MILISEGUNDOS";
cout<<"¿De qué estación quieres partir?: ";
string op;
string op2;
cin>>op;
temp1 = buscarNombre(g, op);
cout<<"¿A qué estación quieres llegar?: ";
cin>>op2;
temp1 = buscarNombre(g, op);
temp2 = buscarNombre(g, op2);
if(temp1!=-1&&temp2!=-1)
{
if(matrix[temp1][temp2] == numeric_limits<int>::max())
	cout << "No se puede llegar a esa estación";
else
	cout << "El tiempo es de: "<< matrix[temp1][temp2] <<" minutos";
}
else
cout<<endl <<"No se encontró alguna de las estaciones";
}//Cierre de 6

else if(n==8)
 break;
else
cout<<"Esa opción no es válida";
}
}//Cierre de while opcion 2
}//Cierre de if 2 menu general
else if(n2==3)
{

generarGrafo(g, "test.csv");
generarNodos(g, "Nodos.csv");

}//Cierre de 3
else if(n2==4)
{

printEdgeWeights(g);
printNodos(g);

}//Cierre de 4
else if(n2==5)
	exit(0);
else
cout<<"Esa opción no es válida";

}//Cierre de while general

    return 0;
}


/*
 [auto_generated]
 libs/numeric/odeint/test/runge_kutta_controlled_concepts.cpp

 [begin_description]
 This file tests the Stepper concepts of odeint with all Controlled Runge-Kutta steppers.
 It's one of the main tests of odeint.
 [end_description]

 Copyright 2012 Karsten Ahnert
 Copyright 2012-2013 Mario Mulansky

 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)
 */

// disable checked iterator warning for msvc
#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_runge_kutta_controlled_concepts

#include <vector>
#include <cmath>
#include <iostream>

#include <boost/numeric/odeint/config.hpp>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/ref.hpp>
#include <boost/bind.hpp>
#include <boost/utility.hpp>
#include <boost/type_traits/add_reference.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/insert_range.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/inserter.hpp>

#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54_classic.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_fehlberg78.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer.hpp>

#include "prepare_stepper_testing.hpp"
#include "dummy_odes.hpp"

using std::vector;

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;

const double result = 2.2; // two steps

const double eps = 1.0e-14;

template< class Stepper , class System >
void check_controlled_stepper_concept( Stepper &stepper , System system , typename Stepper::state_type &x )
{
    typedef Stepper stepper_type;
    typedef typename stepper_type::deriv_type container_type;
    //typedef typename stepper_type::order_type order_type;  controlled_error_stepper don't necessarily have a order (burlish-stoer)
    typedef typename stepper_type::time_type time_type;

    time_type t = 0.0 , dt = 0.1;
    controlled_step_result step_result = stepper.try_step( system , x , t , dt );

    BOOST_CHECK_MESSAGE( step_result == success , "step result: " << step_result ); // error = 0 for constant system -> step size is always too small
}

template< class ControlledStepper , typename T >
struct perform_controlled_stepper_test
{
    typedef T vector_space_type;
    void operator()( void ) const
    {
        using std::abs;
        vector_space_type x;
        x = 2.0;
        ControlledStepper controlled_stepper;
        constant_system_functor_vector_space sys;
#ifndef _MSC_VER
        // dont run this for MSVC due to compiler bug 697006
        check_controlled_stepper_concept( controlled_stepper ,
                                          constant_system_vector_space< vector_space_type , vector_space_type , typename ControlledStepper::time_type >
                                          , x );
#else
        check_controlled_stepper_concept( controlled_stepper , boost::cref( sys ) , x );
#endif
        check_controlled_stepper_concept( controlled_stepper , boost::cref( sys ) , x );
        BOOST_CHECK( (abs( x - result )) < eps );
    }
};

template< class ControlledStepper , typename T >
struct perform_controlled_stepper_test< ControlledStepper , std::vector<T> >
{
    typedef std::vector<T> vector_type;
    void operator()( void )
    {
        using std::abs;
        vector_type x( 1 , 2.0 );
        ControlledStepper controlled_stepper;
        constant_system_functor_standard sys;
#ifndef _MSC_VER
        // dont run this for MSVC due to compiler bug 697006

        check_controlled_stepper_concept( controlled_stepper ,
                                          constant_system_standard< vector_type , vector_type , typename ControlledStepper::time_type > ,
                                          x );
#else
        check_controlled_stepper_concept( controlled_stepper , boost::cref( sys ) , x );
#endif
        check_controlled_stepper_concept( controlled_stepper , boost::cref( sys ) , x );
        BOOST_CHECK( (abs( x[0] - result )) < eps );
    }
};

template< class ControlledStepper >
struct perform_controlled_stepper_test< ControlledStepper , vector_space_type >
{
    void operator()( void ) const
    {
        using std::abs;
        vector_space_type x;
        x = 2.0;
        ControlledStepper controlled_stepper;
        constant_system_functor_vector_space sys;
#ifndef _MSC_VER
        // dont run this for MSVC due to compiler bug 697006
        check_controlled_stepper_concept( controlled_stepper ,
                                          constant_system_vector_space< vector_space_type , vector_space_type , typename ControlledStepper::time_type >
                                          , x );
#else
        check_controlled_stepper_concept( controlled_stepper , boost::cref( sys ) , x );
#endif
        check_controlled_stepper_concept( controlled_stepper , boost::cref( sys ) , x );
        BOOST_CHECK( (abs( x - result )) < eps );
    }
};

template< class ControlledStepper , typename T >
struct perform_controlled_stepper_test< ControlledStepper , boost::array<T,1> >
{
    typedef boost::array<T,1> array_type;
    void operator()( void )
    {
        using std::abs;
        array_type x;
        x[0] = 2.0;
        ControlledStepper controlled_stepper;
        constant_system_functor_standard sys;
#ifndef _MSC_VER
        // dont run this for MSVC due to compiler bug 697006
        check_controlled_stepper_concept( controlled_stepper , constant_system_standard< array_type , array_type , typename ControlledStepper::time_type > , x );
#else
        check_controlled_stepper_concept( controlled_stepper , boost::cref( sys ) , x );
#endif
        check_controlled_stepper_concept( controlled_stepper , boost::cref( sys ) , x );
        BOOST_CHECK( (abs( x[0] - result )) < eps );
    }
};

template< class State > class controlled_stepper_methods : public mpl::vector<
    controlled_runge_kutta< runge_kutta_cash_karp54_classic< State , typename detail::extract_value_type<State>::type > > ,
    controlled_runge_kutta< runge_kutta_dopri5< State , typename detail::extract_value_type<State>::type > > ,
    controlled_runge_kutta< runge_kutta_fehlberg78< State , typename detail::extract_value_type<State>::type > > ,
    bulirsch_stoer< State , typename detail::extract_value_type<State>::type >
    > { };

typedef mpl::copy
<
    container_types ,
    mpl::inserter
    <
        mpl::vector0<> ,
        mpl::insert_range
        <
            mpl::_1 ,
            mpl::end< mpl::_1 > ,
            controlled_stepper_methods< mpl::_2 >
            >
        >
    >::type all_controlled_stepper_methods;

BOOST_AUTO_TEST_SUITE( controlled_runge_kutta_concept_test )

BOOST_AUTO_TEST_CASE_TEMPLATE( controlled_stepper_test , ControlledStepper , all_controlled_stepper_methods )
{
    perform_controlled_stepper_test< ControlledStepper , typename ControlledStepper::state_type > tester;
    tester();
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @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_INEARBYINT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_INEARBYINT_HPP_INCLUDED

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

 /*!

    @ingroup group-arithmetic
    Function object implementing inearbyint capabilities

    Computes the integer conversion of the nearbyint of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    as_integer_t<T> r = inearbyint(x);
    @endcode

    is similar to:

    @code
    as_integer_t<T> r = toint_s(nearbyint(x));
    @endcode

    @par Note:
    Speed can be gained using inearbyint(x, fast_) that uses @ref
    toint in place of @ref toint_s,  but be aware that large values can be not correctly converted
    and that invalid entries lead to undefined results

  **/

  as_integer_t<Value> inearbyint(Value const& x);
} }
#endif

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

#endif


#include <iostream>
#include <fstream>
#include <boost/numeric/ublas/matrix.hpp>
#include <vector>

struct coord
{
  int x;
  int y;
};

struct line
{
  coord coord1;
  coord coord2;
};

struct thermalLines
{
  int maxX;
  int maxY;
  std::vector<line> lines;
};

boost::numeric::ublas::matrix<int> getThermalMap();
thermalLines getCoords();
coord getCoord(std::string *coordString);
void updateThermalMap(boost::numeric::ublas::matrix<int> *thermalMap, coord *coord1, coord *coord2);
int calcDangerousZones(boost::numeric::ublas::matrix<int> *thermalMap);
void addHozLineToThermalMap(boost::numeric::ublas::matrix<int> *thermalMap, coord *coord1, coord *coord2);
void addVerLineToThermalMap(boost::numeric::ublas::matrix<int> *thermalMap, coord *coord1, coord *coord2);

int main()
{
  //start somewhere, one must!
  std::cout << "Hello world" << std::endl;

  thermalLines thermalLines = getCoords();

  boost::numeric::ublas::matrix<int> thermalMap = boost::numeric::ublas::zero_matrix(thermalLines.maxX + 1, thermalLines.maxY + 1);

  for (size_t i = 0; i < thermalLines.lines.size(); i++)
  {
    updateThermalMap(&thermalMap, &thermalLines.lines[i].coord1, &thermalLines.lines[i].coord2);
  }

  int dangerousZones = calcDangerousZones(&thermalMap);
  std::cout << "amount of dangerous zones: " << dangerousZones << std::endl;
  std::cout << "ow sheesh :s" << std::endl;
  return 0;
}

int calcDangerousZones(boost::numeric::ublas::matrix<int> *thermalMap)
{
  int dangerousZones = 0;
  for (size_t r = 0; r < thermalMap->size1(); r++)
  {
    for (size_t c = 0; c < thermalMap->size2(); c++)
    {
      if ((*thermalMap)(r, c) && (*thermalMap)(r, c) > 1)
      {
        int test = (*thermalMap)(r, c);
        dangerousZones++;
      }
    }
  }
  return dangerousZones;
}

thermalLines getCoords()
{
  std::ifstream thermalLinesFile("hello-world.txt");
  if (!thermalLinesFile.is_open())
  {
    std::cout << "No file 'hello-world.txt' found! Did you name it correctly?" << std::endl;
    exit(1);
  }

  int maxX = 0;
  int maxY = 0;
  std::vector<line> lines;

  std::string coordString;
  while (std::getline(thermalLinesFile, coordString))
  {
    std::string delimiter = "->";
    int delimiterPos = coordString.find(delimiter);
    std::string coordString1 = coordString.substr(0, delimiterPos);
    std::string coordString2 = coordString.substr(delimiterPos + delimiter.size());

    line line = {
      coord1 : getCoord(&coordString1),
      coord2 : getCoord(&coordString2)
    };

    if (line.coord1.x > maxX)
    {
      maxX = line.coord1.x;
    }
    if (line.coord2.x > maxX)
    {
      maxX = line.coord2.x;
    }
    if (line.coord1.y > maxY)
    {
      maxY = line.coord1.y;
    }
    if (line.coord2.y > maxY)
    {
      maxY = line.coord2.y;
    }

    lines.push_back(line);
  }
  return {
    maxX : maxX,
    maxY : maxY,
    lines : lines
  };
}

void updateThermalMap(boost::numeric::ublas::matrix<int> *thermalMap, coord *coord1, coord *coord2)
{
  if ((*coord1).x == (*coord2).x || (*coord1).y == (*coord2).y)
  {
    addHozLineToThermalMap(thermalMap, coord1, coord2);
  }
  else
  {
    addVerLineToThermalMap(thermalMap, coord1, coord2);
  }
}

void addHozLineToThermalMap(boost::numeric::ublas::matrix<int> *thermalMap, coord *coord1, coord *coord2)
{
  int rowStart;
  int rowStop;
  if ((*coord1).x < (*coord2).x)
  {
    rowStart = (*coord1).x;
    rowStop = (*coord2).x;
  }
  else
  {
    rowStart = (*coord2).x;
    rowStop = (*coord1).x;
  }

  int columnStart;
  int columnStop;
  if ((*coord1).y < (*coord2).y)
  {
    columnStart = (*coord1).y;
    columnStop = (*coord2).y;
  }
  else
  {
    columnStart = (*coord2).y;
    columnStop = (*coord1).y;
  }

  for (size_t r = rowStart; r <= rowStop; r++)
  {
    for (size_t c = columnStart; c <= columnStop; c++)
    {
      (*thermalMap)(r, c) += 1;
    }
  }
}

void addVerLineToThermalMap(boost::numeric::ublas::matrix<int> *thermalMap, coord *coord1, coord *coord2)
{
  if ((*coord1).x < (*coord2).x && (*coord1).y < (*coord2).y)
  {
    for (coord i = {x : (*coord1).x, y : (*coord1).y}; i.x <= (*coord2).x && i.y <= (*coord2).y; i = {x : i.x + 1, y : i.y + 1})
    {
      (*thermalMap)(i.x, i.y) += 1;
    }
  }
  else if ((*coord1).x > (*coord2).x && (*coord1).y < (*coord2).y)
  {
    for (coord i = {x : (*coord1).x, y : (*coord1).y}; i.x >= (*coord2).x && i.y <= (*coord2).y; i = {x : i.x - 1, y : i.y + 1})
    {
      (*thermalMap)(i.x, i.y) += 1;
    }
  }
  else if ((*coord1).x > (*coord2).x && (*coord1).y > (*coord2).y)
  {
    for (coord i = {x : (*coord1).x, y : (*coord1).y}; i.x >= (*coord2).x && i.y >= (*coord2).y; i = {x : i.x - 1, y : i.y - 1})
    {
      (*thermalMap)(i.x, i.y) += 1;
    }
  }
  else
  {
    for (coord i = {x : (*coord1).x, y : (*coord1).y}; i.x <= (*coord2).x && i.y >= (*coord2).y; i = {x : i.x + 1, y : i.y - 1})
    {
      (*thermalMap)(i.x, i.y) += 1;
    }
  }
}

coord getCoord(std::string *coordString)
{
  coord coordinate;
  int commaPos = coordString->find(',');
  coordinate.x = std::stoi(coordString->substr(0, commaPos));
  coordinate.y = std::stoi(coordString->substr(commaPos + 1));
  return coordinate;
}

day 05/Marnix - C++/hello-world-2/hello-world.cpp


#ifndef POSITIONBASEDDYNAMICS_CCONSTRAINT_H
#define POSITIONBASEDDYNAMICS_CCONSTRAINT_H

#include <memory>
#include <vector>
#include <CVector3.hpp>
#include <physics/CParticle.hpp>
#include <Eigen/Dense>
#include <Common.h>

namespace PBD {

    const static double constraintEpsilon = 0.0001;

template<typename T_real=double>
class CConstraint
{
public:
    typedef std::shared_ptr< CConstraint<T_real> > Ptr;

    typedef const std::shared_ptr< CConstraint<T_real> > ConstPtr;

public:
    CConstraint() = default;

    virtual ~CConstraint() = default;

    CConstraint(const CConstraint& C): m_particles(C.m_particles)
    {}

    CConstraint(CConstraint&& C)noexcept : m_particles(std::move(C.m_particles))
    {}

    CConstraint & operator= (const CConstraint& C)
    {
        m_particles = C.m_particles;
        return *this;
    }

    CConstraint & operator= (CConstraint&& C) noexcept
    {
        m_particles = std::move(C.m_particles);
        return *this;
    }

    virtual bool isSatisfied()=0;// { return true; };

    virtual bool isPredSatisfied()=0;// { return true; };

    virtual bool project()=0;// { return true; };

    std::vector< PBD::CParticle<>* > m_particles;
    T_real m_epsilon;

};

    template<typename T_real=double, typename T_vector=Eigen::Vector3d>
    class CNoPenetrationConstraint: public CConstraint<T_real>
    {
    public:
        typedef std::shared_ptr< CNoPenetrationConstraint > Ptr;

        typedef const std::shared_ptr< CNoPenetrationConstraint > ConstPtr;

        CNoPenetrationConstraint(PBD::CParticle<>* p1, PBD::CParticle<>* p2)
        {
            CConstraint<T_real>::m_particles.push_back(p1);
            CConstraint<T_real>::m_particles.push_back(p2);
            CConstraint<T_real>::m_epsilon = PBD::constraintEpsilon;
        }

        bool isSatisfied()
        {
            //if (CConstraint<T_real>::m_particles.size()!=2) return true;
            return (CConstraint<T_real>::m_particles[0]->m_position -
                    CConstraint<T_real>::m_particles[1]->m_position).norm() >
                   (CConstraint<T_real>::m_particles[0]->m_size + CConstraint<T_real>::m_particles[1]->m_size) * 0.5;
        }

        bool isPredSatisfied()
        {
            //if (CConstraint<T_real>::m_particles.size()!=2) return true;
            return (CConstraint<T_real>::m_particles[0]->m_predPosition -
                    CConstraint<T_real>::m_particles[1]->m_predPosition).norm() >
                   (CConstraint<T_real>::m_particles[0]->m_size + CConstraint<T_real>::m_particles[1]->m_size) * 0.5;
        }

        bool project()
        {
            T_vector posAdjustmentDir =
                    CConstraint<T_real>::m_particles[0]->m_predPosition -
                    CConstraint<T_real>::m_particles[1]->m_predPosition;

            T_real err = posAdjustmentDir.norm() -
                    (CConstraint<T_real>::m_particles[0]->m_size + CConstraint<T_real>::m_particles[1]->m_size)*0.5;

            if (err < CConstraint<T_real>::m_epsilon)
            {
                err -= CConstraint<T_real>::m_epsilon;

                posAdjustmentDir.normalize();
                if (CConstraint<T_real>::m_particles[0]->getMass()>0 && CConstraint<T_real>::m_particles[1]->getMass()>0)
                {
                    CConstraint<T_real>::m_particles[0]->m_predPosition -= posAdjustmentDir * err * 0.5;
                    CConstraint<T_real>::m_particles[1]->m_predPosition += posAdjustmentDir * err * 0.5;
                }
                else if (CConstraint<T_real>::m_particles[0]->getMass()>0)
                    CConstraint<T_real>::m_particles[0]->m_predPosition -= posAdjustmentDir * err * 1.0;
                else if (CConstraint<T_real>::m_particles[1]->getMass()>0)
                    CConstraint<T_real>::m_particles[1]->m_predPosition += posAdjustmentDir * err * 1.0;
            }

            //TODO: Apply friction
            //TODO: Apply restitution

            return isPredSatisfied();
        }

    };

    template<typename T_real=double, typename T_vector=Eigen::Vector3d>
    class CConstantDistanceConstraint: public CConstraint<T_real>
    {
    public:
        typedef std::shared_ptr< CConstantDistanceConstraint > Ptr;

        typedef const std::shared_ptr< CConstantDistanceConstraint > ConstPtr;

        CConstantDistanceConstraint(PBD::CParticle<>* p1, PBD::CParticle<>* p2)
        {
            CConstraint<T_real>::m_particles.push_back(p1);
            CConstraint<T_real>::m_particles.push_back(p2);
            m_targetDistance2 = (p1->m_position - p2->m_position).squaredNorm();
            m_targetDistance = std::sqrt(m_targetDistance2);
            m_targetDistanceTolerance = 0.01;
            m_targetDistanceTolerance2 = m_targetDistanceTolerance * m_targetDistanceTolerance;
            CConstraint<T_real>::m_epsilon = PBD::constraintEpsilon;
            m_damping = 0.99;
        }

        bool isSatisfied()
        {
            T_real err = (CConstraint<T_real>::m_particles[0]->m_position -
                          CConstraint<T_real>::m_particles[1]->m_position).squaredNorm() - m_targetDistance2;

            return -m_targetDistanceTolerance2 < err && err < m_targetDistanceTolerance2;
        }

        bool isPredSatisfied()
        {
            T_real err = (CConstraint<T_real>::m_particles[0]->m_predPosition -
                          CConstraint<T_real>::m_particles[1]->m_predPosition).squaredNorm() - m_targetDistance2;

            return m_targetDistanceTolerance2 < err && err < m_targetDistanceTolerance2;
        }

        bool project()
        {
            T_vector posAdjustmentDir =
                    CConstraint<T_real>::m_particles[0]->m_predPosition -
                    CConstraint<T_real>::m_particles[1]->m_predPosition;

            T_real err = posAdjustmentDir.norm() - m_targetDistance;
            if (err > m_targetDistanceTolerance && !std::isinf(err) && !std::isnan(err))
            {
                err = err  - m_targetDistanceTolerance + CConstraint<T_real>::m_epsilon;
            }
            else if (err < -m_targetDistanceTolerance && !std::isinf(err) && !std::isnan(err))
            {
                err = err + m_targetDistanceTolerance - CConstraint<T_real>::m_epsilon;
            }
            else
            {
                //return isPredSatisfied();
                return true;
            }

            posAdjustmentDir.normalize();
            T_real posAdjustmentDirX = posAdjustmentDir(0);
            T_real posAdjustmentDirY = posAdjustmentDir(1);
            T_real posAdjustmentDirZ = posAdjustmentDir(2);
            if (CConstraint<T_real>::m_particles[0]->getMass()>0 && CConstraint<T_real>::m_particles[1]->getMass()>0)
            {
                CConstraint<T_real>::m_particles[0]->m_predPosition -= posAdjustmentDir * err * 0.5 * m_damping;
                CConstraint<T_real>::m_particles[1]->m_predPosition += posAdjustmentDir * err * 0.5 * m_damping;
            }
            else if (CConstraint<T_real>::m_particles[0]->getMass()>0)
                CConstraint<T_real>::m_particles[0]->m_predPosition -= posAdjustmentDir * err * 1.0 * m_damping;
            else if (CConstraint<T_real>::m_particles[1]->getMass()>0)
                CConstraint<T_real>::m_particles[1]->m_predPosition += posAdjustmentDir * err * 1.0 * m_damping;

            //return true;
            return isPredSatisfied();
        }

        void setTargetDistance( const T_real& d )
        {
            m_targetDistance  = d;
            m_targetDistance2 = d*d;
        }

        void setDistanceTolerance( const T_real& d )
        {
            m_targetDistanceTolerance  = d;
            m_targetDistanceTolerance2 = d*d;
        }

        void setConstraintStiffness ( const T_real& d )
        {
            m_damping = d;
        }

    protected:
        T_real m_targetDistance;
        T_real m_targetDistance2;
        T_real m_targetDistanceTolerance;
        T_real m_targetDistanceTolerance2;
        T_real m_damping;
    };

    template<typename T_real=double, typename T_vector=Eigen::Vector3d, typename T_matrix=Eigen::Matrix3d>
    class CShapeMatchingConstraint: public CConstraint<T_real>
    {
    public:
        typedef std::shared_ptr< CShapeMatchingConstraint > Ptr;

        typedef const std::shared_ptr< CShapeMatchingConstraint > ConstPtr;

        //TODO: Make this parameters global and dependent of the timeStep
        constexpr static T_real MAX_ANGLE_MAGNITUDE = 0.05;
        constexpr static T_real MAX_AXIS_DIFF = 0.3;

        explicit CShapeMatchingConstraint(const std::vector< PBD::CParticle<>* >& particles)
        {
            for (const auto &p:particles)
            {
                CConstraint<T_real>::m_particles.push_back(p);
            }
            CConstraint<T_real>::m_epsilon = PBD::constraintEpsilon;

            m_restCoM = computeCenterOfMass(CConstraint<T_real>::m_particles);
            computeRestEigenVectors(CConstraint<T_real>::m_particles,m_restCovMat);

            //Populate target particle positions w.r.t. initial CoM and Orientation
            for (const auto &p:particles)
            {
                T_matrix wMo = m_restCovMat;                //Rotation matrix that converts world coordinates to local frame
                T_vector pTo = p->m_position - m_restCoM;   //Translation of the point w.r.t. local frame
                m_shapeMatchingPositions.push_back( wMo * pTo ); //Store each particle position w.r.t. local frame
            }

            //Update deformed configuration states
            m_deformedCoM = computePredCenterOfMass(CConstraint<T_real>::m_particles);
            computeEigenVectors(CConstraint<T_real>::m_particles,m_deformedCovMat);
        }

        bool isSatisfied()
        {
            Eigen::Quaterniond restQuat(m_restCovMat);
            Eigen::Quaterniond defQuat(m_deformedCovMat);
            T_real dist = restQuat.angularDistance(defQuat);
            return ( dist < CConstraint<T_real>::m_epsilon );
        }

        bool isPredSatisfied()
        {
            Eigen::Quaterniond restQuat(m_restCovMat);
            Eigen::Quaterniond defQuat(m_deformedCovMat);
            T_real dist = restQuat.angularDistance(defQuat);
            return ( dist < CConstraint<T_real>::m_epsilon );
        }

        T_matrix checkAxisPermutation( const T_matrix& oldMat,  const T_matrix& newMat )
        {
            T_real threshold = 0.01;
            T_matrix res;

            for (uint i=0; i<3; ++i)
            {
                if ( (oldMat.col(0) - newMat.col(0)).norm() < threshold )
                {
                    res.col(i) = oldMat.col(i);
                }
                else if ( (oldMat.col(0) - newMat.col(1)).norm() < threshold )
                {
                    res.col(i) = oldMat.col(i);
                }
                else if ( (oldMat.col(i) - newMat.col(2)).norm() < threshold )
                {
                    res.col(i) = oldMat.col(i);
                }
                else
                {
                    res.col(i) = newMat.col(i);
                }
            }

            return res;
        }

        T_matrix checkAxisInversion( const T_matrix& oldMat,  const T_matrix& newMat )
        {
            T_real threshold = 0.01;
            T_matrix res;

            for (uint i=0; i<3; ++i)
            {
                if ( (oldMat.col(0) - newMat.col(0)).norm() > (1+threshold) )
                {
                    res.col(i) = oldMat.col(i);
                }
                else
                {
                    res.col(i) = newMat.col(i);
                }
            }

            return res;
        }

        bool project()
        {
            //Update the rest configuration descriptors
            m_restCoM = computeCenterOfMass(CConstraint<T_real>::m_particles);
            T_matrix Q, qtmp;
            computeRestEigenVectors(CConstraint<T_real>::m_particles,Q);
            qtmp = checkAxisPermutation( m_restCovMat, Q );
            m_restCovMat = checkAxisInversion  ( m_restCovMat, qtmp );

            //Update the deformed configuration descriptors
            m_deformedCoM = computePredCenterOfMass(CConstraint<T_real>::m_particles);  //CoM in the deformed configuration
            computeEigenVectors(CConstraint<T_real>::m_particles,Q);                    //Q component of QR decompositions of the Covariance in the deformed configuration
            qtmp = checkAxisPermutation( m_deformedCovMat, Q );
            m_deformedCovMat = checkAxisInversion  ( m_deformedCovMat, qtmp );

            uint i = 0;
            double totalDeltas = 0;
            for (const auto& p:CConstraint<T_real>::m_particles)
            {
                //Compute the delta to move each particle to its shape target position in the local frame

                //1 - Convert particle shape target position to world frame
                T_vector tTw = m_deformedCovMat.inverse() * m_shapeMatchingPositions[i] + m_deformedCoM;

                //2 - Calculate the delta
                T_vector deltaWorld = tTw - p->m_predPosition;

                totalDeltas += deltaWorld.norm();

                //3 - Add the delta to the predPosition
                double stiffness = 1.0; //TODO: Make this a parameter
                p->m_predPosition = p->m_predPosition + deltaWorld * 1.0;
                p->m_predOrientation = Eigen::Quaterniond(m_deformedCovMat);
                ++i;

// POSITION DELTA FROM THE PBD PAPERS
//                T_vector x_star = p->m_predPosition;            //particle position in the deformed configuration
//                T_vector ri = x_star - m_restCoM;               //particle offset to the CoM in the rest configuration
//                T_vector posDelta = (Q * ri + c) - x_star;
//                //p->m_predPosition += posDelta;
//                p->m_predPosition = p->m_position + posDelta;
//                p->m_predOrientation = quat * p->m_orientation;
            }

            //return totalDeltas < 0.0001;
            return true;
        }

        //TODO: HIGH Consider particles can have different mass in the com computation
        T_vector computeCenterOfMass( const std::vector< PBD::CParticle<>* >& particles )
        {
            T_vector CoM (T_real(0),T_real(0),T_real(0));

            for (const auto& p:particles)
            {
                CoM += p->m_position;
            }
            CoM /= particles.size();
            return CoM;
        }

        T_vector computePredCenterOfMass( const std::vector< PBD::CParticle<>* >& particles )
        {
            T_vector CoM (T_real(0),T_real(0),T_real(0));

            for (const auto& p:particles)
            {
                CoM += p->m_predPosition;
            }
            CoM /= particles.size();
            return CoM;
        }

        void computeEigenVectors( const std::vector< PBD::CParticle<>* >& particles, T_matrix& eigenvectors )
        {
            T_vector c = computePredCenterOfMass(CConstraint<T_real>::m_particles);
            T_matrix cov;

            cov.setZero();
            for (const auto& p:particles)
            {
//                T_matrix A = 0.2*p->getMass()*p->m_size*p->m_predOrientation.toRotationMatrix();
                T_vector x_star = p->m_predPosition;
                T_vector ri = x_star - m_deformedCoM;
                cov += (x_star - c) * ri.transpose();
//                cov += A + (x_star - c) * ri.transpose();
            }

//            Eigen::SelfAdjointEigenSolver<T_matrix> es(cov);
//            cov = es.eigenvectors();
            Eigen::JacobiSVD<T_matrix> svd(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
            eigenvectors = svd.matrixU();

//            //Normalize cov mat axis
//            cov.col(0).normalize();
//            cov.col(1).normalize();
//            cov.col(2).normalize();
//
//            //Construct a right handed orthonormal basis from the covariance matrix axes
//            eigenvectors.col(0) = cov.col(0);                   //x = first eigenvector
//            eigenvectors.col(2) = cov.col(0).cross(cov.col(1)); //z = x*y
//            eigenvectors.col(2).normalize();
//            eigenvectors.col(1) = cov.col(2).cross(cov.col(0)); //y = z*x
//            eigenvectors.col(1).normalize();
        }

        void computeRestEigenVectors( const std::vector< PBD::CParticle<>* >& particles, T_matrix& eigenvectors )
        {

            T_matrix cov;
            cov.setIdentity();

            T_vector c = computeCenterOfMass(CConstraint<T_real>::m_particles);

            for (const auto& p:particles)
            {
                T_matrix A = 0.2*p->getMass()*p->m_size*p->m_predOrientation.toRotationMatrix();
                T_vector x_star = p->m_position;
                T_vector ri = x_star - m_restCoM;
                cov += (x_star - c) * ri.transpose();
//                cov += A + (x_star - c) * ri.transpose();
            }

//            Eigen::EigenSolver<T_matrix> es(cov);
//            cov = es.eigenvectors().real();
            Eigen::JacobiSVD<T_matrix> svd(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
            eigenvectors = svd.matrixU();

//            //Normalize cov mat axis
//            cov.col(0).normalize();
//            cov.col(1).normalize();
//            cov.col(2).normalize();
//
//            //Make sure it is a right handed orthonormal basis
//            eigenvectors.col(0) = cov.col(0);
//            eigenvectors.col(0).normalize();
//            eigenvectors.col(2) = cov.col(0).cross(cov.col(1));
//            eigenvectors.col(2).normalize();
//            eigenvectors.col(1) = cov.col(2).cross(cov.col(0));
//            eigenvectors.col(1).normalize();

//            Eigen::FullPivHouseholderQR<Eigen::Matrix3d> solver;
//            solver.compute(cov);
//            cov = solver.matrixQ();
        }

        T_vector getCoM()    { return m_restCoM; }
        T_matrix getCovMat() { return m_restCovMat; }
        T_vector getDeformedCoM()    { return m_deformedCoM; }
        T_matrix getDeformedCovMat() { return m_deformedCovMat; }

        std::vector<T_vector> m_shapeMatchingPositions;

    protected:
        T_vector m_restCoM;
        T_matrix m_restCovMat;
        T_vector m_deformedCoM;
        T_matrix m_deformedCovMat;
    };

}

#endif


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2011 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_GEOM_UTIL_HPP
#define MAPNIK_GEOM_UTIL_HPP

// mapnik
#include <mapnik/box2d.hpp>
#include <mapnik/vertex.hpp>

// boost
#include <boost/tuple/tuple.hpp>

// stl
#include <cmath>
#include <vector>

namespace mapnik
{
template <typename T>
bool clip_test(T p,T q,double& tmin,double& tmax)
{
    double r = 0;
    bool result=true;
    if (p<0.0)
    {
        r=q/p;
        if (r>tmax) result=false;
        else if (r>tmin) tmin=r;
    }
    else if (p>0.0)
    {
        r=q/p;
        if (r<tmin) result=false;
        else if (r<tmax) tmax=r;
    } else if (q<0.0) result=false;
    return result;
}

template <typename T,typename Image>
bool clip_line(T& x0,T& y0,T& x1,T& y1,box2d<T> const& box)
{
    double tmin=0.0;
    double tmax=1.0;
    double dx=x1-x0;
    if (clip_test<double>(-dx,x0,tmin,tmax))
    {
        if (clip_test<double>(dx,box.width()-x0,tmin,tmax))
        {
            double dy=y1-y0;
            if (clip_test<double>(-dy,y0,tmin,tmax))
            {
                if (clip_test<double>(dy,box.height()-y0,tmin,tmax))
                {
                    if (tmax<1.0)
                    {
                        x1=static_cast<T>(x0+tmax*dx);
                        y1=static_cast<T>(y0+tmax*dy);
                    }
                    if (tmin>0.0)
                    {
                        x0+=static_cast<T>(tmin*dx);
                        y0+=static_cast<T>(tmin*dy);
                    }
                    return true;
                }
            }
        }
    }
    return false;
}

template <typename Iter>
inline bool point_inside_path(double x,double y,Iter start,Iter end)
{
    bool inside=false;
    double x0=boost::get<0>(*start);
    double y0=boost::get<1>(*start);

    double x1 = 0;
    double y1 = 0;
    while (++start!=end)
    {
        if ( boost::get<2>(*start) == SEG_MOVETO)
        {
            x0 = boost::get<0>(*start);
            y0 = boost::get<1>(*start);
            continue;
        }
        x1=boost::get<0>(*start);
        y1=boost::get<1>(*start);

        if ((((y1 <= y) && (y < y0)) ||
             ((y0 <= y) && (y < y1))) &&
            ( x < (x0 - x1) * (y - y1)/ (y0 - y1) + x1))
            inside=!inside;
        x0=x1;
        y0=y1;
    }
    return inside;
}

inline bool point_in_circle(double x,double y,double cx,double cy,double r)
{
    double dx = x - cx;
    double dy = y - cy;
    double d2 = dx * dx + dy * dy;
    return (d2 <= r * r);
}

template <typename T>
inline T sqr(T x)
{
    return x * x;
}

inline double distance2(double x0,double y0,double x1,double y1)
{
    double dx = x1 - x0;
    double dy = y1 - y0;
    return sqr(dx) + sqr(dy);
}

inline double distance(double x0,double y0, double x1,double y1)
{
    return std::sqrt(distance2(x0,y0,x1,y1));
}

inline double point_to_segment_distance(double x, double y,
                                        double ax, double ay,
                                        double bx, double by)
{
    double len2 = distance2(ax,ay,bx,by);

    if (len2 < 1e-14)
    {
        return distance(x,y,ax,ay);
    }

    double r = ((x - ax)*(bx - ax) + (y - ay)*(by -ay))/len2;
    if ( r < 0 )
    {
        return distance(x,y,ax,ay);
    }
    else if (r > 1)
    {
        return distance(x,y,bx,by);
    }
    double s = ((ay - y)*(bx - ax) - (ax - x)*(by - ay))/len2;
    return std::fabs(s) * std::sqrt(len2);
}

template <typename Iter>
inline bool point_on_path(double x,double y,Iter start,Iter end, double tol)
{
    double x0=boost::get<0>(*start);
    double y0=boost::get<1>(*start);
    double x1 = 0;
    double y1 = 0;
    while (++start != end)
    {
        if ( boost::get<2>(*start) == SEG_MOVETO)
        {
            x0 = boost::get<0>(*start);
            y0 = boost::get<1>(*start);
            continue;
        }
        x1=boost::get<0>(*start);
        y1=boost::get<1>(*start);

        double distance = point_to_segment_distance(x,y,x0,y0,x1,y1);
        if (distance < tol)
            return true;
        x0=x1;
        y0=y1;
    }
    return false;
}

// filters
struct filter_in_box
{
    box2d<double> box_;
    explicit filter_in_box(box2d<double> const& box)
        : box_(box) {}

    bool pass(box2d<double> const& extent) const
    {
        return extent.intersects(box_);
    }
};

struct filter_at_point
{
    box2d<double> box_;
    explicit filter_at_point(coord2d const& pt, double tol=0)
        : box_(pt,pt)
    {
        box_.pad(tol);
    }

    bool pass(box2d<double> const& extent) const
    {
        return extent.intersects(box_);
    }
};

////////////////////////////////////////////////////////////////////////////
template <typename PathType>
double path_length(PathType & path)
{
    double x0 = 0;
    double y0 = 0;
    double x1 = 0;
    double y1 = 0;
    path.rewind(0);
    unsigned command = path.vertex(&x0,&y0);
    if (command == SEG_END) return 0;
    double length = 0;
    while (SEG_END != (command = path.vertex(&x1, &y1)))
    {
        if (command == SEG_CLOSE) continue;
        length += distance(x0,y0,x1,y1);
        x0 = x1;
        y0 = y1;
    }
    return length;
}

namespace label {

template <typename PathType>
bool middle_point(PathType & path, double & x, double & y)
{
    double x0 = 0;
    double y0 = 0;
    double x1 = 0;
    double y1 = 0;
    double mid_length = 0.5 * path_length(path);
    path.rewind(0);
    unsigned command = path.vertex(&x0,&y0);
    if (command == SEG_END) return false;
    double dist = 0.0;
    while (SEG_END != (command = path.vertex(&x1, &y1)))
    {
        if (command == SEG_CLOSE) continue;
        double seg_length = distance(x0, y0, x1, y1);

        if ( dist + seg_length >= mid_length)
        {
            double r = (mid_length - dist)/seg_length;
            x = x0 + (x1 - x0) * r;
            y = y0 + (y1 - y0) * r;
            break;
        }
        dist += seg_length;
        x0 = x1;
        y0 = y1;
    }
    return true;
}

template <typename PathType>
bool centroid(PathType & path, double & x, double & y)
{
    double x0 = 0.0;
    double y0 = 0.0;
    double x1 = 0.0;
    double y1 = 0.0;
    double start_x;
    double start_y;

    path.rewind(0);
    unsigned command = path.vertex(&x0, &y0);
    if (command == SEG_END) return false;

    start_x = x0;
    start_y = y0;

    double atmp = 0.0;
    double xtmp = 0.0;
    double ytmp = 0.0;
    unsigned count = 1;
    while (SEG_END != (command = path.vertex(&x1, &y1)))
    {
        if (command == SEG_CLOSE) continue;
        double dx0 = x0 - start_x;
        double dy0 = y0 - start_y;
        double dx1 = x1 - start_x;
        double dy1 = y1 - start_y;

        double ai = dx0 * dy1 - dx1 * dy0;
        atmp += ai;
        xtmp += (dx1 + dx0) * ai;
        ytmp += (dy1 + dy0) * ai;
        x0 = x1;
        y0 = y1;
        ++count;
    }

    if (count <= 2) {
        x = (start_x + x0) * 0.5;
        y = (start_y + y0) * 0.5;
        return true;
    }

    if (atmp != 0)
    {
        x = (xtmp/(3*atmp)) + start_x;
        y = (ytmp/(3*atmp)) + start_y;
    }
    else
    {
        x = x0;
        y = y0;
    }
    return true;
}

// Compute centroid over a set of paths
template <typename Iter>
bool centroid_geoms(Iter start, Iter end, double & x, double & y)
{
  double x0 = 0.0;
  double y0 = 0.0;
  double x1 = 0.0;
  double y1 = 0.0;
  double start_x = x0;
  double start_y = y0;

  double atmp = 0.0;
  double xtmp = 0.0;
  double ytmp = 0.0;
  unsigned count = 0;

  while (start!=end)
  {
    typename Iter::value_type & path = *start++;
    path.rewind(0);
    unsigned command = path.vertex(&x0, &y0);
    if (command == SEG_END) continue;

    if ( ! count++ ) {
      start_x = x0;
      start_y = y0;
    }

    while (SEG_END != (command = path.vertex(&x1, &y1)))
    {
        if (command == SEG_CLOSE) continue;
        double dx0 = x0 - start_x;
        double dy0 = y0 - start_y;
        double dx1 = x1 - start_x;
        double dy1 = y1 - start_y;
        double ai = dx0 * dy1 - dx1 * dy0;
        atmp += ai;
        xtmp += (dx1 + dx0) * ai;
        ytmp += (dy1 + dy0) * ai;
        x0 = x1;
        y0 = y1;
        ++count;
    }

  }

  if (count == 0) return false;

  if (count <= 2) {
      x = (start_x + x0) * 0.5;
      y = (start_y + y0) * 0.5;
      return true;
  }

  if (atmp != 0)
  {
      x = (xtmp/(3*atmp)) + start_x;
      y = (ytmp/(3*atmp)) + start_y;
  }
  else
  {
      x = x0;
      y = y0;
  }

  return true;
}

template <typename PathType>
bool hit_test(PathType & path, double x, double y, double tol)
{
    bool inside=false;
    double x0 = 0;
    double y0 = 0;
    double x1 = 0;
    double y1 = 0;
    path.rewind(0);
    unsigned command = path.vertex(&x0, &y0);
    if (command == SEG_END) return false;
    unsigned count = 0;
    while (SEG_END != (command = path.vertex(&x1, &y1)))
    {
        if (command == SEG_CLOSE) continue;
        ++count;
        if (command == SEG_MOVETO)
        {
            x0 = x1;
            y0 = y1;
            continue;
        }
        if ((((y1 <= y) && (y < y0)) ||
             ((y0 <= y) && (y < y1))) &&
            (x < (x0 - x1) * (y - y1)/ (y0 - y1) + x1))
            inside=!inside;

        x0 = x1;
        y0 = y1;
    }

    if (count == 0) // one vertex
    {
        return distance(x, y, x0, y0) <= std::fabs(tol);
    }
    return inside;
}

template <typename PathType>
bool interior_position(PathType & path, double & x, double & y)
{
    // start with the centroid
    if (!label::centroid(path, x,y))
        return false;

    // if we are not a polygon, or the default is within the polygon we are done
    if (hit_test(path,x,y,0.001))
        return true;

    // otherwise we find a horizontal line across the polygon and then return the
    // center of the widest intersection between the polygon and the line.

    std::vector<double> intersections; // only need to store the X as we know the y

    double x0 = 0;
    double y0 = 0;
    path.rewind(0);
    unsigned command = path.vertex(&x0, &y0);
    double x1 = 0;
    double y1 = 0;
    while (SEG_END != (command = path.vertex(&x1, &y1)))
    {
        if (command == SEG_CLOSE)
            continue;
        if (command != SEG_MOVETO)
        {
            // if the segments overlap
            if (y0==y1)
            {
                if (y0==y)
                {
                    double xi = (x0+x1)/2.0;
                    intersections.push_back(xi);
                }
            }
            // if the path segment crosses the bisector
            else if ((y0 <= y && y1 >= y) ||
                     (y0 >= y && y1 <= y))
            {
                // then calculate the intersection
                double xi = x0;
                if (x0 != x1)
                {
                    double m = (y1-y0)/(x1-x0);
                    double c = y0 - m*x0;
                    xi = (y-c)/m;
                }

                intersections.push_back(xi);
            }
        }
        x0 = x1;
        y0 = y1;
    }
    // no intersections we just return the default
    if (intersections.empty())
        return true;
    x0=intersections[0];
    double max_width = 0;
    for (unsigned ii = 1; ii < intersections.size(); ++ii)
    {
        double xi=intersections[ii];
        double xc=(x0+xi)/2.0;
        double width = std::fabs(xi-x0);
        if (width > max_width && hit_test(path,xc,y,0))
        {
            x=xc;
            max_width = width;
            break;
        }
    }
    return true;
}

}}

#endif // MAPNIK_GEOM_UTIL_HPP


#ifndef INIT_HPP
#define INIT_HPP

#include <opencv2/opencv.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/assign/list_of.hpp>

namespace bg = boost::geometry;

typedef bg::model::d2::point_xy<int> point;
typedef bg::model::polygon<point> polygon;
typedef bg::model::box<point> box;
typedef boost::geometry::ring_type<polygon>::type ring_type;

void SleepFor(int mill_sec);
void ShowAndWait(cv::Mat image, std::string window_name, int mill_sec);

#endif


#include <boost/test/unit_test.hpp>

#include "util/coordinate_calculation.hpp"

#include <osrm/coordinate.hpp>

#include <cmath>

using namespace osrm;
using namespace osrm::util;

// Regression test for bug captured in #1347
BOOST_AUTO_TEST_CASE(regression_test_1347)
{
    FixedPointCoordinate u(10 * COORDINATE_PRECISION, -100 * COORDINATE_PRECISION);
    FixedPointCoordinate v(10.001 * COORDINATE_PRECISION, -100.002 * COORDINATE_PRECISION);
    FixedPointCoordinate q(10.002 * COORDINATE_PRECISION, -100.001 * COORDINATE_PRECISION);

    double d1 = coordinate_calculation::perpendicularDistance(u, v, q);

    double ratio;
    FixedPointCoordinate nearest_location;
    double d2 = coordinate_calculation::perpendicularDistance(u, v, q, nearest_location, ratio);

    BOOST_CHECK_LE(std::abs(d1 - d2), 0.01);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_CUMSUM_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_CUMSUM_HPP_INCLUDED

#include <boost/simd/swar/functions/cumsum.hpp>
#include <boost/simd/include/functions/simd/insert.hpp>
#include <boost/simd/include/functions/simd/extract.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::cumsum_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type that = a0;
      for(size_t i=1; i!=meta::cardinal_of<A0>::value; ++i)
        that[i] += that[i-1];
      return that;
    }
  };
} } }

#endif


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

#ifndef SRC__RMF_TRAFFIC__BLOCKADE__GEOMETRY_HPP
#define SRC__RMF_TRAFFIC__BLOCKADE__GEOMETRY_HPP

#include <Eigen/Geometry>

namespace rmf_traffic {
namespace blockade {

//==============================================================================
struct Segment
{
  Eigen::Vector2d start;
  Eigen::Vector2d finish;
  double radius;
};

//==============================================================================
struct ConflictInfo
{
  enum Type
  {
    Nothing,
    Conflict,
    Alignment
  };

  Type type = Nothing;

  bool is_nothing() const
  {
    return type == Nothing;
  }

  bool is_conflict() const
  {
    return type == Conflict;
  }

  bool is_alignment() const
  {
    return type == Alignment;
  }

  enum Cap
  {
    Start = 0,
    Finish
  };

  // The following fields will be uninitialized when has_conflict is false
  std::array<bool, 2> include_cap_a;
  std::array<bool, 2> include_cap_b;

  static ConflictInfo nothing()
  {
    return ConflictInfo();
  }
};

//==============================================================================
ConflictInfo detect_conflict(
  const Segment& s_a,
  const Segment& s_b,
  double angle_threshold);

} // namespace blockade
} // namespace rmf_traffic

#endif // SRC__RMF_TRAFFIC__BLOCKADE__GEOMETRY_HPP


#pragma once
#include <vector>
#include <string>
#include <utility>

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

#include <opencv2/opencv.hpp>
//#include <opencv2/videoio.hpp>
#define win64
//#define linux
//#define normalization
#define perspective
#define EPSILON 1e-3
//#define small_rect_def
//#define train_intersection_def

const int G_land_num = 73;
const int G_train_pic_id_num = 3300;
const int G_nShape = 47;
const int G_nVerts = 11510;
const int G_nFaces = 11540;
const int G_test_num = 77;
const int G_iden_num = 77;
const int G_inner_land_num = 58;
const int G_outer_land_num = 15;
const int G_line_num = 84;
//const int G_jaw_land_num = 20;

const float G_rand_rot_border = 0.1;
const float G_rand_tslt_border = 10;
const float G_rand_s_border = 10;
const float G_rand_f_border = 100;

const int G_rnd_rot = 5;
const int G_rnd_tslt = 5;
const int G_rnd_exp = 15;
const int G_rnd_user = 5;
const int G_rnd_camr = 5;
const int G_trn_factor = 35;

#ifdef perspective
const int G_tslt_num = 3;
#endif // perspective
#ifdef normalizatoin
const int G_tslt_num = 2;
#endif // normalizatoin

const int G_angle_num = 3;
const int G_target_type_size = G_nShape - 1 + G_tslt_num + G_angle_num + 2 * G_land_num;

const int G_dde_K = 5;

const float pi = acos(-1);

const float G_norm_face_rect_ave = 120;
const float G_norm_face_rect_sig = 60;

const int G_pixel_batch_feature_size = 2;

const int G_lk_batch_size = 3;
const int G_lk_step = 20;

struct Target_type {
	Eigen::VectorXf exp;

#ifdef perspective
	Eigen::Vector3f tslt;
#endif // perspective
#ifdef normalization
	Eigen::RowVector3f tslt;
#endif
	//Eigen::Matrix3f rot;
	Eigen::Vector3f angle;
	Eigen::MatrixX2f dis;
	Eigen::VectorXi land_cor;
};

struct DataPoint
{
	cv::Mat image;
	//cv::Rect face_rect;
	std::vector<cv::Point2d> landmarks;
	//std::vector<cv::Point2d> init_shape;
	Target_type shape;
	Eigen::VectorXf user;
	Eigen::RowVector2f center;
	Eigen::RowVector2f centeroid;
	Eigen::MatrixX2f land_2d;
#ifdef perspective
	float fcs;
#endif // perspective
#ifdef normalization
	Eigen::MatrixX3f s;
#endif

	Eigen::VectorXi land_cor;
};

float cal_3d_vtx(
	Eigen::MatrixXf &bldshps,
	Eigen::VectorXf &user, Eigen::VectorXf &exp, int vtx_idx, int axis);

//void recal_dis(DataPoint &data, Eigen::MatrixXf &bldshps);
void recal_dis_ang(DataPoint &data, Eigen::MatrixXf &bldshps);

void cal_mesh(DataPoint &data, Eigen::MatrixXf &bldshps, Eigen::MatrixX3f &mesh);
void cal_3d_land(DataPoint &data, Eigen::MatrixXf &bldshps, Eigen::MatrixX3f &land_3d);

void test_load_mesh(std::vector <DataPoint> &data, Eigen::MatrixXf &bldshps, int idx, std::string path);
void test_load_3d_land(std::vector <DataPoint> &data, Eigen::MatrixXf &bldshps, int idx, std::string path);

void draw_point(cv::Mat& img, cv::Point2f fp, cv::Scalar color);

void draw_delaunay(cv::Mat& img, cv::Subdiv2D& subdiv, cv::Scalar delaunay_color);
void draw_voronoi(cv::Mat& img, cv::Subdiv2D& subdiv);

void test_del_tri(Eigen::MatrixX2f &points, cv::Mat& img, cv::Subdiv2D subdiv);

void cal_del_tri(
	Eigen::MatrixX2f &points, cv::Mat& img, std::vector<cv::Vec6f> &triangleList);

void cal_del_tri(
	const std::vector<cv::Point2d> &points, cv::Rect &rect,
	std::vector<cv::Vec6f> &triangleList, Eigen::MatrixX3i &tri_idx);
//void test_del_tri(
//	std::vector < std::vector <cv::Mat_<uchar> > >& imgs,
//	iden *ide, std::vector<std::pair<int, std::pair <int, int> > > &del_tri);

double dis_cv_pt(cv::Point2d pointO, cv::Point2d pointA);
double cal_cv_area(cv::Point2d point0, cv::Point2d point1, cv::Point2d point2);

//void update_2d_land(DataPoint &data, Eigen::MatrixXf &bldshps);
//void update_2d_ang_land(DataPoint &data, Eigen::MatrixXf &bldshps);
//void update_2d_land_0ide(DataPoint &data, Eigen::MatrixXf &exp_r_t_all_matrix);
void update_2d_land_ang_0ide(DataPoint &data, Eigen::MatrixXf &exp_r_t_all_matrix);

void cal_2d_land_i_ang_0ide(
	std::vector<cv::Point2d> &ans, const Target_type &data, Eigen::MatrixXf &exp_r_t_all_matrix,
	const DataPoint &data_dp);
//void cal_2d_land_i(
//	std::vector<cv::Point2d> &ans, const Target_type &data, Eigen::MatrixXf &bldshps, DataPoint &ini_data);
Target_type shape_difference(const Target_type &s1, const Target_type &s2);
Target_type shape_adjustment(Target_type &shape, Target_type &offset);

void show_image(cv::Mat img, cv::Rect rect, std::vector<cv::Point2d> landmarks);
void show_image_0rect(cv::Mat img, std::vector<cv::Point2d> landmarks);
void show_image_land_2d(cv::Mat img, Eigen::MatrixX2f &land);

void draw_land_img(cv::Mat image, std::vector<cv::Point2d> landmarks);

void save_video(cv::Mat img, std::vector<cv::Point2d> landmarks, cv::VideoWriter &output_video);
void save_video(cv::Mat img, std::vector<cv::Point2f> landmarks, cv::VideoWriter &output_video);
//void cal_2d_land_i_0dis(
//	std::vector<cv::Point2d> &ans, Eigen::MatrixXf &bldshps, DataPoint &data);
void cal_2d_land_i_0dis_ang(
	std::vector<cv::Point2d> &ans, Eigen::MatrixXf &bldshps, DataPoint &data);

void save_for_debug(DataPoint &temp, std::string name);

void print_datapoint(DataPoint &data);
void print_target(Target_type &data);
void save_datapoint(DataPoint &temp, std::string save_name);

void cal_exp_r_t_all_matrix(
	Eigen::MatrixXf &bldshps, DataPoint &data, Eigen::MatrixXf &result);

void target2vector(Target_type &data, Eigen::VectorXf &ans);
void vector2target(Eigen::VectorXf &data, Target_type &ans);

float cal_3d_vtx_0ide(
	Eigen::MatrixXf &exp_r_t_all_matrix, Eigen::VectorXf &exp, int vtx_idx, int axis);

//Eigen::RowVector3f get_uler_angle(Eigen::Matrix3f R);
//Eigen::Matrix3f get_r_from_angle(float angle, int axis);
//Eigen::Matrix3f get_r_from_angle(const Eigen::Vector3f &angle);

Eigen::Vector3f get_uler_angle_zyx(Eigen::Matrix3f R);
Eigen::Matrix3f get_r_from_angle_zyx(const Eigen::Vector3f &angle);

void update_training_data(DataPoint &data, std::vector<DataPoint> &training_data, Eigen::MatrixXf &exp_r_t_all_matrix);

void rect_scale(cv::Rect &rect, double scale);
void normalize_gauss_face_rect(cv::Mat image, cv::Rect &rect);

uchar get_batch_feature(int patch_size, cv::Mat img, cv::Point p);

uchar get_sobel_batch_feature(cv::Mat img, cv::Point p);
std::pair<float, float> cal_sobel(cv::Mat img, cv::Point p);

cv::Point lk_get_pos_next(int batch_feature_size, cv::Point p, cv::Mat frame_last, cv::Mat frame_now);

void shape_err_print(DataPoint &data, DataPoint &data_ref, Eigen::VectorXf &ave_er);

float show_dis_part(Eigen::MatrixX2f &dis);


// Copyright (c) 2019 herenvarno
//
// This software is released under the MIT License.
// https://opensource.org/licenses/MIT

#ifndef __SIMPLE_CYCLES_HPP__
#define __SIMPLE_CYCLES_HPP__

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/subgraph.hpp>
#include <boost/property_map/property_map.hpp>
#include <queue>
#include <set>
#include <stack>
#include <unordered_map>
#include <vector>

using namespace std;

namespace bglex {

/**
 * Find the simple cycles in a graph
 */
template <typename G>
vector<vector<typename G::vertex_descriptor>> simple_cycles(G g_) {

  using namespace boost;
  // typedef typename G::vertex_descriptor Vertex;
  // typedef typename G::edge_descriptor Edge;
  typedef adjacency_list<vecS, vecS, directedS, property<vertex_color_t, int>,
                         property<edge_index_t, int>>
      SimpleGraph;
  typedef subgraph<SimpleGraph> SubGraph;
  typedef SimpleGraph::vertex_descriptor Vertex;
  typedef SimpleGraph::vertex_iterator VertexIter;
  typedef SimpleGraph::edge_descriptor Edge;
  typedef SimpleGraph::edge_iterator EdgeIter;
  typedef SimpleGraph::out_edge_iterator OutEdgeIter;

  vector<vector<typename G::vertex_descriptor>> cycles;

  // Define a aux function _unblock()
  auto _unblock = [](Vertex thisnode, set<Vertex> &blocked,
                     std::unordered_map<Vertex, set<Vertex>> &B) {
    std::stack<Vertex> s;
    s.push(thisnode);
    while (!s.empty()) {
      Vertex node = s.top();
      s.pop();
      if (blocked.find(node) != blocked.end()) {
        blocked.erase(node);
        for (auto &n : B[node]) {
          s.push(n);
        }
        B[node].clear();
      }
    }
  };

  // Find strong connected components with at least two vertices in it.

  std::unordered_map<typename G::vertex_descriptor, Vertex> g2subg;
  std::unordered_map<Vertex, typename G::vertex_descriptor> subg2g;

  SubGraph subg(num_vertices(g_));
  typename G::vertex_iterator vi, vi_end;
  int i = 0;
  for (tie(vi, vi_end) = vertices(g_); vi != vi_end; vi++, i++) {
    g2subg[*vi] = i;
    subg2g[i] = *vi;
  }
  typename G::edge_iterator ei, ei_end;
  for (tie(ei, ei_end) = edges(g_); ei != ei_end; ei++) {
    typename G::vertex_descriptor src, dest;
    src = source(*ei, g_);
    dest = target(*ei, g_);
    add_edge(g2subg[src], g2subg[dest], subg);
  }

  vector<int> component(num_vertices(subg));
  int num = strong_components(
      subg, make_iterator_property_map(component.begin(),
                                       get(boost::vertex_index, subg)));

  stack<vector<Vertex>> sccs;
  for (int i = 0; i < num; i++) {
    vector<Vertex> temp;
    for (int j = 0; j < component.size(); j++) {
      if (component[j] == i) {
        temp.push_back(j);
      }
    }
    if (temp.size() > 1) {
      sccs.push(temp);
    }
  }

  // Record self-loops and remove them
  vector<Vertex> self_loops;
  VertexIter svi, svi_end;
  for (tie(svi, svi_end) = vertices(subg); svi != svi_end; svi++) {
    Vertex vd = *svi;
    Edge ed;
    bool exist = false;
    tie(ed, exist) = edge(vd, vd, subg);
    if (exist) {
      self_loops.push_back(vd);
    }
  }
  for (auto vd : self_loops) {
    remove_edge(vd, vd, subg);
  }

  // main loop
  while (!sccs.empty()) {
    auto scc = sccs.top();
    sccs.pop();
    auto sccG = subg.create_subgraph();
    for (auto s : scc) {
      add_vertex(s, sccG);
    }

    auto startnode = scc[0];
    vector<Vertex> path = {startnode};
    set<Vertex> blocked;
    set<Vertex> closed;
    blocked.insert(startnode);
    std::unordered_map<Vertex, set<Vertex>> B;
    std::vector<pair<Vertex, vector<Vertex>>> q;

    vector<Vertex> nbrs0;
    OutEdgeIter oei, oei_end;
    for (tie(oei, oei_end) = out_edges(sccG.global_to_local(startnode), sccG);
         oei != oei_end; oei++) {
      Vertex nextnode = sccG.local_to_global(target(*oei, sccG));
      nbrs0.push_back(nextnode);
    }
    q.push_back({startnode, nbrs0});
    while (!q.empty()) {
      Vertex thisnode = q.back().first;
      vector<Vertex> &nbrs = q.back().second;
      if (!nbrs.empty()) {
        Vertex nextnode = nbrs.back();
        nbrs.pop_back();
        if (nextnode == startnode) {
          cycles.push_back(path);
          for (auto p : path) {
            closed.insert(p);
          }
        } else if (blocked.find(nextnode) == blocked.end()) {
          path.push_back(nextnode);
          vector<Vertex> new_nbrs;
          for (tie(oei, oei_end) =
                   out_edges(sccG.global_to_local(nextnode), sccG);
               oei != oei_end; oei++) {
            Vertex xx = sccG.local_to_global(target(*oei, sccG));
            new_nbrs.push_back(xx);
          }
          q.push_back({nextnode, new_nbrs});
          closed.erase(nextnode);
          blocked.insert(nextnode);
          continue;
        }
      }

      if (nbrs.size() == 0) {
        if (closed.find(thisnode) != closed.end()) {
          _unblock(thisnode, blocked, B);
        } else {
          for (tie(oei, oei_end) =
                   out_edges(sccG.global_to_local(thisnode), sccG);
               oei != oei_end; oei++) {
            Vertex nextnode = sccG.local_to_global(target(*oei, sccG));
            if (B[nextnode].find(thisnode) == B[nextnode].end()) {
              B[nextnode].insert(thisnode);
            }
          }
        }
      }
      q.pop_back();
      path.pop_back();
    }

    auto H = subg.create_subgraph();
    for (auto s : scc) {
      add_vertex(s, sccG);
    }
    vector<int> c(num_vertices(H));
    int n = strong_components(
        H, make_iterator_property_map(c.begin(), get(boost::vertex_index, H)));

    for (int i = 0; i < n; i++) {
      vector<Vertex> temp;
      for (int j = 0; j < c.size(); j++) {
        if (c[j] == i) {
          temp.push_back(j);
        }
      }
      if (temp.size() > 1) {
        sccs.push(temp);
      }
    }
  }

  // Final translate
  for (int i = 0; i < cycles.size(); i++) {
    for (int j = 0; j < cycles[i].size(); j++) {
      cycles[i][j] = subg2g[cycles[i][j]];
    }
  }
  return cycles;
}

} // namespace bglex

#endif


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

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/random.hpp>
#include <boost/graph/depth_first_search.hpp>

#include <boost/random/linear_congruential.hpp>

using namespace boost;
using namespace std;

int cpt = 0;

class my_dfs_visitor:public default_dfs_visitor {
public:
  template < typename Vertex, typename Graph >
  void discover_vertex(const Vertex& u, const Graph& g) {
    cpt++;
    cout << cpt << "\t" << u << "\n";
  }
};

int main(int _, char* args[]) {
  // create a typedef for the Graph type
  typedef adjacency_list<vecS, vecS> Graph;

  minstd_rand ran(time(0));

  GraphvizDigraph g;
  read_graphviz(args[1], g);
  //  generate_random_graph(g, atoi(args[1]), atoi(args[2]), ran);
  //  write_graphviz(cout, g);
  default_dfs_visitor vis;
  // my_dfs_visitor vis;
  time_t t1, t2;
  time(&t1);
  depth_first_search(g, visitor(vis));
  time(&t2);
  double dif = difftime(t2, t1);
  cout << dif << " seconds.\n";

  return 0;
}

/*
Local Variables:
compile-command: "g++ -O3 -o cpp_bench -Wall -lbgl-viz bench.cpp"
End:
*/


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Test_SignalProcessing
#include <boost/test/unit_test.hpp>
#include "../SignalProcessing.hpp"

BOOST_AUTO_TEST_CASE(SanityCheck_DirectConvolution1) {
    std::vector<int> v1{1, 2, 3};
    std::vector<int> v2{1};
    std::vector<int> v = direct_conv(v1, v2);
    BOOST_REQUIRE(v.size() == v1.size() + v2.size() - 1);
    for (size_t i = 0; i < v.size(); i++) {
        BOOST_REQUIRE(v[i] == v1[i]);
    }
}

BOOST_AUTO_TEST_CASE(SanityCheck_DirectConvolution2) {
    //>>> np.convolve([1,-1,0,10, 3], [3, 9, -2]  ==> array([  3,   6, -11,  32,  99,   7,  -6])
    std::vector<int> v1{1, -1, 0, 10, 3};
    std::vector<int> v2{3, 9, -2};
    std::vector<int> desired_res{3, 6, -11, 32, 99, 7, -6};

    std::vector<int> v = direct_conv(v1, v2);
    BOOST_REQUIRE(desired_res.size() == v.size());
    for (size_t i = 0; i < v.size(); i++) {
        BOOST_REQUIRE(desired_res[i] == v[i]);
    }
}


#include <unistd.h>
#include <boost/filesystem.hpp>
#include "coins.h"
#include "util.h"
#include "init.h"
#include "primitives/transaction.h"
#include "crypto/equihash.h"
#include "chainparams.h"
#include "pow.h"
#include "sodium.h"
#include "streams.h"

#include "zcbenchmarks.h"

#include "zcash/Zcash.h"
#include "zcash/IncrementalMerkleTree.hpp"

using namespace libzcash;

struct timeval tv_start;

void timer_start()
{
    gettimeofday(&tv_start, 0);
}

double timer_stop()
{
    double elapsed;
    struct timeval tv_end;
    gettimeofday(&tv_end, 0);
    elapsed = double(tv_end.tv_sec-tv_start.tv_sec) +
        (tv_end.tv_usec-tv_start.tv_usec)/double(1000000);
    return elapsed;
}

double benchmark_sleep()
{
    timer_start();
    sleep(1);
    return timer_stop();
}

double benchmark_parameter_loading()
{
    // FIXME: this is duplicated with the actual loading code
    boost::filesystem::path pk_path = ZC_GetParamsDir() / "z4-proving.key";
    boost::filesystem::path vk_path = ZC_GetParamsDir() / "z4-verification.key";

    timer_start();

    auto newParams = ZCJoinSplit::Unopened();

    newParams->loadVerifyingKey(vk_path.string());
    newParams->setProvingKeyPath(pk_path.string());
    newParams->loadProvingKey();

    double ret = timer_stop();

    delete newParams;

    return ret;
}

double benchmark_create_joinsplit()
{
    uint256 pubKeyHash;

    /* Get the anchor of an empty commitment tree. */
    uint256 anchor = ZCIncrementalMerkleTree().root();

    timer_start();
    CPourTx pourtx(*pzcashParams,
                   pubKeyHash,
                   anchor,
                   {JSInput(), JSInput()},
                   {JSOutput(), JSOutput()},
                   0,
                   0);
    double ret = timer_stop();

    assert(pourtx.Verify(*pzcashParams, pubKeyHash));
    return ret;
}

double benchmark_verify_joinsplit(const CPourTx &joinsplit)
{
    timer_start();
    uint256 pubKeyHash;
    joinsplit.Verify(*pzcashParams, pubKeyHash);
    return timer_stop();
}

double benchmark_solve_equihash()
{
    CBlock pblock;
    CEquihashInput I{pblock};
    CDataStream ss(SER_NETWORK, PROTOCOL_VERSION);
    ss << I;

    unsigned int n = Params(CBaseChainParams::MAIN).EquihashN();
    unsigned int k = Params(CBaseChainParams::MAIN).EquihashK();
    crypto_generichash_blake2b_state eh_state;
    EhInitialiseState(n, k, eh_state);
    crypto_generichash_blake2b_update(&eh_state, (unsigned char*)&ss[0], ss.size());

    uint256 nonce;
    randombytes_buf(nonce.begin(), 32);
    crypto_generichash_blake2b_update(&eh_state,
                                    nonce.begin(),
                                    nonce.size());

    timer_start();
    std::set<std::vector<unsigned int>> solns;
    EhOptimisedSolve(n, k, eh_state, solns);
    return timer_stop();
}

double benchmark_verify_equihash()
{
    CChainParams params = Params(CBaseChainParams::MAIN);
    CBlock genesis = Params(CBaseChainParams::MAIN).GenesisBlock();
    CBlockHeader genesis_header = genesis.GetBlockHeader();
    timer_start();
    CheckEquihashSolution(&genesis_header, params);
    return timer_stop();
}


#include <Eigen/Core>
#include <log2plot/logger.h>

int main()
{
    Eigen::Vector3d t;

    log2plot::Logger logger(LOG2PLOT_EXAMPLE_PATH);

    // save translation with Latex legend
    logger.save(t, "eigen_t", "[t_x,t_y,t_z]", "Position");

    for(int i=0;i<100;++i)
    {
       t[0] = cos(i*0.01);
       t[1] = sin(i*0.01);
       t[2] = 0.01*i;

        // log
        logger.update();

        if(i % 20 == 0)
          logger.writeStep();
    }

    // default script path + verbose
    logger.plot(true);

}