/*
Maximilien Danisch
Mai 2016
http://bit.ly/maxdan94
maximilien.danisch@telecom-paristech.fr

Info:
Feel free to use these lines as you wish. This program computes the exact density-friendly decomposition.

To compile:
g++ exactDF.cpp -fopenmp -fpermissive -o exactDF -O3

To execute:
./exactDF ncpu iter net.txt rates.txt pavafit.txt cuts.txt exact.txt

- nthreads is the number of threads to use
- iter is the number of iterations over all edges to perform
- net.txt should contain the graph (one edge on each line: 2 unsigned separated by a space)
- rates.txt will contain the density value for each node
- pavafit.txt will contain the profile given by the PAVA fit, that is the week approximation of the density-friendly ("size density density-upperbound" on each line)
- cuts.txt will contain the profile given by correct cuts, that is the strong approximation of the density-friendly ("size density density-upperbound" on each line)
- exact.txt will contain the exact density-friendly ("size density" on each line (it is not necesarily in decreasing order of density))
Some information will be printed in the terminal.

*/

#include <boost/graph/edge_list.hpp>
#include <fstream>
#include <iostream>
#include <string>
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/lexical_cast.hpp>
#include <vector>
#include <algorithm>
#include <utility>
#include <stdio.h>
#include <stdlib.h>
#include <omp.h>
#include <time.h>

#define NLINKS 500000000 //maximum number of edges for memory allocation, will increase if needed

typedef struct {
	unsigned s;
	unsigned t;
  double a;//alpha value of edge (s,t)
} edge;

typedef struct {
	unsigned n;
	double r;//rate value of node n
} node;

typedef struct {
	unsigned n;//number of nodes
	unsigned e;//number of edges
	unsigned *map;//correspondance between old and new nodeID
	edge *edges;//list of all edges
	node *nodes;//value associated to each node
  double *ne;//ne[i]=number of edges from i to nodes before (used for pava)
	unsigned *cd;//cumulative degree
	unsigned *cuts;
  unsigned iter;//number of iterations
} optim;

//compute the maximum of three unsigned
inline unsigned max3(unsigned a,unsigned b,unsigned c){
	a=(a>b) ? a : b;
	return (a>c) ? a : c;
}

optim* readedgelist(char* edgelist){
	unsigned e1=NLINKS;
	optim *opt=(optim*)malloc(sizeof(optim));
	FILE *file;

	opt->n=0;
	opt->e=0;
	file=fopen(edgelist,"r");
	opt->edges=(edge*)malloc(e1*sizeof(edge));
	while (fscanf(file,"%u %u", &(opt->edges[opt->e].s), &(opt->edges[opt->e].t))==2) {
		opt->n=max3(opt->n,opt->edges[opt->e].s,opt->edges[opt->e].t);
		if (opt->e++==e1) {
			e1+=NLINKS;
			opt->edges=(edge*)realloc(opt->edges,e1*sizeof(edge));
		}
	}
	fclose(file);
	opt->n++;
	opt->edges=(edge*)realloc(opt->edges,opt->e*sizeof(edge));
	return opt;
}

void relabel(optim *opt) {
	unsigned i,j;
	unsigned *newlabel;

	newlabel=(unsigned*)malloc(opt->n*sizeof(unsigned));
	for (i=0;i<opt->n;i++) {
		newlabel[i]=opt->n;
	}
	opt->map=(unsigned*)malloc(opt->n*sizeof(unsigned));
	j=0;
	for (i=0;i<opt->e;i++) {
		if (newlabel[opt->edges[i].s]==opt->n){
			newlabel[opt->edges[i].s]=j;
			opt->map[j++]=opt->edges[i].s;
		}
		if (newlabel[opt->edges[i].t]==opt->n){
			newlabel[opt->edges[i].t]=j;
			opt->map[j++]=opt->edges[i].t;
		}
		opt->edges[i].s=newlabel[opt->edges[i].s];
		opt->edges[i].t=newlabel[opt->edges[i].t];
	}
	opt->n=j;
	free(newlabel);
	opt->map=(unsigned*)realloc(opt->map,opt->n*sizeof(unsigned));
}

//Step 1: Frank-Wolf gradiant descent

//initialize the optim datastructure
void init(optim *opt){
  opt->iter=0;
	opt->nodes=(node*)malloc(opt->n*sizeof(node));
  for (unsigned k=0;k<opt->n;k++){
    opt->nodes[k].n=k;
    opt->nodes[k].r=0;
  }
	for (unsigned k=0;k<opt->e;k++){
    opt->edges[k].a=.5;
		opt->nodes[opt->edges[k].s].r+=.5;
		opt->nodes[opt->edges[k].t].r+=.5;
	}
}

//one pass over all edges
void onepass(optim *opt){
	unsigned i,j,k;
	double gamma;
	opt->iter++;
	gamma=2./(2.+opt->iter);
	#pragma omp parallel for private(i,j,k)
	for (k=0;k<opt->e;k++){//parfor
		i=opt->edges[k].s;
		j=opt->edges[k].t;
		if (opt->nodes[i].r<opt->nodes[j].r){
			#pragma omp atomic update
			opt->nodes[i].r+=gamma*(1-opt->edges[k].a);//carefull
			#pragma omp atomic update
			opt->nodes[j].r-=gamma*(1-opt->edges[k].a);
			opt->edges[k].a=(1.-gamma)*opt->edges[k].a+gamma;
		}
		else if (opt->nodes[i].r>opt->nodes[j].r){
			#pragma omp atomic update
			opt->nodes[i].r-=gamma*opt->edges[k].a;//carefull
			#pragma omp atomic update
			opt->nodes[j].r+=gamma*opt->edges[k].a;
			opt->edges[k].a*=(1.-gamma);
		}
	}
}

//to print in file the value for each stub: NOT USED
void print_alphas(optim* opt, char* alphas){
	FILE *file=fopen(alphas,"w");
	unsigned long long k;
	for (k=0;k<opt->e;k++){
		fprintf(file,"%u %u %e\n",opt->map[opt->edges[k].s],opt->map[opt->edges[k].t],opt->edges[k].a);
	}
	fclose(file);
}

void freeoptim(optim *opt){
	free(opt->map);
	free(opt->edges);
	free(opt->nodes);
  free(opt->ne);
	free(opt->cuts);
	free(opt);
}

//Step 2: Isotonic regression with PAVA

//used for quicksort (greatest hit in CS before this algorithm)
static int compare_nodes(void const *a, void const *b){
	if (((node*)a)->r <= ((node*)b)->r)
		return 1;
	return -1;
}
//used for quicksort (greatest hit in CS before this algorithm)
static int compare_edges(void const *a, void const *b){
	edge *pa = (edge *)a;
	edge *pb = (edge *)b;
	if ((*pa).s>(*pb).s)
		return 1;
	if ((*pa).s<(*pb).s)
		return -1;
	if ((*pa).t<=(*pb).t)
		return 1;
	return -1;
}

void prepava(optim *opt){
  unsigned u,v;
  unsigned *newlabel=(unsigned*)malloc(opt->n*sizeof(unsigned));
  qsort(opt->nodes,opt->n,sizeof(node),compare_nodes);

  for (unsigned i=0;i<opt->n;i++){
    newlabel[opt->nodes[i].n]=i;
  }
  for (unsigned i=0;i<opt->e;i++){
		u=newlabel[opt->edges[i].s];
		v=newlabel[opt->edges[i].t];
		if (u<v){
			opt->edges[i].s=u;
			opt->edges[i].t=v;
		}
		else {
			opt->edges[i].s=v;
			opt->edges[i].t=u;
			opt->edges[i].a=1-opt->edges[i].a;
		}
	}
  //free(newlabel);
  qsort(opt->edges,opt->e,sizeof(edge),compare_edges);

	opt->cd=(unsigned*)calloc((opt->n+1),sizeof(unsigned));
	for (unsigned i=0;i<opt->e;i++){
		opt->cd[opt->edges[i].s+1]++;
	}
	for (unsigned i=0;i<opt->n;i++){
		opt->cd[i+1]+=opt->cd[i];
	}

  opt->ne=(double*)calloc(opt->n,sizeof(double));
  for (unsigned i=0;i<opt->e;i++){
    u=opt->edges[i].s;
    v=opt->edges[i].t;
    opt->ne[(u>v)?u:v]++;
  }
}

//to print in file the value for each node
void print_rates(optim* opt,char* rates){
	FILE *file=fopen(rates,"w");
	unsigned i;
	for (i=0;i<opt->n;i++){
		fprintf(file,"%u %e\n",opt->map[opt->nodes[i].n],opt->nodes[i].r);
	}
	fclose(file);
}

//fit data structure:
typedef struct {
	unsigned n;//total number of aggregated points
	unsigned *nag;//nag[i]=number of points aggregated in i
	double *val;//val[i]=value of the aggregated points
} isoreg;

//Pool Adjacent Violators Algorithm. Values to fit in vect and size of vect.
isoreg *pava(double *vect,unsigned n){
	isoreg *fit=(isoreg*)malloc(sizeof(isoreg));
	unsigned *nag=(unsigned*)malloc(n*sizeof(unsigned));
	double *val=(double*)malloc(n*sizeof(double));
	unsigned i,j;

	nag[0]=1;
	val[0]=vect[0];
	j=0;
	for (i=1;i<n;i++){
		j+=1;
		val[j]=vect[i];
		nag[j]=1;
		while ((j>0) && (val[j]>val[j-1]-1e-10)){//do val[j]>val[j-1] to have a non-increasing monotonic regression.
			val[j-1]=(nag[j]*val[j]+nag[j-1]*val[j-1])/(nag[j]+nag[j-1]);
			nag[j-1]+=nag[j];
			j--;
		}
	}
	fit->n=j+1;
	fit->nag=nag;
	fit->val=val;
	return fit;
}

//printing the result in file output: "nag val" on each line
void print_fit(isoreg *fit,char* output){
	FILE *file=fopen(output,"w");
	for (unsigned i=0;i<fit->n;i++){
		fprintf(file,"%u %.10le\n",fit->nag[i],fit->val[i]);
	}
	fclose(file);
}

void freeisoreg(isoreg *fit){
	free(fit->nag);
	free(fit->val);
	free(fit);
}

