#include <math.h>
#include <algorithm>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/bc_clustering.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/bind.hpp>
#include <iomanip>
#include <time.h>
#include <socialgraph.h>

void SocialGraph::setGraph(std::ifstream &fin){
	//read file
	facebook::Data data = facebook::readFile(fin, name);

	//create map for name and id
	map = facebook::createMap(data);

	//create graph
	g = facebook::createGraph(data, map);

	for(int i=0; i<num_vertices(g); i++){
		put(vertex_name, g, i, name[i]);
	}

	forces.resize(num_vertices(g));

	//create centrality graph
	cg = g;
}

void SocialGraph::setGraph2(std::ifstream &fin){
	facebook::Name t_name;
	facebook::Data data = facebook::readFile(fin, t_name);

	map2 = facebook::createMap(data);

	g2 = facebook::createGraph(data, map2);
}

void SocialGraph::setWeightedGraph(){
	WeightedGraph twg(num_vertices(g));

	property_map<WeightedGraph, edge_weight_t>::type weightmap = get(edge_weight, twg);
	EdgeIterator e, e_end;
	for(tie(e, e_end) = edges(cg); e != e_end; e++){
		WeightedEdgeDescriptor e1; bool inserted;
		tie(e1, inserted) = add_edge(source(*e, cg), target(*e, cg), twg);
		weightmap[e1] = 1;
	}

	wg = twg;
}

void SocialGraph::setNodes(){
	nodes.resize(num_vertices(g));
	for(int i=0; i<num_vertices(g); i++){
		nodes[i].x = (rand() % 200) -100;
		nodes[i].y = (rand() % 200) -100;
		nodes[i].dx = 0;
		nodes[i].dy = 0;
	}
}

void SocialGraph::setNode(int i, int x, int y){
	nodes[i].x = x;
	nodes[i].y = y;
}

Graph SocialGraph::getGraph(){
	return g;
}

Graph SocialGraph::getGraph2(){
	return g2;
}

WeightedGraph SocialGraph::getWeightedGraph(){
	return wg;
}

facebook::Name SocialGraph::getName(){
	return name;
}

Graph SocialGraph::getClusterGraph(){
	return cg;
}

Graph SocialGraph::getClusterGraph2(){
	return cg2;
}

Nodes SocialGraph::getNodes(){
	return nodes;
}

void SocialGraph::repF(int i){
	for(int j=i+1; j<nodes.size(); j++){
		float nx = nodes[i].x - nodes[j].x;
		float ny = nodes[i].y - nodes[j].y;
		float r = sqrt(pow(nx, 2) + pow(ny, 2));
		if(r > 0){
			float f = 900 / (r * r);
			forces[i].x += f * nx / r;
			forces[i].y += f * ny / r;
			forces[j].x -= f * nx / r;
			forces[j].y -= f * ny / r;
		}
	}
}

void SocialGraph::repulsiveForce(){
	for(int i=0; i<nodes.size(); i++){
		repF(i);
	}
}

void SocialGraph::attractiveForce(Graph temp_g){
	EdgeIterator e, e_end;
	for(tie(e, e_end) = edges(temp_g); e != e_end; e++){
		int s = source(*e, temp_g);
		int t = target(*e, temp_g);
		float nx = nodes[s].x - nodes[t].x;
		float ny = nodes[s].y - nodes[t].y;
		float r = sqrt(pow(nx, 2) + pow(ny, 2));
		if(r > 0){
			float f = -(r * r) / 30;
			forces[s].x += f * nx / r;
			forces[s].y += f * ny / r;
			forces[t].x -= f * nx / r;
			forces[t].y -= f * ny / r;

			/*
			forces[s].x += f * nx / (r * out_degree(s,temp_g));
			forces[s].y += f * ny / (r * out_degree(s,temp_g));
			forces[t].x -= f * nx / (r * out_degree(t,temp_g));
			forces[t].y -= f * ny / (r * out_degree(t,temp_g));
			*/
		}
	}
}

float SocialGraph::limitForce(){
	float dt = .2;
	float damping = 0.01;
	float kinetic = 0.0;

	for(int i=0; i<nodes.size(); i++){
		nodes[i].dx = (nodes[i].dx + dt * forces[i].x) * damping;
		nodes[i].dy = (nodes[i].dy + dt * forces[i].y) * damping;

		forces[i].x = 0;
		forces[i].y = 0;

		if(nodes[i].dx > 1) nodes[i].dx = 1;
		if(nodes[i].dx < -1) nodes[i].dx = -1;
		if(nodes[i].dy > 1) nodes[i].dy = 1;
		if(nodes[i].dy < -1) nodes[i].dy = -1;

/*
		if(nodes[i].dx > 10) nodes[i].dx = 10;
		if(nodes[i].dx < -10) nodes[i].dx = -10;
		if(nodes[i].dy > 10) nodes[i].dy = 10;
		if(nodes[i].dy < -10) nodes[i].dy = -10;
*/

		nodes[i].x = nodes[i].x + dt * nodes[i].dx;
		nodes[i].y = nodes[i].y + dt * nodes[i].dy;

		kinetic += sqrt(pow(nodes[i].dx, 2) + pow(nodes[i].dy, 2));
	}

	return kinetic;
}

float SocialGraph::springModel(Graph temp_g){
	float kinetic = 0.0;

	repulsiveForce();
	attractiveForce(temp_g);

	kinetic = limitForce();

	return kinetic;
}

void changeColor(float &r, float &g, float &b){
	if(r < 1.0){
		r += .5;
	}
	else{
		r = 0.0;
		if(g < 1.0){
			g += .5;
		}
		else{
			g = 0.0;
			if(b < .5){
				b += .5;
			}
			else
				b = 0.0;
		}
	}
}

void SocialGraph::clusterColor(Graph tempg){
	float r = 0.0;
	float g = 0.0;
	float b = 0.0;

	wg = ::createWeightedGraph(tempg);

	for(int i=0; i<num_vertices(wg); i++){
		for(int j=i+1; j<num_vertices(wg); j++){
			if(edge(i,j,wg).second == 0 && edge(j,i,wg).second != 0)
				remove_edge(j,i,wg);
			else if(edge(i,j,wg).second != 0 && edge(j,i,wg).second == 0)
				remove_edge(i,j,wg);
		}
	}

	std::vector<int> visited(num_vertices(wg), 0);
	int cluster = 0;
	for(int i=0; i<num_vertices(wg); i++){
		if(visited[i] == 0){
			cluster++;

			std::vector<WeightedVertexDescriptor> p(num_vertices(wg));
			std::vector<int> d(num_vertices(wg));
			dijkstra_shortest_paths(wg, i, predecessor_map(&p[0]).distance_map(&d[0]));

			changeColor(r,g,b);
			//std::cout << r << " " << g << " " << b << std::endl;
			for(int j=0; j<num_vertices(wg); j++){
				if(d[j] != INT_MAX){
					visited[j] = 1;
					nodes[j].r = r;
					nodes[j].g = g;
					nodes[j].b = b;
					nodes[j].cluster = cluster;
				}
			}
		}
	}
	std::cout << "number of clusters: " << cluster << std::endl;
}

bool ContainsNode(std::vector<int> visited, int node){
	for(int i=0; i<visited.size(); i++){
		if(visited[i] == node)
			return true;
	}
	return false;
}

void SocialGraph::DepthFirst(std::vector<int> &visited, int max, std::vector<int> &hits, int &max_short, int t){
	int back = visited.back();
	graph_traits<WeightedGraph>::out_edge_iterator e, e_end;

	for(tie(e, e_end) = out_edges(back, wg); e != e_end; e++){
		int node = target(*e, wg);

		if(ContainsNode(visited, node)) continue;

		if(node == t){
			visited.push_back(node);
			for(int i=1; i<visited.size()-1; i++){
				hits[visited[i]]++;
			}
			max_short++;

			int n = (int) visited.size() -1;
			visited.erase(visited.begin() + n);

			break;
		}

		visited.push_back(node);

		if(visited.size() <= max)
			DepthFirst(visited, max, hits, max_short, t);

		int n = (int)visited.size() - 1;
		visited.erase(visited.begin() + n);
	}
}

void SocialGraph::ShortestPaths(int s, std::vector<double> &centrality){
	std::vector<WeightedVertexDescriptor> p(num_vertices(wg));
	std::vector<int> d(num_vertices(wg));
	dijkstra_shortest_paths(wg, s, predecessor_map(&p[0]).distance_map(&d[0]));

	for(int i=0; i<num_vertices(wg); i++){
		//std::cout << i << " " << d[i] << std::endl;
		if(d[i] > 1 && d[i] != INT_MAX){
			std::vector<int> hits(num_vertices(wg), 0);
			int max_short = 0;

			std::vector<int> visited;
			visited.push_back(s);
			DepthFirst(visited, d[i], hits, max_short, i);

			for(int j=0; j<num_vertices(wg); j++){
				if(max_short > 0)
					centrality[j] += (double)hits[j]/(double)max_short;
			}
		}
	}
}

void SocialGraph::setMatrix(){
	Matrix matrix(num_vertices(g));

	for(int i=0; i<num_vertices(g); i++){
		std::vector<int> list(num_vertices(g));
		for(int j=0; j<num_vertices(g); j++){
			if(edge(i, j, g).second == 1)
				list[j] = 1;
			else
				list[j] = 0;
		}
		matrix[i] = list;
	}

	matrixlist.push_back(matrix);
}

void SocialGraph::addMatrix(){
	Matrix matrix1 = matrixlist[0];
	Matrix matrix2 = matrixlist[matrixlist.size() -1];
	Matrix temp(num_vertices(g));

	for(int i=0; i<num_vertices(g); i++){
		std::vector<int> list(num_vertices(g), 0);
		for(int j=0; j<num_vertices(g); j++){
			for(int k=0; k<num_vertices(g); k++){
				list[j] += matrix1[i][k] * matrix2[k][j];
			}
		}
		temp[i] = list;
	}

	matrixlist.push_back(temp);
}

void SocialGraph::printMatrix(){
	Matrix matrix = matrixlist[matrixlist.size() -1];

	for(int i=0; i<num_vertices(g); i++){
		for(int j=0; j<num_vertices(g); j++)
			std::cout << matrix[i][j] << " ";
		std::cout << std::endl;
	}
}

std::vector<double> SocialGraph::Dependency(int s){
	std::vector<WeightedVertexDescriptor> p(num_vertices(wg));
	std::vector<int> d(num_vertices(wg));
	dijkstra_shortest_paths(wg, s, predecessor_map(&p[0]).distance_map(&d[0]));

	std::vector<double> centrality(num_vertices(g), 0.0);

	for(int i=0; i<num_vertices(g); i++){
		//if(d[i] > 1 && d[i] != INT_MAX){
		if(d[i] > 1 && d[i] < 4){
			while(d[i] > matrixlist.size())
				addMatrix();

			//number of shortest paths
			int num = matrixlist[d[i]-1][s][i];

			std::vector<double> temp(num_vertices(g), 0.0);
			for(int j=1; j<d[i]; j++){
				for(int k=0; k<num_vertices(g); k++)
					temp[k] += matrixlist[j-1][s][k] * matrixlist[d[i]-1-j][k][i];
			}
			for(int k=0; k<num_vertices(g); k++)
				centrality[k] += (double)temp[k] / (double)num;
		}
	}

	return centrality;
}

MatrixScore SocialGraph::AllDependency(){
	MatrixScore all_centrality;

	for(int i=0; i<num_vertices(g); i++){
		std::vector<double> centrality = Dependency(i);
		all_centrality.push_back(centrality);
	}

	for(int i=0; i<num_vertices(g); i++){
		for(int j=0; j<num_vertices(g); j++){
			if(edge(i, j, g).second == 1)
				all_centrality[i][j] = 0;
		}
	}

	return all_centrality;
}

int SocialGraph::convertNode(int s){
	std::string source = map.left.find(s)->second;
	int t = map2.right.find(source)->second;
	return t;
}

Matrix SocialGraph::getDistance(){
	return distance;
}

void SocialGraph::setAvgDegree(){
	float avg=0.0;

	for(int i=0; i<num_vertices(g); i++){
		avg += out_degree(i, g);
	}
	avg = avg / num_vertices(g);

	avgDegree = avg;
}

float SocialGraph::getAvgDegree(){
	return avgDegree;
}

Edges SocialGraph::newEdgesList(){
	Edges edges;
	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			if(edge(i, j, g).second == 0){
				std::string source = map.left.find(i)->second;
				std::string target = map.left.find(j)->second;
				int s = map2.right.find(source)->second;
				int t = map2.right.find(target)->second;
				if(s > 0 && t > 0 && s < num_vertices(g2) + 1 && t < num_vertices(g2)){
					if(edge(s, t, g2).second == 1){
						Edge e(i, j);
						edges.push_back(e);
					}
				}
			}
		}
	}
	return edges;
}

