#pragma once
#include "../../stdafx.h"
#include "../glLibLocals.h"
#include "../glLibMath/glLibMathVec.h"

#include "../glLibObjects/glLibObject.h"
//class glLibObject;

template <typename type_edge> class glLibExtAlgGraphNode {
	friend class glLibExtAlgGraphSolver;
	private:
		type_edge _distance;
		bool _infinitely_far;
		glLibExtAlgGraphNode<type_edge>* _previous;
		map<glLibExtAlgGraphNode<type_edge>*,type_edge> _edges;
	public:
		string name;
		type_edge* position;
	public:
		glLibExtAlgGraphNode(string new_name="", type_edge* new_position=NULL, unsigned long position_size=0);
		~glLibExtAlgGraphNode();
		void connect(glLibExtAlgGraphNode<type_edge>* node, type_edge distance);
};
template <typename type_edge> class glLibExtAlgGraph {
	friend class glLibExtAlgGraphSolver;
	private:
		list<glLibExtAlgGraphNode<type_edge>*> _nodes;
		bool _must_delete;
	public:
		glLibExtAlgGraph();
		glLibExtAlgGraph(glLibObject* object);
		~glLibExtAlgGraph();
		glLibExtAlgGraphNode<type_edge>* get_node_index(unsigned long target_index);
		void add_node(glLibExtAlgGraphNode<type_edge>* new_node);
};

class glLibExtAlgGraphSolver {
	private:
		glLibExtFlagAlg _algorithm_type;
	public:
		glLibExtAlgGraphSolver(glLibExtFlagAlg algorithm_type);
		template <typename type_edge> bool solve(glLibExtAlgGraph<type_edge>* graph, unsigned long index_node_start, unsigned long index_node_end, list<glLibExtAlgGraphNode<type_edge>*>* path) {
			glLibExtAlgGraphNode<type_edge>* node_start = graph->get_node_index(index_node_start);
			glLibExtAlgGraphNode<type_edge>*   node_end = graph->get_node_index(  index_node_end);
			return solve(graph, node_start, node_end, path);
		}
		template <typename type_edge> bool solve(glLibExtAlgGraph<type_edge>* graph, glLibExtAlgGraphNode<type_edge>* node_start, glLibExtAlgGraphNode<type_edge>* node_end, list<glLibExtAlgGraphNode<type_edge>*>* path) {
			if (_algorithm_type==GLLIBEXT_GRAPH_SOLVER_DIJKSTRA) {
				typedef typename list<glLibExtAlgGraphNode<type_edge>*>::const_iterator type_node_citer;
				typedef typename list<glLibExtAlgGraphNode<type_edge>*>::iterator        type_node_iter;
				type_node_citer const_vert_iter;
				type_node_iter        vert_iter;

				glLibExtAlgGraphNode<type_edge> *current_node, *u, *v;

				list<glLibExtAlgGraphNode<type_edge>*> Q;
				for (const_vert_iter=graph->_nodes.begin(); const_vert_iter!=graph->_nodes.end(); ++const_vert_iter) {
					current_node = *const_vert_iter;
					if (current_node==node_start) {
						current_node->_infinitely_far = false;
						current_node->_distance = 0;
					}
					else {
						current_node->_infinitely_far = true;
					}
					current_node->_previous = NULL;
					Q.push_back(current_node);
				}

				bool found = false;
				while (Q.size()>0) {
					//Find the node with the shortest distance u;
					u = NULL;
					for (const_vert_iter=Q.begin(); const_vert_iter!=Q.end(); ++const_vert_iter) {
						current_node = *const_vert_iter;
						if (current_node->_infinitely_far) continue;
						if ( (u==NULL) || (current_node->_distance<u->_distance) ) {
							u = current_node;
						}
					}

					//If we can't get anywhere, return false.
					if (u==NULL) {
						//printf("Stuck!\n");
						break;
					}
					//If we're done, return true.
					if (u==node_end) {
						//printf("Reached end!\n");
						found = true;
						break;
					}
					//printf("Nearest is %s\n",u->name.c_str());

					//Remove u from Q;
					for (vert_iter=Q.begin(); vert_iter!=Q.end(); ++vert_iter) {
						current_node = *vert_iter;
						if (current_node==u) {
							Q.remove(current_node);
							break;
						}
					}

					//Relax
					typedef typename map<glLibExtAlgGraphNode<type_edge>*,type_edge>::const_iterator type_map_citer;
					type_map_citer edge_iter;
					for (edge_iter=u->_edges.begin(); edge_iter!=u->_edges.end(); ++edge_iter) {
						v = (*edge_iter).first;
						//printf("  %s is connected to %s\n",u->name.c_str(),v->name.c_str());
						type_edge alt = u->_distance + u->_edges[v];
						if ( (v->_infinitely_far) || (alt<v->_distance) ) {
							v->_infinitely_far = false;
							v->_distance = alt;
							v->_previous = u;
						}
					}
				}

				if (found) {
					//We reached the goal.  Finalize the path and return.
					path->clear();
					u = node_end;
					while (u!=NULL) {
						path->push_front(u);
						u = u->_previous;
					}
				}

				return found;
			}
			return false;
		}
};