//Step 3: Checking if the cuts given by PAVA are correct

//printing the result in file output: "nag val" on each line
isoreg* mkcut(isoreg* fit, optim *opt){
	unsigned j1,j2,ncuts=0;
	double *r=(double*)malloc(opt->n*sizeof(double));
	double *r2=(double*)malloc(opt->n*sizeof(double));
	double min, max;
	edge ed;
	unsigned *d=(unsigned*)calloc(opt->n,sizeof(unsigned));
	unsigned *d2=(unsigned*)malloc(opt->n*sizeof(unsigned));
	isoreg *fit2=(isoreg*)malloc(sizeof(isoreg));
	fit2->nag=(unsigned*)calloc(opt->n,sizeof(unsigned));
	fit2->val=(double*)calloc(opt->n,sizeof(double));
	fit2->n=0;

	opt->cuts=(unsigned*)malloc(opt->n*sizeof(unsigned));

	for (unsigned k=0;k<opt->n;k++){
		r[k]=opt->nodes[k].r;
		r2[k]=r[k];
	}

	j1=0;
	j2=0;
	for (unsigned i=0;i<fit->n;i++){
		fit2->nag[ncuts]+=fit->nag[i];
		fit2->val[ncuts]+=fit->nag[i]*fit->val[i];
		j2+=fit->nag[i];
		for (unsigned u=j1;u<j2;u++){
			d2[u]=0;
			for (unsigned k=opt->cd[u]+d[u];k<opt->cd[u+1];k++){
				ed=opt->edges[k];
				if (ed.t>=j2){
					r2[u]-=ed.a;
					r2[ed.t]+=ed.a;
					d2[u]++;
				}
				else{
					break;
				}
			}
		}
		min=opt->nodes[0].r;
		for (unsigned k=j1;k<j2;k++){
			min=(min<r2[k])?min:r2[k];
		}
		max=0;
		for (unsigned k=j2;k<opt->n;k++){
			max=(max>r2[k])?max:r2[k];
		}
		if (max<min){
			for (unsigned k=j1;k<j2;k++){
				opt->cuts[k]=ncuts;
			}
			fit2->val[ncuts]/=fit2->nag[ncuts];
			for (unsigned u=j1;u<j2;u++){
				d[u]+=d2[u];
			}
			for (unsigned k=j1;k<opt->n;k++){///////////
				r[k]=r2[k];
			}
			ncuts++;
			j1=j2;
		}
		else{
			for (unsigned k=j1;k<opt->n;k++){///////////
				r2[k]=r[k];
			}
		}
	}
	fit2->n=ncuts;//+1;
	return fit2;
}

//Step 4: Density-Friendly in each indepent subgraphs

typedef struct {
	unsigned s;
	unsigned t;
} edge2;

typedef struct {
	unsigned n;//number of nodes
	unsigned e;//number of edges
	unsigned l;//number of loops
	edge2 *edges;//list of edges
	unsigned *loops;//loops[i]=number of loops of node i
	unsigned *deg;//deg[i]=degree of node i
} subgraph;

void freesubgraph(subgraph* sg){
	free(sg->edges);
	free(sg->loops);
	free(sg->deg);
	free(sg);
}

subgraph **mkindep(optim* opt,unsigned *nsub){
	*nsub=opt->cuts[opt->n-1]+1;
	subgraph **sgs=(subgraph**)malloc((*nsub)*sizeof(subgraph*));
	unsigned *newlabel=(unsigned*)malloc(opt->n*sizeof(unsigned));
	unsigned j,p,u,v,u2,v2;
	for (unsigned k=0;k<*nsub;k++){
		sgs[k]=(subgraph*)malloc(sizeof(subgraph));
		sgs[k]->n=0;
		sgs[k]->e=0;
		sgs[k]->l=0;
	}
	j=0;
	for (unsigned k=0;k<opt->n;k++){
		sgs[opt->cuts[k]]->n++;
		newlabel[k]=j++;
		if ((k<opt->n-1) && (opt->cuts[k] != opt->cuts[k+1])){
			j=0;
		}
	}
	for (unsigned k=0;k<opt->e;k++){
		u=opt->edges[k].s;
		v=opt->edges[k].t;
		p=opt->cuts[v];
		if (opt->cuts[u]==p){
			sgs[p]->e++;
		}
	}
	for (unsigned k=0;k<*nsub;k++){
		sgs[k]->edges=(edge2*)malloc(sgs[k]->e*sizeof(edge2));
		sgs[k]->loops=(unsigned*)calloc(sgs[k]->n,sizeof(unsigned));
		sgs[k]->deg=(unsigned*)calloc(sgs[k]->n,sizeof(unsigned));
		sgs[k]->e=0;
	}
	for (unsigned k=0;k<opt->e;k++){
		u=opt->edges[k].s;
		v=opt->edges[k].t;
		u2=newlabel[u];
		v2=newlabel[v];
		p=opt->cuts[v];
		if (opt->cuts[u]==p){
			sgs[p]->edges[sgs[p]->e].s=u2;
			sgs[p]->edges[sgs[p]->e++].t=v2;
			sgs[p]->deg[u2]++;
			sgs[p]->deg[v2]++;
		}
		else{
			sgs[p]->loops[v2]++;
			sgs[p]->l++;
		}
	}
	return sgs;
}

subgraph *allocsubgraph(unsigned n,unsigned e){
	subgraph *sg=(subgraph*)malloc(sizeof(subgraph));
	sg->n=n;
	sg->e=0;
	sg->l=0;
	sg->deg=(unsigned*)calloc(n,sizeof(unsigned));
	sg->loops=(unsigned*)calloc(n,sizeof(unsigned));
	sg->edges=(edge2*)malloc(e*sizeof(edge2));
	return sg;
}

using namespace boost;
typedef adjacency_list_traits < vecS, vecS, directedS > Traits; //the associated types of the adjacency_list class
typedef adjacency_list < vecS, vecS, directedS,
	    property < vertex_name_t, unsigned,
	    property < vertex_index_t, long,
	    property < vertex_color_t, boost::default_color_type,
	    property < vertex_distance_t, double,///////////???
	    property < vertex_predecessor_t, Traits::edge_descriptor > > > > >,
	    property < edge_capacity_t, double,
	    property < edge_residual_capacity_t, double,
	    property < edge_reverse_t, Traits::edge_descriptor > > > > Graph; //the associated properties of the ajacency_list

void AddEdge(Traits::vertex_descriptor &v1, Traits::vertex_descriptor &v2, property_map < Graph, edge_reverse_t >::type &rev, const double capacity, Graph &g) {
  Traits::edge_descriptor e1 = add_edge(v1, v2, g).first;
  Traits::edge_descriptor e2 = add_edge(v2, v1, g).first;
  put(edge_capacity, g, e1, capacity);
   rev[e1] = e2;
   rev[e2] = e1;
}

void ReReadGraph(Traits::vertex_descriptor &s, Traits::vertex_descriptor &t, subgraph *sg, Graph &g) {
	graph_traits<Graph>::vertex_descriptor u, v;
	property_map < Graph, edge_reverse_t >::type rev = get(edge_reverse, g);
	double alpha=(sg->e+sg->l)/((double)(sg->n))+1./(((double)sg->n)*((double)sg->n));
	double capacity1=2*alpha, capacity2, capacity3=1.;
	for (unsigned i=0; i<sg->n; i++) {
		u=add_vertex(g);
		capacity2=(sg->deg[i]+2.*sg->loops[i]);
		AddEdge(s,u, rev, capacity2, g);
		AddEdge(u,t, rev, capacity1, g);
	}
	for(unsigned i=0; i<sg->e; i++){
		u=vertex(sg->edges[i].s+2,g);
		v=vertex(sg->edges[i].t+2,g);
		AddEdge(u,v,rev, capacity3, g);
		AddEdge(v,u,rev, capacity3, g);
	}
}

void maxflow(subgraph* sg,FILE *file) {
	Traits::vertex_descriptor s, t;
	Graph g;
	property_map < Graph, vertex_color_t >::type col = get(vertex_color, g);
	s=add_vertex(g);
  t=add_vertex(g);
	ReReadGraph(s, t, sg, g);
	double flow = boykov_kolmogorov_max_flow(g ,s, t);
	//std::cout << "number of nodes: " << sg->n << std::endl;
	//std::cout << "number of edges: " << sg->e << std::endl;
	//std::cout << "number of loops: " << sg->l << std::endl;
	//std::cout << "flow: " << flow << std::endl;

	bool *isin=(bool *)calloc(sg->n,sizeof(bool));
	unsigned n_nodes=0;
	for(unsigned i=0;i<sg->n;i++){
		if (col[vertex(i+2,g)]==4){
			isin[i]=1;
			n_nodes++;
			//std::cout << i << " " << col[vertex(i+2,g)] << std::endl;
		}
	}
	g.clear();
	if (n_nodes>0){
		subgraph *sg1=allocsubgraph(n_nodes,sg->e);
		subgraph *sg2=allocsubgraph(sg->n-n_nodes,sg->e);
		unsigned *newlabel=(unsigned*)malloc(sg->n*sizeof(unsigned));
		unsigned j1=0,j2=0;
		for(unsigned i=0;i<sg->n;i++){
			if (isin[i]){
				sg1->loops[j1]=sg->loops[i];
				sg1->l+=sg->loops[i];
				newlabel[i]=j1++;
			}
			else{
				sg2->loops[j2]=sg->loops[i];
				sg2->l+=sg->loops[i];
				newlabel[i]=j2++;
			}
		}
		j1=0,j2=0;
		for(unsigned i=0;i<sg->e;i++){
			if (isin[sg->edges[i].s] && isin[sg->edges[i].t]){
				sg1->edges[j1].s=newlabel[sg->edges[i].s];
				sg1->edges[j1].t=newlabel[sg->edges[i].t];
				sg1->deg[sg1->edges[j1].s]++;
				sg1->deg[sg1->edges[j1].t]++;
				j1++;
			}
			else if (isin[sg->edges[i].s] && (isin[sg->edges[i].t]==0)){
				sg2->loops[newlabel[sg->edges[i].t]]++;
				sg2->l++;
			}
			else if ((isin[sg->edges[i].s]==0) && isin[sg->edges[i].t]){
				sg2->loops[newlabel[sg->edges[i].s]]++;
				sg2->l++;
			}
			else{
				sg2->edges[j2].s=newlabel[sg->edges[i].s];
				sg2->edges[j2].t=newlabel[sg->edges[i].t];
				sg2->deg[sg2->edges[j2].s]++;
				sg2->deg[sg2->edges[j2].t]++;
				j2++;
			}
		}
		sg1->e=j1;
		sg2->e=j2;
		free(isin);
		freesubgraph(sg);
		maxflow(sg1,file);
		maxflow(sg2,file);
	}
	else {
		//<< "n,e,d = "
		#pragma omp critical
		{
			fprintf(file,"%u %le\n",sg->n,((double)(sg->e+sg->l))/sg->n);
			//fprintf(file,"%u %u %u %le\n",sg->n,sg->e,sg->l,((double)(sg->e+sg->l))/sg->n);
			//std::cout << sg->n << " " << sg->e << " " << sg->l << " " << ((double)(sg->e+sg->l))/sg->n << std::endl;
		}
		free(isin);
		freesubgraph(sg);
	}
}