void SocialGraph::setNewEdges(){
	Edges edges = newEdgesList();

	for(int i=0; i<edges.size(); i++){
		int s = edges[i].first;
		int t = edges[i].second;

		Edge e; e.first = s; e.second = t;
		if(distance[s][t] < num_vertices(g) && distance[s][t] > 0){
			while(newEdges.size() <= distance[s][t]){
				Edges es;
				newEdges.push_back(es);
			}
			newEdges[distance[s][t]].push_back(e);
		}
		else
			newEdges[0].push_back(e);
	}

	int count = 0;
	float allavg=0.0;
	for(int i=0; i<newEdges.size(); i++){
		count += newEdges[i].size();
		for(int j=0; j<newEdges[i].size(); j++){
			int s = newEdges[i][j].first;
			int t = newEdges[i][j].second;
			allavg += out_degree(s, g) + out_degree(t, g);
		}
	}
	std::cout << "new edges(all_new) = " << count << std::endl;
	numNewEdges = count;
	allavg = allavg/(count*2);
	std::cout << "avg degree(all_new) = " << allavg << std::endl;
	std::cout << std::endl;

	for(int i=0; i<newEdges.size(); i++){
		std::cout << "new edges (" << i << ") = " << newEdges[i].size() << std::endl;
		float avgnew = 0.0;
		for(int j=0; j<newEdges[i].size(); j++){
			int s = newEdges[i][j].first;
			int t = newEdges[i][j].second;
			avgnew += out_degree(s, g) + out_degree(t, g);
		}
		avgnew = avgnew/(newEdges[i].size()*2);
		std::cout << "avg degree (" << i << ") = " << avgnew << std::endl;
	}
	std::cout << std::endl;

	count = 0;
	int count2 = 0;
	float avg=0.0;
	float avg2=0.0;
	for(int i=0; i<newEdges[3].size(); i++){
		int flag = 0;
		int s = newEdges[3][i].first;
		int t = newEdges[3][i].second;
		for(int j=0; j<newEdges[2].size(); j++){
			int s2 = newEdges[2][j].first;
			int t2 = newEdges[2][j].second;
			if(s2 == s){
				if(distance[t2][t] == 1){
					flag = 1;
				}
			}
			else if(t2 == s){
				if(distance[s2][t] == 1){
					flag = 1;
				}
			}
			else if(s2 == t){
				if(distance[t2][s] == 1){
					flag = 1;
				}
			}
			else if(t2 == t){
				if(distance[s2][s] == 1){
					flag = 1;
				}
			}
			if(flag == 1){
				count++;
				avg += out_degree(s,g) + out_degree(t, g);
				break;
			}
		}
		if(flag == 0){
			count2++;
			avg2 += out_degree(s, g) + out_degree(t, g);
		}
	}
	std::cout << "new edges(3)<-(2) = " << count << std::endl;
	avg = avg / (count * 2);
	std::cout << "avg degree of (3)<-(2) = " << avg << std::endl;

	std::cout << "new edges(3)!(2) = " << count2 << std::endl;
	avg2 = avg2 / (count2 * 2);
	std::cout << "avg degree of (3)!(2) = " << avg2 << std::endl;
}

std::vector<Edges> SocialGraph::getNewEdges(){
	return newEdges;
}

//new functions
void SocialGraph::initialize(int argc, char *argv[]){
	std::ifstream fin;
	fin.open(argv[1]);
	facebook::Data data = facebook::readFile(fin, name);
	map = facebook::createMap(data);
	g = facebook::createGraph(data, map);
	for(int i=0; i<num_vertices(g); i++){
		put(vertex_name, g, i, name[i]);
	}
	forces.resize(num_vertices(g));
	fin.close();

	distance = ::createAllPairsShortestPaths(::createWeightedGraph(g));
	setAvgDegree();

	//initialize graph for t+1
	fin.open(argv[2]);
	Graph temp_g = ::createGraph(fin);
	g2 = g;
	fin.close();

	new_edges = ::newEdgeList(g, temp_g);
	new_matrix = ::newList2Matrix(g, new_edges);
	countd2=0;
	for(int i=0; i<new_edges.size(); i++){
		if(distance[new_edges[i].first][new_edges[i].second] < num_vertices(g)){
			if(distance[new_edges[i].first][new_edges[i].second] == 2){
				countd2++;
			}
			add_edge(new_edges[i].first, new_edges[i].second, g2);
			add_edge(new_edges[i].second, new_edges[i].first, g2);
		}
	}

	distance2 = ::createAllPairsShortestPaths(::createWeightedGraph(g2));

	setNodes();
	setAvgDegree();

	std::cout << "number of vertices: " << num_vertices(g) << std::endl;
	std::cout << "number of edges: " << num_edges(g)/2 << std::endl;
	std::cout << "number of new edges: " << new_edges.size() << std::endl;
	std::cout << "number of new edges at disance 2: " << countd2 << std::endl;

	#pragma omp parallel for
	for(int i=0; i<2; i++){
		if(i == 0)
			cg = ::ClusterEdgeCentrality(g, num_vertices(g));
		if(i == 1)
			cg2 = ::ClusterEdgeCentrality(g2, num_vertices(g2));
	}

	sg = g;
}

void SocialGraph::test(){
	Scores q;

	probabilityAA = ::AdamicAdar(g);
	q = ::SortScores(probabilityAA);

	//use this to count how many were the same as
	//betweenness centrality based method
	Edges daa;
	Edges dbc;

	int a=0;
	for(int i=0; i<new_edges.size(); i++){
	//for(int i=0; i<countd2; i++){
		int s = q[i].second.first;
		int t = q[i].second.second;
		if(edge(s, t, g2).second == 1){
			a++;
			daa.push_back(Edge(s,t));
		}
	}
	std::cout << "adamic-adar:" << a << std::endl;

	std::cout << "new edges of distance > 2: " << a << std::endl;
	std::cout << "average node degree: " << avgDegree << std::endl;

	std::vector<double> vbc(num_vertices(g), 0.0);
	std::vector<double> ebc(num_edges(g), 0.0);
	::VertexBetweennessCentrality(g, vbc, ebc);

	float beta = 0;
	probability = ::BCBasedLP(g, distance, vbc, beta);
	Scores sbc = ::SortScores(probability);
	a=0;
	int b=0;
	for(int i=0; i<new_edges.size(); i++){
	//for(int i=0; i<countd2; i++){
		int s = sbc[i].second.first;
		int t = sbc[i].second.second;
		if(edge(s, t, g2).second == 1 && (probability[s][t] > 0 || probability[t][s] > 0)){
			detected_edges.push_back(Edge(s,t));
			a++;
			dbc.push_back(Edge(s,t));
			if(distance[s][t] > 2)
				b++;
		}
	}
	std::cout << a << ":" << b << std::endl;

	//check how many were the same
	int overlap=0;
	for(int i=0; i<dbc.size(); i++){
		int s = dbc[i].first;
		int t = dbc[i].second;
		for(int j=0; j<daa.size(); j++){
			int s1 = daa[j].first;
			int t1 = daa[j].second;
			if((s1 == s && t1 == t) || (s1 == t && t1 == s))
				overlap++;
		}
	}
	std::cout << "overlap: " << overlap << "/" << dbc.size() << " = " << (float)overlap/dbc.size() << std::endl;

	vertex_bc = vbc;

	//calculate number of paths at what distance
	std::vector<int> count;
	for(int x=0; x<num_vertices(g); x++){
		for(int y=x+1; y<num_vertices(g); y++){
			if(distance[x][y] < num_vertices(g)){
				while(count.size() <= distance[x][y]){
					count.push_back(0);
				}
				count[distance[x][y]]++;
			}
		}
	}
	for(int i=0; i<count.size(); i++){
		std::cout << i << ":" << count[i] << std::endl;
	}

	::NormalizePr(probability, distance);
	::NormalizePr(probabilityAA, distance);

	edge_bc = ::EdgeCentrality(g, ebc);

	//finds probability of correct new edges detected
	//based on how many new edges detected of one individual
	std::vector<int> nEdges(num_vertices(g),0);
	for(int i=0; i<new_edges.size(); i++){
		int s = new_edges[i].first;
		int t = new_edges[i].second;
		if(distance[s][t] == 2){
			nEdges[s]++;
			nEdges[t]++;
		}
	}
	int necount=0; int necorrect=0;
	for(int i=0; i<num_vertices(g); i++){
		necount += nEdges[i];
		if(nEdges[i] > 0){
			Scores nscores = SortIndividualScores(probability, i);

			for(int j=0; j<nEdges[i]; j++){
				int s = nscores[j].second.first;
				int t = nscores[j].second.second;
				if(edge(s,t,g2).second == 1)
					necorrect++;
			}
		}
	}
	std::cout << necorrect << "/" << necount << std::endl;

	//change size of vertex and edge betweenness centrality
	//for visualization
	vbc = sortDouble(vertex_bc);
	double minbc = vbc[0.05*vbc.size()];
	double maxbc = vbc[0.95*vbc.size()];
	double diff = maxbc - minbc;
	for(int i=0; i<num_vertices(g); i++){
		if(vertex_bc[i] < minbc)
			vertex_bc[i] = 10;
		else if(vertex_bc[i] > maxbc)
			vertex_bc[i] = 25;
		else
			vertex_bc[i] = 10 + 15 * (vertex_bc[i] - minbc)/diff;
	}

	ebc = sortDouble(ebc);
	minbc = ebc[0];
	maxbc = ebc[ebc.size()*.5];
	diff = maxbc - minbc;

	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			if(edge(i,j,g).second == 1){
			if(edge_bc[i][j] < minbc){
				edge_bc[i][j] = 5;
				edge_bc[j][i] = 5;
			}
			else if(edge_bc[i][j] > maxbc){
				edge_bc[i][j] = 1;
				edge_bc[j][i] = 1;
			}
			else{
				edge_bc[i][j] = 5.0 - 4.0 * (edge_bc[i][j] - minbc)/diff;
				edge_bc[j][i] = edge_bc[i][j];
			}
			}
		}
	}

	//copy of above for future graph
	std::vector<double> vbc2(num_vertices(g2), 0.0);
	std::vector<double> ebc2(num_edges(g2), 0.0);
	::VertexBetweennessCentrality(g2, vbc2, ebc2);
	vertex_bc2 = vbc2;
	edge_bc2 = ::EdgeCentrality(g2, ebc2);

	vbc2 = sortDouble(vertex_bc2);
	minbc = vbc2[0.05*vbc2.size()];
	maxbc = vbc2[0.95*vbc2.size()];
	diff = maxbc - minbc;
	for(int i=0; i<num_vertices(g); i++){
		if(vertex_bc2[i] < minbc)
			vertex_bc2[i] = 10;
		else if(vertex_bc2[i] > maxbc)
			vertex_bc2[i] = 25;
		else
			vertex_bc2[i] = 10 + 15 * (vertex_bc2[i] - minbc)/diff;
	}

	ebc2 = sortDouble(ebc2);
	minbc = ebc2[0];
	maxbc = ebc2[ebc2.size()*.5];
	diff = maxbc - minbc;

	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			if(edge(i,j,g2).second == 1){
			if(edge_bc2[i][j] < minbc){
				edge_bc2[i][j] = 5;
				edge_bc2[j][i] = 5;
			}
			else if(edge_bc2[i][j] > maxbc){
				edge_bc2[i][j] = 1;
				edge_bc2[j][i] = 1;
			}
			else{
				edge_bc2[i][j] = 5.0 - 4.0 * (edge_bc2[i][j] - minbc)/diff;
				edge_bc2[j][i] = edge_bc2[i][j];
			}
			}
		}
	}

}

void SocialGraph::showDetected(){
	static int k=0;

	for(int i=0; i<num_vertices(g); i++){
		nodes[i].r = 0; nodes[i].g = 0; nodes[i].b = 0;
	}
	vizN.clear();

	k++;
	if(k == detected_edges.size())
		k = 0;

	int s = detected_edges[k].first;
	int t = detected_edges[k].second;
	nodes[s].b = 1.0; nodes[t].b = 1.0;

	vizN.push_back(s); vizN.push_back(t);

	for(int i=0; i<num_vertices(g); i++){
		if(distance[s][i] + distance[i][t] == distance[s][t]
		&& s != i && t != i){
			nodes[i].g = 1.0;
			vizN.push_back(i);
		}
	}

	std::cout << "changed " << k << ", normalized bc score = " << probability[s][t] << std::endl;
}

Graph SocialGraph::getSG(){
	return sg;
}

Graph SocialGraph::getSG2(){
	return sg2;
}

void SocialGraph::selectSG(int s){
	if(s != -1){

	Graph tempg(num_vertices(g));

	std::vector<int> nhbr;
	nhbr.push_back(s);
	for(int t=0; t<num_vertices(g); t++){
		if(distance[s][t] == 2 || distance[s][t] == 1){
			nhbr.push_back(t);

			//make nodes closer to selected node
			//for visualization
			/*
			nodes[t].x = nodes[s].x + (20.0*rand()/RAND_MAX - 10.0);
			nodes[t].y = nodes[s].y + (20.0*rand()/RAND_MAX - 10.0);
			*/
		}
	}

	for(int x=0; x<nhbr.size()-1; x++){
		for(int y=x+1; y<nhbr.size(); y++){
			int i = nhbr[x];
			int j = nhbr[y];
			if(edge(i,j,g).second == 1 && edge(i,j,tempg).second != 1){
				add_edge(i,j,tempg);
				add_edge(j,i,tempg);
			}
		}
	}

	sg = tempg;

	/*
	for(int i=0; i<num_vertices(g); i++){
		if(i == s){
			nodes[i].r = 1; nodes[i].g = 0; nodes[i].b = 0;
		}
		else{
			nodes[i].r = 0; nodes[i].g = 0; nodes[i].b = 0;
		}
	}
	*/

	}
}

void SocialGraph::selectFutureSG(int s){
	sg2 = sg;
	sgEdges.clear();

	int count=0;
	for(int i=0; i<num_vertices(g); i++){
		if(distance[s][i] == 2 && edge(s,i,g2).second == 1){
			add_edge(s,i,sg2);
			add_edge(i,s,sg2);
			count++;
			sgEdges.push_back(Edge(s,i));
		}
	}
	std::cout << count << " new edges" << std::endl;
}

float SocialGraph::SpringSG(int s, char flag){
	float kinetic = 0.0;

	if(s !=  -1){

	repulsiveForce();
	if(flag == 0)
		attractiveForce(sg);
	else if(flag == 1 || flag == 2){
		attractiveForce(sg);
		for(int i=0; i<num_vertices(g); i++){
			if(distance[s][i] == 2){
				float nx = nodes[s].x - nodes[i].x;
				float ny = nodes[s].y - nodes[i].y;
				float r = sqrt(pow(nx,2) + pow(ny,2));
				nx = nx / r;
				ny = ny / r;
				if(r>0 &&
				((flag==1)?(probability[s][i]):(probabilityAA[s][i])) >= 1){
				//if(r > 0 && (probability[s][i] >= 1)){
					float f = -2 * (r*r)/30;
					forces[s].x += f * nx;
					forces[s].y += f * ny;
					forces[i].x -= f * nx;
					forces[i].y -= f * ny;
				}
			}
		}
	}
	else if(flag == 3)
		attractiveForce(sg2);

	float dt = 1.0;
	float damping = 0.01;

	for(int i=0; i<nodes.size(); i++){
		nodes[i].dx = (nodes[i].dx + dt * forces[i].x) * damping;
		nodes[i].dy = (nodes[i].dy + dt * forces[i].y) * damping;

		forces[i].x = 0;
		forces[i].y = 0;

		if(nodes[i].dx > 1) nodes[i].dx = 1;
		if(nodes[i].dx < -1) nodes[i].dx = -1;
		if(nodes[i].dy > 1) nodes[i].dy = 1;
		if(nodes[i].dy < -1) nodes[i].dy = -1;

		if(i != s){
			nodes[i].x = nodes[i].x + dt * nodes[i].dx;
			nodes[i].y = nodes[i].y + dt * nodes[i].dy;
		}

		kinetic += sqrt(pow(nodes[i].dx, 2) + pow(nodes[i].dy, 2));
	}

	}

	return kinetic;

}

