/*
 * airwaysNode.cxx
 *
 *  Created on: May 12, 2014
 *  Author: Diego Cáceres
 *
 *  Modified by: Alfredo Morales Pinzón
 */

#include <iostream>

#include "airwaysNode.h"

namespace airways
{

Node::Node() : m_id(0), m_children(), m_edge(NULL), m_coords(0, 0, 0), m_level(-1), m_mark(false)
{
}

Node::Node(const Vec3& coord) : m_id(0), m_children(), m_edge(NULL), m_level(-1), m_mark(false)
{
	this->m_coords = coord;
	this->m_level = 0;
}

Node::Node(Node* node) : m_id(0), m_children(), m_mark(false)
{
	this->m_coords = node->m_coords;
	this->m_level = node->m_level;
	this->m_edge = new Edge(node->m_edge);
}

Node::~Node()
{
}

//Getters

const std::vector<Node*>& Node::GetChildren() const
{
	return this->m_children;
}

const unsigned int Node::GetId() const
{
	return this->m_id;
}

const Vec3& Node::GetCoords() const
{
	return this->m_coords;
}

Edge* Node::GetEdge() const
{
	return this->m_edge;
}

Node* Node::GetFather() const
{
	return this->father;
}

unsigned int Node::GetLevel() const
{
	return this->m_level;
}

int Node::GetDepthById(unsigned int idNode) const
{
	if( this->m_id == idNode)
		return 0;

	vec_nodes::const_iterator it = this->m_children.begin();
	for( ; it != this->m_children.end(); ++it)
	{
		if((*it)->HasNodeId(idNode))
			return 1 + (*it)->GetDepthById(idNode);
	}

	std::cout << "GetDepthById - The idNode: " << idNode <<  "does not exist" << std::endl;

	return -1;
}

unsigned int Node::GetWeight() const
{
	if(this->IsLeaf())
		return 1;

	unsigned int weight = 0;

	vec_nodes::const_iterator it = this->m_children.begin();
	for( ; it != this->m_children.end(); ++it)
		weight += (*it)->GetWeight();

	return 1 + weight;
}

unsigned int Node::GetHeight() const
{
	unsigned int heightChildren = 0;

	vec_nodes::const_iterator it=this->m_children.begin();
	for( ; it != this->m_children.end(); ++it)
	{
		unsigned int actualHeight = (*it)->GetHeight();
		if(actualHeight > heightChildren)
			heightChildren = actualHeight;
	}

	// Return my heigh plus my children height
	return 1 + heightChildren;
}

unsigned int Node::GetNumberOfChildren() const
{
	return this->m_children.size();
}

unsigned int Node::GetNumberOfNonMarkedChildren( unsigned int depth ) const
{
	if(depth > 0)
	{
		unsigned int nonMarked = 0;

		for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
		{
			if( !(*it)->m_mark )
				++nonMarked;
			nonMarked += (*it)->GetNumberOfNonMarkedChildren( depth - 1 );
		}
		return nonMarked;
	}
	return 0;
}

unsigned int Node::GetNumberOfLeafs() const
{
	if(this->IsLeaf())
		return 1;

	unsigned int leafs = 0;
	vec_nodes::const_iterator it = this->m_children.begin();
	for( ; it != this->m_children.end(); ++it)
		leafs += (*it)->GetNumberOfLeafs();

	return leafs;
}

Node* Node::GetNodeById(unsigned int nId)
{
	// Base case
	if( m_id == nId)
		return this;

	Node* node_searched = NULL;

	// Search in the children
	vec_nodes::iterator it_children = m_children.begin();
	for( ; it_children != m_children.end() && node_searched == NULL; ++it_children)
		node_searched = (*it_children)->GetNodeById(nId);

	return node_searched;
}

Node* Node::GetClosestBranchNodeToPoint(float point_x, float point_y, float point_z, float &distance)
{
	// Get the actual distance
	distance = GetBranchDistanceToPoint(point_x, point_y, point_z);

	// Make as closest "this" node
	Node* node_closest = this;

	// Iterate over the children
	vec_nodes::iterator it_children = m_children.begin();
	for( ; it_children != m_children.end(); ++it_children)
	{
		// Get actual child distance
		float distance_actual = 0;
		Node* bestNodeChild = (*it_children)->GetClosestBranchNodeToPoint(point_x, point_y, point_z, distance_actual);

		// If the child distance is smaller then switch
		if(distance_actual < distance)
		{
			node_closest = bestNodeChild;
			distance = distance_actual;
		}
	}

	return node_closest;
}

Node* Node::GetClosestNodeToPoint(Vec3 position, double& distance)
{
	// Calculate actual distance to point
	Vec3 vector_diff = position - this->m_coords;
	distance = vector_diff.Norm();

	if(this->IsLeaf())
		return this;

	// Variables
	double actualDistance = -1;
	Node* node_Best = this;
	Node* node_Actual = NULL;

	// Check the children and get the best
	for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
	{
		node_Actual = (*it)->GetClosestNodeToPoint(position, actualDistance);

		if(actualDistance < distance)
		{
			node_Best = node_Actual;
			distance = actualDistance;
		}
	}

	if(actualDistance == -1)
		std::cout << "GetClosestNodeToPoint NOT FOUND!" << std::endl;

	return node_Best;
}

float Node::GetBranchDistanceToPoint(int voxel_x, int voxel_y, int voxel_z)
{
	Vec3 voxel(voxel_x,voxel_y, voxel_z);
	if(this->m_edge != NULL)
		return this->m_edge->GetSmallestDistanceToPoint(voxel);
	else
	{
		Vec3 vector_diff = this->m_coords - voxel;
		return vector_diff.Norm();
	}
}

bool Node::HasMinimumLevels(int levels)
{
	if(levels == 1)
		return true;

	bool minimum = false;

	vec_nodes::iterator it=this->m_children.begin();
	for( ; it != this->m_children.end() && !minimum; ++it)
		if((*it)->HasMinimumLevels(levels-1))
			minimum = true;

	return minimum;
}

bool Node::IsLeaf() const
{
	return this->m_children.empty();
}

bool Node::IsMarked() const
{
	return this->m_mark;
}

bool Node::HasMarkedChildren() const
{
	if(this->IsLeaf())
		return false;

	// Check the children
	vec_nodes::const_iterator it = this->m_children.begin();
	for( ; it != this->m_children.end(); ++it)
		if( (*it)->IsMarked() || (*it)->HasMarkedChildren())
			return true;

	return false;
}

bool Node::HasNode(Node* node_searched) const
{
	if(this->m_id == node_searched->m_id)
		return true;

	bool found = false;

	vec_nodes::const_iterator it = this->m_children.begin();
	for( ; it != this->m_children.end() && !found; ++it)
		if( (*it)->HasNode(node_searched) )
			found = true;

	return found;
}

bool Node::HasNodeId(unsigned int id_node_searched) const
{
	// If this is the node then return true
	if(this->m_id == id_node_searched)
		return true;

	// Search in the children
	bool found = false;
	vec_nodes::const_iterator it = this->m_children.begin();
	for( ; it != this->m_children.end() && !found; ++it)
		if( (*it)->HasNodeId(id_node_searched) )
			found = true;

	return found;
}

bool Node::IsPointInfluencedByNode(float point_x, float point_y, float point_z) const
{
	if(this->m_edge)
		return m_edge->IsPointInfluencedByEdge(point_x, point_y, point_z);

	return false;
}

void Node::GetBrancheNodesInfluencingPoint(float point_x, float point_y, float point_z, vec_nodes& vec_nodes_influence)
{
	if(this->IsPointInfluencedByNode(point_x, point_y, point_z))
		vec_nodes_influence.push_back(this);

	vec_nodes::const_iterator it = this->m_children.begin();
	for( ; it != this->m_children.end(); ++it)
		(*it)->GetBrancheNodesInfluencingPoint(point_x, point_y, point_z, vec_nodes_influence);
}

void Node::GetChildrenUptoDepth(unsigned int Q, vec_nodes& fathers)
{
	if(Q > 0)
	{
		vec_nodes::const_iterator it = this->m_children.begin();
		for( ; it != this->m_children.end(); ++it)
		{
			fathers.push_back((*it));
			(*it)->GetChildrenUptoDepth(Q-1, fathers);
		}
		/*// Add this node
		fathers.push_back(this);

		// If the depth is greater that 1 then add the children of this node
		if(Q > 1)
		{
			vec_nodes::const_iterator it = this->m_children.begin();
			for( ; it != this->m_children.end(); ++it)
				(*it)->GetChildrenUptoDepth(Q-1, fathers);
		}*/
	}
}

void Node::CompareSubTreesOrkiszMorales(Node* nodeB, unsigned int Q, unsigned int F, vec_nodes& commonA, vec_nodes& commonB, vec_nodes& nonCommonA, vec_nodes& nonCommonB, map_id_vecnodes& map_A_to_B, map_id_vecnodes& map_B_to_A, std::vector< std::pair< pair_nodes, pair_nodes > >& vector_pair_edges)
{
	//std::cout << "---------------------------------------------------------" << std::endl;
	//std::cout << "Comparing [A,B]: " << this->m_id << "," << nodeB->m_id << std::endl;

	// Variables
	double distanceAtoB = 0;
	double distanceBtoA = 0;
	Node* node_bestA = NULL;
	Node* node_bestB = NULL;

	// Algorithm
	// The algorithm is recursive it is implemented the node class. The idea is to overlap the initial nodes of the subtrees
	// and find the best branch correspondence for the first level of the trees.
	// Hypothesis: It is supposed that the roots of the trees are common, so the algorithm does not run in the root nodes
	// 1. Translate one of the subtree so that the initial nodes overlap.
	// 2. While one of the trees has non-marked child
	//   2.1 Compare each child to all the branches is the other tree.
	//   2.2 Select the pair, child and branch in the other tree, that has the lowest distance function.
	//   2.3 Add the pair and the distance to the final result.
	//   2.4 Mark the nodes
	//   2.5 Find and mark the omitted nodes in the similar branch. If the selected branch has more that 2 nodes it means that intermediary nodes do not have correspondence.
	//   2.6 Run the process (First step) using the found pair of nodes.

	//1. Translate one of the subtree so that the initial nodes overlap.
	// The translation must be applied later

	Vec3 vector_trans_B_to_A = this->m_coords - nodeB->m_coords;

	// ------------------------
	// ------------------------
	// Making the matching faster
	std::multimap<double, pair_nodes> map_dist_pairNodes_A_to_B = this->CalculateDistanceToAllBranches_FatherAndFamily(nodeB, Q, F);
	std::multimap<double, pair_nodes> map_dist_pairNodes_B_to_A = nodeB->CalculateDistanceToAllBranches_FatherAndFamily(this, Q, F);

	// ------------------------
	// ------------------------

	while( this->GetNumberOfNonMarkedChildren( Q ) > 0 && nodeB->GetNumberOfNonMarkedChildren( Q ) > 0 )
	{
		//   2.1 Compare each child to all the branches is the other tree.

		//     2.1.1 Get best translated branch from A to B
		//std::pair<pair_nodes,double> pair_A_to_B = this->GetClosestBranch_FatherAndFamily(nodeB);
		//     2.1.2 Get best translated branch from B to A
		//std::pair<pair_nodes,double> pair_B_to_A = nodeB->GetClosestBranch_FatherAndFamily(this);

		// ------------------------
		// ------------------------
		// Making the matching faster
		// Get the next best pair
		std::pair<double,pair_nodes> pair_A_to_B = GetPairWithClosestDistance_FirstNodeNonMarked(map_dist_pairNodes_A_to_B);
		std::pair<double,pair_nodes> pair_B_to_A = GetPairWithClosestDistance_FirstNodeNonMarked(map_dist_pairNodes_B_to_A);

		if( ! pair_A_to_B.second.first || ! pair_A_to_B.second.second )
			std::cout << "There is no closest-non-marked branch!" << std::endl;

		distanceAtoB = pair_A_to_B.first;
		distanceBtoA = pair_B_to_A.first;

		//std::map<double, pair_nodes> map_dist_pairNodes_A_to_B = this->CalculateDistanceToAllBranches_FatherAndFamily(nodeB);
		//std::map<double, pair_nodes> map_dist_pairNodes_B_to_A = nodeB->CalculateDistanceToAllBranches_FatherAndFamily(this);
		// ------------------------
		// ------------------------


		// Assign the distances
		//distanceAtoB = pair_A_to_B.second;
		//distanceBtoA = pair_B_to_A.second;

		//   2.2 Select the pair, child and branch in the other tree, that has the lowest distance function.
		if(distanceAtoB < distanceBtoA)
		{
			//node_bestA = pair_A_to_B.first.first;
			//node_bestB = pair_A_to_B.first.second;
			node_bestA = pair_A_to_B.second.first;
			node_bestB = pair_A_to_B.second.second;

			//std::cout << "Best Pair [A',B]: " << node_bestA->m_id << "," << node_bestB->m_id << std::endl;

			// 2.2.1 Check that there are no topological problems before adding the pair
			// Search all the nodes in the tree A linked to the found best node in B
			vec_nodes nodesA_linkedToBestB = map_B_to_A[node_bestB->m_id];

			// Look for topological problems
			bool topo_problem = false;
			for(vec_nodes::iterator it = nodesA_linkedToBestB.begin(); it != nodesA_linkedToBestB.end() && !topo_problem; ++it)
			{
				if( ! ( (*it)->HasNode( node_bestA ) || node_bestA->HasNode( (*it) ) ) )
				{
					topo_problem = true;
					//std::cout << "Topological problem adding node:" << node_bestA->m_id << std::endl;
				}
			}

			if(! topo_problem)
			{
				// 2.3 Add the pair and the distance to the final result.
				commonA.push_back(node_bestA);
				commonB.push_back(node_bestB);
				map_A_to_B[node_bestA->m_id].push_back(node_bestB);
				map_B_to_A[node_bestB->m_id].push_back(node_bestA);

				// Add the edge link
				pair_nodes pair_edgeA(this, node_bestA);
				pair_nodes pair_edgeB(nodeB, node_bestB);
				std::pair< pair_nodes, pair_nodes > pair_edge(pair_edgeA, pair_edgeB);
				vector_pair_edges.push_back(pair_edge);


				// 2.4 Mark the nodes
				node_bestA->Mark();
				node_bestB->Mark();

				// 2.5 Find and mark the omitted nodes in the similar branch. If the selected branch has more that 2 nodes it means that intermediary nodes do not have correspondence.
				Node* node_nextA = this->GetNextNodeInPathToNode(node_bestA);
				//bool markedA = node_nextA->MarkNodesUntilNode(node_bestA);
				//if(!markedA)
				//	std::cout << "NodeA not found for marking [A]:" << node_bestA->m_id << std::endl;

				Node* node_nextB = nodeB->GetNextNodeInPathToNode(node_bestB);
				//bool markedB = node_nextB->MarkNodesUntilNode(node_bestB);
				//if(!markedB)
				//	std::cout << "NodeB not found for marking [B]:" << node_bestB->m_id << std::endl;

				// 2.6 Run the process (First step) using the found pair of nodes.
				node_bestA->CompareSubTreesOrkiszMorales(node_bestB, Q, F, commonA, commonB, nonCommonA, nonCommonB, map_A_to_B, map_B_to_A, vector_pair_edges);
			}
			else
			{
				// 2.4 Mark the wrong node
				node_bestA->Mark();
				nonCommonA.push_back(node_bestA);
			}
		}
		else
		{
			//node_bestA = pair_B_to_A.first.second;
			//node_bestB = pair_B_to_A.first.first;
			node_bestA = pair_B_to_A.second.second;
			node_bestB = pair_B_to_A.second.first;

			//std::cout << "Best Pair [A,B']: " << node_bestA->m_id << "," << node_bestB->m_id << std::endl;

			// 2.29 Check that there are no topological problems before adding the pair
			// 2.291 Search all the nodes in the A tree linked to the found best node in B
			vec_nodes nodesB_linkedToBestA = map_A_to_B[node_bestA->m_id];

			// 2.292 Look for topological problems
			bool topo_problem = false;
			for(vec_nodes::iterator it = nodesB_linkedToBestA.begin(); it != nodesB_linkedToBestA.end() && !topo_problem; ++it)
			{
				if( ! ( (*it)->HasNode(node_bestB) || node_bestB->HasNode( (*it) ) ) )
				{
					topo_problem = true;
					//std::cout << "Topological problem adding node:" << node_bestB->m_id << std::endl;
				}
			}

			if(! topo_problem)
			{
				// 2.3 Add the pair and the distance to the final result.
				commonA.push_back(node_bestA);
				commonB.push_back(node_bestB);
				map_A_to_B[node_bestA->m_id].push_back(node_bestB);
				map_B_to_A[node_bestB->m_id].push_back(node_bestA);

				// Add the edge link
				pair_nodes pair_edgeA(this, node_bestA);
				pair_nodes pair_edgeB(nodeB, node_bestB);
				std::pair< pair_nodes, pair_nodes > pair_edge(pair_edgeA, pair_edgeB);
				vector_pair_edges.push_back(pair_edge);

				// 2.4 Mark the nodes
				node_bestA->Mark();
				node_bestB->Mark();

				// 2.5 Find and mark the omitted nodes in the similar branch. If the selected branch has more that 2 nodes it means that intermediary nodes do not have correspondence.
				Node* node_nextA = this->GetNextNodeInPathToNode(node_bestA);
				//bool markedA = node_nextA->MarkNodesUntilNode(node_bestA);
				//if(!markedA)
				//	std::cout << "NodeA not found for marking [A]:" << node_bestA->m_id << std::endl;

				Node* node_nextB = nodeB->GetNextNodeInPathToNode(node_bestB);
				//bool markedB = node_nextB->MarkNodesUntilNode(node_bestB);
				//if(!markedB)
				//	std::cout << "NodeB not found for marking [B]:" << node_bestB->m_id << std::endl;

				// 2.6 Run the process (First step) using the found pair of nodes.
				//node_bestB->CompareSubTreesOrkiszMorales(node_bestA, commonB, commonA, nonCommonB, nonCommonA, map_B_to_A, map_A_to_B, vector_pair_edges);
				node_bestA->CompareSubTreesOrkiszMorales(node_bestB, Q, F, commonA, commonB, nonCommonA, nonCommonB, map_A_to_B, map_B_to_A, vector_pair_edges);
			}
			else
			{
				// 2.4 Mark the wrong node
				node_bestB->Mark();
				nonCommonB.push_back(node_bestB);
			}
		}

	}
	//std::cout << "OUT Comparing [A,B]: " << this->m_id << "," << nodeB->m_id << std::endl;
	//std::cout << "---------------------------------------------------------" << std::endl;
}

std::pair< std::pair<Node*, Node*>, double> Node::GetBestBranches_ActualTranslatedOneLevelBranches_To_AllPossibleBranches(Node* nodeB)
{
	// Variables
	double distance = -1;
	double distance_bestFinal = -1;
	Node* node_A_bestFinal = NULL;
	Node* node_B_bestFinal = NULL;
	bool first = true;

	// Get the translation vector for root ovelapping
	Vec3 vector_trans_A_to_B = nodeB->m_coords - this->m_coords;

	// Iterate over my children
	for(vec_nodes::iterator it_A = this->m_children.begin(); it_A != this->m_children.end(); ++it_A)
	{
		if( !(*it_A)->IsMarked() )
		{
			// Get the position of the actual child of this node
			Vec3 position_actual = (*it_A)->m_coords;

			// Translate the position
			Vec3 position_translated = position_actual + vector_trans_A_to_B;

			// Find the "closest" node in the subtree nodeB using a distance function
			// a. Distance to ending point
			//Node* bestNode_actual = nodeB->GetClosestRelativeToPoint(position_translated, distance);
			// b. Distance point to point
			Node* bestNode_actual = nodeB->GetClosestRelativeToBranch( (*it_A)->m_edge, vector_trans_A_to_B, distance);

			if(first)
			{
				first = false;
				node_A_bestFinal = (*it_A);
				node_B_bestFinal = bestNode_actual;
				distance_bestFinal = distance;
			}
			else if(distance < distance_bestFinal)
			{
				node_A_bestFinal = (*it_A);
				node_B_bestFinal = bestNode_actual;
				distance_bestFinal = distance;
			}
		}
	}

	if(first)
		std::cout << "GetBestTranslatedBranchFromOtherTree NOT FOUND!" << std::endl;

	pair_nodes pair_best(node_A_bestFinal, node_B_bestFinal);
	std::pair<pair_nodes,double> best_pair_score(pair_best, distance_bestFinal);
	return best_pair_score;
}

std::multimap< double, std::pair<Node*, Node*> > Node::CalculateDistanceToAllBranches_FatherAndFamily(Node* node_gradfather, unsigned int Q, unsigned int F)
{
	// "this" is a grandfather node
	// Variables
	std::multimap< double, pair_nodes > map_dist_pairNodeNode;
	double distance_actual = -1;

	// Get the translation vector for root overlapping
	Vec3 vector_trans_A_to_B_grandfathers = node_gradfather->m_coords - this->m_coords;

	// Get all the possible "fathers" based on Q
	vec_nodes fathers;
	this->GetChildrenUptoDepth(Q, fathers);
	//fathers = this->m_children;

	// Iterate over my children which actually are fathers

	//vec_nodes::iterator it_father = this->m_children.begin();
	vec_nodes::iterator it_father = fathers.begin();
	//for( ; it_father != this->m_children.end(); ++it_father)
	for( ; it_father != fathers.end(); ++it_father)
	{
		if( ! (*it_father)->IsMarked() )
		{

			Node* node_actualBest = node_gradfather->GetClosestRelativeFatherAndFamily( this, (*it_father), F, vector_trans_A_to_B_grandfathers, distance_actual);

			pair_nodes pair_actual((*it_father), node_actualBest);

			std::pair<double,pair_nodes> actual_dist_pair(distance_actual,pair_actual);
			map_dist_pairNodeNode.insert(actual_dist_pair);
		}
	}

	return map_dist_pairNodeNode;
}


/*std::pair< std::pair<Node*, Node*>, double> Node::GetClosestBranch_FatherAndFamily(Node* node_gradfather)
{
	// "this" is a grandfather node
	// Variables
	double distance_actual = -1;
	double distance_bestFinal = -1;
	Node* node_A_bestFinal = NULL;
	Node* node_B_bestFinal = NULL;
	bool first = true;

	// Get the translation vector for root overlapping
	Vec3 vector_trans_A_to_B_grandfathers = node_gradfather->m_coords - this->m_coords;

	// Iterate over my children which actually are fathers
	vec_nodes::iterator it_father = this->m_children.begin();
	for( ; it_father != this->m_children.end(); ++it_father)
	{
		if( !(*it_father)->IsMarked() )
		{
			// Get the position of the actual child of this node
			//Vec3 position_actualFather = (*it_father)->m_coords;

			// Find the "closest" node in the subtree nodeB using a distance function
			// a. Distance to ending point
			// Translate the position
			//Vec3 position_translated = position_actualFather + vector_trans_A_to_B_grandfathers;
			//Node* bestNode_actual = nodeB->GetClosestRelativeToPoint(position_translated, distance);
			// b. Distance point to point
			Node* node_actualBest = node_gradfather->GetClosestRelativeFatherAndFamily( (*it_father), vector_trans_A_to_B_grandfathers, distance_actual);

			if(first)
			{
				first = false;
				node_A_bestFinal = (*it_father);
				node_B_bestFinal = node_actualBest;
				distance_bestFinal = distance_actual;
			}
			else if(distance_actual < distance_bestFinal)
			{
				node_A_bestFinal = (*it_father);
				node_B_bestFinal = node_actualBest;
				distance_bestFinal = distance_actual;
			}
		}
	}

	if(first)
		std::cout << "GetBestTranslatedBranchFromOtherTree NOT FOUND!" << std::endl;

	pair_nodes pair_best(node_A_bestFinal, node_B_bestFinal);
	std::pair<pair_nodes,double> best_pair_score(pair_best, distance_bestFinal);
	return best_pair_score;
}
*/

Node* Node::GetClosestRelativeToPoint(Vec3 position, double& distance)
{
	if(this->IsLeaf())
	{
		distance = -1;
		return NULL;
	}

	distance = -1;
	bool first = true;
	double actualDistance = -1;
	Node* node_Best = NULL;
	Node* node_Actual = NULL;

	for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
	{
		node_Actual = (*it)->GetClosestNodeToPoint(position, actualDistance);

		if(first)
		{
			first = false;
			node_Best = node_Actual;
			distance = actualDistance;
		}
		else if(actualDistance < distance)
		{
			node_Best = node_Actual;
			distance = actualDistance;
		}
	}

	if(first)
		std::cout << "GetClosestRelativeToPoint NOT FOUND!" << std::endl;

	if(!node_Best)
		std::cout << "GetClosestRelativeToPoint NULL!" << std::endl;

	return node_Best;
}

Node* Node::GetClosestRelativeToBranch(Edge* branch, Vec3 vector_translation, double& distance)
{
	if(this->IsLeaf())
	{
		distance = -1;
		//std::cout << "GetClosestRelativeToBranch -- This node is a leaf" << std::endl;
		return NULL;
	}

	distance = -1;
	bool first = true;
	double actualDistance = -1;
	Node* node_Best = NULL;
	Node* node_Actual = NULL;

	for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
	{
		node_Actual = (*it)->GetClosestBranchToTranslatedBranch(branch,vector_translation, actualDistance, NULL);

		if(first)
		{
			first = false;
			node_Best = node_Actual;
			distance = actualDistance;
		}
		else if(actualDistance < distance)
		{
			node_Best = node_Actual;
			distance = actualDistance;
		}
	}

	if(first)
		std::cout << "GetClosestRelativeToPoint NOT FOUND!" << std::endl;

	if(!node_Best)
		std::cout << "GetClosestRelativeToPoint NULL!" << std::endl;

	return node_Best;
}

Node* Node::GetClosestRelativeFatherAndFamily(Node* node_grandfather, Node* node_father, unsigned int F, Vec3 vector_translation, double& distance)
{
	// "this" is a grandfather node

	distance = -1;
	if(this->IsLeaf())
		return NULL;

	bool first = true;
	double actualDistance = -1;
	Node* node_Best = NULL;
	Node* node_Actual = NULL;

	// Iterate over the children of this granfather which means
	// iterate over the fathers
	vec_nodes::const_iterator it_father = this->m_children.begin();

	for( ; it_father != this->m_children.end(); ++it_father)
	{
		node_Actual = (*it_father)->GetClosestFatherAndFamilyToTranslatedFatherAndFamily(node_grandfather, node_father, F, vector_translation, actualDistance, NULL);

		if(first)
		{
			first = false;
			node_Best = node_Actual;
			distance = actualDistance;
		}
		else if(actualDistance < distance)
		{
			node_Best = node_Actual;
			distance = actualDistance;
		}
	}

	if(first)
		std::cout << "GetClosestRelativeToPoint NOT FOUND!" << std::endl;

	if(!node_Best)
		std::cout << "GetClosestRelativeToPoint NULL!" << std::endl;

	return node_Best;
}

Node* Node::GetClosestBranchToTranslatedBranch(Edge* branch, Vec3 vector_translation, double& distance, Edge* edgeSuperior)
{
	Node* node_best = this;
	Node* node_Actual = NULL;
	Edge* edge_composed = NULL;

	// Joint the superior edge with this edge
	edge_composed = this->ConcatenateEdgeToSuperiorEdge(edgeSuperior);

	distance = edge_composed->GetDistanceToTranslatedEdge(branch, vector_translation);
	distance += branch->GetDistanceToTranslatedEdge(edge_composed, -vector_translation);
	//distance = edge_composed->GetDistanceWeigthedToTranslatedEdge(branch, vector_translation);
	//distance += branch->GetDistanceWeigthedToTranslatedEdge(edge_composed, -vector_translation);
	//std::cout << "DW[thisId,toBranchTargetId,Dist]:[" << this->m_id << "," << branch->GetTarget()->GetId() << "," << distance << "]";

	// Lance the recursion and the the minumum distance and node
	double distance_actual = -1;

	// Check the children and get the best
	for(vec_nodes::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
	{
		Edge* edge_forChild = new Edge(edge_composed);
		node_Actual = (*it)->GetClosestBranchToTranslatedBranch(branch, vector_translation, distance_actual, edge_forChild);

		if(distance_actual < distance)
		{
			node_best = node_Actual;
			distance = distance_actual;
		}
		delete(edge_forChild);
	}

	return node_best;
}

Node* Node::GetClosestFatherAndFamilyToTranslatedFatherAndFamily(Node* node_grandfather, Node* node_father, unsigned int F, Vec3 vector_translation, double& distance, Edge* edgeSuperior)
{
	// "this" is a father node_father
	Node* node_best = this;		// Best matching node is "this" in the beginnig.
	Node* node_Actual = NULL;
	Edge* edge_composed = NULL;

	// Joint the superior edge with this edge
	edge_composed = this->ConcatenateEdgeToSuperiorEdge(edgeSuperior);

	//distance = edge_composed->GetDistanceToTranslatedEdge(branch, vector_translation);
	//distance += branch->GetDistanceToTranslatedEdge(edge_composed, -vector_translation);

	//Edge* branch_father = node_father->m_edge;
	Edge* branch_father = node_father->GetEdgeToAncestor(node_grandfather, NULL);
	if( branch_father == NULL )
		std::cout << "No branch father" << std::endl;


	distance = edge_composed->GetDistanceWeigthedToTranslatedEdge(branch_father, vector_translation);
	distance += branch_father->GetDistanceWeigthedToTranslatedEdge(edge_composed, -vector_translation);

	// Find the family distance from the node_father family to this family
	double distance_family_A_to_B = 0;
	// Add the distances to the family, up to one generation by the moment
	Vec3 vector_translation_child_A_to_B = this->m_coords - node_father->m_coords; // Vector that align the fathers

	// Get the family up to depth F
	vec_nodes family;
	node_father->GetChildrenUptoDepth(F, family);
	//vec_nodes family = node_father->m_children;

	vec_nodes::iterator it_child_other = family.begin();
	for(; it_child_other != family.end(); ++it_child_other)
	{
		// Create the "child" edge
		Edge* childEdge = (*it_child_other)->GetEdgeToAncestor(node_father, NULL);
		//Edge* childEdge = (*it_child_other)->m_edge;

		double distance_child = 0;
		if(this->IsLeaf())
		{
			// The distance of the each point edge to the root point because the fathers are aligned
			distance_child = childEdge->GetDistanceToPoint(this->m_coords);
		}
		else
		{
			this->GetClosestRelativeToBranch(childEdge,vector_translation_child_A_to_B, distance_child);
		}
		distance_family_A_to_B += distance_child;
	}

	distance += distance_family_A_to_B;

	// Find the family distance from this family to node_father family
	double distance_family_B_to_A = 0;

	// Add the distances to the family, up to one generation by the moment
	Vec3 vector_translation_child_B_to_A = node_father->m_coords - this->m_coords; // Vector that align the fathers

	// Get the family up to depth F
	vec_nodes my_family;
	this->GetChildrenUptoDepth(F, my_family);
	//vec_nodes my_family = this->m_children;

	vec_nodes::iterator it_child_my = my_family.begin();
	for(; it_child_my != my_family.end(); ++it_child_my)
	{
		// Create the "child" edge
		Edge* my_childEdge = (*it_child_my)->GetEdgeToAncestor(this, NULL);
		//Edge* my_childEdge = (*it_child_my)->m_edge;

		double distance_child = 0;
		if(node_father->IsLeaf())
		{
			// The distance of the each point edge to the root point because the fathers are aligned
			distance_child = my_childEdge->GetDistanceToPoint(node_father->m_coords);
		}
		else
		{
			node_father->GetClosestRelativeToBranch(my_childEdge, vector_translation_child_B_to_A, distance_child);
		}
		distance_family_B_to_A += distance_child;
	}

	distance += distance_family_B_to_A;

	//std::cout << "DW[thisId,toBranchTargetId,Dist]:[" << this->m_id << "," << branch->GetTarget()->GetId() << "," << distance << "]";

	// Lance the recursion and the minimum distance and node_father
	double distance_actual = -1;

	// Check the children and get the best
	vec_nodes::iterator it = this->m_children.begin();
	for( ; it != this->m_children.end(); ++it)
	{
		Edge* edge_forChild = new Edge(edge_composed);
		node_Actual = (*it)->GetClosestFatherAndFamilyToTranslatedFatherAndFamily(node_grandfather, node_father, F, vector_translation, distance_actual, edge_forChild);

		if(distance_actual < distance)
		{
			node_best = node_Actual;
			distance = distance_actual;
		}
		delete(edge_forChild);
	}

	return node_best;
}

const Node* Node::GetClosestCommonAscendingNode() const
{
	Node* node_closestCommonAscendingNode = NULL;

	if(this->m_edge)
	{
		if(this->m_edge->GetSource()->IsMarked() || this->m_edge->GetSource()->HasMarkedChildren())
			node_closestCommonAscendingNode =  this->m_edge->GetSource();
		else
			return this->m_edge->GetSource()->GetClosestCommonAscendingNode();
	}

	return node_closestCommonAscendingNode;
}

bool Node::MarkNodesUntilNode(Node* nodeB)
{
	this->Mark();

	if(this->m_id == nodeB->m_id)
		return true;

	bool found = false;
	bool actualFound = false;
	for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
	{
		actualFound = (*it)->MarkNodesUntilNode(nodeB);

		if(actualFound)
			found = true;
	}

	return found;
}

Node* Node::GetNextNodeInPathToNode(Node* node_searched)
{
	Node* nextNode = NULL;
	for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end() && !nextNode; ++it)
	{
		if( (*it)->HasNode(node_searched) )
			nextNode = (*it);
	}

	return nextNode;
}

void Node::GetPathToNode(Node* node_end, vec_nodes& vector_pathNodes)
{
	if(!this->HasNode(node_end))
		return;

	vector_pathNodes.push_back(this);
	for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
		(*it)->GetPathToNode(node_end, vector_pathNodes);
}

void Node::MarkPathFromNodeToNode(Node* node_begin, Node* node_end)
{
	if(this->m_id == node_begin->m_id)
	{
		if(this->HasNode(node_end))
			this->MarkPathNodesToNode(node_end);
	}
	else
		for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
			(*it)->MarkPathFromNodeToNode(node_begin, node_end);
}

void Node::MarkPathNodesToNode(Node* node_end)
{
	if(this->HasNode(node_end))
		this->Mark();

	for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
		(*it)->MarkPathNodesToNode(node_end);
}

Node* Node::GetDetachedRootCopy()
{
	Node* node_copy = new Node(this);

	// Change properties for root node
	Node* root_detached = new Node();
	root_detached->m_coords = node_copy->m_edge->GetSource()->GetCoords();
	root_detached->m_edge = NULL;
	vec_nodes vector_new;
	vector_new.push_back(node_copy);
	root_detached->m_children = vector_new;

	// Change properties for the copy node
	node_copy->m_edge->SetSource(root_detached);

	return root_detached;
}

bool Node::CompareNodes(Node* nodeTreeB)
{
	if (this == NULL && nodeTreeB == NULL)
		return true;
	else if (this == NULL || nodeTreeB == NULL)
		return false;
	vec_nodes::iterator it = this->m_children.begin();
	for (; it != this->m_children.end(); ++it)
	{
		if ((*it)->m_mark)
			continue;
		bool suc = false;
		vec_nodes::iterator it2 = nodeTreeB->m_children.begin();
		for (; it2 != nodeTreeB->m_children.end(); ++it2)
			suc = (*it)->CompareNodes(*it2);
		if (!suc)
			return false;
	} //for
	if (*this != *nodeTreeB)
		return false;
	this->m_mark = true;
	nodeTreeB->m_mark = true;
	return true;
}

Edge* Node::GetComposedEdge(unsigned int idNode)
{
	// If this is the node, then a copy edge (to the parent) is returned
	if(this->m_id == idNode)
	{
		//std::cout << "This is the searched idNode: " << this->m_id << std::endl;
		Edge* nEdge = new Edge(this->m_edge);
		return nEdge;
	}


	Edge* composedEdge = NULL;

	for(std::vector<Node*>::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
	{
		// If one of my children has the composed node, then my edge must be added
		// and the composed edge is returned
		Edge* childEdge = (*it)->GetComposedEdge(idNode);
		if(childEdge)
		{
			//std::cout << "My id: " << this->m_id << ", Looking for: " << idNode << std::endl;
			return AddChildEdgeInformation(childEdge);
		}
	}

	return composedEdge;
}

Edge* Node::AddChildEdgeInformation(Edge* nEdge)
{
	// If this is the root then the edge to be compose is null, so the same edge must be returned
	if(this->m_edge == NULL)
	{
		//std::cout << "This is the root node" << std::endl;
		return nEdge;
	}

	// New information vector
	vec_pair_posVox_rad vector_newInformation;
	vec_pair_posVox_rad actualEdgeInformation = this->m_edge->GetEdgeInfo();
	vec_pair_posVox_rad composedInformation = nEdge->GetEdgeInfo();

	// Check that last and initial information, from this and the other edge respectively, are equal.
	if(actualEdgeInformation[actualEdgeInformation.size()-1].first != composedInformation[0].first)
	{

		std::cout << "actualEdgeInformation[0;end]: [" << actualEdgeInformation[0].first << ";" << actualEdgeInformation[actualEdgeInformation.size()-1].first << "], Points: " << actualEdgeInformation.size() << std::endl;
		std::cout << "ActualEdge[source;target]: [" << this->m_edge->GetSource()->GetCoords() << "];[" << this->m_edge->GetTarget()->GetCoords() << "]" << std::endl;
		std::cout << "-- -- -- " << std::endl;
		std::cout << "composedInformation[0;end]: [" << composedInformation[0].first << ";" << composedInformation[composedInformation.size()-1].first << "], Points: " << composedInformation.size() << std::endl;
		std::cout << "AddChildEdge[source;target]: [" << nEdge->GetSource()->GetCoords() << "];[" << nEdge->GetTarget()->GetCoords() << "]" << std::endl;
		std::cout << "End and initial information are not equal! : " << actualEdgeInformation[actualEdgeInformation.size()-1].first << " , " << composedInformation[0].first << std::endl;


		std::cout << "Print composed edge information" << std::endl;
		std::cout << "Composed [source, target] " << nEdge->GetSource()->GetCoords() << "];[" << nEdge->GetTarget()->GetCoords() << "]" << std::endl;
		std::cout << "Composed information using iterator:" << std::endl;

		for(vec_pair_posVox_rad::const_iterator it_composed = nEdge->GetEdgeInfo().begin(); it_composed != nEdge->GetEdgeInfo().end(); ++it_composed)
			std::cout << "[" << (*it_composed).first << "], ";
	}

	// Change the source
	nEdge->SetSource(this->m_edge->GetSource());

	// Change properties
	nEdge->SetLength(nEdge->GetLength()+this->m_edge->GetLength());
	nEdge->SetEDistance(nEdge->GetEDistance()+this->m_edge->GetEDistance());

	// Create the new information vector and calculate the new radii attributes
	for(vec_pair_posVox_rad::iterator it_actual = actualEdgeInformation.begin(); it_actual != actualEdgeInformation.end(); ++it_actual)
		vector_newInformation.push_back((*it_actual));

	bool connectionVoxel = true;
	for(vec_pair_posVox_rad::iterator it_composed = composedInformation.begin(); it_composed != composedInformation.end(); ++it_composed)
	{
		if(connectionVoxel)
			connectionVoxel = false;
		else
			vector_newInformation.push_back((*it_composed));
	}

	// Set the new information
	nEdge->SetSkeletonPairVector(vector_newInformation);

	return nEdge;
}

Edge* Node::GetEdgeToAncestor(Node* node_ancestor, Edge* edge)
{
	Edge* edge_composed = NULL;

	// Compose the edge
	if(edge == NULL)
		edge_composed = new Edge(this->m_edge);
	else
		edge_composed = edge->ConcatenateToSuperior(new Edge(this->m_edge));

	// Check if the father is the ancestor
	if(this->m_edge->GetSource()->GetId() == node_ancestor->GetId())
		return edge_composed;

	return this->m_edge->GetSource()->GetEdgeToAncestor(node_ancestor, edge_composed);
}

Edge* Node::ConcatenateEdgeToSuperiorEdge(Edge* superiorEdge)
{
	if(superiorEdge == NULL)
		return new Edge(this->m_edge);

	// Check concatenation condition in the nodes
	if(superiorEdge->GetTarget()->GetCoords() != this->m_edge->GetSource()->GetCoords())
	{
		std::cout << "Node::ConcatenateEdgeToSuperiorEdge - superiorEdge.target != actualEdge.source. Actual idNode:" << this->m_id << std::endl;
		std::cout << "[superiorEdge.target;actualEdge.source]: [" << superiorEdge->GetTarget()->GetCoords() << ";" << this->m_edge->GetSource()->GetCoords() << "]" << std::endl;
		return NULL;
	}

	// Check concatenation condition in the edge information
	vec_pair_posVox_rad vectorInfo_thisEdge = this->m_edge->GetEdgeInfo(); // Actual edge information
	vec_pair_posVox_rad vectorInfo_superiorEdge = superiorEdge->GetEdgeInfo(); // Superior edge information

	// Check that last superior edge position is equal to first initial actual edge position
	if(vectorInfo_superiorEdge[vectorInfo_superiorEdge.size()-1].first != vectorInfo_thisEdge[0].first)
	{
		std::cout << "Node::ConcatenateEdgeToSuperiorEdge - superiorEdge.info[end] != actualEdge.info[begin]. Actual idNode:" << this->m_id << std::endl;
		return NULL;
	}

	// Change the superior edge target, the superior length, and the euclidian distance
	superiorEdge->SetTarget(this->m_edge->GetTarget());
	superiorEdge->SetLength(superiorEdge->GetLength()+this->m_edge->GetLength()-1);
	superiorEdge->SetEDistance(superiorEdge->GetEDistance()+this->m_edge->GetEDistance());

	// Add the position information
	vec_pair_posVox_rad::iterator it_actualInfo = vectorInfo_thisEdge.begin();
	++it_actualInfo; // Skip the first position because is it shared between both edges
	for(; it_actualInfo != vectorInfo_thisEdge.end(); ++it_actualInfo)
		vectorInfo_superiorEdge.push_back((*it_actualInfo));

	// Set the new information
	superiorEdge->SetSkeletonPairVector(vectorInfo_superiorEdge);

	return superiorEdge;
}

bool Node::FindSimilarEdgeByAccumulation(Node* nodeB, Node* nodeBAcc)
{
	bool firstTime = false;
	if (nodeBAcc == NULL)
	{
		nodeBAcc = new Node();
		nodeBAcc->m_edge = new Edge();
		firstTime = true;
	} //if
	nodeBAcc->m_coords = nodeB->m_coords;
	nodeBAcc->m_level = this->m_level;
	Edge* edgeBAcc = nodeBAcc->m_edge;
	Edge* edgeB = nodeB->m_edge;
	edgeBAcc->m_angle = edgeB->m_angle;
	edgeBAcc->m_eDistance = edgeB->m_eDistance;
	if (!firstTime)
	{
		edgeBAcc->m_length += edgeB->m_length;
		//edgeBAcc->m_aRadius = (edgeB->m_aRadius + edgeBAcc->m_aRadius) / 2.0;
		edgeBAcc->m_maxRadius =	edgeB->m_maxRadius > edgeBAcc->m_maxRadius ? edgeB->m_maxRadius : edgeBAcc->m_maxRadius;
		edgeBAcc->m_minRadius =	edgeB->m_minRadius < edgeBAcc->m_minRadius ? edgeB->m_minRadius : edgeBAcc->m_minRadius;
	} //fi
	else
	{
		edgeBAcc->m_length = edgeB->m_length;
		edgeBAcc->m_aRadius = edgeB->m_aRadius;
		edgeBAcc->m_maxRadius = edgeB->m_maxRadius;
		edgeBAcc->m_minRadius = edgeB->m_minRadius;
	} //else

	if (*this == *nodeBAcc)
		return true;
	Node* aux = NULL;
	bool suc = false;
	vec_nodes::iterator it = nodeB->m_children.begin();
	for (; it != nodeB->m_children.end(); ++it)
	{
		aux = *it;
		if (this->FindSimilarEdgeByAccumulation(aux, nodeBAcc))
		{
			suc = true;
			break;
		} //if
	} //for
	if (suc)
	{
		nodeB = aux;
		return true;
	}//if
	return false;
}

bool Node::FindMarked(Node* result)
{
	vec_nodes::iterator it = this->m_children.begin();
	for (; it != this->m_children.end(); ++it)
		if ((*it)->m_mark)
		{
			result = *it;
			return true;
		} //if
		else if ((*it)->FindMarked(result))
			return true;
	result = NULL;
	return false;
}

bool Node::MarkPath(Node* nodeB)
{
	if (this == NULL || nodeB == NULL)
		return false;
	//comparing addresses
	if (this == nodeB)
	{
		nodeB->m_mark = true;
		return true;
	} //if
	vec_nodes::iterator it = this->m_children.begin();
	for (; it != this->m_children.end(); ++it)
		if ((*it)->MarkPath(nodeB))
		{
			(*it)->m_mark = true;
			return true;
		} //if
	return false;

}

unsigned int Node::GetCost(Node* nodeB)
{
	if (this == NULL || nodeB == NULL)
		return 0;
	unsigned int maxCost = 0;
	if (*this == *nodeB)
	{
		vec_nodes::iterator it = this->m_children.begin();
		for (; it != this->m_children.end(); ++it)
		{
			vec_nodes::iterator it2 = nodeB->m_children.begin();
			for (; it2 != nodeB->m_children.end(); ++it2)
			{
				unsigned int currentCost = (*it)->GetCost(*it2);
				if (currentCost > maxCost)
					maxCost = currentCost;
			} //for
		} //for
		maxCost += 1;
	} //if
	return maxCost;
}


//Setters and Modifiers

void Node::SetId(unsigned int nM_id)
{
	m_id = nM_id;
}


void Node::AddChild(Node* node)
{
	this->m_children.push_back(node);
}

void Node::SetChildren(std::vector<Node*>& children)
{
	this->m_children = children;
}

void Node::SetFather(Node* nFather)
{
	father = nFather;
}

void Node::SetEdge(Edge* edge)
{
	this->m_edge = edge;
}

void Node::SetCoords(const Vec3& coords)
{
	this->m_coords = coords;
}

void Node::SetLevel(const int& level)
{
	this->m_level = level;
}

bool Node::DeleteNodeById(unsigned int nId)
{
	if(this->IsLeaf())
		return false;

	bool deleted = false;

	vec_nodes::iterator it = this->m_children.begin();
	while(it != this->m_children.end() && !deleted)
	{
		if((*it)->m_id == nId)
		{
			it = m_children.erase(it);
			deleted = true;
		}
		else
			++it;
	}

	if(!deleted)
	{
		for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end() && !deleted; ++it)
		{
			deleted = (*it)->DeleteNodeById(nId);
		}
	}

	return deleted;
}

void Node::MergeNodes(Node* nodeB)
{
	if (nodeB == NULL)
		return;
	this->m_coords = nodeB->m_coords;
	Edge* edgeA = this->m_edge;
	Edge* edgeB = nodeB->m_edge;
	edgeA->m_angle = edgeB->m_angle;
	edgeA->m_eDistance = edgeB->m_eDistance;
	edgeA->m_length += edgeB->m_length;
	edgeA->m_aRadius = (edgeB->m_aRadius + edgeA->m_aRadius) / 2.0;
	edgeA->m_maxRadius = edgeB->m_maxRadius > edgeA->m_maxRadius ? edgeB->m_maxRadius : edgeA->m_maxRadius;
	edgeA->m_minRadius = edgeB->m_minRadius < edgeA->m_minRadius ? edgeB->m_minRadius : edgeA->m_minRadius;
	edgeA->m_vec_pair_posVox_rad.insert(edgeA->m_vec_pair_posVox_rad.end(), edgeB->m_vec_pair_posVox_rad.begin(), edgeB->m_vec_pair_posVox_rad.end());
}

void Node::Mark()
{
	this->m_mark = true;
}

void Node::UnMark()
{
	this->m_mark = false;
}

void Node::UpdateLevels(unsigned int level)
{
	this->m_level = level;
	level++;
	vec_nodes::iterator it = this->m_children.begin();
	for (; it != this->m_children.end(); ++it)
		(*it)->UpdateLevels(level);
}

void Node::printUpToLevel(int level)
{
	// Print myself
	cout << "level" << level << " " << m_coords << " || " << endl;
	if(level == 1)
		return;

	// Print my children
	for (vec_nodes::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
	{
		(*it)->printUpToLevel(level-1);
	}
}

void Node::printNodeAndChildrenIds( )
{
	std::cout << "------------------------------------------" << std::endl;
	std::cout << "Id: " << this->m_id << std::endl;

	// Print children
	for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
		std::cout << (*it)->m_id << ";";

	// Recursion
	for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
		(*it)->printNodeAndChildrenIds( );
}

void Node::printChildrenIds( )
{
	std::cout << "------------------------------------------" << std::endl;
	std::cout << "Id: " << this->m_id << std::endl;

	// Print children
	for(vec_nodes::const_iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
		std::cout << (*it)->m_id << ";";
}

void Node::createQuaternionsUpToLevel(int level, int requiredLevel)
{
	//cout << "Level:" << requiredLevel-level <<endl;

	if(level >= 2 && this->HasMinimumLevels(2))
	{
		// 1. Take the vector to each son (SP - The vector that points from the son to the parent [P-S]).
		// 2. For each son take each vector from son to grandson (SG - the vector that points from the son to the grandson [G-S])
		// 3. For each combination SP-SG calculate the quaternion
		for(vec_nodes::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
		{
			// Create vector from son to parent (SP = [P - S])
			Vec3 sp(this->m_coords - (*it)->m_coords);

			// Get the mean radius for the parent-son branch
			double radMeanSP=(*it)->m_edge->GetARadius();

			// Get distance son to parent
			double distSonParent = (*it)->m_edge->GetEDistance();

			// Get the vectors from son to each grandson (SG = [G - S] )
			for(vec_nodes::iterator itps = (*it)->m_children.begin(); itps != (*it)->m_children.end(); ++itps)
			{
				// Build the sg vector and get the quaternion
				Vec3 sg((*itps)->m_coords - (*it)->m_coords);
				Quaternion q(sp, sg);

				// Get distance son to grandson
				double distSonGrandson = (*itps)->m_edge->GetEDistance();

				// Get the mean radius for the son-grandson branch
				double radMeanSG=(*itps)->m_edge->GetARadius();

				//cout << "Vectors:" << ps << sg <<endl;
				//cout << "Q:" << q << ", Distance=" << (*itps)->m_edge->GetEDistance() << endl << endl;
				// Printing
				// Level | PositionSon | SPvector | SGvector | Quaternion | distanceSonParent | distanceSonGrandSon | meanRadiusSP | meanRadiusSG
				cout << requiredLevel-level << " " << (*it)->m_coords << " " << sp << " " << sg << " " << q << " " << distSonParent << " " << distSonGrandson << " " << radMeanSP << " " << radMeanSG << endl;
			}
		}

		for(vec_nodes::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
		{
			(*it)->createQuaternionsUpToLevel(level-1, requiredLevel);
		}
	}
}

std::pair<double, std::pair<Node*, Node*> > Node::GetPairWithClosestDistance_FirstNodeNonMarked(std::multimap<double, pair_nodes> map_dist_pairNodes)
{
	// Variables
	bool pairFound = false;
	std::pair<Node*, Node*> pairNodes_null (NULL, NULL);
	std::pair<double, std::pair<Node*, Node*> > pair_closest_dist_pairNodes (-1, pairNodes_null);

	std::multimap<double, pair_nodes>::iterator it = map_dist_pairNodes.begin();

	for( ; it != map_dist_pairNodes.end() && !pairFound; ++it)
	{
		if( ! (*it).second.first->IsMarked() )
		{
			pair_closest_dist_pairNodes.first=(*it).first;
			pair_closest_dist_pairNodes.second=(*it).second;
			pairFound = true;
		}
	}

	return pair_closest_dist_pairNodes;
}

void Node::getStasticsBifurcations(int* p)
{
	int numBif = this->m_children.size();
	if(numBif>0 && numBif<=6)
		p[numBif-1]=p[numBif-1]+1;

	for(vec_nodes::iterator it = this->m_children.begin(); it != this->m_children.end(); ++it)
	{
		(*it)->getStasticsBifurcations(p);
	}
}

//Operator Overloads

Node &Node::operator=(Node & node)
{
	this->m_children = node.m_children;
	this->m_edge = node.m_edge;
	this->m_coords = node.m_coords;
	this->m_level = node.m_level;
	this->m_mark = node.m_mark;
	return *this;
}

const Node&Node::operator=(const Node& node)
{
	this->m_children = node.m_children;
	this->m_edge = node.m_edge;
	this->m_coords = node.m_coords;
	this->m_level = node.m_level;
	this->m_mark = node.m_mark;
	return *this;
}

bool Node::operator ==(const Node& node)
{
	bool comp = false;
	if (this->m_edge == NULL && this->m_edge != NULL)
		return false;
	else if (this->m_edge != NULL && this->m_edge == NULL)
		return false;
	else if (this->m_edge == NULL && this->m_edge == NULL)
		comp = true;
	else
		comp = ((*this->m_edge) == (*node.m_edge));
	bool ret = (/*(this->m_coords == node.m_coords)
     && (this->m_children.size() == node.m_children.size())
     && */(this->m_level == node.m_level) && comp);
	return ret;
}

bool Node::operator !=(const Node& node)
{
	bool comp = false;
	if (this->m_edge == NULL && this->m_edge != NULL)
		return false;
	else if (this->m_edge != NULL && this->m_edge == NULL)
		return false;
	else if (this->m_edge == NULL && this->m_edge == NULL)
		comp = true;
	else
		comp = ((*this->m_edge) == (*node.m_edge));
	return (/*(this->m_coords != node.m_coords)
     && (this->m_children.size() != node.m_children.size())
     && */(this->m_level != node.m_level) && !comp);
}

bool Node::operator <(const Node& node)
{
	bool comp = false;
	if (this->m_edge == NULL && this->m_edge != NULL)
		return false;
	else if (this->m_edge != NULL && this->m_edge == NULL)
		return false;
	else if (this->m_edge == NULL && this->m_edge == NULL)
		comp = false;
	else
		comp = ((*this->m_edge) < (*node.m_edge));
	return comp;
}

bool Node::operator >(const Node& node)
{
	bool comp = false;
	if (this->m_edge == NULL && this->m_edge != NULL)
		return false;
	else if (this->m_edge != NULL && this->m_edge == NULL)
		return false;
	else if (this->m_edge == NULL && this->m_edge == NULL)
		comp = false;
	else
		comp = ((*this->m_edge) > (*node.m_edge));
	return comp;
}

} /* namespace airways */