template <typename type_edge> glLibExtAlgGraphNode<type_edge>::glLibExtAlgGraphNode(string new_name, type_edge* new_position, unsigned long position_size) {
	name = new_name;
	#ifdef GLLIB_ERROR_MODE_REPORT_ERRORS
	if ((new_position==NULL)&&(position_size!=0)) {
		throw glLibError("If a position is not specified, then the position's size must be zero!");
	}
	if ((new_position!=NULL)&&(position_size==0)) {
		throw glLibError("If a position is specified, then the position's size must not be zero!");
	}
	#endif
	if (position_size!=0) {
		position = new type_edge[position_size];
		for (unsigned long i=0;i<position_size;++i) {
			position[i] = new_position[i];
		}
	}
	else {
		position = NULL;
	}
}
template <typename type_edge> glLibExtAlgGraphNode<type_edge>::~glLibExtAlgGraphNode() {
	if (position!=NULL) delete [] position;
}
template <typename type_edge> void glLibExtAlgGraphNode<type_edge>::connect(glLibExtAlgGraphNode<type_edge>* node, type_edge distance) {
	_edges[node] = distance;
	node->_edges[this] = distance;
	//TODO: check if the edge already exists and throw exception if so
}

template <typename type_edge> glLibExtAlgGraph<type_edge>::glLibExtAlgGraph() {
	_must_delete = false;
}
template <typename type_edge> glLibExtAlgGraph<type_edge>::glLibExtAlgGraph(glLibObject* object) {
	_must_delete = true;

	map<GLuint,glLibExtAlgGraphNode<type_edge>*> object_nodes;
	for (GLuint i=0;i<object->numof_vertices;++i) {
		type_edge position[3];
		position[0] = object->array_vertices[3*i  ];
		position[1] = object->array_vertices[3*i+1];
		position[2] = object->array_vertices[3*i+2];
		glLibExtAlgGraphNode<type_edge>* node = new glLibExtAlgGraphNode<type_edge>("node"+to_string(i),position,3);
		add_node(node);
		object_nodes[i] = node;
	}

	for (unsigned long mesh_index=0;mesh_index<object->numof_meshes;++mesh_index) {
		glLibInternal_mesh* mesh = object->meshes[mesh_index];
		unsigned long vpf = mesh->verts_per_face;
		GLuint* face = new GLuint[vpf];

		unsigned long num_vertices = mesh->numof_indices;
		unsigned long face_counter = 0;
		for (unsigned long i=0; i<num_vertices; ++i) {
			face[face_counter++] = mesh->vindices[i];
			if (face_counter==vpf) {
				glLibExtAlgGraphNode<type_edge> *node1, *node2;
				type_edge delta_vec[3];

				for (face_counter=0;face_counter<vpf-1;++face_counter) {
					node1 = object_nodes[face[face_counter]];
					node2 = object_nodes[face[face_counter+1]];
					glLibMathVec::subtract3(node1->position,node2->position,delta_vec);
					node1->connect(node2,(type_edge)glLibMathVec::length3(delta_vec));
				}
				node1 = object_nodes[face[face_counter]];
				node2 = object_nodes[face[0]];
				glLibMathVec::subtract3(node1->position,node2->position,delta_vec);
				node1->connect(node2,(type_edge)glLibMathVec::length3(delta_vec));

				face_counter = 0;
			}
		}

		delete [] face;
	}
}
template <typename type_edge> glLibExtAlgGraph<type_edge>::~glLibExtAlgGraph() {
	if (_must_delete) {
		typedef typename list<glLibExtAlgGraphNode<type_edge>*>::const_iterator type_node_citer;
		type_node_citer iter;
		for (iter=_nodes.begin(); iter!=_nodes.end(); ++iter) {
			delete (*iter);
		}
	}
}
template <typename type_edge> glLibExtAlgGraphNode<type_edge>* glLibExtAlgGraph<type_edge>::get_node_index(unsigned long target_index) {
	typedef typename list<glLibExtAlgGraphNode<type_edge>*>::const_iterator type_node_citer;
	type_node_citer iter;
	unsigned long index = 0;
	for (iter=_nodes.begin(); iter!=_nodes.end(); ++iter) {
		if (index==target_index) return *iter;
		index++;
	}
	#ifdef GLLIB_ERROR_MODE_REPORT_ERRORS
	throw glLibError("Node index "+to_string(target_index)+" was not found in the graph!");
	#endif
	return NULL;
}
template <typename type_edge> void glLibExtAlgGraph<type_edge>::add_node(glLibExtAlgGraphNode<type_edge>* new_node) {
	_nodes.push_back(new_node);
}