Edges SocialGraph::getSGEdges(){
	return sgEdges;
}

std::vector<double> SocialGraph::getVBC(){
	return vertex_bc;
}

std::vector<double> SocialGraph::getVBC2(){
	return vertex_bc2;
}

MatrixScore SocialGraph::getEBC(){
	return edge_bc;
}

MatrixScore SocialGraph::getEBC2(){
	return edge_bc2;
}

MatrixScore SocialGraph::getProbability(){
	return probability;
}

MatrixScore SocialGraph::getProbabilityAA(){
	return probabilityAA;
}


/*
 * lstmlayer.cpp
 *
 * Feed-forward:
 *    x(t) -- inputs
 *    h(t-1) -- hidden state, aka outputs on prev timestep
 *    c(t) -- cell state
 *
 *    Phase 1: I/O gates
 *      zz_a(t) = ww_xa*x(t) + ww_ha * h(t-1) + bb_a  -- input
 *      zz_i(t) = ww_xi*x(t) + ww_hi * h(t-1) + bb_i  -- input gate
 *      zz_f(t) = ww_xf*x(t) + ww_hf * h(t-1) + bb_g  -- forget gate
 *      zz_o(t) = ww_xo*x(t) + ww_ho * h(t-1) + bb_o  -- output gate
 *
 *      a(t) = activ1 (zz_a(t))
 *      i(t) = activ2 (zz_i(t))
 *      f(t) = activ2 (zz_f(t))
 *      o(t) = activ2 (zz_o(t))
 *
 *    Phase 2: State
 *      c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
 *
 *    Phase 3: Output
 *      h(t) = elem_prod(o(t), activ1(c(t))
 *
 * Back propagation:
 *
 */
#include <boost/assert.hpp>

#include "core/utils.h"
#include "core/random.h"
#include "core/functions.h"
#include "lstmlayer.h"

using namespace std;
using namespace boost;
using namespace yann;

namespace yann {

// prefixes for read/write
static const char g_ltsm_names_suffix[LstmLayer::Gate_Max + 1] = "aifo";

////////////////////////////////////////////////////////////////////////////////////////////////
//
// LstmLayer_Context implementation
//
class LstmLayer_Context :
    public Layer::Context
{
  typedef Layer::Context Base;

  friend class LstmLayer;

public:
  LstmLayer_Context(
      const MatrixSize & output_size,
      const MatrixSize & max_batch_size) :
    Base(output_size, max_batch_size),
    _pos(0)
  {
    init();
  }

  LstmLayer_Context(
      const RefVectorBatch & output) :
    Base(output),
    _pos(0)
  {
    init();
  }

  // Layer::Context overwrites
  virtual void reset_state()
  {
    Base::reset_state();

    _pos = 0;
  }

private:
  void init()
  {
    for(auto & zz_gate: _zz_gate) {
      zz_gate.resize(get_batch_size(), get_output_size());
    }
    for(auto & gate: _gate) {
      gate.resize(get_batch_size(), get_output_size());
    }

    _cc.resize(get_batch_size(), get_output_size());
    _activ_cc.resize(get_batch_size(), get_output_size());

    _hh.resize(get_batch_size(), get_output_size());
  }

protected:
  MatrixSize  _pos;
  VectorBatch _zz_gate[LstmLayer::Gate_Max]; // pre-activation function results for each gate
  VectorBatch _gate[LstmLayer::Gate_Max];    // post-activation function results for each gate
  VectorBatch _cc;                      // cell state (save it for each step)
  VectorBatch _activ_cc;                // activ1(c(t))
  VectorBatch _hh;                      // hidden state aka output (save it for each step)
}; // class LstmLayer_Context

////////////////////////////////////////////////////////////////////////////////////////////////
//
// LstmLayer_TrainingContext implementation
//
class LstmLayer_TrainingContext :
    public LstmLayer_Context
{
  typedef LstmLayer_Context Base;
  friend class LstmLayer;

public:
  LstmLayer_TrainingContext(
      const MatrixSize & input_size,
      const MatrixSize & output_size,
      const MatrixSize & batch_size,
      const unique_ptr<Layer::Updater> & updater) :
    Base(output_size, batch_size)
  {
    init(updater, input_size);
  }

  LstmLayer_TrainingContext(
      const MatrixSize & input_size,
      const RefVectorBatch & output,
      const unique_ptr<Layer::Updater> & updater) :
    Base(output)
  {
    init(updater, input_size);
  }

  // Layer::Context overwrites
  virtual void start_epoch()
  {
    Base::start_epoch();

    for(auto & ww_x_updater: _ww_x_updater) {
      YANN_SLOW_CHECK(ww_x_updater);
      ww_x_updater->start_epoch();
    }
    for(auto & ww_h_updater: _ww_h_updater) {
      YANN_SLOW_CHECK(ww_h_updater);
      ww_h_updater->start_epoch();
    }
    for(auto & bb_updater: _bb_updater) {
      YANN_SLOW_CHECK(bb_updater);
      bb_updater->start_epoch();
    }
  }

  virtual void reset_state()
  {
    Base::reset_state();

    for(auto & delta_ww_x: _delta_ww_x) {
      delta_ww_x.setZero();
    }
    for(auto & delta_ww_h: _delta_ww_h) {
      delta_ww_h.setZero();
    }
    for(auto & delta_bb: _delta_bb) {
      delta_bb.setZero();
    }

    for(auto & ww_x_updater: _ww_x_updater) {
      YANN_SLOW_CHECK(ww_x_updater);
      ww_x_updater->reset();
    }
    for(auto & ww_h_updater: _ww_h_updater) {
      YANN_SLOW_CHECK(ww_h_updater);
      ww_h_updater->reset();
    }
    for(auto & bb_updater: _bb_updater) {
      YANN_SLOW_CHECK(bb_updater);
      bb_updater->reset();
    }

    _gradient_hh.setZero();
    _gradient_cc.setZero();
  }

private:
  void init(
      const unique_ptr<Layer::Updater> & updater,
      const MatrixSize & input_size)
  {
    YANN_CHECK_GT(updater, 0);
    YANN_CHECK_GT(input_size, 0);

    // deltas
    for(auto & delta_ww_x: _delta_ww_x) {
      delta_ww_x.resize(input_size, get_output_size());
    }
    for(auto & delta_ww_h: _delta_ww_h) {
      delta_ww_h.resize(get_output_size(), get_output_size());
    }
    for(auto & delta_bb: _delta_bb) {
      delta_bb.resize(get_output_size());
    }

    // updaters
    for(auto & ww_x_updater: _ww_x_updater) {
      ww_x_updater = updater->copy();
      YANN_SLOW_CHECK(ww_x_updater);
      ww_x_updater->init(input_size, get_output_size());
    }
    for(auto & ww_h_updater: _ww_h_updater) {
      ww_h_updater = updater->copy();
      YANN_SLOW_CHECK(ww_h_updater);
      ww_h_updater->init(get_output_size(), get_output_size());
    }
    for(auto & bb_updater: _bb_updater) {
      bb_updater = updater->copy();
      YANN_SLOW_CHECK(bb_updater);
      bb_updater->init(1, get_output_size()); // RowMajor
    }

    // gate radients
    _gradient_hh.resize(get_output_size());
    _gradient_cc.resize(get_output_size());
    _gradient_gate.resize(get_output_size());
    _gradient_zz_gate.resize(get_output_size());
    _tmp_derivative.resize(get_output_size());
  }

private:
  Matrix _delta_ww_x[LstmLayer::Gate_Max];
  Matrix _delta_ww_h[LstmLayer::Gate_Max];
  Vector _delta_bb[LstmLayer::Gate_Max];

  unique_ptr<Layer::Updater> _ww_x_updater[LstmLayer::Gate_Max];
  unique_ptr<Layer::Updater> _ww_h_updater[LstmLayer::Gate_Max];
  unique_ptr<Layer::Updater> _bb_updater[LstmLayer::Gate_Max];

  Vector _gradient_hh;
  Vector _gradient_cc;
  Vector _gradient_gate;
  Vector _gradient_zz_gate;
  Vector _tmp_derivative;
}; // class LstmLayer_TrainingContext

}; // namespace yann

////////////////////////////////////////////////////////////////////////////////////////////////
//
// yann::LstmLayer implementation
//
yann::LstmLayer::LstmLayer(
    const MatrixSize & input_size,
    const MatrixSize & output_size) :
    _gate_activation_function(new SigmoidFunction()),
    _io_activation_function(new TanhFunction())
{
  YANN_CHECK_GT(input_size, 0);

  for(auto & ww_x: _ww_x) {
    ww_x.resize(input_size, output_size);
  }
  for(auto & ww_h: _ww_h) {
    ww_h.resize(output_size, output_size);
  }
  for(auto & bb: _bb) {
    bb.resize(output_size);
  }
}

yann::LstmLayer::~LstmLayer()
{
}

void yann::LstmLayer::set_values(
    const Matrix (& ww_x)[Gate_Max],
    const Matrix (& ww_h)[Gate_Max],
    const Vector (& bb)[Gate_Max])
{
  for(auto ii = 0; ii < Gate_Max; ++ii) {
    YANN_CHECK(is_same_size(ww_x[ii], _ww_x[ii]));
    YANN_CHECK(is_same_size(ww_h[ii], _ww_h[ii]));
    YANN_CHECK(is_same_size(bb[ii], _bb[ii]));

    _ww_x[ii] = ww_x[ii];
    _ww_h[ii] = ww_h[ii];
    _bb[ii] = bb[ii];
  }
}

void yann::LstmLayer::set_activation_functions(
    const std::unique_ptr<ActivationFunction> & gate_activation_function,
    const std::unique_ptr<ActivationFunction> & io_activation_function)
{
  YANN_CHECK(gate_activation_function);
  YANN_CHECK(io_activation_function);
  _gate_activation_function = gate_activation_function->copy();
  _io_activation_function = io_activation_function->copy();
}

// Layer overwrites
bool yann::LstmLayer::is_valid() const
{
  if(!Base::is_valid()) {
    return false;
  }
  if(!_io_activation_function || !_gate_activation_function) {
    return false;
  }
  for(auto ii = 0; ii < Gate_Max; ++ii) {
    if(!is_same_size(_ww_x[0], _ww_x[ii])) {
      return false;
    }
    if(!is_same_size(_ww_h[0], _ww_h[ii])) {
      return false;
    }
    if(!is_same_size(_bb[0], _bb[ii])) {
      return false;
    }
  }

  return true;
}

std::string yann::LstmLayer::get_name() const
{
  return "LstmLayer";
}

string yann::LstmLayer::get_info() const
{
  YANN_CHECK(is_valid());

  ostringstream oss;
  oss << Base::get_info()
      << ", state activation: " << _io_activation_function->get_info()
      << ", output activation: " << _gate_activation_function->get_info()
      ;
  return oss.str();
}

bool yann::LstmLayer::is_equal(const Layer & other, double tolerance) const
{
  if(!Base::is_equal(other, tolerance)) {
    return false;
  }
  auto the_other = dynamic_cast<const LstmLayer*>(&other);
  if(the_other == nullptr) {
    return false;
  }
  // TOOD: add deep compare
  if(_io_activation_function->get_info() != the_other->_io_activation_function->get_info()) {
    return false;
  }
  if(_gate_activation_function->get_info() != the_other->_gate_activation_function->get_info()) {
    return false;
  }
  for(auto ii = 0; ii < Gate_Max; ++ii) {
    if(!_ww_x[ii].isApprox(the_other->_ww_x[ii], tolerance)) {
      return false;
    }
    if(!_ww_h[ii].isApprox(the_other->_ww_h[ii], tolerance)) {
      return false;
    }
    if(!_bb[ii].isApprox(the_other->_bb[ii], tolerance)) {
      return false;
    }
  }
  return true;
}

MatrixSize yann::LstmLayer::get_input_size() const
{
  YANN_SLOW_CHECK(is_valid());
  return _ww_x[Gate_A].rows();
}

MatrixSize yann::LstmLayer::get_output_size() const
{
  YANN_SLOW_CHECK(is_valid());
  return _ww_x[Gate_A].cols();
}

unique_ptr<Layer::Context> yann::LstmLayer::create_context(const MatrixSize & batch_size) const
{
  YANN_CHECK(is_valid());
  return make_unique<LstmLayer_Context>(get_output_size(), batch_size);
}
unique_ptr<Layer::Context> yann::LstmLayer::create_context(const RefVectorBatch & output) const
{
  YANN_CHECK(is_valid());
  return make_unique<LstmLayer_Context>(output);
}
unique_ptr<Layer::Context> yann::LstmLayer::create_training_context(
    const MatrixSize & batch_size, const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  YANN_CHECK(updater);
  return make_unique<LstmLayer_TrainingContext>(
      get_input_size(), get_output_size(), batch_size, updater);
}
unique_ptr<Layer::Context> yann::LstmLayer::create_training_context(
    const RefVectorBatch & output, const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  YANN_CHECK(updater);
  return make_unique<LstmLayer_TrainingContext>(
      get_input_size(), output, updater);
}

