#include "Graph.h"
#include <iostream>
#include <fstream>
#include <sstream>

using namespace std;

double distance(Mesh* mesh1, Mesh* mesh2){
	//cout << "enter distance" << endl;
	double d = 0;
	for (unsigned int i = 0; i < mesh1->verts.size(); i++){

		float d0 = pow((mesh1->verts[i]->coords[0] - mesh2->verts[i]->coords[0]), 2);
		float d1 = pow((mesh1->verts[i]->coords[1] - mesh2->verts[i]->coords[1]), 2);
		float d2 = pow((mesh1->verts[i]->coords[2] - mesh2->verts[i]->coords[2]), 2);

		d += sqrt(d0 + d1 + d2);

	}
	d /= mesh1->verts.size();

	return d;
}

void CenterMesh(Mesh* mesh){ //parts kept as vector of vertice ids
	//compute center of mass
	float* m1 = new float[3]; //center of mass
	float* sum = new float[3]; //[0] is sum of x coords and so on
	for (int i = 0; i < 3; i++)
		sum[i] = 0.0f;
	for (int i = 0; i < mesh->verts.size(); i++)
		for (int c = 0; c < 3; c++)
			sum[c] += mesh->verts[i]->coords[c];
	for (int c = 0; c < 3; c++)
		m1[c] = sum[c] / (float)mesh->verts.size();
	//desired translation that moves shape center to (0,0,0)
	for (int i = 0; i < mesh->verts.size(); i++)
		for (int c = 0; c < 3; c++)
			mesh->verts[i]->coords[c] -= m1[c];
}

void Graph::center_all(){
	for (unsigned int i = 0; i < allmeshes.size(); i++) {
		CenterMesh(allmeshes[i]);
	}
}

void Graph::writeDists(string filename){
	cout << "writing " << filename << endl;
	std::ofstream out;

	out.open(filename, std::ios::app);
	if (out.is_open())
	{
		//get all pairs
		//for (unsigned int i = k; i < allmeshes.size(); i++) {
		for (unsigned int i = 0; i < allmeshes.size(); i++) {
			for (unsigned int j = i + 1; j < allmeshes.size(); j++) {


				//cout << "i j: " << i << " " << j << endl;
				//cout << "allmeshes size: " << allmeshes.size();
				float distbtw = distance(allmeshes[i], allmeshes[j]);

				out << i << " ";
				out << j << " ";
				out << distbtw << '\n';

				//out.flush();

			}
		}

	}
	out.close();

	cout << "done writing " << filename << endl;
}

void Graph::readMeshes(string foldername,int noofmeshes, string format){
	cout << "reading foldername: " << foldername << " format: " << format << endl;
	for (int i = 0; i < noofmeshes; i++){
		Mesh* currmesh = new Mesh();
		currmesh->readID(i, foldername, format);
		allmeshes.push_back(currmesh);
	}
	if (format == "dancer")
		this->center_all();
}

void Graph::initPath(string filename, int noofelems, int startNode, int endNode, int partindex){
	//cout << "creating graph from : " << filename << endl;

	fstream myfile(filename, std::ios_base::in);

	for (int i = 0; i < noofelems; i++){
		Node* currNode = new Node;
		currNode->meshId = i;
		currNode->prev = -1;
		currNode->currDist = FLT_MAX;
		currNode->isVisited = false;
		currNode->next = -1;

		nodes.push_back(currNode);
	}

	for (string line; getline(myfile, line);)   //read stream line by line
	{
		istringstream in(line);      //make a stream for the line itself

		int mesh1;
		int mesh2;
		double dist;
		in >> mesh1;
		in >> mesh2;
		int i = 0;
		while (i <= partindex){
			in >> dist;
			i++;
			//	cout << dist << endl;
		}
		//square the distance to part the vertices
		dist = (dist*dist);
		//	cout << "sq" << dist << endl;

		nodes[mesh1]->neighbors.push_back(mesh2);
		nodes[mesh1]->distances.push_back(dist);

		nodes[mesh2]->neighbors.push_back(mesh1);
		nodes[mesh2]->distances.push_back(dist);
	}
}

vector<Node*> Graph::createPath(string filename, int noofelems, int startNode, int endNode,int partindex){ //read from file and create graph structure that consists of Nodes
	//cout << "creating graph from : " << filename << endl;
	fstream myfile(filename, std::ios_base::in);

	vector<Node* > graph;
	for (int i = 0; i < noofelems; i++){
		Node* currNode = new Node;
		currNode->meshId = i;
		currNode->prev = -1;
		currNode->currDist = FLT_MAX;
		currNode->isVisited = false;
		currNode->next = -1;

		graph.push_back(currNode);
	}

	for (string line; getline(myfile, line);)   //read stream line by line
	{
		istringstream in(line);      //make a stream for the line itself

		int mesh1;
		int mesh2;
		double dist;
		in >> mesh1;
		in >> mesh2;
		int i = 0;
		while (i <= partindex){
			in >> dist;
			i++;
		//	cout << dist << endl;
		}
		//square the distance to part the vertices
		dist = (dist*dist);
	//	cout << "sq" << dist << endl;

		graph[mesh1]->neighbors.push_back(mesh2);
		graph[mesh1]->distances.push_back(dist);

		graph[mesh2]->neighbors.push_back(mesh1);
		graph[mesh2]->distances.push_back(dist);


	}
	//shortest path

	this->nodes = graph;
	shortestPath(graph, startNode);

	//cout << "created the path" << endl;

	vector<Node*> mypath = printPath(graph, startNode, endNode);


	//cout << "printed the path" << endl;

	//heuristic paths
	//heuristicPath(graph, startNode, endNode);
	//vector<Node*> mypath = printHeuristicPath(graph, startNode, endNode);
	return mypath;

}