int main(int argc,char** argv){
	optim* opt;
  isoreg *fit,*fit2;
	subgraph** sgs;
	unsigned nsgs;
	unsigned nthreads=atoi(argv[1]);
	unsigned rep=atoi(argv[2]);
  char* edgelist=argv[3];
  char* rates=argv[4];
  char* pavafit=argv[5];
	char* cuts=argv[6];
	char* exact=argv[7];

  omp_set_num_threads(nthreads);

	time_t t1,t2,t3;
	t1=time(NULL);
	printf("- Reading edgelist from file %s\n",edgelist);
	opt=readedgelist(edgelist);
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));
	printf("- Building the datastructure\n");
	t1=time(NULL);
	relabel(opt);
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));
	printf("- Building the datastructure\n");
	printf("- Number of nodes = %u\n",opt->n);
	printf("- Number of edges = %u\n",opt->e);
	printf("- Computing the locally densest decomposition\n");

  printf("- Step 1: Frank-Wolf gradiant descent (%u iterations)\n",rep);
	t1=time(NULL);
	init(opt);
	for (unsigned i=0;i<rep;i++){
		//printf("%u\n",i);
		onepass(opt);
	}
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));
  //print_alphas(opt,alphas);

  printf("- Step 2: Isotonic regression with PAVA\n");
	t1=time(NULL);
  prepava(opt);
  fit=pava(opt->ne,opt->n);
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));

  print_fit(fit,pavafit);
  print_rates(opt,rates);

  printf("- Step 3: Checking if the %u cuts given by PAVA are correct\n",fit->n);
	t1=time(NULL);
	fit2=mkcut(fit,opt);
	freeisoreg(fit);
  print_fit(fit2,cuts);
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));

  printf("- Step 4: Density-Friendly in each %u indepent subgraphs\n",fit2->n-1);
	freeisoreg(fit2);
	t1=time(NULL);
	sgs=mkindep(opt,&nsgs);
	freeoptim(opt);
	FILE *file=fopen(exact,"w");
	unsigned k;
	#pragma omp parallel for schedule(dynamic) private(k)
	for(k=0;k<nsgs;k++){
		maxflow(sgs[k],file);
	}
	fclose(file);
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));

	return 0;
}


/*
 * poh_color_bfs.hpp
 * Author: Aven Bross
 *
 * Implementation of Poh path 3-coloring algorithm for triangulated plane graphs
 * that uses a breadth first search to find chordless paths.
 */

#ifndef __POH_COLOR_BFS_HPP
#define __POH_COLOR_BFS_HPP

// STL headers
#include <vector>
#include <queue>
#include <stdexcept>
#include <utility>
#include <string>

// Basic graph headers
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

// Local project headers
#include "incidence_list_helpers.hpp"

/*
 * poh_color_bfs_recursive
 *
 * inputs: A weakly triangulated planar graph with vertex indices (predfined
 *     boost property), a valid planar embedding of the graph modeling the boost
 *     PlanarEmbedding concept, a read-write-able vertex property map to store
 *     vertex colors, a read-write-able vertex property map to store integer
 *     marks for BFS, a read-write-able vertex property map to store the parent
 *     vertex for backtracking after the BFS, the first and last vertex of two
 *     disjoint colored paths P=p_0...p_n and Q=q_0...q_m such that p_0...p_nq_m
 *     ...q_0 is a cycle, an unsigned int count such that all vertex marks
 *     assigned so far are less than count, and finally the third color that has
 *     not been used on the paths P and Q.
 *
 * output: The coloring vertex property will contain a valid path 3-coloring of
 *     the subgraph bounded by p_0...p_nq_m...q_0 such that no interior vertex
 *     shares a color with a neighbor in P or Q.
 */

namespace {
    template<
            typename graph_t,
            typename planar_embedding_t,
            typename color_map_t,
            typename mark_map_t,
            typename parent_map_t,
            typename color_t,
            typename vertex_t
                = typename boost::graph_traits<graph_t>::vertex_descriptor
        >
    void poh_color_bfs_recursive(
            const graph_t & graph,
            const planar_embedding_t & planar_embedding,
            color_map_t & color_map,
            mark_map_t & mark_map,
            parent_map_t & parent_map,
            vertex_t p_0, vertex_t p_n, vertex_t q_0,
            vertex_t q_m, std::size_t count, color_t new_color
        )
    {
        color_t p_color = color_map[p_0], q_color = color_map[q_0];
        vertex_t t_0 = p_0, t_l = p_n;

        // Remove triangles from the end until we find an interior vertex t_l
        do {
            if(p_0 == p_n && q_0 == q_m) return;

            // Find the edge between p_n and q_m in q_m's incidence list
            auto edge_iter = find_edge_iterator(
                    q_m, p_n, planar_embedding, graph
                );

            // Find the neighbor t_l counterclockwise from p_n around q_m
            if(edge_iter == planar_embedding[q_m].end()) {
                throw std::runtime_error("No edge between p_n and q_m).");
            }
            else if(++edge_iter == planar_embedding[q_m].end()) {
                t_l = get_incident_vertex(
                        q_m, *planar_embedding[q_m].begin(), graph
                    );
            }
            else {
                t_l = get_incident_vertex(q_m, *edge_iter, graph);
            }

            // If t_l is in P or Q we have found a colored face and remove it
            if(color_map[t_l] == p_color) {
                p_n = t_l;
            }
            else if(color_map[t_l] == q_color) {
                q_m = t_l;
            }
        } while(color_map[t_l] == p_color || color_map[t_l] == q_color);

        vertex_t current_vertex, p_i = p_0, q_j = q_0;

        // Perform a BFS from t_l to find and color a path T between P and Q
        {
            std::queue<vertex_t> bfs_queue;
            std::size_t bfs_mark = count++;
            bfs_queue.push(t_l);
            mark_map[t_l] = bfs_mark;
            parent_map[t_l] = t_l;

            while(t_0 == p_0 && !bfs_queue.empty()) {
                current_vertex = bfs_queue.front();
                bfs_queue.pop();

                auto edge_iter = planar_embedding[current_vertex].begin();
                vertex_t last_neighbor = get_incident_vertex(
                        current_vertex, *edge_iter, graph
                    );

                // Loop through the neighbors of current_vertex
                do {
                    // If we hit the end of the list, wrap to the start
                    if(++edge_iter == planar_embedding[current_vertex].end())
                        edge_iter = planar_embedding[current_vertex].begin();

                    vertex_t neighbor = get_incident_vertex(
                            current_vertex, *edge_iter, graph
                        );

                    std::size_t mark = mark_map[neighbor];
                    color_t color = color_map[neighbor];
                    color_t last_color = color_map[last_neighbor];

                    // If we hit an unmarked vertex, add it to the queue
                    if(
                            mark != bfs_mark && color != p_color &&
                            color != q_color
                        )
                    {
                        parent_map[neighbor] = current_vertex;
                        mark_map[neighbor] = bfs_mark;
                        bfs_queue.push(neighbor);
                    }
                    // If we find an edge P to Q, we have completed T
                    else if(color == q_color && last_color == p_color) {
                        t_0 = current_vertex;
                        p_i = last_neighbor;
                        q_j = neighbor;

                        break;
                    }

                    last_neighbor = neighbor;
                } while(edge_iter != planar_embedding[current_vertex].begin());
            }

            if(t_0 == p_0) {
                throw std::runtime_error(
                        "BFS failed to find edge between paths."
                    );
            }
        }

        // If the edge p_iq_j is a chord we must color the rest
        if(p_i != p_0 || q_j != q_0) {
            poh_color_bfs_recursive(
                    graph, planar_embedding, color_map, mark_map, parent_map,
                    p_0, p_i, q_0, q_j, count, new_color
                );
        }

        color_map[current_vertex] = new_color;

        // Backtrack through BFS tree from t_0 to color the path T
        while(parent_map[current_vertex] != current_vertex) {
            current_vertex = parent_map[current_vertex];
            color_map[current_vertex] = new_color;
        }

        // Color the subgraph bounded by p_i...p_n and T
        poh_color_bfs_recursive(
                graph, planar_embedding, color_map, mark_map, parent_map,
                p_i, p_n, t_0, t_l, count, q_color
            );

        // Color the subgraph bounded by T and q_m...q_j
        poh_color_bfs_recursive(
                graph, planar_embedding, color_map, mark_map, parent_map,
                q_m, q_j, t_l, t_0, count, p_color
            );
    }
}

/*
 * poh_color_bfs
 *
 * inputs: A weakly triangulated planar graph with vertex indices (predfined
 *     boost property), a valid planar embedding of the graph modeling the boost
 *     PlanarEmbedding concept, a read-write-able vertex property map to store
 *     vertex colors, pairs of bidirectional iterators for lists of vertices
 *     of two disjoint colored paths P=p_0...p_n and Q=q_0...q_m such that
 *     p_0...p_nq_m...q_0 is a cycle, and three colors for the path 3-coloring
 *     (no vertex should be assigned any of these three colors before calling
 *     this function).
 *
 * output: The coloring vertex property will contain a valid path 3-coloring of
 *     the subgraph bounded by p_0...p_nq_m...q_0 such that vertices in P are
 *     recieve the first color, vertices in Q the second color, and no interior
 *     vertex shares a color with a neighbor in P or Q.
 */