template<typename InputType>
void yann::LstmLayer::feedforward_internal(
    const InputType & input,
    Context * context,
    enum OperationMode mode) const
{
  auto ctx = dynamic_cast<LstmLayer_Context *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  YANN_CHECK_LE(get_batch_size(input) + ctx->_pos, ctx->get_batch_size());
  YANN_CHECK_LE(get_batch_size(input), get_batch_size(ctx->get_output()));
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());

  for(MatrixSize ii = 0; ii < get_batch_size(input); ++ii, ++ctx->_pos) {
    // Phase 1: I/O gates
    //  zz_a(t) = ww_xa*x(t) + ww_ha * h(t-1) + bb_a  -- input
    //  zz_i(t) = ww_xi*x(t) + ww_hi * h(t-1) + bb_i  -- input gate
    //  zz_f(t) = ww_xf*x(t) + ww_hf * h(t-1) + bb_g  -- forget gate
    //  zz_o(t) = ww_xo*x(t) + ww_ho * h(t-1) + bb_o  -- output gate
    auto in  = get_batch(input, ii);
    auto out = get_batch(ctx->get_output(), ctx->_pos);
    auto hh  = get_batch(ctx->_hh, ctx->_pos);
    if(ctx->_pos > 0) {
      const auto & hh_prev = get_batch(ctx->_hh, ctx->_pos - 1);
      for(auto jj = 0; jj < Gate_Max; ++jj) {
        auto zz_gate = get_batch(ctx->_zz_gate[jj], ctx->_pos);
        zz_gate.noalias() = MatrixFunctions<InputType>::product(in, _ww_x[jj]);
        zz_gate.noalias() += MatrixFunctions<RefConstMatrix>::product(hh_prev, _ww_h[jj]) + _bb[jj];
      }
    } else {
      for(auto jj = 0; jj < Gate_Max; ++jj) {
        auto zz_gate = get_batch(ctx->_zz_gate[jj], ctx->_pos);
        zz_gate.noalias() = MatrixFunctions<InputType>::product(in, _ww_x[jj]);
        zz_gate.noalias() += _bb[jj];
      }
    }

    //  a(t) = activ1 (zz_a(t))
    //  i(t) = activ2 (zz_i(t))
    //  f(t) = activ2 (zz_f(t))
    //  o(t) = activ2 (zz_o(t))
    for(auto jj = 0; jj < Gate_Max; ++jj) {
      const auto zz_gate = get_batch(ctx->_zz_gate[jj], ctx->_pos);
      auto gate = get_batch(ctx->_gate[jj], ctx->_pos);
      if(jj > 0) {
        _gate_activation_function->f(zz_gate, gate, Operation_Assign);
      } else {
        _io_activation_function->f(zz_gate, gate, Operation_Assign);
      }
    }

    // Phase 2: State
    //  c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
    const auto gate_aa = get_batch(ctx->_gate[Gate_A], ctx->_pos);
    const auto gate_ii = get_batch(ctx->_gate[Gate_I], ctx->_pos);
    const auto gate_ff = get_batch(ctx->_gate[Gate_F], ctx->_pos);
    auto cc = get_batch(ctx->_cc, ctx->_pos);
    if(ctx->_pos > 0) {
      const auto & cc_prev = get_batch(ctx->_cc, ctx->_pos - 1);
      cc.array() = gate_ii.array() * gate_aa.array() + gate_ff.array() * cc_prev.array();
    } else {
      cc.array() = gate_ii.array() * gate_aa.array();
    }

    // Phase 3: Output
    //  h(t) = elem_prod(o(t), activ1(c(t))
    const auto gate_oo = get_batch(ctx->_gate[Gate_O], ctx->_pos);
    auto activ_cc = get_batch(ctx->_activ_cc, ctx->_pos);
    _io_activation_function->f(cc, activ_cc, Operation_Assign);
    hh.array() = gate_oo.array() * activ_cc.array();

    // done!
    switch(mode) {
    case Operation_Assign:
      out.noalias() = hh;
      break;
    case Operation_PlusEqual:
      out.noalias() += hh;
      break;
    }
  }
}

void yann::LstmLayer::feedforward(
    const RefConstVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  feedforward_internal(input, context, mode);
}

void yann::LstmLayer::feedforward(
    const RefConstSparseVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  feedforward_internal<RefConstSparseMatrix>(input, context, mode);
}

template<
  typename MainInputType,
  typename InputType,
  typename GradientInputType,
  typename ContextType
>
void yann::LstmLayer::backprop_gate(
    enum IOGates gate,
    const std::unique_ptr<ActivationFunction> & activation_function,
    const RefConstVector & gradient_gate,
    const InputType & input,
    boost::optional<GradientInputType> gradient_in,
    ContextType * ctx) const
{
  YANN_SLOW_CHECK(activation_function);
  YANN_SLOW_CHECK(ctx);

  const auto zz_gate = get_batch(ctx->_zz_gate[gate], ctx->_pos);
  const auto & ww_x = _ww_x[gate];
  const auto & ww_h = _ww_h[gate];
  auto & gradient_zz_gate = ctx->_gradient_zz_gate;
  auto & tmp_derivative = ctx->_tmp_derivative;

  // gate(t) = activ(zz_gate(t))
  //
  // grad(zz_gate) = grad(gate) * activ'(zz_gate)
  activation_function->derivative(zz_gate, tmp_derivative);
  gradient_zz_gate.array() = gradient_gate.array() * tmp_derivative.array();

  // zz_gate(t) = ww_x_gate*x(t) + ww_h_gate * h(t-1) + bb_gate
  //
  // grad(ww_x_gate) = grad(zz_gate) * d zz_gate/dww_x_gate = grad(zz_gate) * x(t)
  // grad(ww_h_gate) = grad(zz_gate) * d zz_gate/dww_h_gate = grad(zz_gate) * h(t-1)
  // grad(b) = grad(zz_gate) * d zz_gate/db = grad(zz_gate)
  YANN_SLOW_CHECK(is_same_size(ctx->_delta_ww_x[gate], _ww_x[gate]));
  YANN_SLOW_CHECK(is_same_size(ctx->_delta_ww_h[gate], _ww_h[gate]));
  YANN_SLOW_CHECK(is_same_size(ctx->_delta_bb[gate], _bb[gate]));
  ctx->_delta_ww_x[gate] += MatrixFunctions<MainInputType>::product(input.transpose(), gradient_zz_gate);
  if(ctx->_pos > 0) {
    auto prev_hh = get_batch(ctx->_hh, ctx->_pos - 1);
    ctx->_delta_ww_h[gate] += MatrixFunctions<RefConstMatrix>::product(prev_hh.transpose(), gradient_zz_gate);
  }
  ctx->_delta_bb[gate].noalias() += gradient_zz_gate;

  // zz_gate(t) = ww_x_gate*x(t) + ww_h_gate * h(t-1) + bb_gate
  //
  // grad(h) = grad(zz_gate) * d zz_gate/dh = grad(zz_gate) * transp(ww_h_gate)
  if(ctx->_pos > 0) {
    auto & gradient_hh = ctx->_gradient_hh;
    gradient_hh.noalias() += MatrixFunctions<RefConstMatrix>::product(gradient_zz_gate, ww_h.transpose());
  }

  // zz_gate(t) = ww_x_gate*x(t) + ww_h_gate * h(t-1) + bb_gate
  //
  //  grad(x) = grad(zz_gate) * d zz_gate/dx = grad(zz_gate) * transp(ww_x_gate)
  if(gradient_in) {
    (*gradient_in).noalias() += MatrixFunctions<RefConstMatrix>::product(gradient_zz_gate, ww_x.transpose());
  }
}

template<typename InputType>
void yann::LstmLayer::backprop_internal(
    const RefConstVectorBatch & gradient_output,
    const InputType & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  auto ctx = dynamic_cast<LstmLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  YANN_CHECK_GT(get_batch_size(gradient_output), 0);
  YANN_CHECK_LE(get_batch_size(gradient_output), ctx->_pos);
  YANN_CHECK_EQ(get_batch_item_size(gradient_output), get_output_size());
  YANN_CHECK_EQ(get_batch_size(input), get_batch_size(gradient_output));
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK(!gradient_input || is_same_size(input, *gradient_input));

  // Zero out since we will be adding to it
  if(gradient_input) {
    gradient_input->setZero();
  }

  for(MatrixSize ii = get_batch_size(gradient_output) - 1; ii >= 0 && (--ctx->_pos) >= 0; --ii) {
    const auto in = get_batch(input, ii);
    auto gradient_in = gradient_input ? make_optional(get_batch(*gradient_input, ii)) : boost::none;
    const auto gate_aa  = get_batch(ctx->_gate[Gate_A], ctx->_pos);
    const auto gate_ii  = get_batch(ctx->_gate[Gate_I], ctx->_pos);
    const auto gate_ff  = get_batch(ctx->_gate[Gate_F], ctx->_pos);
    const auto gate_oo  = get_batch(ctx->_gate[Gate_O], ctx->_pos);
    auto & gradient_gate = ctx->_gradient_gate;

    // note that output gradient also has the internal feedback loop
    auto & gradient_hh = ctx->_gradient_hh;
    gradient_hh += get_batch(gradient_output, ii);

    // State cell
    //
    //  h(t) = elem_prod(o(t), activ1(c(t))
    //  grad(c) = grad(h) * d h/dactiv1(c) * activ1'(c) = elem_prod(grad(h), o, activ1(c)
    const auto cc = get_batch(ctx->_cc, ctx->_pos);
    auto & gradient_cc = ctx->_gradient_cc;
    auto & tmp_derivative = ctx->_tmp_derivative;
    _io_activation_function->derivative(cc, tmp_derivative);
    // add to the gradient from previous step
    gradient_cc.array() += gradient_hh.array() * gate_oo.array() * tmp_derivative.array();

    // Gate O: Output gate:
    //
    //  h(t) = elem_prod(o(t), activ1(c(t))
    //  grad(o) = grad(h) * dh/do = elem_prod(grad(h), activ1(c))
    const auto activ_cc = get_batch(ctx->_activ_cc, ctx->_pos);
    gradient_gate.array() = gradient_hh.array() * activ_cc.array();
    gradient_hh.setZero(); // we will update it in backprop_gate for next step
    backprop_gate<InputType>(Gate_O, _gate_activation_function, gradient_gate, in, gradient_in, ctx);

    // Gate A
    //
    //  c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
    //  grad(a) = grad(c) * dc/da = elem_prod(grad(c), i(t))
    gradient_gate.array() = gradient_cc.array() * gate_ii.array();
    backprop_gate<InputType>(Gate_A, _io_activation_function, gradient_gate, in, gradient_in, ctx);

    // Gate I: Input gate
    //
    //  c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
    //  grad(i) = grad(c) * dc/di = elem_prod(grad(c), a(t))
    gradient_gate.array() = gradient_cc.array() * gate_aa.array();
    backprop_gate<InputType>(Gate_I, _gate_activation_function, gradient_gate, in, gradient_in, ctx);

    // Gate F: Forget gate
    //
    //  c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
    //  grad(f) = grad(c) * dc/df = elem_prod(grad(c), c(t-1))
    if(ctx->_pos > 0) {
      const auto prev_cc = get_batch(ctx->_activ_cc, ctx->_pos - 1);
      gradient_gate.array() = gradient_cc.array() * prev_cc.array();
    } else {
      gradient_gate.setZero();
    }
    backprop_gate<InputType>(Gate_F, _gate_activation_function, gradient_gate, in, gradient_in, ctx);

    // State gradient for prev step
    //
    //  c(t) = elem_prod(i(t), a(t)) + elem_prod(f(t), c(t-1))
    //  grad(c(t-1)) = grad(c) * dc(t)/dc(t-1) = elem_prod(grad(c(t)), f(t))
    if(ctx->_pos > 0) {
      gradient_cc.array() = gradient_cc.array() * gate_ff.array();
    }
  }
}

void yann::LstmLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  backprop_internal(gradient_output, input, gradient_input, context);
}

void yann::LstmLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstSparseVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  // <RefConstSparseMatrix> is required for MatrixFunctions<>::product
  backprop_internal<RefConstSparseMatrix>(gradient_output, input, gradient_input, context);
}

void yann::LstmLayer::init(enum InitMode mode, boost::optional<InitContext> init_context)
{
  switch (mode) {
  case InitMode_Zeros:
    for(auto & ww_x: _ww_x) {
      ww_x.setZero();
    }
    for(auto & ww_h: _ww_h) {
      ww_h.setZero();
    }
    for(auto & bb: _bb) {
      bb.setZero();
    }
    break;
  case InitMode_Random:
    {
      unique_ptr<RandomGenerator> gen01 = RandomGenerator::normal_distribution(0, 1,
          init_context ? optional<Value>(init_context->seed()) : boost::none);
      for(auto & ww_x: _ww_x) {
        gen01->generate(ww_x);
      }
      for(auto & ww_h: _ww_h) {
        gen01->generate(ww_h);
      }
      for(auto & bb: _bb) {
        gen01->generate(bb);
      }
    }
    break;
  }
}

void yann::LstmLayer::update(Context * context, const size_t & tests_num)
{
  auto ctx = dynamic_cast<LstmLayer_TrainingContext *>(context);
  YANN_CHECK(ctx);

  for(auto ii = 0; ii < Gate_Max; ++ii) {
    YANN_SLOW_CHECK(ctx->_ww_x_updater[ii]);
    YANN_SLOW_CHECK(ctx->_ww_h_updater[ii]);
    YANN_SLOW_CHECK(ctx->_bb_updater[ii]);

    ctx->_ww_x_updater[ii]->update(ctx->_delta_ww_x[ii], tests_num, _ww_x[ii]);
    ctx->_ww_h_updater[ii]->update(ctx->_delta_ww_h[ii], tests_num, _ww_h[ii]);
    ctx->_bb_updater[ii]->update(ctx->_delta_bb[ii], tests_num, _bb[ii]);
  }
}

// the format is (no new lines):
// (wxa:<_ww_x[Gate_A]>,wxh:<_ww_h[Gate_A]>,ba:<_bb[Gate_A]>,
//  wxi:<_ww_x[Gate_I]>,wxi:<_ww_h[Gate_I]>,bi:<_bb[Gate_I]>,
//  wxf:<_ww_x[Gate_F]>,wxf:<_ww_h[Gate_F]>,bf:<_bb[Gate_F]>,
//  wxo:<_ww_x[Gate_O]>,wxo:<_ww_h[Gate_O]>,bo:<_bb[Gate_O]>)
void yann::LstmLayer::read(std::istream & is)
{
  Base::read(is);

  read_char(is, '(');

  for(auto ii = 0; ii < Gate_Max; ++ii) {
    const char & suffix =  g_ltsm_names_suffix[ii];
    if(ii > 0) {
      read_char(is, ',');
    }
    read_object(is, string("wx") + suffix, _ww_x[ii]);
    read_char(is, ',');
    read_object(is, string("wh") + suffix, _ww_h[ii]);
    read_char(is, ',');
    read_object(is, string("b") + suffix, _bb[ii]);
  }
  read_char(is, ')');
}