void Graph::shortestPath(vector<Node*> graph, int startid){

	//cout << "starting shortest path" << endl;
	//dist from source to source
	priority_queue < Node*, vector<Node*>, mycomparison > myqueue;

	for (unsigned int i = 0; i < graph.size(); i++){

		myqueue.push(graph[i]);


	}
	graph[startid]->currDist = 0;

	myqueue.push(graph[startid]);
	//cout << "queue size: " << myqueue.size() << endl;
	while (!myqueue.empty()){
		Node* u = myqueue.top();
		myqueue.pop();
		//cout << "queue size: " << myqueue.size() << endl;

		//cout << "popped : " << u->meshId << endl;
		//cout << "neighbors size: " << u->neighbors.size() << endl;

		//for each neighbor of u
		for (unsigned int i = 0; i < u->neighbors.size(); i++){

			float newdistance = u->currDist + u->distances[i];
			//cout << "newdist: " << newdistance << " prev dist: " << graph[u->neighbors[i]]->currDist << endl;

			if (newdistance < graph[u->neighbors[i]]->currDist){

				graph[u->neighbors[i]]->currDist = newdistance;

				graph[u->neighbors[i]]->prev = u->meshId;

				//myqueue.push(graph[u->neighbors[i]]);
			}

		}
		//update queue
		std::make_heap(const_cast<Node**>(&myqueue.top()),
			const_cast<Node**>(&myqueue.top()) + myqueue.size(),
			mycomparison());
	}
	//cout << "end of shortest path" << endl;
}

vector<Node*> Graph::printPath(vector<Node*> graph, int startNode, int endNode){

	vector<Node*> path;
	path.push_back(graph[endNode]);

	//cout << "got my path now printing" << endl;
	Node* currNode = graph[endNode];
//	cout << "startnode: " << startNode << " endNode: " << endNode << endl;
	while (currNode->meshId != startNode){

		path.push_back(graph[currNode->prev]);

	//	cout << currNode->meshId << endl;
	//	cout << "dist: " << currNode->currDist << endl;
		currNode = graph[currNode->prev];

	}
	cout << currNode->meshId << endl;

	//cout << "mypath: " << endl;
	//for (unsigned int i = 0; i < path.size(); i++){
	//	cout << path[i]->meshId << " ";
	//}

	return path;
}


void CenterPart(vector<int> part, Mesh* mesh){ //parts kept as vector of vertice ids
	//compute center of mass
	float* m1 = new float[3]; //center of mass
	float* sum = new float[3]; //[0] is sum of x coords and so on
	for (int i = 0; i < 3; i++)
		sum[i] = 0.0f;
	for (int i = 0; i < part.size(); i++)
		for (int c = 0; c < 3; c++)
			sum[c] += mesh->verts[part[i]]->coords[c];
	for (int c = 0; c < 3; c++)
		m1[c] = sum[c] / (float)part.size();
	//desired translation that moves shape center to (0,0,0)
	for (int i = 0; i < part.size(); i++)
		for (int c = 0; c < 3; c++)
			mesh->verts[part[i]]->coords[c] -= m1[c];
}
double DistBtwParts(vector<int> part, Mesh* mesh1, Mesh* mesh2){ //parts kept as vector of vertice ids

	double d = 0;
	for (unsigned int i = 0; i < part.size(); i++){

		float d0 = pow((mesh1->verts[part[i]]->coords[0] - mesh2->verts[part[i]]->coords[0]), 2);
		float d1 = pow((mesh1->verts[part[i]]->coords[1] - mesh2->verts[part[i]]->coords[1]), 2);
		float d2 = pow((mesh1->verts[part[i]]->coords[2] - mesh2->verts[part[i]]->coords[2]), 2);

		d += sqrt(d0 + d1 + d2);
		//d += d0 + d1 + d2;
	}
	d /= part.size();
	return d;
}


void Graph::WritePartDists(string filename, vector<vector<int>>& part_verts){

	cout << "writing " << filename << endl;
	//got all meshes, got the part list, center given part and write its distance
	std::ofstream out;
	out.open(filename, std::ios::app);
	int noofparts = part_verts.size();

	//cout << "how many parts: "<<part_verts.size() << endl;
	if (out.is_open())
	{
		for (unsigned int i = 0; i < allmeshes.size(); i++) {
			for (unsigned int j = i + 1; j < allmeshes.size(); j++) {
				//cout << "i j " <<i <<" " << j << endl;
				out << i << " ";
				out << j << " ";

				for (int t = 0; t < noofparts; t++){
					//cout << "t: " << t << endl;
					CenterPart(part_verts[t], allmeshes[i]);
					CenterPart(part_verts[t], allmeshes[j]);

					double distbtw = DistBtwParts(part_verts[t], allmeshes[i], allmeshes[j]);
				//	distbtw = distbtw*distbtw;

				//	cout << distbtw << endl;

					out << distbtw << " ";	
				}

				out << '\n';
			//	out.flush();
			}
		}
	}

	out.close();
	cout << "done writing " << filename << endl;
}