template<
        typename graph_t,
        typename planar_embedding_t,
        typename color_map_t,
        typename mark_map_t,
        typename parent_map_t,
        typename vertex_iterator_t,
        typename color_t
    >
void poh_color_bfs(
        const graph_t & graph,
        const planar_embedding_t & planar_embedding,
        vertex_iterator_t p_begin, vertex_iterator_t p_end,
        vertex_iterator_t q_begin, vertex_iterator_t q_end,
        color_t c_0, color_t c_1, color_t c_2,
        mark_map_t & mark_map,
        parent_map_t & parent_map,
        color_map_t & color_map
    )
{
    // Color the path P
    for(vertex_iterator_t p_iter = p_begin; p_iter != p_end; ++p_iter) {
        color_map[*p_iter] = c_0;
    }

    // Color the path Q
    for(vertex_iterator_t q_iter = q_begin; q_iter != q_end; ++q_iter) {
        color_map[*q_iter] = c_1;
    }

    // Construct the path 3-coloring
    poh_color_bfs_recursive(
            graph, planar_embedding, color_map, mark_map, parent_map,
            *p_begin, *(--p_end), *q_begin, *(--q_end), 1, c_2
        );
}

/*
 * A wrapper that automatically constructs fast property maps for the parent_map
 * and neighbor_range_map, but requires that graph_t is some definition of
 * boost::adjacency_list.
 */

template<
        typename graph_t,
        typename planar_embedding_t,
        typename vertex_iterator_t,
        typename color_t,
        typename color_map_t
    >
void poh_color_bfs(
        const graph_t & graph,
        const planar_embedding_t & planar_embedding,
        vertex_iterator_t p_begin, vertex_iterator_t p_end,
        vertex_iterator_t q_begin, vertex_iterator_t q_end,
        color_t c_0, color_t c_1, color_t c_2,
        color_map_t & color_map
    )
{
    // Vertex type
    typedef typename boost::graph_traits<graph_t>::vertex_descriptor vertex_t;

    // Vertex property map to store vertex marks
    typedef boost::iterator_property_map<
            std::vector<int>::iterator,
            typename boost::property_map<
                    graph_t, boost::vertex_index_t
                >::const_type
        > integer_property_map_t;

    // Vertex property map to store BFS tree
    typedef boost::iterator_property_map<
            typename std::vector<vertex_t>::iterator,
            typename boost::property_map<
                    graph_t, boost::vertex_index_t
                >::const_type
        > parent_map_t;

    // Construct a vertex property map to store vertex marks
    std::vector<int> mark_storage(num_vertices(graph));
    integer_property_map_t mark_map(
            mark_storage.begin(), boost::get(boost::vertex_index, graph)
        );

    // Construct a vertex property map to store neighbor ranges
    std::vector<vertex_t> parent_storage(num_vertices(graph));
    parent_map_t parent_map(
            parent_storage.begin(), boost::get(boost::vertex_index, graph)
        );

    // Construct the path 3-coloring
    poh_color_bfs(
            graph,
            planar_embedding,
            p_begin, p_end,
            q_begin, q_end,
            c_0, c_1, c_2,
            mark_map,
            parent_map,
            color_map
        );
}

#endif


// ======================================================================
/*!
 * \file NFmiWebMercatorArea.cpp
 * \brief Implementation of class NFmiWebMercatorArea
 */
// ======================================================================
/*!
 * \class NFmiWebMercatorArea
 *
 * Provides for equidistance cylindrical projection. Maps geodetic
 * coordinates (in degrees) to rectangular mercator xy coordinates
 * (in meters) and vice versa.
 *
 * Projection is based on EPSG:3857
 */
// ======================================================================

#include "NFmiWebMercatorArea.h"
#include <boost/functional/hash.hpp>
#include <fmt/format.h>
#include <macgyver/Exception.h>
#include <limits>

using namespace std;

const double kSemiAxis = 6378137.0;

// ----------------------------------------------------------------------
/*!
 * Default constructor
 */
// ----------------------------------------------------------------------