// the format is (no new lines):
// (wxa:<_ww_x[Gate_A]>,wxh:<_ww_h[Gate_A]>,ba:<_bb[Gate_A]>,
//  wxi:<_ww_x[Gate_I]>,wxi:<_ww_h[Gate_I]>,bi:<_bb[Gate_I]>,
//  wxf:<_ww_x[Gate_F]>,wxf:<_ww_h[Gate_F]>,bf:<_bb[Gate_F]>,
//  wxo:<_ww_x[Gate_O]>,wxo:<_ww_h[Gate_O]>,bo:<_bb[Gate_O]>)
void yann::LstmLayer::write(std::ostream & os) const
{
  Base::write(os);

  os << "(";
  for(auto ii = 0; ii < Gate_Max; ++ii) {
    const char & suffix =  g_ltsm_names_suffix[ii];
    if(ii > 0) {
      os << ",";
    }
    write_object(os, string("wx") + suffix, _ww_x[ii]);
    os << ",";
    write_object(os, string("wh") + suffix, _ww_h[ii]);
    os << ",";
    write_object(os, string("b") + suffix, _bb[ii]);
  }
  os << ")";
}


#ifndef TRIUMF_BNMR_SLR_STR_EXP_HPP
#define TRIUMF_BNMR_SLR_STR_EXP_HPP

#include <boost/math/quadrature/tanh_sinh.hpp>
#include <cmath>
#include <triumf/bnmr/slr/common.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// spin-lattice relaxation (SLR)
namespace slr {

/// pulsed stretched exponential integral (from 0 to time_p)
template <typename T = double>
T pulsed_str_exp_integral(T time, T time_p, T nuclear_lifetime, T slr_rate,
                          T beta) {
  // integrand for the numeric integral
  auto integrand = [=](T t_p) {
    return std::exp(-(time - t_p) / nuclear_lifetime) *
           std::exp(-std::pow(slr_rate * (time - t_p), beta));
  };
  // create the integrator for tanh-sinh quadrature
  static boost::math::quadrature::tanh_sinh<T> integrator;
  // evaluate the integral from 0 to time_p
  T Q = integrator.integrate(integrand, 0.0, time_p);
  return Q;
}

/// pulsed stretched exponential
template <typename T = double>
T pulsed_str_exp(T time, T nuclear_lifetime, T pulse_length, T asymmetry,
                 T slr_rate, T beta) {
  if (time == 0.0) {
    return asymmetry;
  } else if (time > 0.0 and time <= pulse_length) {
    return asymmetry *
           pulsed_str_exp_integral<T>(time, time, nuclear_lifetime, slr_rate,
                                      beta) /
           normalization<T>(time, nuclear_lifetime);
  } else if (time > pulse_length) {
    return (asymmetry *
            pulsed_str_exp_integral<T>(time, pulse_length, nuclear_lifetime,
                                       slr_rate, beta) /
            normalization<T>(pulse_length, nuclear_lifetime)) /
           std::exp(-(time - pulse_length) / nuclear_lifetime);
  } else {
    return 0.0;
  }
}

/// pulsed stretched exponential (ROOT)
template <typename T = double> T pulsed_str_exp(const T *x, const T *par) {
  return pulsed_str_exp<T>(*x, par[0], par[1], par[2], par[3], par[4]);
}

} // namespace slr

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SLR_STR_EXP_HPP


///////////////////////////////////////////////////////////////////////////////
// common.hpp                                                                //
//                                                                           //
//  Copyright 2010 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_PEARSON_CHISQ_COMMON_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_PEARSON_CHISQ_COMMON_HPP_ER_2010

#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/common/df_formula.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/common/asy_distribution.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/common/chisq_summand_formula.hpp>

#endif


#include "./include/rsa.h"
#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <string>

int main(int argc, char const *argv[])
{
    RSA::key_generator key_gen(100);
    auto k1 = key_gen.get_public_key_1();
    auto k2 = key_gen.get_public_key_2();
    auto k3 = key_gen.get_private_key();

    std::cout << "Chave pública 1:\t\t" << k1 << std::endl;
    std::cout << "Chave pública 2:\t\t" << k2 << std::endl;
    std::cout << "Chave privada:\t\t\t" << k3 << std::endl;

    std::cout << "Chave privada (BRUTADA):\t" << RSA::brent_key_breaker(k1, k2) << std::endl;

    return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2018 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// 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 "main.h"
#include <Eigen/SparseCore>

template<int ExpectedDim,typename Xpr>
void check_dim(const Xpr& ) {
  STATIC_CHECK( Xpr::NumDimensions == ExpectedDim );
}

#if EIGEN_HAS_CXX11
template<template <typename,int,int> class Object>
void map_num_dimensions()
{
  typedef Object<double, 1, 1> ArrayScalarType;
  typedef Object<double, 2, 1> ArrayVectorType;
  typedef Object<double, 1, 2> TransposeArrayVectorType;
  typedef Object<double, 2, 2> ArrayType;
  typedef Object<double, Eigen::Dynamic, 1> DynamicArrayVectorType;
  typedef Object<double, 1, Eigen::Dynamic> DynamicTransposeArrayVectorType;
  typedef Object<double, Eigen::Dynamic, Eigen::Dynamic> DynamicArrayType;

  STATIC_CHECK(ArrayScalarType::NumDimensions == 0);
  STATIC_CHECK(ArrayVectorType::NumDimensions == 1);
  STATIC_CHECK(TransposeArrayVectorType::NumDimensions == 1);
  STATIC_CHECK(ArrayType::NumDimensions == 2);
  STATIC_CHECK(DynamicArrayVectorType::NumDimensions == 1);
  STATIC_CHECK(DynamicTransposeArrayVectorType::NumDimensions == 1);
  STATIC_CHECK(DynamicArrayType::NumDimensions == 2);

  typedef Eigen::Map<ArrayScalarType> ArrayScalarMap;
  typedef Eigen::Map<ArrayVectorType> ArrayVectorMap;
  typedef Eigen::Map<TransposeArrayVectorType> TransposeArrayVectorMap;
  typedef Eigen::Map<ArrayType> ArrayMap;
  typedef Eigen::Map<DynamicArrayVectorType> DynamicArrayVectorMap;
  typedef Eigen::Map<DynamicTransposeArrayVectorType> DynamicTransposeArrayVectorMap;
  typedef Eigen::Map<DynamicArrayType> DynamicArrayMap;

  STATIC_CHECK(ArrayScalarMap::NumDimensions == 0);
  STATIC_CHECK(ArrayVectorMap::NumDimensions == 1);
  STATIC_CHECK(TransposeArrayVectorMap::NumDimensions == 1);
  STATIC_CHECK(ArrayMap::NumDimensions == 2);
  STATIC_CHECK(DynamicArrayVectorMap::NumDimensions == 1);
  STATIC_CHECK(DynamicTransposeArrayVectorMap::NumDimensions == 1);
  STATIC_CHECK(DynamicArrayMap::NumDimensions == 2);
}

template<typename Scalar, int Rows, int Cols>
using TArray = Array<Scalar,Rows,Cols>;

template<typename Scalar, int Rows, int Cols>
using TMatrix = Matrix<Scalar,Rows,Cols>;

#endif

void test_num_dimensions()
{
  int n = 10;
  ArrayXXd A(n,n);
  CALL_SUBTEST( check_dim<2>(A) );
  CALL_SUBTEST( check_dim<2>(A.block(1,1,2,2)) );
  CALL_SUBTEST( check_dim<1>(A.col(1)) );
  CALL_SUBTEST( check_dim<1>(A.row(1)) );

  MatrixXd M(n,n);
  CALL_SUBTEST( check_dim<0>(M.row(1)*M.col(1)) );

  SparseMatrix<double> S(n,n);
  CALL_SUBTEST( check_dim<2>(S) );
  CALL_SUBTEST( check_dim<2>(S.block(1,1,2,2)) );
  CALL_SUBTEST( check_dim<1>(S.col(1)) );
  CALL_SUBTEST( check_dim<1>(S.row(1)) );

  SparseVector<double> s(n);
  CALL_SUBTEST( check_dim<1>(s) );
  CALL_SUBTEST( check_dim<1>(s.head(2)) );

  #if EIGEN_HAS_CXX11
  CALL_SUBTEST( map_num_dimensions<TArray>() );
  CALL_SUBTEST( map_num_dimensions<TMatrix>() );
  #endif
}


// Group E - Excel Visualization
//
// by Scott Sidoli
//
// 6-15-19
//
// Main.cpp
//
// In this work we use the excel visualization functionality to produce spreadsheet output for the
// four batches. It should be noted that some of the includes need to have the path adjusted to work
// other machines. We describe the four batches, create row labels for the S value and column labels
// are the call/put prices for a given batch. We use an if-else loop inside a for-loop to produce the
// matrix output and then send the output to an excel spreadsheet. We include all our option class
// functionality.

#include "C:\Users\ssido\OneDrive\Desktop\Level9\Level9\Level9Code\Level9Code\UtilitiesDJD\ExcelDriver\ExcelDriverLite.hpp"
#include "C:\Users\ssido\OneDrive\Desktop\Level9\Level9\Level9Code\Level9Code\UtilitiesDJD\ExcelDriver\Utilities.hpp"
#include "C:\Users\ssido\OneDrive\Desktop\Level9\Level9\Level9Code\Level9Code\UtilitiesDJD\VectorsAndMatrices\Vector.hpp"
#include "C:\Users\ssido\OneDrive\Desktop\Level9\Level9\Level9Code\Level9Code\UtilitiesDJD\ExceptionClasses\DatasimException.hpp"
#include <iostream>
#include "EuropeanOption.hpp"
#include "MeshArray.hpp"
#include <boost/tuple/tuple_io.hpp>
#include <iostream>
#include <string>
#include <vector>
#include <list>
#include <fstream>
#include <cmath>
#include <boost/numeric/ublas/matrix.hpp>
#include "C:\Users\ssido\OneDrive\Desktop\Level9\Level9\Level9Code\Level9Code\UtilitiesDJD\VectorsAndMatrices\NestedMatrix.hpp"

using NumericMatrix = boost::numeric::ublas::matrix<double>; // using NumericMatrix = NestedMatrix<double>;

using namespace std;

int main()
{
	curr_stock_price S_start = (curr_stock_price) 10.0;
	curr_stock_price S_end = (curr_stock_price) 50.0;
	curr_stock_price S_interval = (curr_stock_price) 1.0;

	vector<curr_stock_price> S_array = MeshArray(S_start, S_end, S_interval);

	// Batch 1
	Time T1 = (Time) 0.25;
	Strike_Price K1 = (Strike_Price)65;
	Volatility sig1 = (Volatility) 0.30;
	rate r1 = (rate) 0.08;
	cost_of_carry b1 = (cost_of_carry) 0.08;
	curr_stock_price S1 = (curr_stock_price) 60.0;

	// Create option and set parameters
	EuropeanOption option1;
	option1.SetOption(T1, K1, sig1, r1, b1, S1);

	// Batch 2
	Time T2 = (Time) 1.0;
	Strike_Price K2 = (Strike_Price) 100.0;
	Volatility sig2 = (Volatility) 0.2;
	rate r2 = (rate) 0.00;
	cost_of_carry b2 = (cost_of_carry) 0.00;
	curr_stock_price S2 = (curr_stock_price) 100.0;

	// Create option and set parameters
	EuropeanOption option2;
	option2.SetOption(T2, K2, sig2, r2, b2, S2);

	// Batch 3
	Time T3 = (Time) 1.0;
	Strike_Price K3 = (Strike_Price) 10.0;
	Volatility sig3 = (Volatility) 0.50;
	rate r3 = (rate) 0.12;
	cost_of_carry b3 = (cost_of_carry) 0.12;
	curr_stock_price S3 = (curr_stock_price) 5.0;

	// Create option and set parameters
	EuropeanOption option3;
	option3.SetOption(T3, K3, sig3, r3, b3, S3);

	// Batch 4
	Time T4 = (Time) 30.0;
	Strike_Price K4 = (Strike_Price) 100.0;
	Volatility sig4 = (Volatility) 0.30;
	rate r4 = (rate) 0.08;
	cost_of_carry b4 = (cost_of_carry) 0.08;
	curr_stock_price S4 = (curr_stock_price) 100.0;

	// Create option and set parameters
	EuropeanOption option4;
	option4.SetOption(T4, K4, sig4, r4, b4, S4);

	// Now we create Row and column labels Rows labeled by S-value, column by Batch num call/put
	stringstream ss;
	string str;
	list<string> rowLabels;
	for (unsigned int i = 0; i < S_array.size(); ++i)
	{
		ss << i + 10;
		ss >> str;
		rowLabels.push_back(str);
		ss.clear();
	}

	list<string> colLabels{ "Batch 1 Call", "Batch 1 Put", "Batch 2 Call", "Batch 2 Put", "Batch 3 Call", "Batch 3 Put",
	"Batch 4 Call", "Batch 4 Put"};

	// Now we write the sheet name
	string sheetName("Option Prices");

	NumericMatrix PriceMatrix(rowLabels.size(), colLabels.size());
	for (unsigned int i = 0; i < PriceMatrix.size1(); i++)
	{
		for (unsigned int j = 0; j < PriceMatrix.size2(); j++)
		{
			if (j == 0)
			{
				option1.SetOption(S_array[i]);
				PriceMatrix(i, j) = option1.CallPriceEuro();
			}
			else if ( j == 1)
			{
				option1.SetOption(S_array[i]);
				PriceMatrix(i, j) = option1.PutPriceEuro();
			}
			else if (j == 2)
			{
				option2.SetOption(S_array[i]);
				PriceMatrix(i, j) = option2.CallPriceEuro();
			}
			else if (j == 3)
			{
				option2.SetOption(S_array[i]);
				PriceMatrix(i, j) = option2.PutPriceEuro();
			}
			else if (j == 4)
			{
				option3.SetOption(S_array[i]);
				PriceMatrix(i, j) = option3.CallPriceEuro();
			}
			else if (j == 5)
			{
				option3.SetOption(S_array[i]);
				PriceMatrix(i, j) = option3.PutPriceEuro();
			}
			else if (j == 6)
			{
				option4.SetOption(S_array[i]);
				PriceMatrix(i, j) = option4.CallPriceEuro();
			}
			else if (j == 7)
			{
				option4.SetOption(S_array[i]);
				PriceMatrix(i, j) = option4.PutPriceEuro();
			}
		}
	}

	ExcelDriver& excel = ExcelDriver::Instance();
	excel.MakeVisible(true);

	long row = 1;
	long col = 1;
	excel.AddMatrix<NumericMatrix>(PriceMatrix, sheetName, rowLabels, colLabels, row, col);
	return 0;
}


#include "matrix_utils.h"
#include <Eigen/SparseCholesky>
#include <Eigen/LU>

namespace rsurfaces
{
    namespace MatrixUtils
    {
        void TripleTriplets(const std::vector<Triplet> &orig, std::vector<Triplet> &output)
        {
            for (const Triplet &t : orig)
            {
                output.push_back(Triplet(3 * t.row(), 3 * t.col(), t.value()));
                output.push_back(Triplet(3 * t.row() + 1, 3 * t.col() + 1, t.value()));
                output.push_back(Triplet(3 * t.row() + 2, 3 * t.col() + 2, t.value()));
            }
        }

        void TripleMatrix(const Eigen::MatrixXd &M, Eigen::MatrixXd &out)
        {
            for (int i = 0; i < M.rows(); i++)
            {
                for (int j = 0; j < M.cols(); j++)
                {
                    out(3 * i, 3 * j) = M(i, j);
                    out(3 * i + 1, 3 * j + 1) = M(i, j);
                    out(3 * i + 2, 3 * j + 2) = M(i, j);
                }
            }
        }

        void MatrixIntoColumn(const Eigen::MatrixXd &M, Eigen::VectorXd &out)
        {
            int rows = M.rows();
            int cols = M.cols();

            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    int ind = cols * i + j;
                    out(ind) = M(i, j);
                }
            }
        }

        void ColumnIntoMatrix(const Eigen::VectorXd &v, Eigen::MatrixXd &out)
        {
            int rows = out.rows();
            int cols = out.cols();

            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    int ind = cols * i + j;
                    out(i, j) = v(ind);
                }
            }
        }

        void SolveSparseSystem(const Eigen::SparseMatrix<double> &M, Eigen::VectorXd &rhs, Eigen::VectorXd &output)
        {
            Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
            solver.compute(M);
            if (solver.info() != Eigen::Success)
            {
                // decomposition failed
                std::cout << "Sparse decomposition failed. Exiting." << std::endl;
                std::exit(1);
            }
            Eigen::VectorXd x = solver.solve(rhs);
            if (solver.info() != Eigen::Success)
            {
                // solving failed
                std::cout << "Sparse solve failed. Exiting." << std::endl;
                std::exit(1);
            }
            output = x;
        }

        void SolveDenseSystem(const Eigen::MatrixXd &M, Eigen::VectorXd &rhs, Eigen::VectorXd &output)
        {
            Eigen::PartialPivLU<Eigen::MatrixXd> solver = M.partialPivLu();
            output = solver.solve(rhs);
        }

        void SolveDenseSystem(Eigen::PartialPivLU<Eigen::MatrixXd> &solver, const Eigen::MatrixXd &M, Eigen::VectorXd &rhs, Eigen::VectorXd &output)
        {
            solver.compute(M);
            output = solver.solve(rhs);
        }

    } // namespace MatrixUtils
} // namespace rsurfaces


#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
//#define EIGEN_SUPERLU_SUPPORT

#include "Debug.h"
#include "SolverEigen.h"
#include "SolverTime.h"
#include "ComputerTime.h"
#include "util.h"
#include <chrono>

//#include <Eigen/SuperLUSupport>
#include <Eigen/SparseExtra>
#include <Eigen/IterativeSolvers>

extern SolverTime      solverTime;
extern ComputerTime    computerTime;

using namespace std;
using namespace Eigen;

SolverEigen::SolverEigen()
{
  STABILISED = false;

  update_precond = 1;

  if (debug) cout << " SolverEigen constructor\n\n";
}

SolverEigen::~SolverEigen()
{
  if (debug)  cout << " SolverEigen destructor\n\n";

  free();
}

int SolverEigen::initialise(int p1, int p2, int p3)
{
   nRow = nCol = p3;

   //cout << " nRow = " << nRow << endl;

   soln.resize(nRow);
   soln.setZero();

   rhsVec   = soln;
   solnPrev = soln;

  return 0;
}

int SolverEigen::setSolverAndParameters()
{
    ///////////////////////
    // Create the linear solver and set various options
    ///////////////////////

    ///////////////////////
    //Set operators. Here the matrix that defines the linear system
    //also serves as the preconditioning matrix.
    ///////////////////////

    return 0;
}

void SolverEigen::zeroMtx()
{
  //cout << " nRow = " << nRow << endl;
  //printVector(rhsVec);

  //cout << " nRow = " << nRow << endl;
  //cout << mtx << endl;
  //mtx.setZero();
  mtx *= 0.0;
  rhsVec.setZero();

  matB *= 0.0;
  rhsVec2.setZero();

  //cout << " nRow = " << nRow << endl;

  return;
}

int SolverEigen::free()
{
  return 0;
}

void SolverEigen::printInfo()
{
  //cout << "Eigen solver:  nRow = " << nRow << "\n";
  //cout << "               nnz  = " <<  << "\n\n";
  //printVector(rhsVec);
  //rhsVec.setZero();
  //cout << " nRow = " << nRow << endl;
  //cout << mtx << endl;

  return;
}

void SolverEigen::printMatrixPatternToFile()
{
    /*
    ofstream fout("matrix-pattern2.dat");

    if(fout.fail())
    {
      cout << " Could not open the Output file" << endl;
      exit(1);
    }

    fout << totalDOF << setw(10) << totalDOF << endl;

    for(int k=0; k<mtx.outerSize(); ++k)
    for(SparseMatrixXd::InnerIterator it(mtx,k); it; ++it)
      printf("%9d \t %9d \n", it.row(), it.col());

    //for(int ii=0;ii<nRow;ii++)
      //for(int jj=0;jj<nRow;jj++)
        //printf("%5d \t %5d \t %12.6f \n",mtx.coeffRef(ii,jj));

    fout.close();
    */

   FILE * pFile;
   int n;
   char name [100];

   cout << mtx.nonZeros() << endl;
   //pFile = fopen ("Stokes.dat","w");
   pFile = fopen ("Poisson.dat","w");

    fprintf(pFile, "%9d \t %9d \t %9d \n", nRow, nCol, 0 );

    for(int k=0; k<mtx.outerSize(); ++k)
    {
      for(SparseMatrixXd::InnerIterator it(mtx,k); it; ++it)
      {
        if(it.row() == it.col())
          fprintf(pFile, "%9d \t %9d \t %20.16f \n", it.row(), it.col(), it.value());
      }
    }

   fclose (pFile);

   pFile = fopen ("rhsVec.dat","w");

    for(int k=0; k<nRow; ++k)
      fprintf(pFile, "%9d \t %14.8f \n", k, rhsVec(k));

   fclose (pFile);

   pFile = fopen ("refsoln.dat","w");

    for(int k=0; k<nRow; ++k)
      fprintf(pFile, "%9d \t %14.8f \n", k, soln(k));

   fclose (pFile);
  return;
}

void SolverEigen::printMatrix(int dig, int dig2, bool gfrmt, int indent, bool interactive)
{
  printInfo();

  cout << mtx << endl;
  printf("\n\n");

  return;
}

double SolverEigen::giveMatrixCoefficient(int row, int col)
{
  return  mtx.coeff(row,col);
}

int SolverEigen::factorise()
{
  char fct[] = "SolverEigen::factorise";

  if (currentStatus != ASSEMBLY_OK) { prgWarning(1,fct,"assemble matrix first!"); return 1; }

  if (checkIO)
  {
    // search for "nan" entries in matrix coefficients

    //if (prgNAN(mtx.x.x,NE)) prgError(1,fct,"nan matrix coefficient!");
  }

  computerTime.go(fct);

  currentStatus = FACTORISE_OK;

  solverTime.total     -= solverTime.factorise;
  solverTime.factorise += computerTime.stop(fct);
  solverTime.total     += solverTime.factorise;

  return 0;
}

int  SolverEigen::solve()
{
  char fct[] = "SolverEigen::solve";

  time_t tstart, tend;

  if (currentStatus != FACTORISE_OK) { prgWarning(1,fct,"factorise matrix first!"); return 1; }

  //algoType = 1;

  if(algoType == 1)
  {
    //cout << " Solving with Eigen::SimplicialLDLT " << endl;

    SimplicialLDLT<SparseMatrix<double> > solver;

    //cout << mtx << endl;
    //printVector(rhsVec);

    printf("nnz =  %d \n ", mtx.nonZeros() );

    tstart = time(0);

    solver.compute(mtx);

    soln = solver.solve(rhsVec);

    //printVector(soln);
    //
    //computeConditionNumber();

    //VectorXd x0 = VectorXd::LinSpaced(nRow, 0.0, 1.0);

    //double  cond = myCondNum(mtx, x0, 50, solver);

    //printf("\n Matrix condition number = %12.6E \n\n\n", cond);

    //soln = solver.solveWithGuess(rhsVec, soln);
    //cout << solver.info() << '\t' << solver.error() << '\t' << solver.iterations() << endl;
    tend = time(0);
    printf("It took %8.4f second(s) \n ", difftime(tend, tstart) );
  }

  if( algoType == 2 )
  {
    cout << " Solving with Eigen::BiCGSTAB " << endl;

    //ConjugateGradient<SparseMatrixXd, Lower|Upper >   solver;
    //ConjugateGradient<SparseMatrixXd>   solver;

    BiCGSTAB<SparseMatrixXd, IncompleteLUT<double> > solver;

    //BiCGSTAB<SparseMatrixXd > solver;

    //ConjugateGradient<SparseMatrixXd, Lower, IncompleteLUT<double> >   solver;

    solver.preconditioner().setDroptol(1.0e-3);
    solver.preconditioner().setFillfactor(2);

    //BiCGSTAB<SparseMatrixXd> solver;

    //GMRES<SparseMatrixXd, IncompleteLUT<double> > solver;

    //GMRES<SparseMatrixXd> solver;

    //solver.set_restart(100);
    //solver.setEigenv(10);

    solver.setMaxIterations(1000);
    solver.setTolerance(1.0e-8);

    //tstart = time(0);
    auto  time1 = chrono::steady_clock::now();

    //cout << " iiiiiiiiiiiiiii " << endl;
    solver.compute(mtx);
    //cout << " iiiiiiiiiiiiiii " << endl;

    //printf("\n\n");
    //printVector(rhsVec);

    //soln = solver.solveWithGuess(rhsVec, solnGuess);
    soln = solver.solve(rhsVec);

    //printf("\n\n");
    //printVector(soln);

    //tend = time(0);
    //printf("It took %8.4f second(s) \n ", difftime(tend, tstart) );

    auto  time2 = chrono::steady_clock::now();
    auto  duration = chrono::duration_cast<chrono::milliseconds>(time2-time1).count();

    cout << " Error       = " << solver.error() << endl;
    cout << " Iterations  = " << solver.iterations() << endl;
    cout << " Time (ms)   = " << duration << endl;
    //cout << solver.info() << '\t' << solver.error() << '\t' << solver.iterations() << endl;
  }

  solnPrev = soln;

  //printf("\n\n");
  //printVector(soln);

  //cout << "SolverEigen::solve()  took "<< difftime(tend, tstart) <<" second(s)."<< endl;

  if(checkIO)
  {
    // search for "nan" entries in solution vector

    //if (prgNAN(RHS,N)) prgError(1,fct,"nan entry in solution vector!");
  }

  return 0;
}

int SolverEigen::factoriseAndSolve()
{
  if(currentStatus != ASSEMBLY_OK)
  {
    cerr << " assemble matrix first! " << endl;
    return 1;
  }

  factorise();

  return solve();
}

int SolverEigen::assembleMatrixAndVector(vector<int>& row, vector<int>& col, MatrixXd& Klocal, VectorXd& Flocal)
{
  int ii, jj;
  for(ii=0;ii<row.size();ii++)
  {
    rhsVec[row[ii]] += Flocal(ii);
    for(jj=0;jj<col.size();jj++)
    {
      mtx.coeffRef(row[ii], col[jj]) += Klocal(ii,jj);
    }
  }

  return 0;
}

int SolverEigen::assembleMatrixAndVector(int start, int c1, vector<int>& vec1, vector<int>& vec2, MatrixXd& Klocal, VectorXd& Flocal)
{
  // subroutine for mixed formulation
  // vec1 - for variable 'u'
  // vec2 - for variable 'p'

  int ii, jj, aa, bb, size1, size2;

  //printVector(vec1);
  //printVector(vec2);

  size1 = vec1.size();
  size2 = vec2.size();

  for(ii=0;ii<size1;ii++)
  {
    rhsVec[vec1[ii]] += Flocal(ii);
    for(jj=0;jj<size1;jj++)
       mtx.coeffRef(vec1[ii], vec1[jj]) += Klocal(ii, jj);

    for(jj=0;jj<size2;jj++)
    {
       aa = start + vec2[jj];
       bb = size1 + jj;
       mtx.coeffRef(vec1[ii], aa) += Klocal(ii, bb);
       mtx.coeffRef(aa, vec1[ii]) += Klocal(bb, ii);
    }
  }

  for(ii=0;ii<size2;ii++)
  {
    aa = start + vec2[ii];
    rhsVec[aa] += Flocal(size1+ii);
  }

  if(STABILISED)
  {
    for(ii=0;ii<size2;ii++)
    {
      aa = start + vec2[ii];
      bb = size1 + ii;
      for(jj=0;jj<size2;jj++)
      {
        mtx.coeffRef(aa, start+vec2[jj]) += Klocal(bb, size1+jj);
      }
    }
  }

  return 0;
}

int SolverEigen::assembleMatrixAndVector(int start, int c1, vector<int>& forAssy, MatrixXd& Klocal, VectorXd& Flocal)
{
  int ii, jj, aa, bb, size1, r, c;

  //printVector(forAssy);

  size1 = forAssy.size();

  for(ii=0; ii<size1; ii++)
  {
    aa = forAssy[ii];
    if( aa != -1 )
    {
      r = start + aa;
      rhsVec[r] += Flocal(ii);

      //cout << ii << '\t' << aa << '\t' << r << endl;

      for(jj=0; jj<size1; jj++)
      {
        bb = forAssy[jj];
        if( bb != -1 )
          mtx.coeffRef(r, start+bb) += Klocal(ii,jj);
      }
    }
  }

  //cout << " start = " << start << '\t' << c1 << endl;

  return 0;
}