NFmiWebMercatorArea::NFmiWebMercatorArea()
    : NFmiArea(),
      itsBottomLeftLatLon(),
      itsTopRightLatLon(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect()
{
}

// ----------------------------------------------------------------------
/*!
 * Constructor
 *
 * \param theBottomLeftLatLon Undocumented
 * \param theTopRightLatLon Undocumented
 * \param theTopLeftXY Undocumented
 * \param theBottomRightXY Undocumented
 */
// ----------------------------------------------------------------------

NFmiWebMercatorArea::NFmiWebMercatorArea(const NFmiPoint& theBottomLeftLatLon,
                                         const NFmiPoint& theTopRightLatLon,
                                         const NFmiPoint& theTopLeftXY,
                                         const NFmiPoint& theBottomRightXY,
                                         bool usePacificView)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsBottomLeftLatLon(theBottomLeftLatLon),
      itsTopRightLatLon(theTopRightLatLon),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect()
{
  try
  {
    Init();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Copy constructor
 *
 * \param theLatLonArea The other object being copied
 */
// ----------------------------------------------------------------------

NFmiWebMercatorArea::NFmiWebMercatorArea(const NFmiWebMercatorArea& theLatLonArea)

    = default;

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 * \todo Should return an boost::shared_ptr instead
 */
// ----------------------------------------------------------------------

NFmiArea* NFmiWebMercatorArea::Clone() const
{
  try
  {
    return new NFmiWebMercatorArea(*this);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param fKeepWorldRect Undocumented
 */
// ----------------------------------------------------------------------

void NFmiWebMercatorArea::Init(bool fKeepWorldRect)
{
  try
  {
    if (itsTopRightLatLon.X() < itsBottomLeftLatLon.X())
      itsTopRightLatLon += NFmiPoint(360., 0.);

    if (!fKeepWorldRect)
      itsWorldRect =
          NFmiRect(LatLonToWorldXY(itsBottomLeftLatLon), LatLonToWorldXY(itsTopRightLatLon));

    itsXScaleFactor = Width() / itsWorldRect.Width();
    itsYScaleFactor = Height() / itsWorldRect.Height();

    NFmiArea::Init(fKeepWorldRect);

    const char* fmt = "+proj=webmerc +R={}";
    itsProjStr = fmt::format(fmt, kRearth);
    itsSpatialReference = std::make_shared<Fmi::SpatialReference>(itsProjStr);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theLatLonPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::LatLonToWorldXY(const NFmiPoint& theLatLonPoint) const
{
  try
  {
    // Limit Y-values to prevent infinity

    double y = std::max(std::min(theLatLonPoint.Y(), 89.9999), -89.9999);

    return NFmiPoint(kSemiAxis * FmiRad(theLatLonPoint.X()),
                     kSemiAxis * log(tan(FmiRad(45. + 0.5 * y))));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theLatLonPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::ToXY(const NFmiPoint& theLatLonPoint) const
{
  try
  {
    // Transforms input geodetic coordinates (longitude,latitude) into local (relative)
    // coordinates on xy-plane.
    double xLocal, yLocal;

    // Transform input geodetic coordinates into world coordinates (meters) on xy-plane.
    NFmiPoint latlon(FixLongitude(theLatLonPoint.X()), theLatLonPoint.Y());
    NFmiPoint xyWorld(LatLonToWorldXY(latlon));

    // Finally, transform world xy-coordinates into local xy-coordinates
    xLocal = Left() + itsXScaleFactor * (xyWorld.X() - itsWorldRect.Left());
    yLocal = Top() + itsYScaleFactor * (itsWorldRect.Bottom() - xyWorld.Y());

    return NFmiPoint(xLocal, yLocal);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::WorldXYToLatLon(const NFmiPoint& theXYPoint) const
{
  try
  {
    // Computes the geodetic coordinates (in degrees) from the input (metric) world xy coordinates

    double worldY = theXYPoint.Y();
    double lon = NFmiLongitude(FmiDeg(theXYPoint.X() / kSemiAxis), PacificView()).Value();
    double lat = FmiDeg(2.0 * atan(exp(worldY / kSemiAxis)) - 0.5 * kPii);

    return NFmiPoint(lon, lat);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::XYToWorldXY(const NFmiPoint& theXYPoint) const
{
  try
  {
    // Transform local xy-coordinates into world xy-coordinates (meters).
    double xWorld = itsWorldRect.Left() + (theXYPoint.X() - Left()) / itsXScaleFactor;
    double yWorld = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / itsYScaleFactor;
    return NFmiPoint(xWorld, yWorld);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::WorldXYToXY(const NFmiPoint& theWorldXYPoint) const
{
  try
  {
    double x = itsXScaleFactor * (theWorldXYPoint.X() - itsWorldRect.Left()) + Left();
    double y = Top() - itsYScaleFactor * (theWorldXYPoint.Y() - itsWorldRect.Bottom());
    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::ToLatLon(const NFmiPoint& theXYPoint) const
{
  try
  {
    // Transforms input local xy-coordinates into geodetic coordinates
    // (longitude,latitude) on globe.
    return WorldXYToLatLon(XYToWorldXY(theXYPoint));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

double NFmiWebMercatorArea::XScale() const
{
  try
  {
    return 1. / itsXScaleFactor;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

double NFmiWebMercatorArea::YScale() const
{
  try
  {
    return 1. / itsYScaleFactor;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theBottomLeftLatLon Undocumented
 * \param theTopRightLatLon Undocumented
 * \return Undocumented
 * \todo Should return an boost::shared_ptr instead
 */
// ----------------------------------------------------------------------

NFmiArea* NFmiWebMercatorArea::NewArea(const NFmiPoint& theBottomLeftLatLon,
                                       const NFmiPoint& theTopRightLatLon,
                                       bool allowPacificFix) const
{
  try
  {
    if (allowPacificFix)
    {
      PacificPointFixerData fixedPointData =
          NFmiArea::PacificPointFixer(theBottomLeftLatLon, theTopRightLatLon);
      return new NFmiWebMercatorArea(fixedPointData.itsBottomLeftLatlon,
                                     fixedPointData.itsTopRightLatlon,
                                     TopLeft(),
                                     BottomRight(),
                                     fixedPointData.fIsPacific);
    }

    return new NFmiWebMercatorArea(
        theBottomLeftLatLon, theTopRightLatLon, TopLeft(), BottomRight(), PacificView());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Write the object to the given output stream
 *
 * \param file The output stream to write to
 * \return The output stream written to
 */
// ----------------------------------------------------------------------

std::ostream& NFmiWebMercatorArea::Write(std::ostream& file) const
{
  try
  {
    NFmiArea::Write(file);
    file << itsBottomLeftLatLon;
    file << itsTopRightLatLon;

    // Dummies to replace old removed variables
    file << "0 0\n0 0\n";

    file << itsXScaleFactor << " ";
    file << itsYScaleFactor << std::endl;
    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Read new object contents from the given input stream
 *
 * \param file The input stream to read from
 * \return The input stream read from
 */
// ----------------------------------------------------------------------

std::istream& NFmiWebMercatorArea::Read(std::istream& file)
{
  try
  {
    double dummy;

    NFmiArea::Read(file);
    file >> itsBottomLeftLatLon;
    file >> itsTopRightLatLon;
    PacificView(NFmiArea::IsPacificView(itsBottomLeftLatLon, itsTopRightLatLon));

    file >> dummy >> dummy >> dummy >> dummy;  // old removed variables

    file >> itsXScaleFactor;
    file >> itsYScaleFactor;

    itsWorldRect =
        NFmiRect(LatLonToWorldXY(itsBottomLeftLatLon), LatLonToWorldXY(itsTopRightLatLon));

    Init();

    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const std::string NFmiWebMercatorArea::AreaStr() const
{
  try
  {
    std::ostringstream out;
    out << "webmercator:" << BottomLeftLatLon().X() << ',' << BottomLeftLatLon().Y() << ','
        << TopRightLatLon().X() << ',' << TopRightLatLon().Y();
    return out.str();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Return Well Known Text representation of the GCS
 *
 * We shall return EPSG:3857, but may have to return the following if using the plain
 * EPSG does not work for all software
 *
 * PROJCS["WGS 84 / Pseudo-Mercator",
 *    GEOGCS["WGS 84",
 *        DATUM["WGS_1984",
 *            SPHEROID["WGS 84",6378137,298.257223563,
 *                AUTHORITY["EPSG","7030"]],
 *            AUTHORITY["EPSG","6326"]],
 *        PRIMEM["Greenwich",0,
 *            AUTHORITY["EPSG","8901"]],
 *        UNIT["degree",0.0174532925199433,
 *            AUTHORITY["EPSG","9122"]],
 *        AUTHORITY["EPSG","4326"]],
 *    PROJECTION["Mercator_1SP"],
 *    PARAMETER["central_meridian",0],
 *    PARAMETER["scale_factor",1],
 *    PARAMETER["false_easting",0],
 *    PARAMETER["false_northing",0],
 *    UNIT["metre",1,
 *        AUTHORITY["EPSG","9001"]],
 *    AXIS["X",EAST],
 *    AXIS["Y",NORTH],
 *    EXTENSION["PROJ4","+proj=merc +a=6378137 +b=6378137 +lat_ts=0.0 +lon_0=0.0 +x_0=0.0 +y_0=0
 * +k=1.0 +units=m +nadgrids=@null +wktext  +no_defs"], AUTHORITY["EPSG","3857"]]
 *
 */
// ----------------------------------------------------------------------

const std::string NFmiWebMercatorArea::WKT() const
{
  return "EPSG:3857";
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison with an NFmiWebMercatorArea.
 *
 * \param theArea The other area being compared to
 * \return True, if the NFmiWebMercatorArea parts are equivalent
 * \todo Use static_cast instead of C-style cast
 */
// ----------------------------------------------------------------------

bool NFmiWebMercatorArea::operator==(const NFmiArea& theArea) const
{
  try
  {
    return *this == static_cast<const NFmiWebMercatorArea&>(theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison
 *
 * \param theArea The other area being compared to
 * \return True, if the areas are equivalent
 * \todo Investigate whether NFmiArea::operator== should also be called.
 */
// ----------------------------------------------------------------------

bool NFmiWebMercatorArea::operator==(const NFmiWebMercatorArea& theArea) const
{
  try
  {
    if ((itsBottomLeftLatLon == theArea.itsBottomLeftLatLon) &&
        (itsTopRightLatLon == theArea.itsTopRightLatLon) &&
        (itsXScaleFactor == theArea.itsXScaleFactor) &&
        (itsYScaleFactor == theArea.itsYScaleFactor) && (itsWorldRect == theArea.itsWorldRect))
      return true;

    return false;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Hash value
 */
// ----------------------------------------------------------------------

std::size_t NFmiWebMercatorArea::HashValue() const
{
  try
  {
    std::size_t hash = NFmiArea::HashValue();
    boost::hash_combine(hash, itsBottomLeftLatLon.HashValue());
    boost::hash_combine(hash, itsTopRightLatLon.HashValue());
    boost::hash_combine(hash, boost::hash_value(itsXScaleFactor));
    boost::hash_combine(hash, boost::hash_value(itsYScaleFactor));
    boost::hash_combine(hash, itsWorldRect.HashValue());
    return hash;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================


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

#ifndef SRC_MATH_HPP_
#define SRC_MATH_HPP_

#include <Eigen/SparseCholesky>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>

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

namespace wrapd {
namespace math {

using VecX = Eigen::VectorXd;
using VecXi = Eigen::VectorXi;
using MatX = Eigen::MatrixXd;
using MatXi = Eigen::MatrixXi;
using MatX2 = Eigen::MatrixX2d;
using Mat2X = Eigen::Matrix2Xd;
using SpMat = Eigen::SparseMatrix<double, Eigen::RowMajor>;
using DiagMat = Eigen::DiagonalMatrix<double, Eigen::Dynamic>;
using RowVec3 = Eigen::RowVector3d;

using Triplet = Eigen::Triplet<double>;
using Triplets = std::vector<Triplet>;

using Doubles = std::vector<double>;

using Cholesky = Eigen::SimplicialLDLT< Eigen::SparseMatrix<double>, Eigen::Lower>;

using AlignedBox = Eigen::AlignedBox<double, 3>;

// Singular value decomposition
template <typename TYPE>
using JacobiSVD = Eigen::JacobiSVD<TYPE>;
static constexpr int ComputeFullU = Eigen::ComputeFullU;
static constexpr int ComputeFullV = Eigen::ComputeFullV;

// Mapping between vectors and matrices
template <typename TYPE>
using Map = Eigen::Map<TYPE>;

// Vectors of doubles
template <int DIM>
using Vec = Eigen::Matrix<double, DIM, 1>;
using Vec2 = Vec<2>;
using Vec3 = Vec<3>;
using Vec4 = Vec<4>;

// Vectors of floats
template <int DIM>
using Vecf = Eigen::Matrix<float, DIM, 1>;
using Vec2f = Vecf<2>;
using Vec3f = Vecf<3>;
using Vec4f = Vecf<4>;

// Vectors of integers
template <int DIM>
using Veci = Eigen::Matrix<int, DIM, 1>;
using Vec1i = Veci<1>;
using Vec2i = Veci<2>;
using Vec3i = Veci<3>;
using Vec4i = Veci<4>;

template <int ROWS, int COLS>
using Mat = Eigen::Matrix<double, ROWS, COLS>;

using Mat1x3 = Mat<1, 3>;
using Mat2x2 = Mat<2, 2>;
using Mat2x3 = Mat<2, 3>;
using Mat3x2 = Mat<3, 2>;
using Mat3x3 = Mat<3, 3>;
using Mat3x4 = Mat<3, 4>;

inline double clamp(const double minval, const double val, const double maxval) {
    double retval = val;
    if (val < minval) {
        retval = minval;
    }
    if (val > maxval) {
        retval = maxval;
    }
    return retval;
}

inline void svd(
        const math::Mat2x2& F,
        math::Vec2& sigma,
        math::Mat2x2& U,
        math::Mat2x2& V,
        bool prevent_flips = true) {
    math::JacobiSVD<math::Mat2x2> svd(F, math::ComputeFullU | math::ComputeFullV);
    sigma = svd.singularValues();
    U = svd.matrixU();
    V = svd.matrixV();

    if (prevent_flips && (U*V.transpose()).determinant() < 0.) {
        math::Mat2x2 J = math::Mat2x2::Identity();
        J(1, 1) = -1.0;
        if (U.determinant() < 0.) {
            U = U * J;
            sigma[1] = -sigma[1];
        }
        if (V.determinant() < 0.0) {
            math::Mat2x2 Vt = V.transpose();
            Vt = J * Vt;
            V = Vt.transpose();
            sigma[1] = -sigma[1];
        }
    }
}

inline void polar(
        const math::Mat2x2& F,
        math::Mat2x2& R,
        math::Mat2x2& S,
        bool prevent_flips = true) {
    math::Vec2 sigma;
    math::Mat2x2 U;
    math::Mat2x2 V;
    svd(F, sigma, U, V, prevent_flips);
    R = U * V.transpose();
    S = V * sigma.asDiagonal() * V.transpose();
}

inline math::Mat2x2 rot(const math::Mat2x2& A) {
    math::Vec2 b(A(0, 0) + A(1, 1), A(0, 1) - A(1, 0));
    b.normalize();
    math::Mat2x2 R2;
    R2(0, 0) = R2(1, 1) = b(0);
    R2(0, 1) = b(1);
    R2(1, 0) = -b(1);
    return R2;
}

inline math::Mat2x2 sym(const math::Mat2x2& A) {
    math::Mat2x2 R = rot(A);
    return R.transpose() * A;
}

}  // namespace math
} // namespace wrapd

#endif  // SRC_MATH_HPP_


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "cotmatrix.h"
#include <vector>

// For error printing
#include <cstdio>
#include "cotmatrix_entries.h"

// Bug in unsupported/Eigen/SparseExtra needs iostream first
#include <iostream>

template <typename DerivedV, typename DerivedF, typename Scalar>
IGL_INLINE void igl::cotmatrix(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  Eigen::SparseMatrix<Scalar>& L)
{
  using namespace Eigen;
  using namespace std;

  L.resize(V.rows(),V.rows());
  Matrix<int,Dynamic,2> edges;
  int simplex_size = F.cols();
  // 3 for triangles, 4 for tets
  assert(simplex_size == 3 || simplex_size == 4);
  if(simplex_size == 3)
  {
    // This is important! it could decrease the comptuation time by a factor of 2
    // Laplacian for a closed 2d manifold mesh will have on average 7 entries per
    // row
    L.reserve(10*V.rows());
    edges.resize(3,2);
    edges <<
      1,2,
      2,0,
      0,1;
  }else if(simplex_size == 4)
  {
    L.reserve(17*V.rows());
    edges.resize(6,2);
    edges <<
      1,2,
      2,0,
      0,1,
      3,0,
      3,1,
      3,2;
  }else
  {
    return;
  }
  // Gather cotangents
  Matrix<Scalar,Dynamic,Dynamic> C;
  cotmatrix_entries(V,F,C);

  vector<Triplet<Scalar> > IJV;
  IJV.reserve(F.rows()*edges.rows()*4);
  // Loop over triangles
  for(int i = 0; i < F.rows(); i++)
  {
    // loop over edges of element
    for(int e = 0;e<edges.rows();e++)
    {
      int source = F(i,edges(e,0));
      int dest = F(i,edges(e,1));
      IJV.push_back(Triplet<Scalar>(source,dest,C(i,e)));
      IJV.push_back(Triplet<Scalar>(dest,source,C(i,e)));
      IJV.push_back(Triplet<Scalar>(source,source,-C(i,e)));
      IJV.push_back(Triplet<Scalar>(dest,dest,-C(i,e)));
    }
  }
  L.setFromTriplets(IJV.begin(),IJV.end());
}

#include "massmatrix.h"
#include "pinv.h"
#include "cotmatrix_entries.h"
#include "diag.h"
#include "massmatrix.h"
#include <Eigen/Geometry>

template <
  typename DerivedV,
  typename DerivedI,
  typename DerivedC,
  typename Scalar>
IGL_INLINE void igl::cotmatrix(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedI> & I,
  const Eigen::MatrixBase<DerivedC> & C,
  Eigen::SparseMatrix<Scalar>& L,
  Eigen::SparseMatrix<Scalar>& M,
  Eigen::SparseMatrix<Scalar>& P)
{
  typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MatrixXS;
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorXS;
  typedef Eigen::Index Index;
  // number of vertices
  const Index n = V.rows();
  // number of polyfaces
  const Index m = C.size()-1;
  assert(V.cols() == 2 || V.cols() == 3);
  std::vector<Eigen::Triplet<Scalar> > Lfijv;
  std::vector<Eigen::Triplet<Scalar> > Mfijv;
  std::vector<Eigen::Triplet<Scalar> > Pijv;
  // loop over vertices; set identity for original vertices
  for(Index i = 0;i<V.rows();i++) { Pijv.emplace_back(i,i,1); }
  // loop over faces
  for(Index p = 0;p<C.size()-1;p++)
  {
    // number of faces/vertices in this simple polygon
    const Index np = C(p+1)-C(p);
    // Working "local" list of vertices; last vertex is new one
    // this needs to have 3 columns so Eigen doesn't complain about cross
    // products below.
    Eigen::Matrix<Scalar,Eigen::Dynamic,3> X = decltype(X)::Zero(np+1,3);
    for(Index i = 0;i<np;i++){ X.row(i).head(V.cols()) = V.row(I(C(p)+i)); };
    // determine weights definig position of inserted vertex
    {
      MatrixXS A = decltype(A)::Zero(np+1,np);
      // My equation (38) would be A w = b.
      VectorXS b = decltype(b)::Zero(np+1);
      for(Index k = 0;k<np;k++)
      {
        const RowVector3S Xkp1mk = X.row((k+1)%np)-X.row(k);
        const RowVector3S Xkp1mkck = Xkp1mk.cross(X.row(k));
        for(Index i = 0;i<np;i++)
        {
          b(i) -= 2.*(X.row(i).cross(Xkp1mk)).dot(Xkp1mkck);
          for(Index j = 0;j<np;j++)
          {
            A(i,j) += 2.*(X.row(j).cross(Xkp1mk)).dot(X.row(i).cross(Xkp1mk));
          }
        }
      }
      A.row(np).setConstant(1);
      b(np) = 1;
      const VectorXS w =
        Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd>(A).solve(b);
      X.row(np) = w.transpose()*X.topRows(np);
      // scatter w into new row of P
      for(Index i = 0;i<np;i++) { Pijv.emplace_back(n+p,I(C(p)+i),w(i)); }
    }
    // "local" fan of faces. These could be statically cached, but this will
    // not be the bottleneck.
    Eigen::MatrixXi F(np,3);
    for(Index i = 0;i<np;i++)
    {
      F(i,0) = i;
      F(i,1) = (i+1)%np;
      F(i,2) = np;
    }
    // Cotangent contributions
    MatrixXS K;
    igl::cotmatrix_entries(X,F,K);
    // Massmatrix entried
    VectorXS Mp;
    {
      Eigen::SparseMatrix<Scalar> M;
      igl::massmatrix(X,F,igl::MASSMATRIX_TYPE_DEFAULT,M);
      Mp = M.diagonal();
    }
    // Scatter into fine Laplacian and mass matrices
    const auto J = [&n,&np,&p,&I,&C](Index i)->Index{return i==np?n+p:I(C(p)+i);};
    // Should just build Mf as a vector...
    for(Index i = 0;i<np+1;i++) { Mfijv.emplace_back(J(i),J(i),Mp(i)); }
    // loop over faces
    for(Index f = 0;f<np;f++)
    {
      for(Index c = 0;c<3;c++)
      {
        const Index i = F(f,(c+1)%3);
        const Index j = F(f,(c+2)%3);
        // symmetric off-diagonal
        Lfijv.emplace_back(J(i),J(j),K(f,c));
        Lfijv.emplace_back(J(j),J(i),K(f,c));
        // diagonal
        Lfijv.emplace_back(J(i),J(i),-K(f,c));
        Lfijv.emplace_back(J(j),J(j),-K(f,c));
      }
    }
  }
  P.resize(n+m,n);
  P.setFromTriplets(Pijv.begin(),Pijv.end());
  Eigen::SparseMatrix<Scalar> Lf(n+m,n+m);
  Lf.setFromTriplets(Lfijv.begin(),Lfijv.end());
  Eigen::SparseMatrix<Scalar> Mf(n+m,n+m);
  Mf.setFromTriplets(Mfijv.begin(),Mfijv.end());
  L = P.transpose() * Lf * P;
  // "unlumped" M
  const Eigen::SparseMatrix<Scalar> PTMP = P.transpose() * Mf * P;
  // Lump M
  const VectorXS Mdiag = PTMP * VectorXS::Ones(n,1);
  igl::diag(Mdiag,M);

  MatrixXS Vf = P*V;
  Eigen::MatrixXi Ff(I.size(),3);
  {
    Index f = 0;
    for(Index p = 0;p<C.size()-1;p++)
    {
      const Index np = C(p+1)-C(p);
      for(Index c = 0;c<np;c++)
      {
        Ff(f,0) = I(C(p)+c);
        Ff(f,1) = I(C(p)+(c+1)%np);
        Ff(f,2) = V.rows()+p;
        f++;
      }
    }
    assert(f == Ff.rows());
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::cotmatrix<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::SparseMatrix<double, 0, int>&, Eigen::SparseMatrix<double, 0, int>&, Eigen::SparseMatrix<double, 0, int>&);
// generated by autoexplicit.sh
template void igl::cotmatrix<Eigen::Matrix<double, -1, -1, 1, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 1, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::SparseMatrix<double, 0, int>&);
template void igl::cotmatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 4, 0, -1, 4>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 4, 0, -1, 4> > const&, Eigen::SparseMatrix<double, 0, int>&);
template void igl::cotmatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::SparseMatrix<double, 0, int>&);
template void igl::cotmatrix<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::SparseMatrix<double, 0, int>&);
#endif


/*
 * EigenvalueCovariance.cpp
 *
 *  Created on: May 6, 2015
 *      Author: dbazazian
 */

     #include <iostream>
     #include <Eigen/Eigenvalues>
     #include <Eigen/Dense>
     #include <vector>
     #include <math.h>
     #include <cmath>
     #include <fstream>
     #include <string>
     #include <vector>
     #include <pcl/io/io.h>
     #include <pcl/io/pcd_io.h>
     #include <pcl/point_types.h>
     #include <pcl/features/integral_image_normal.h>
     #include <pcl/features/normal_3d.h>
     #include <pcl/common/common_headers.h>
     #include <pcl/features/integral_image_normal.h>
     #include <pcl/features/normal_3d.h>
     #include <pcl/visualization/cloud_viewer.h>
     #include <pcl/filters/passthrough.h>
     #include <pcl/ModelCoefficients.h>
     #include <pcl/filters/project_inliers.h>
     #include <pcl/features/shot_omp.h>
     #include "pcl/features/fpfh.h"
     #include <pcl/io/ply_io.h>

using namespace std;
using namespace Eigen;

int
main (int argc, char*argv[])
{
	  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);

	 // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/Wedge/W0.2S45T45.pcd", *cloud);
	 // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/TwoPlane22.pcd", *cloud);
	  //  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/CubeSharpEdge.pcd", *cloud);
	  // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/OnePlane.pcd", *cloud);
	   // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/bunny.pcd",*cloud);
	   // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/Statue.pcd", *cloud);
	     // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/dragon.pcd", *cloud);
	  // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/IntersectionThreePlanes.pcd", *cloud);

	  // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AddingNoise/Bunny03Noise50.pcd", *cloud);
	  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/TetrahedronMultiple.pcd", *cloud);

	    // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/SpherMultiple.pcd", *cloud);

	      // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/trim-starC.pcd", *cloud);
	      //	pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/VaseC.pcd", *cloud);
	       //  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/twirlC.pcd", *cloud);
	      	//  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/fandiskC.pcd", *cloud);
	      //pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/sharp_sphereC.pcd", *cloud);

	   std::cout << "Number of points in the Cube Input cloud is:"<< cloud->points.size() << std::endl;

	  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Normals (new pcl::PointCloud<pcl::PointXYZRGBA>);
	  Normals->resize(cloud->size());

	  // K nearest neighbor search
	  int KNumbersNeighbor = 10; // numbers of neighbors 7 , 120
	  std::vector<int> NeighborsKNSearch(KNumbersNeighbor);
	  std::vector<float> NeighborsKNSquaredDistance(KNumbersNeighbor);

	  int* NumbersNeighbor = new  int [cloud ->points.size ()];
	  pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
	  kdtree.setInputCloud (cloud);
	  pcl::PointXYZRGBA searchPoint;

	  double* SmallestEigen = new  double [cloud->points.size() ];
	  double* MiddleEigen = new  double [cloud->points.size() ];
	  double* LargestEigen = new  double [cloud->points.size() ];

	  double* DLS = new  double [cloud->points.size() ];
	  double* DLM = new  double [cloud->points.size() ];
	  double* DMS = new  double [cloud->points.size() ];
	  double* Sigma = new  double [cloud->points.size() ];

//		std::vector<double> SmallestEigen;
//		std::vector<double> MiddleEigen;
//		std::vector<double> LargestEigen;
//
//		std::vector<double> DLS;
//		std::vector<double> DML;
//		std::vector<double> DMS;

	    //  ************ All the Points of the cloud *******************
	for (size_t i = 0; i < cloud ->points.size (); ++i) {

	searchPoint.x =   cloud->points[i].x;
	searchPoint.y =   cloud->points[i].y;
	searchPoint.z =   cloud->points[i].z;

	if ( kdtree.nearestKSearch (searchPoint, KNumbersNeighbor, NeighborsKNSearch, NeighborsKNSquaredDistance) > 0 ) {
		 NumbersNeighbor[i]= NeighborsKNSearch.size (); }
	    else { NumbersNeighbor[i] = 0; }

	float Xmean; float Ymean; float Zmean;
	float sum= 0.00;
// Computing Covariance Matrix
	for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
      sum += cloud->points[ NeighborsKNSearch[ii] ].x; }
	Xmean = sum / NumbersNeighbor[i] ;
	sum= 0.00;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += cloud->points[NeighborsKNSearch[ii] ].y;}
			Ymean = sum / NumbersNeighbor[i] ;
		 sum= 0.00;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += cloud->points[NeighborsKNSearch[ii] ].z;}
			Zmean = sum / NumbersNeighbor[i] ;

			float	CovXX;  float CovXY; float CovXZ; float CovYX; float CovYY; float CovYZ; float CovZX; float CovZY; float CovZZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].x - Xmean )  );}
			CovXX = sum / ( NumbersNeighbor[i]-1) ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );}
			CovXY = sum / ( NumbersNeighbor[i]-1) ;

			CovYX = CovXY ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovXZ= sum / ( NumbersNeighbor[i]-1) ;

			CovZX = CovXZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );}
			CovYY = sum / ( NumbersNeighbor[i]-1) ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovYZ = sum / ( NumbersNeighbor[i]-1) ;

			CovZY = CovYZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].z - Zmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovZZ = sum / ( NumbersNeighbor[i]-1) ;

// Computing Eigenvalue and EigenVector
   Matrix3f Cov;
   Cov << CovXX, CovXY, CovXZ, CovYX, CovYY, CovYZ, CovZX, CovZY, CovZZ;

  SelfAdjointEigenSolver<Matrix3f> eigensolver(Cov);
  if (eigensolver.info() != Success) abort();

  double EigenValue1 = eigensolver.eigenvalues()[0];
  double EigenValue2 = eigensolver.eigenvalues()[1];
  double EigenValue3 = eigensolver.eigenvalues()[2];

  double Smallest = 0.00; double Middle = 0.00; double Largest= 0.00;
  if (EigenValue1<  EigenValue2 ) { Smallest =  EigenValue1 ; } else { Smallest = EigenValue2 ; }
  if (EigenValue3<  Smallest ) { Smallest =  EigenValue3 ; }

  if(EigenValue1 <= EigenValue2 && EigenValue1 <= EigenValue3) {
	  Smallest = EigenValue1;
  if(EigenValue2 <= EigenValue3) {Middle = EigenValue2; Largest = EigenValue3;}
  else {Middle = EigenValue3; Largest = EigenValue2;}
  }

  if(EigenValue1 >= EigenValue2 && EigenValue1 >= EigenValue3)
  {
	  Largest = EigenValue1;
  if(EigenValue2 <= EigenValue3) { Smallest = EigenValue2; Middle = EigenValue3; }
  else {Smallest = EigenValue3; Middle = EigenValue2;}
  }

  if ((EigenValue1 >= EigenValue2 && EigenValue1 <= EigenValue3) || (EigenValue1 <= EigenValue2 && EigenValue1 >= EigenValue3))
  {
	  Middle = EigenValue1;
  if(EigenValue2 >= EigenValue3){Largest = EigenValue2; Smallest = EigenValue3;}
  else{Largest = EigenValue3; Smallest = EigenValue2;}
  }

SmallestEigen[i]= Smallest ;
MiddleEigen[i]= Middle;
LargestEigen[i]= Largest;

DLS[i] =    std::abs ( SmallestEigen[i] / LargestEigen[i]) ;          // std::abs ( LargestEigen[i] -  SmallestEigen[i] ) ;
DLM[i] = std::abs ( MiddleEigen[i] /  LargestEigen[i]) ;             // std::abs (  LargestEigen[i] - MiddleEigen[i] ) ;
DMS[i] = std::abs ( SmallestEigen[i] / MiddleEigen[i]) ;       // std::abs ( MiddleEigen[i] -  SmallestEigen[i] ) ;
Sigma[i] = (SmallestEigen[i] ) / ( SmallestEigen[i] + MiddleEigen[i] + LargestEigen[i] ) ;
	} // For each point of the cloud

	  std::cout<< " Computing Sigma is Done! " << std::endl;
// Color Map For the difference of the eigen values

	  double MaxD=0.00 ;
	  double MinD= cloud ->points.size ();
	  int Ncolors=256;

	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
	  	if (  Sigma [i] < MinD) MinD= Sigma [i];
	  	if (  Sigma[i] > MaxD) MaxD = Sigma [i];
	  }

	  std::cout<< " Minimum is :" << MinD<< std::endl;
	  std::cout<< " Maximum  is :" << MaxD << std::endl;

//   *****************************************
	/*
	  // computing the standard deviation
	 double ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += Sigma [i] ;}
	  double avg = ss / cloud ->points.size () ;
	  ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += (Sigma [i] -  avg ) * (  Sigma [i] -  avg ) ;}
	  double stddvtion =   sqrt (  ss  /  ( cloud ->points.size () - 1 )  ) ;

	  std::cout<< " Standard Deviation is :" << stddvtion << std::endl;

	   MaxD = ( 2 )* stddvtion;
	  //MaxD = 10* stddvtion;

	  // Color table
		double line;
		double code[Ncolors][3];
	   ifstream colorcode ( "/Path/TO/ArtificialPointClouds/JetColorDensity/ColorCodes256.txt" );
	   //store color codes in array
	    int i=0,j=0;
	    while( colorcode>> line ) {
	    code[i][j]=line;
	    j++;
	    if (j == 3)
	    i++;
	}
	    code[1][0] = 0;
	    code[1][1] = 0;
	    code[1][2] = 135.468;

	    // jet color map

	int level = 0;
	float step = ( ( MaxD -  MinD) / Ncolors ) ;
	    for (size_t i = 0; i < cloud ->points.size (); ++i) {
if (  SmallestEigen [i] <= MaxD ) {
	    level = floor( (SmallestEigen [i] - MinD ) /  step ) ;

	    cloud->points[i].r = code[ level ][0];
	    cloud->points[i].g =  code[ level ][1];
	    cloud->points[i].b =  code[ level ][2];
} // if sigma less than Max
	    }
*/
//    *****************************************

//   *****************************************
	    int Edgepoints = 0;
	    // Red and white (khaki)

	    for (size_t i = 0; i < cloud ->points.size (); ++i) {
	    	    cloud->points[i].r = 240;
	    	    cloud->points[i].g =  230 ;
	    	    cloud->points[i].b =  140;
	            }

	  int level = 0;
	  float step = ( ( MaxD -  MinD) / Ncolors ) ;
	 //  level = floor( (Sigma [i] - MinD ) /  step ) ;
for (size_t i = 0; i < cloud ->points.size (); ++i) {
	  if ( Sigma [i] > ( MinD + ( 6* step) ) ) {  //6*step
	    cloud->points[i].r = 255;
	    cloud->points[i].g =  0 ;
	    cloud->points[i].b =  0;

	    //    Dim gray....
//		    cloud->points[i].r = 105;
//		    cloud->points[i].g = 105 ;
//		    cloud->points[i].b = 105;
	    Edgepoints ++;
        }
     }

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

std::cout<< " Number of Edge points  is :" << Edgepoints << std::endl;

	    // writing the Sigma on the disk
//	    	   		std::ofstream ofsSigma;
//	    	   		ofsSigma.open("/Path/TO/SigmaDragon.txt");
//	    	            for (size_t i = 0; i < cloud ->points.size (); ++i) {
//	    	            	ofsSigma << Sigma [i]<< ","<< std::endl ;
//	    	                    }

  	pcl::PLYWriter writePLY;
 //  writePLY.write ("/Path/TO/RatioSmallestEigen22.ply", *cloud,  false);
	 //   writePLY.write ("/Path/TO/CloudEigeJnetTwirl.ply", *cloud,  false);
	// writePLY.write ("/Path/TO/CloudEigeJnetDragon.ply", *cloud,  false);
  // writePLY.write ("/Path/TO/EigenTwoPlane90N10.ply", *cloud,  false);
  	 // writePLY.write ("/Path/TO/DragonRedWhite.ply", *cloud,  false);
  	// writePLY.write ("/Path/TO/IntersectionThreePlanes.ply", *cloud,  false);
  	// writePLY.write ("/Path/TO/BunnyNoise50.ply", *cloud,  false);
  	//writePLY.write ("/Path/TO/BunnyEdges.ply", *cloud,  false);
  	 writePLY.write ("/Path/TO/TetahedronMultiple.ply", *cloud,  false);

  pcl::visualization::CloudViewer viewer("Cloud Viewer");
  viewer.showCloud(cloud);
  while (!viewer.wasStopped ())
  {}

  return 0;
  }


#ifndef _GLOB_H_
#define _GLOB_H_

#include <sys/resource.h>

#include <exception>
#include <iostream>
#include <utility>
#include <cstdlib>
#include <fstream>
#include <memory>
#include <string>
#include <mutex>
#include <list>
#include <map>
#include <random>
#include <omp.h>
#include <iomanip>
#include <stdexcept>
#include <chrono>
#include <cmath>
#include <cassert>
#include <algorithm>

#include <json.hpp>
#include <progressBar.hpp>

#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>

#include <CGAL/Line_2.h>
#include <CGAL/Origin.h>
#include <CGAL/Polygon_2.h>
//#include <CGAL/Cartesian.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>  //new
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Delaunay_mesher_2.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/squared_distance_2.h>
#include <CGAL/Aff_transformation_2.h>
#include <CGAL/Delaunay_mesh_face_base_2.h>
#include <CGAL/Delaunay_mesh_size_criteria_2.h>
#include <CGAL/Triangulation_face_base_with_info_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>

#define CGAL_HAS_THREADS

#include <GeographicLib/Geodesic.hpp>
#include <GeographicLib/Constants.hpp>
#include <GeographicLib/GeoCoords.hpp>
#include <GeographicLib/LocalCartesian.hpp>

#include <osrm/match_parameters.hpp>
#include <osrm/nearest_parameters.hpp>
#include <osrm/route_parameters.hpp>
#include <osrm/table_parameters.hpp>
#include <osrm/trip_parameters.hpp>
#include <osrm/coordinate.hpp>
#include <osrm/engine_config.hpp>
#include <osrm/json_container.hpp>
#include <osrm/osrm.hpp>
#include <osrm/status.hpp>

struct FaceInfo2 {
	FaceInfo2() {}
	int nesting_level;
	bool in_domain()
	{
		return nesting_level%2 == 1;
	}
};

typedef CGAL::Exact_predicates_inexact_constructions_kernel      K;//new
//typedef CGAL::Simple_cartesian<double> K;

typedef CGAL::Aff_transformation_2<K> Transformation;

typedef CGAL::Line_2<K>     Line2D;
typedef CGAL::Point_2<K>    Point2D;
typedef CGAL::Triangle_2<K> Triangle2D;//new
typedef CGAL::Vector_2<K>   Vector2D;
typedef CGAL::Polygon_2<K>  Polygon2D;

typedef CGAL::Triangulation_vertex_base_2<K>                      Vb;
typedef CGAL::Triangulation_face_base_with_info_2<FaceInfo2,K>    Fbb;//new
//typedef CGAL::Delaunay_mesh_face_base_2<K>                        Fb;
typedef CGAL::Constrained_triangulation_face_base_2<K,Fbb>        Fb;//new
typedef CGAL::Triangulation_data_structure_2<Vb, Fb>              Tds;

typedef CGAL::Exact_predicates_tag                                Itag;//new
typedef CGAL::Constrained_Delaunay_triangulation_2<K, Tds, Itag>  CDT;//new
//typedef CGAL::Constrained_Delaunay_triangulation_2<K, Tds>        CDT;

typedef CGAL::Delaunay_mesh_size_criteria_2<CDT>                  Mesh_2_criteria;

using json=nlohmann::json;
using namespace GeographicLib;

extern bool        g_showProgressBar;
extern float       g_closeEnough;
extern float       g_randomWalkwayRadius;
extern float       g_attractionRadius;
extern uint32_t    g_epochInitSim;
extern uint32_t    g_currTimeSim;
extern std::string g_baseDir;
extern uint32_t    g_AgentsMem;
extern float       g_deltaT;

//Variables globales para medir tiempo
extern uint32_t g_timeExecMakeAgents;
extern uint32_t g_timeExecCal;
extern uint32_t g_timeExecSim;

extern std::vector<std::string> g_logZonesDensity;
extern std::vector<uint32_t>    g_logUsePhone;
extern std::vector<std::string> g_logVelocity;

//enum model_t {ShortestPath=0, FollowTheCrowd=1, RandomWalkway=2, WorkingDay, SNITCH=666};
enum model_t {Residents=0, Visitors_I=1, Visitors_II=2};

extern std::map<std::string, model_t> model_map;

#endif


#ifndef GP_HPP_INCLUDED
#define GP_HPP_INCLUDED
#include <vector>
#include <memory>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Cholesky>

namespace boat {
double normal_lnp(double x, double mean, double variance);
typedef enum { SQUARED_EXP, MATERN52 } kernel_t;

class GPParams{
  public:
  GPParams():mean_(0.0), amplitude_(1.0), default_noise_(0.1),
  kernel_(MATERN52) {}

  void amplitude(double amplitude) {
    amplitude_ = amplitude;
  }

  void stdev(double stdev) {
    amplitude_ = stdev * stdev;
  }

  void default_noise(double default_noise) {
    assert(default_noise >= 0.0);
    //default_noise_ = std::max(default_noise, 1e-4);
    default_noise_ = default_noise;
  }

  void mean(double mean) {
    mean_ = mean;
  }

  void linear_scales(const std::vector<double>& linear_scales){
    linear_scales_ = linear_scales;
    inv_linear_scales_ = Eigen::VectorXd::Map(linear_scales.data(), linear_scales.size())
                           .array()
                           .inverse();
  }

  void kernel(kernel_t kernel){
    kernel_ = kernel;
  }

  double stdev() const {
    return sqrt(amplitude_);
  }

  double amplitude() const {
    return amplitude_;
  }

  double mean() const {
    return mean_;
  }

  double default_noise() const {
    return default_noise_;
  }

  const Eigen::VectorXd& inv_linear_scales() const {
    return inv_linear_scales_;
  }

  const std::vector<double>& linear_scales() const {
    return linear_scales_;
  }

  kernel_t kernel() const {
    return kernel_;
  }

  private:
  double mean_;
  double amplitude_;
  double default_noise_;
  std::vector<double> linear_scales_;
  Eigen::VectorXd inv_linear_scales_;
  kernel_t kernel_;
};

struct GaussianDistrib {
  double mu_;
  double var_;
  double mu() {
    return mu_;
  }
  double var() {
    return var_;
  }
  double stdev() {
    return sqrt(var_);
  }
};

class GP {
  friend class TreedGPS;
  public:
  GP();
  GP(GPParams params);
  void set_params(GPParams params);
  int num_dims() const;
  double observe(const std::vector<double>& x_new, double y_new);
  double observe(const std::vector<double>& x_new, double y_new, double noise);

  double predict_mean(const std::vector<double>& x_new) const;
  GaussianDistrib predict_distrib(const std::vector<double>& x_new) const;
  void print() const;

   //private:
   double observe_i(const std::vector<double>& x_new,
                    double y_new, double noise_var);
   void compute_cholesky() const;
   int at_index(const Eigen::RowVectorXd& v) const;
   void remove_observation(int i);
   Eigen::MatrixXd covariance(const Eigen::MatrixXd& x1,
                              const Eigen::MatrixXd& x2) const;

  void compute_cholesky_from_scratch() const;

  // Parameters
  GPParams params_;

  // Processed data
  Eigen::MatrixXd observed_x_;
  Eigen::VectorXd observed_y_;
  Eigen::VectorXd noise_var_;

  mutable Eigen::MatrixXd covariance_cholesky_;
  mutable Eigen::VectorXd alpha_;
};

class TreedGPS {
  public:
  TreedGPS();
  TreedGPS(GPParams params, int observation_thresh = 64, int overlap = 3);
  TreedGPS(const TreedGPS& other);
  TreedGPS& operator=(const TreedGPS& other);
  void set_params(GPParams params);

  double observe(const std::vector<double>& x_new, double y_new);
  double observe(const std::vector<double>& x_new, double y_new, double noise);

  double predict_mean(const std::vector<double>& x_new) const;
  GaussianDistrib predict_distrib(const std::vector<double>& x_new) const;

  private:
  bool side(const std::vector<double>& x_new) const;

  //Lots of helpers for the splitting
  std::pair<std::vector<int>, std::vector<int>> compute_thresh_and_divide(double max_diff);
  std::pair<std::vector<int>, std::vector<int>> compute_thresh_props();
  void check_thresh();
  void copy_observation(bool s, int i);
  void compute_children_cholesky();

  int observation_thresh_;
  int overlap_;

  int thresh_dim_;
  double thresh_;
  double inv_ls_along_thresh_dim_;
  std::unique_ptr<GP> gp_;
  std::unique_ptr<TreedGPS> left_;
  std::unique_ptr<TreedGPS> right_;
};
}
#endif  // GP_HPP_INCLUDED


/**
 * Copyright (c) 2021 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "multipolymesh.hpp"

#include "math/math.hpp"

#include <boost/numeric/ublas/matrix.hpp>

namespace geometry
{
namespace ublas = boost::numeric::ublas;

FacePlaneCrs::FacePlaneCrs(const math::Point3& p1,
                           const math::Point3& p2,
                           const math::Point3& p3)
{
    // get base vectors
    math::Point3 n1 = math::normalize(p2 - p1);
    normal_ = math::normalize(math::crossProduct(p2 - p1, p3 - p1));
    math::Point3 n2 = math::normalize(math::crossProduct(normal_, n1));

    p2g_ = math::identity4();

    auto col1 = ublas::column(p2g_, 0);
    auto col2 = ublas::column(p2g_, 1);
    auto col3 = ublas::column(p2g_, 2);
    auto col4 = ublas::column(p2g_, 3);
    ublas::subrange(col1, 0, 3) = n1;
    ublas::subrange(col2, 0, 3) = n2;
    ublas::subrange(col3, 0, 3) = normal_;
    ublas::subrange(col4, 0, 3) = p1;

    g2p_ = math::matrixInvert(p2g_);
}

} // namespace geometry