int SolverEigen::assembleMatrixAndVectorMixed(int var2_offset, vector<int>& forAssyVar1, vector<int>& forAssyVar2, MatrixXd& Kuu, MatrixXd& Kup, MatrixXd& Kpu, MatrixXd& Kpp, VectorXd& Fu, VectorXd& Fp)
{
  int ii, jj, row, col;

  int size1 = forAssyVar1.size();
  int size2 = forAssyVar2.size();

  for(ii=0; ii<size1; ii++)
  {
    row = forAssyVar1[ii];
    if( row != -1 )
    {
      rhsVec[row] += Fu(ii);

      for(jj=0; jj<size1; jj++)
      {
        col = forAssyVar1[jj];
        if( col != -1 )
          mtx.coeffRef(row, col) += Kuu(ii,jj);
      }

      for(jj=0; jj<size2; jj++)
      {
        col = forAssyVar2[jj];
        if( col != -1 )
        {
          col += var2_offset;
          mtx.coeffRef(row, col) += Kup(ii,jj);
        }
      }
    }
  }

  for(ii=0; ii<size2; ii++)
  {
    row = forAssyVar2[ii];
    if( row != -1 )
    {
      row += var2_offset;

      rhsVec[row] += Fp(ii);

      for(jj=0; jj<size1; jj++)
      {
        col = forAssyVar1[jj];
        if( col != -1 )
        {
          mtx.coeffRef(row, col) += Kpu(ii,jj);
        }
      }

      for(jj=0; jj<size2; jj++)
      {
        col = forAssyVar2[jj];
        if( col != -1 )
        {
          col += var2_offset;
          mtx.coeffRef(row, col) += Kpp(ii,jj);
        }
      }
    }
  }

  return 0;
}

int SolverEigen::assembleMatrixAndVectorMixed2(int var2_offset, vector<int>& forAssyVar1, vector<int>& forAssyVar2, MatrixXd& Kup, MatrixXd& Kpu, MatrixXd& Kpp, VectorXd& Fu, VectorXd& Fp)
{
  int ii, jj, row, col;

  int size1 = forAssyVar1.size();
  int size2 = forAssyVar2.size();

  for(ii=0; ii<size1; ii++)
  {
    row = forAssyVar1[ii];
    if( row != -1 )
    {
      rhsVec[row] += Fu(ii);

      for(jj=0; jj<size2; jj++)
      {
        col = forAssyVar2[jj];
        if( col != -1 )
        {
          //cout << ii << '\t' << row << '\t' << jj << '\t' << col << endl;
          matB.coeffRef(row, col) += Kup(ii,jj);
          //matC.coeffRef(col, row) += Kpu(jj,ii);
          //cout << "collllllll" << endl;
        }
      }
    }
  }

  for(ii=0; ii<size2; ii++)
  {
    row = forAssyVar2[ii];
    if( row != -1 )
    {
      rhsVec2[row] += Fp(ii);

      for(jj=0; jj<size1; jj++)
      {
        col = forAssyVar1[jj];
        if( col != -1 )
        {
          matC.coeffRef(row, col) += Kpu(ii,jj);
        }
      }

      for(jj=0; jj<size2; jj++)
      {
        col = forAssyVar2[jj];
        if( col != -1 )
        {
          //cout << ii << '\t' << row << '\t' << jj << '\t' << col << endl;
          matD.coeffRef(row, col) += Kpp(ii,jj);
          //cout << "rowwwwwwwww" << endl;
        }
      }

    }
  }

  return 0;
}

int SolverEigen::assembleVector(int start, int c1, vector<int>& vec1, VectorXd& Flocal)
{
  int ii, jj;

  for(ii=0;ii<vec1.size();ii++)
  {
    rhsVec[vec1[ii]] += Flocal(ii);
  }

  return 0;
}


// #include <iostream>
// #include <boost/random/mersenne_twister.hpp>
// #include <boost/random/uniform_int.hpp>
// #include <boost/random/uniform_real.hpp>
//
// #include "ParticleFilter.h"
//
// using std::cout;
// using std::endl;
//
// /// Randomly initialize all particles
// int ParticleFilter::init(int num_particles, double x_min, double x_max,
// 	      double y_min, double y_max,
// 	      double heading_min, double heading_max,
// 	      double velocity_min, double velocity_max)
// {
//      num_particles_ = num_particles;
//
//      x_min_ = x_min;
//      x_max_ = x_max;
//      y_min_ = y_min;
//      y_max_ = y_max;
//      heading_min_ = heading_min;
//      heading_max_ = heading_max;
//      velocity_min_ = velocity_min;
//      velocity_max_ = velocity_max;
//
//      boost::uniform_int<> x_dist(x_min_, x_max_);
//      boost::uniform_int<> y_dist(y_min_, y_max_);
//      boost::uniform_int<> heading_dist(heading_min_, heading_max_);
//      boost::uniform_int<> x_velocity_dist(velocity_min_, velocity_max_);
//      boost::uniform_int<> y_velocity_dist(velocity_min_, velocity_max_);
//
//      for (int i = 0 ; i < num_particles_ ; i++) {
// 	  double x=0, y=0, heading=0, x_velocity=0, y_velocity;
//
// 	  x = x_dist(gen_);
// 	  y = y_dist(gen_);
// 	  heading = heading_dist(gen_);
// 	  x_velocity = x_velocity_dist(gen_);
// 	  y_velocity = y_velocity_dist(gen_);
//
// 	  cv::Point center(x,y);
// 	  cv::Point2d velocity(x_velocity,y_velocity);
//
// 	  Particle particle(center,heading,velocity,1.0/num_particles_);
// 	  particles_.push_back(particle);
//      }
//
//      return 0;
// }
//
// /// Use motion model to update particles
// int ParticleFilter::kinematic_step()
// {
//      std::vector<Particle>::iterator it;
//      for (it = particles_.begin(); it != particles_.end(); it++) {
// 	  it->kinematic_step();
//      }
//      return 0;
// }
//
// int ParticleFilter::importance_weight_update(cv::Point target)
// {
//      double champ = -9999;
//      double wNorm = 0;
//      std::vector<Particle>::iterator it;
//      for (it = particles_.begin(); it != particles_.end(); it++) {
// 	  double w = it->importance_weight_update(target);
// 	  wNorm += w;
//
// 	  if (w > champ) {
// 	       champ = w;
// 	  }
//      }
//
//      wMax_ = champ;
//
//      for (it = particles_.begin(); it != particles_.end(); it++) {
// 	  it->normalize_importance_weight(wNorm);
//      }
//
//      return 0;
// }
//
// int ParticleFilter::importance_weight_update(cv::Point target, cv::Point2d velocity)
// {
//      double champ = -9999;
//      double wNorm = 0;
//      std::vector<Particle>::iterator it;
//      for (it = particles_.begin(); it != particles_.end(); it++) {
// 	  double w = it->importance_weight_update(target, velocity);
// 	  wNorm += w;
//
// 	  if (w > champ) {
// 	       champ = w;
// 	  }
//      }
//
//      wMax_ = champ;
//
//      for (it = particles_.begin(); it != particles_.end(); it++) {
// 	  it->normalize_importance_weight(wNorm);
//      }
//
//      return 0;
// }
//
//
//
// int ParticleFilter::resample_wheel()
// {
//      boost::uniform_int<> index_select(0, particles_.size()-1);
//      boost::uniform_real<> beta_select(0, 2*wMax_);
//
//      std::vector<Particle> newParticles;
//
//      int index = index_select(gen_);
//      double beta = 0;
//
//      //std::vector<Particle>::iterator it;
//      //for (it = particles_.begin(); it != particles_.end(); it++) {
//      for (int i = 0 ; i < num_particles_; i++) {
// 	  beta += beta_select(gen_);
// 	  while (beta > particles_[index].not_normalized_weight()) {
// 	       beta -= particles_[index].not_normalized_weight();
// 	       index = (index + 1) % particles_.size();
// 	  }
//
// 	  Particle part1 = particles_[index];
// 	  Particle part2 = part1;
//
// 	  boost::uniform_int<> x1_velocity_dist(velocity_min_, velocity_max_);
// 	  boost::uniform_int<> y1_velocity_dist(velocity_min_, velocity_max_);
//
// 	  boost::uniform_int<> x2_velocity_dist(velocity_min_, velocity_max_);
// 	  boost::uniform_int<> y2_velocity_dist(velocity_min_, velocity_max_);
//
// 	  cv::Point2d velocity1, velocity2;
// 	  velocity1.x = x1_velocity_dist(gen_);
// 	  velocity1.y = y1_velocity_dist(gen_);
//
// 	  velocity2.x = x2_velocity_dist(gen_);
// 	  velocity2.y = y2_velocity_dist(gen_);
//
// 	  part1.set_velocity(velocity1);
// 	  part2.set_velocity(velocity2);
//
// 	  newParticles.push_back(part1);
// 	  newParticles.push_back(part2);
//      }
//      particles_ = newParticles;
//      return 0;
// }
//
//
// int ParticleFilter::getParticles(std::vector<Particle> &particles)
// {
//      particles = particles_;
//      return 0;
// }


#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include <std_srvs/Empty.h>
#include <aquacore/StateMsg.h>
#include <dynamic_reconfigure/server.h>
#include <aqua_utils/DepthFilterConfig.h>
#include <aquacore/EmptyBool.h>
#include <boost/thread/mutex.hpp>
#include <list>
#include <algorithm>
#include <cmath>

typedef std::pair<ros::Time, float> EntryType;
typedef dynamic_reconfigure::Server<aqua_utils::DepthFilterConfig> ReconfigureServer;

/**
 * A re-implementation of depth_filter.py
 *
 * NOTE: cannot find easy way to implement waitTillConstantDepth in roscpp,
 *       since this service handler must be isolated from others, so that
 *       node does not block. This might be doable with multi-threaded spinners,
 *       or with service server that has a dedicated handler thread, but
 *       good luck figuring out kinks with the former or the syntax of the latter:
 *
 *       http://www.ros.org/wiki/roscpp/Overview/Callbacks%20and%20Spinning
 */
class DepthFilter {
public:
#ifdef ENABLE_WAIT_TILL_CONSTANT_DEPTH
  bool waitTillConstantDepth(std_srvs::Empty::Request& req,
      std_srvs::Empty::Response& res) {
    // WARNING: may likely block subsequent ROS callbacks!
    constant_depth_off_mutex.lock();
    constant_depth_off_mutex.unlock();
    return true;
  };
#endif

  bool hasReachedConstantDepth(aquacore::EmptyBool::Request& req,
      aquacore::EmptyBool::Response& res) {
    res.result = has_reached_constant_depth;
    has_reached_constant_depth = false;
    return true;
  };

  void dyncfgCB(aqua_utils::DepthFilterConfig& config, uint32_t level) {
    min_window_entries = config.min_window_entries;
    window_size_sec = ros::Duration(config.window_size_sec);
    cutoff_sigma_multiplier = config.cutoff_sigma_multiplier;
    N_sigma_cutoff_m = config.N_sigma_cutoff_m;
  };

  void stateCB(const aquacore::StateMsg::ConstPtr& data) {
    state_mutex.lock();

    EntryType curr_entry = std::make_pair(data->header.stamp, data->Depth);

    // Remove outdated entries from sliding window
    if (window.size() > 0) {
      std::list<EntryType>::iterator outdated_i = window.end();
      std::list<EntryType>::iterator i = window.begin();
      for (; i != window.end(); i++) {
        // Stop loop when found entry within sliding window
        if (curr_entry.first - i->first <= window_size_sec) {
          outdated_i = i;
          break;
        }
      }
      window.erase(window.begin(), outdated_i);
    }

    // Compute mean and standard deviation of depth values
    window.push_back(curr_entry);
    size_t num_entries = window.size();
    if ((int) num_entries > min_window_entries) {
      double sum = 0.0;
      double sum_sqrd = 0.0;
      for (std::list<EntryType>::iterator v = window.begin(); v != window.end(); v++) {
        sum += v->second;
        sum_sqrd += v->second * v->second;
      }
      double mean_depth = sum/num_entries;
      double stdev_depth = sqrt(sum_sqrd/num_entries - mean_depth*mean_depth);
      constant_depth = (stdev_depth * cutoff_sigma_multiplier < N_sigma_cutoff_m);
      //ROS_INFO("n: %d, u: %.4f, s: %.4f, c: %d", (int) num_entries, mean_depth, stdev_depth, constant_depth);

      // Publish mean depth (a.k.a. filtered depth)
      filtered_depth_msg.data = mean_depth;
      filtered_depth_pub.publish(filtered_depth_msg);

      // Publish flag indicating whether depth is constant or not
      constant_depth_msg.data = constant_depth;
      constant_depth_pub.publish(constant_depth_msg);

#ifdef ENABLE_WAIT_TILL_CONSTANT_DEPTH
      // Update constant-depth-OFF mutex
      if (!has_prev_constant_depth) {
        if (!constant_depth) {
          constant_depth_off_mutex.lock();
        }
      } else if (constant_depth && !prev_constant_depth) {
        constant_depth_off_mutex.unlock();
      } else if (!constant_depth && prev_constant_depth) {
        constant_depth_off_mutex.lock();
      }
      prev_constant_depth = constant_depth;
      has_prev_constant_depth = true;
#endif

      if (constant_depth) {
        has_reached_constant_depth = true;
      }
    } // if (num_entries > min_window_entries)

    state_mutex.unlock();
  };

  bool resetHandler(std_srvs::Empty::Request& req,
      std_srvs::Empty::Response& res) {
    reset();
    return true;
  };

  void reset() {
    state_mutex.lock();

    constant_depth = false;
#ifdef ENABLE_WAIT_TILL_CONSTANT_DEPTH
    prev_constant_depth = false;
    has_prev_constant_depth = false;
#endif
    has_reached_constant_depth = false;

    window.clear();

    state_mutex.unlock();
  };

  DepthFilter() : nh("~"),
      constant_depth(false),
#ifdef ENABLE_WAIT_TILL_CONSTANT_DEPTH
      prev_constant_depth(false), has_prev_constant_depth(false),
#endif
      has_reached_constant_depth(false) {
    double _window_size_sec;
    nh.param<int>("min_window_entries", min_window_entries, 4);
    nh.param<double>("window_size_sec", _window_size_sec, 2.0);
    nh.param<double>("cutoff_sigma_multiplier", cutoff_sigma_multiplier, 3);
    nh.param<double>("N_sigma_cutoff_m", N_sigma_cutoff_m, 0.15);
    window_size_sec = ros::Duration(_window_size_sec);

    filtered_depth_pub = nh.advertise<std_msgs::Float32>("/aqua/filtered_depth", 100);
    constant_depth_pub = nh.advertise<std_msgs::Bool>("/aqua/constant_depth_flag", 100);
    constant_depth_query_svc = nh.advertiseService(
        "/aqua/has_reached_constant_depth", &DepthFilter::hasReachedConstantDepth, this);
    state_sub = nh.subscribe("/aqua/state", 100, &DepthFilter::stateCB, this);

    reset_svc = nh.advertiseService("reset", &DepthFilter::resetHandler, this);

#ifdef ENABLE_WAIT_TILL_CONSTANT_DEPTH
    constant_depth_blocking_svc = nh.advertiseService(
        "/aqua/wait_till_constant_depth", &DepthFilter::waitTillConstantDepth, this);
#endif

    dyncfg_server = new ReconfigureServer(dyncfg_mutex, nh);
    dyncfg_server->setCallback(bind(&DepthFilter::dyncfgCB, this, _1, _2));

    reset();
  };

protected:
  ros::NodeHandle nh;

  bool constant_depth;
#ifdef ENABLE_WAIT_TILL_CONSTANT_DEPTH
  bool prev_constant_depth;
  bool has_prev_constant_depth;
#endif
  bool has_reached_constant_depth;

  int min_window_entries;
  ros::Duration window_size_sec;
  double cutoff_sigma_multiplier;
  double N_sigma_cutoff_m;

  std::list<EntryType> window;
#ifdef ENABLE_WAIT_TILL_CONSTANT_DEPTH
  boost::mutex constant_depth_off_mutex;
#endif

  boost::mutex state_mutex;

  ros::Publisher filtered_depth_pub;
  ros::Publisher constant_depth_pub;
  ros::ServiceServer reset_svc;
#ifdef ENABLE_WAIT_TILL_CONSTANT_DEPTH
  ros::ServiceServer constant_depth_blocking_svc;
#endif
  ros::ServiceServer constant_depth_query_svc;
  ros::Subscriber state_sub;

  std_msgs::Float32 filtered_depth_msg;
  std_msgs::Bool constant_depth_msg;

  ReconfigureServer* dyncfg_server;
  boost::recursive_mutex dyncfg_mutex;
};

int main(int argc, char** argv) {
  ros::init(argc, argv, "depth_filter");
  DepthFilter filter;
  ros::spin();
  return 0;
};


// Copyright (C) 2004-2006 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_RANDOM_LAYOUT_HPP
#define BOOST_GRAPH_RANDOM_LAYOUT_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/mpl/if.hpp>
#include <boost/graph/point_traits.hpp>

namespace boost {

template<typename Graph, typename PositionMap,
         typename RandomNumberGenerator>
void
random_graph_layout
 (const Graph& g, PositionMap position_map,
  typename property_traits<PositionMap>::value_type const& origin,
  typename property_traits<PositionMap>::value_type const& extent,
  RandomNumberGenerator& gen)
{
  typedef typename property_traits<PositionMap>::value_type Point;
  typedef typename graph::point_traits<Point>::component_type Dimension;

  typedef typename mpl::if_<is_integral<Dimension>,
                            uniform_int<Dimension>,
                            uniform_real<Dimension> >::type distrib_t;
  typedef typename mpl::if_<is_integral<Dimension>,
                            RandomNumberGenerator&,
                            uniform_01<RandomNumberGenerator, Dimension> >
    ::type gen_t;

  gen_t my_gen(gen);
  distrib_t x(origin[0], origin[0] + extent[0]);
  distrib_t y(origin[1], origin[1] + extent[1]);
  typename graph_traits<Graph>::vertex_iterator vi, vi_end;
  for(tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi) {
    position_map[*vi][0] = x(my_gen);
    position_map[*vi][1] = y(my_gen);
  }
}

} // end namespace boost

#endif // BOOST_GRAPH_RANDOM_LAYOUT_HPP


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

#ifndef YANNQ_ACTIVATIONLAYER_HH
#define YANNQ_ACTIVATIONLAYER_HH

#include <complex>
#include <fstream>
#include <random>
#include <vector>
#include <tuple>
#include <type_traits>

#include <Eigen/Dense>

#include "AbstractLayer.hpp"
#include "activations.hpp"

/**
 * Misc: Because of bugs in Intel C compiler (ICC), constexpr static member
 * variable of template class does not work. If there's any update in ICC
 * regarding this bug, one may update name function to return such a member
 * variable.
 * */

namespace yannq {

template <typename T, class Activation>
class ActivationLayer
	: public AbstractLayer<T>
{
public:
	using Scalar = T;
	using Vector = typename AbstractLayer<T>::Vector;
	using Matrix = typename AbstractLayer<T>::Matrix;
	using VectorRef = typename AbstractLayer<T>::VectorRef;
	using VectorConstRef = typename AbstractLayer<T>::VectorConstRef;

private:

	Activation f_;

public:
	ActivationLayer()
	{
	}

	template<typename ...Ts,
		typename disable_if<
            std::is_same<
                typename std::remove_reference<typename get_nth_type<0, Ts...>::type>::type,
                ActivationLayer<T, Activation>
			>::value, int>::type = 0>
	ActivationLayer(Ts&&... args)
		: f_(std::forward<Ts>(args)...)
	{
	}

	ActivationLayer(const ActivationLayer&) = default;
	ActivationLayer(ActivationLayer&&) = default;

	ActivationLayer& operator=(const ActivationLayer&) = default;
	ActivationLayer& operator=(ActivationLayer&&) = default;

	std::string name() const override
	{
		return std::string("Activation Layer");
	}

	bool operator==(const ActivationLayer& rhs) const
	{
		return f_.name() == rhs.f_.name();
	}

	uint32_t paramDim() const override { return 0; }

	Vector getParams() const override
	{
		return Vector{};
	}

	void setParams(VectorConstRef pars) override
	{
		(void)pars;
	}

	uint32_t outputDim(uint32_t inputDim) const override
	{
		return inputDim;
	}

	// Feedforward
	void forward(const VectorConstRef& input, VectorRef output) override
	{
		assert(input.size() == output.size());
		f_.operator()(input, output);
	}

	// Computes derivative.
	void backprop(const VectorConstRef& prev_layer_output,
			const VectorConstRef& this_layer_output,
			const VectorConstRef& dout,
			VectorRef din, VectorRef /*der*/) override
	{
		din.resize(prev_layer_output.size());
		f_.ApplyJacobian(prev_layer_output, this_layer_output, dout, din);
	}

	nlohmann::json desc() const override
	{
		nlohmann::json layerpar;
		layerpar["name"] = name();
		layerpar["activation"] = f_.name();

		return layerpar;
	}

	template<class Archive>
	void serialize(Archive& ar)
	{
		ar(f_);
	}
};

template<typename T>
using Identity = ActivationLayer<T, activation::Identity<T> >;

template<typename T>
using LnCosh = ActivationLayer<T, activation::LnCosh<T> >;

template<typename T>
using Tanh = ActivationLayer<T, activation::Tanh<T> >;

template<typename T>
using WeakTanh = ActivationLayer<T, activation::WeakTanh<T> >;

template<typename T>
using Sigmoid = ActivationLayer<T, activation::Sigmoid<T> >;

template<typename T>
using ReLU = ActivationLayer<T, activation::ReLU<T> >;

template<typename T>
using LeakyReLU = ActivationLayer<T, activation::LeakyReLU<T> >;

template<typename T>
using HardTanh = ActivationLayer<T, activation::HardTanh<T> >;

template<typename T>
using SoftShrink = ActivationLayer<T, activation::SoftShrink<T> >;

template<typename T>
using LeakyHardTanh = ActivationLayer<T, activation::LeakyHardTanh<T> >;

template<typename T>
using SoftSign = ActivationLayer<T, activation::SoftSign<T> >;

template<typename T>
using Cos = ActivationLayer<T, activation::Cos<T> >;

}  // namespace yannq

#endif


//////////////////////////////////////////////////////////////////
// (c) Copyright 2008-  by Jeongnim Kim
//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
//   National Center for Supercomputing Applications &
//   Materials Computation Center
//   University of Illinois, Urbana-Champaign
//   Urbana, IL 61801
//   e-mail: jnkim@ncsa.uiuc.edu
//
// Supported by
//   National Center for Supercomputing Applications, UIUC
//   Materials Computation Center, UIUC
//////////////////////////////////////////////////////////////////
// -*- C++ -*-
/**@file radfunc.cpp
 * @brief Implement a code to debug radial functors for Jastrow orbitals.
 *
 * Currently, the test uses pade and wm functors.
 */
#include <iostream>
#include <iomanip>
#include <vector>
#include <algorithm>
#include <cmath>
#include <string>
#include <boost/random.hpp>
using namespace std;
#include "Utilities/OhmmsInfo.h"
#include "Message/Communicate.h"
#include "Message/OpenMP.h"
#include "QMCWaveFunctions/Jastrow/WMFunctor.h"
#include "QMCWaveFunctions/Jastrow/DerivWMFunctor.h"
#include "QMCWaveFunctions/Jastrow/DerivPadeFunctors.h"
using namespace qmcplusplus;

template<typename FT, typename DFT>
struct RadFunctorTest
{
  FT&  u;
  DFT& ub;
  FT& up;
  FT& um;
  RadFunctorTest(FT& u_in, DFT& ub_in, FT& up_in, FT& um_in)
    : u(u_in), ub(ub_in), up(up_in), um(um_in) {}

  void run(double r, double delta)
  {
    double v=u.f(r);
    double dv=u.df(r);
    double ub_p=up.evaluate(r);
    double ub_m=um.evaluate(r);
    cout << "checking derivative functor ";
    cout <<  (ub_p-ub_m)/(2*delta)-ub.f(r) << endl;
    double du,d2udr2;
    cout << "checking Functor::evaulate ";
    double e=u.evaluate(r,du,d2udr2);
    double dv_p=u.f(r+delta);
    double dv_m=u.f(r-delta);
    cout << "  dudr = " << (dv_p-dv_m)/(2*delta)-du;
    cout << " d2udr2 = " << (dv_p+dv_m-2.0*v)/(delta*delta)-d2udr2 << endl;
    cout << "checking DFunctor::evaulate ";
    e=ub.evaluate(r,du,d2udr2);
    v=ub.f(r);
    dv=ub.df(r);
    dv_p=ub.f(r+delta);
    dv_m=ub.f(r-delta);
    cout << " dudr = " << (dv_p-dv_m)/(2*delta)-du;
    cout << " d2udr2 = " << (dv_p+dv_m-2.0*v)/(delta*delta)-d2udr2 << endl;
  }
};

int main(int argc, char** argv)
{
  //histograms<boost::mt19937>();
  //histograms<boost::lagged_fibonacci607>();
  OHMMS::Controller->initialize(argc,argv);
  OhmmsInfo Welcome(argc,argv,OHMMS::Controller->rank());
  const double delta=0.001;
  double rc=5.0;
  double r=3.5;
  double b=3.0;
  if(argc>3)
  {
    b=atof(argv[1]);
    r=atof(argv[2]);
    rc=atof(argv[3]);
  }
  else
  {
    cout << "Using default values" << endl;
    cout << "Usage : radfunc B distance cutoff-distance " << endl;
  }
  cout << "rc= " << rc << " distance= " << r << endl;
  cout << "Printing differences: small numbers are good." << endl;
  //test WMFunctors
  {
    typedef WMFunctor<double> RadFunctor;
    typedef DWMDBFunctor<double> DerivRadFunctor;
    RadFunctor u(b,rc);
    RadFunctor up(b+delta,rc);
    RadFunctor um(b-delta,rc);
    DerivRadFunctor ub(b,rc);
    cout << endl << "Testing WM functors " << endl;
    RadFunctorTest<RadFunctor,DerivRadFunctor> test(u,ub,up,um);
    test.run(r,delta);
  }
  //test PadeFunctors
  {
    typedef PadeFunctor<double> RadFunctor;
    typedef DPadeDBFunctor<double> DerivRadFunctor;
    RadFunctor u(-0.5,b);
    RadFunctor up(-0.5,b+delta);
    RadFunctor um(-0.5,b-delta);
    DerivRadFunctor ub(-0.5,b);
    cout << endl << "Testing pade functors " << endl;
    RadFunctorTest<RadFunctor,DerivRadFunctor> test(u,ub,up,um);
    test.run(r,delta);
  }
  OHMMS::Controller->finalize();
  return 0;
}

/***************************************************************************
 * $RCSfile$   $Author: jnkim $
 * $Revision: 1770 $   $Date: 2007-02-17 17:45:38 -0600 (Sat, 17 Feb 2007) $
 * $Id: OrbitalBase.h 1770 2007-02-17 23:45:38Z jnkim $
 ***************************************************************************/


#ifndef INCLUDE_SLAM_HPP
#define INCLUDE_SLAM_HPP

#include <iostream>
#include <iomanip>
#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <pangolin/pangolin.h>

#define MATRIX_SIZE 50

struct RotationMatrix {
    Eigen::Matrix3d matrix = Eigen::Matrix3d::Identity();
};

struct TranslationVector {
    Eigen::Vector3d trans = Eigen::Vector3d(0, 0, 0);
};

struct QuaternionDraw {
    Eigen::Quaterniond q;
};

std::ostream &operator<<(std::ostream &out, const RotationMatrix &r);
std::istream &operator>>(std::istream &in, RotationMatrix &r);

std::ostream &operator<<(std::ostream &out, const TranslationVector &t);
std::istream &operator>>(std::istream &in, TranslationVector &t);

std::ostream &operator<<(std::ostream &out, const QuaternionDraw quat);
std::istream &operator>>(std::istream &in, const QuaternionDraw quat);

void eigenMatrix();
void eigenGeometry();
void visualizeGeometry();

#endif // !