/*
 * airwaysTree.txx
 *
 *  Created on: May 3, 2014
 *      Author: caceres@creatis.insa-lyon.fr
 *      Modifications by: Alfredo Morales Pinzón
 */

#ifndef _AIRWAYS_TREE_TXX_
#define _AIRWAYS_TREE_TXX_

#include "airwaysTree.h"

namespace airways
{

AirwaysTree::AirwaysTree() :  m_root(NULL)
{

}

//Ceron
AirwaysTree::AirwaysTree(TInputImage::Pointer img, TInputImage::Pointer img_skele, Node* root, bool shouldUpdate){
    this->m_root = root;
    this->m_root->SetLevel(0);
    this->m_img = img;
    this->m_skeleton = img_skele;
    //TODO: Should this calculate levels and populate edges and nodes vectors?
    FillTree(root);
    if(shouldUpdate) this->UpdateEdges();
}

void AirwaysTree::FillTree(Node* node){
    //std::cout << "Calls fill tree for " << node->GetCoords() << std::endl;
    this->m_nodes.push_back(node);
    node->SetId(this->m_nodes.size());
    if(node->GetEdge() != NULL){
        this->m_edges.push_back(node->GetEdge());
        node->GetEdge()->UpdateEdgeInfo();
    }
    int level = node->GetLevel();
    //std::cout << "Node is at level " << level << std::endl;
    vec_nodes::const_iterator it = node->GetChildren().begin();
	for (; it != node->GetChildren().end(); ++it){
        (*it)->SetLevel(level+1);
        this->FillTree(*it);
    }
}

AirwaysTree::AirwaysTree(TInputImage::Pointer img, TInputImage::Pointer img_sk, Node* root)
{
	this->m_root = root;
	this->m_root->SetLevel(0);
	this->m_skeleton = img_sk;
	this->m_img = img;
	this->UpdateEdges();
}

AirwaysTree::AirwaysTree(TInputImage::Pointer image_airwaySegmentation, TInputImage::Pointer image_skeletonDM, EdgeVector& vector_edges, Edge* edge_trachea)
{
	std::cout << "AirwaysTree::AirwaysTree(TInputImage::Pointer img, TInputImage::Pointer img_sk, EdgeVector& tEdgeVector, Edge* trachea)" << std::endl;
	std::cout << "Edges to add:" << vector_edges.size() << std::endl;

	if(edge_trachea == NULL)
		std::cout << "The trachea edge is NULL" << std::endl;

	// Set the attribtes
	this->m_skeleton = image_skeletonDM;
	this->m_img = image_airwaySegmentation;
	this->m_root = edge_trachea->m_source;
	this->m_root->SetLevel(0);

	this->m_nodes.push_back(edge_trachea->m_source);	// Add the first node, the root to the nodes of the tree
	edge_trachea->m_source->SetId(this->m_nodes.size());// Set the root id

	// Insert the trachea edge
	this->Insert(edge_trachea);

	// Insert all edges
	unsigned int iniSize = vector_edges.size();

	while (!vector_edges.empty())
	{
		bool oneEdgeAdded = false;
		EdgeVector::iterator it = vector_edges.begin();
		while (it != vector_edges.end())
		{
			if (this->Insert(*it))
			{
				std::cout << "Edges added: [" << (*it)->GetSource()->GetCoords() << "]->[" << (*it)->GetTarget()->GetCoords() << "], missing: " << vector_edges.size()-1 << std::endl;
				oneEdgeAdded = true;
				it = vector_edges.erase(it);
			}
			else
				++it;
		} //while

		// If there are edges to be added but they could not be added
		if(!oneEdgeAdded)
		{
			std::cout << "Edges not added:" << vector_edges.size() << std::endl;
			for (EdgeVector::iterator it = vector_edges.begin(); it != vector_edges.end(); ++it)
				std::cout << "Edge missing: [" << (*it)->GetSource()->GetCoords() << "]->[" << (*it)->GetTarget()->GetCoords() << "]" << std::endl;
			break;
		}
		// AMP: I did not find any explanation for this "exception"
		/*count++;
		if (count == iniSize)
		{
			std::cout << "handled exception: arwaysTree.txx, AirwaysTree" << std::endl;
			break;
		} //if
		 */
	} //while
	this->UpdateEdges();

	std::cout << "AirwaysTree::AirwaysTree(TInputImage::Pointer img, TInputImage::Pointer img_sk, EdgeVector& tEdgeVector, Edge* trachea) ...  OK" << std::endl;
}

AirwaysTree::~AirwaysTree()
{

}

Node* AirwaysTree::GetRoot() const
{
	return this->m_root;
}

std::string AirwaysTree::GetImageName() const
{
	return (this->m_img_name);
}

std::string AirwaysTree::GetResultPath() const
{
	return (this->m_result_path);
}

TInputImage::Pointer AirwaysTree::GetSegmentedImage() const
{
	return this->m_img;
}

TInputImage::Pointer AirwaysTree::GetSkeletonImage() const
{
	return this->m_skeleton;
}

const Node* AirwaysTree::GetNode(const Vec3 nodeCoords) const
{
	return this->GetNode(nodeCoords);
}

Node* AirwaysTree::GetNodeById(unsigned int nId)
{
	return this->m_root->GetNodeById(nId);
}

int AirwaysTree::GetDepthById(unsigned int nId)
{
	return this->m_root->GetDepthById(nId);
}

unsigned int AirwaysTree::CountMarkedNodes(Node* current) const
{
	if (current == NULL)
		current = this->m_root;
	Node* aux = NULL;
	bool found = current->FindMarked(aux);
	if (!found && !current->IsMarked())
		return 0;
	if (aux != NULL && found)
		current = aux;
	unsigned int count = 1;
	vec_nodes::const_iterator it = current->GetChildren().begin();
	for (; it != current->GetChildren().end(); ++it)
		count += this->CountMarkedNodes(*it);
	return count;
}

unsigned int AirwaysTree::GetWeight( ) const
{
	if (this->m_root == NULL)
		return 0;

	return this->m_root->GetWeight();
}

unsigned int AirwaysTree::GetHeight(Node* current) const
{
	if (current == NULL)
		current = this->m_root;
	if (current->IsLeaf())
		return 1;
	unsigned int max = 0;
	vec_nodes::const_iterator it = current->GetChildren().begin();
	for (; it != current->GetChildren().end(); ++it)
	{
		unsigned int cMax = this->GetHeight(*it);
		if (cMax > max)
			max = cMax;
	} //rof
	if (current == this->m_root)
		return max;
	return ++max;
}

unsigned int AirwaysTree::GetLevels(Node* current, unsigned int cLevel) const
{
	if (current == NULL)
		current = this->m_root;
	if (current->IsLeaf())
		return current->GetLevel();
	unsigned int maxLevel = cLevel;
	vec_nodes::const_iterator it = current->GetChildren().begin();
	for (; it != current->GetChildren().end(); ++it)
	{
		unsigned int level = this->GetLevels(*it, maxLevel);
		if (level > maxLevel)
			maxLevel = level;
	} //rof
	return maxLevel;
}

unsigned int AirwaysTree::GetNumberOfLeafs( ) const
{
	if(!this->m_root)
		return 0;

	return this->m_root->GetNumberOfLeafs( );
}

const EdgeVector& AirwaysTree::GetEdges() const
{
	return this->m_edges;
}

const vec_nodes& AirwaysTree::GetNodes() const
{
	return this->m_nodes;
}

const Node* AirwaysTree::GetClosestBrachNodeToIndex(float point_x, float point_y, float point_z) const
{
	float distance = 0;
	Node* closestNode = this->m_root->GetClosestBranchNodeToPoint(point_x, point_y, point_z, distance);
	return closestNode;
}

void AirwaysTree::GetBrancheNodesInfluencingPoint(float point_x, float point_y, float point_z, vec_nodes& vec_nodes_influence)
{
	if(this->m_root)
	{
		std::cout << "AirwaysTree::GetBrancheNodesInfluencingPoint" << std::endl;
		this->m_root->GetBrancheNodesInfluencingPoint(point_x, point_y, point_z, vec_nodes_influence);
	}
}

AirwaysTree& AirwaysTree::GetTreeFromMarkedNodes(Node* currentNode, AirwaysTree* tree)
{
	if (tree == NULL)
	{
		if (!this->m_root->IsMarked())
			return *(new AirwaysTree());
		currentNode = this->m_root;
		tree = new AirwaysTree();
		Node* current = new Node(currentNode->GetCoords());
		tree->SetRoot(current);
		vec_nodes::const_iterator it = currentNode->GetChildren().begin();
		for (; it != currentNode->GetChildren().end(); ++it)
			this->GetTreeFromMarkedNodes(*it, tree);
		this->ImageReconstruction(tree->m_img);
		//this->ImageReconstruction(tree->m_skeleton);
		return *tree;
	} //fi
	if (currentNode->IsMarked())
	{
		Edge* edge = new Edge();
		*edge = *currentNode->GetEdge();
		Node* target = new Node(currentNode->GetCoords());
		edge->m_target = target;
		tree->Insert(edge);
		vec_nodes::const_iterator it = currentNode->GetChildren().begin();
		for (; it != currentNode->GetChildren().end(); ++it)
			this->GetTreeFromMarkedNodes(*it, tree);
	} //fi
	return *(new AirwaysTree());
}

AirwaysTree& AirwaysTree::GetSubTreeFromNode(Node* node, AirwaysTree* tree)
{
	if (tree == NULL)
	{
		tree = new AirwaysTree();
		Node* current = new Node(node->GetCoords());
		tree->SetRoot(current);
		vec_nodes::const_iterator it = node->GetChildren().begin();
		for (; it != node->GetChildren().end(); ++it)
			this->GetSubTreeFromNode(*it, tree);
		this->ImageReconstructionFromNode(tree->m_img, node);
		//this->ImageReconstructionFromNode(tree->m_skeleton, node, true);
		return *tree;
	} //fi
	Edge* edge = new Edge();
	*edge = *node->GetEdge();
	Node* target = new Node(node->GetCoords());
	edge->m_target = target;
	tree->Insert(edge);
	vec_nodes::const_iterator it = node->GetChildren().begin();
	for (; it != node->GetChildren().end(); ++it)
		this->GetSubTreeFromNode(*it, tree);

	return *(new AirwaysTree());
}

AirwaysTree& AirwaysTree::GetSubTreeByLevels(const int& level, Node* node,
		AirwaysTree* tree)
{
	if (tree == NULL)
	{
		node = this->m_root;
		tree = new AirwaysTree();
		Node* current = new Node(node->GetCoords());
		tree->SetRoot(current);
		if (node->GetLevel() <= level)
		{
			node->Mark();
			vec_nodes::const_iterator it = node->GetChildren().begin();
			for (; it != node->GetChildren().end(); ++it)
				this->GetSubTreeByLevels(level, *it, tree);
		} //fi
		if (node->GetLevel() == level)
		{
			DuplicatorType::Pointer duplicator = DuplicatorType::New();
			duplicator->SetInputImage(this->m_img);
			duplicator->Update();
			tree->m_img = duplicator->GetOutput();
			duplicator->SetInputImage(this->m_skeleton);
			duplicator->Update();
			tree->m_skeleton = duplicator->GetOutput();
		} //if
		else
		{
			this->ImageReconstruction(tree->m_img);
			//this->ImageReconstruction(tree->m_skeleton, true);
		} //else
		this->UnMarkAll();
		return *tree;
	} //fi
	if (node->GetLevel() <= level)
	{
		node->Mark();
		Edge* edge = new Edge();
		*edge = *node->GetEdge();
		Node* target = new Node(node->GetCoords());
		edge->m_target = target;
		tree->Insert(edge);
		vec_nodes::const_iterator it = node->GetChildren().begin();
		for (; it != node->GetChildren().end(); ++it)
			this->GetSubTreeByLevels(level, *it, tree);
		if (node->GetChildren().size() == 1)
		{
			Node* child = *(node->GetChildren().begin());
			node->MergeNodes(child);
			const Edge* edge = (child)->GetEdge();
			//removing from nodeVector and edgeVector
			EdgeVector::iterator it2 = this->m_edges.begin();
			for (; it2 != this->m_edges.end(); ++it2)
				if (edge == *it2)
				{
					this->m_edges.erase(it2);
					break;
				} //if
			vec_nodes::iterator it3 = this->m_nodes.begin();
			for (; it3 != this->m_nodes.end(); ++it3)
				if (child == *it3)
				{
					this->m_nodes.erase(it3);
					break;
				} //if
		} //if
	} //if
	return *(new AirwaysTree());
}

AirwaysTree& AirwaysTree::GetSubTreeByLength(const double& length, Node* node,
		AirwaysTree* tree, double cLenght)
{
	if (tree == NULL)
	{
		node = this->m_root;
		tree = new AirwaysTree();
		Node* current = new Node(node->GetCoords());
		tree->SetRoot(current);
		node->Mark();
		vec_nodes::const_iterator it = node->GetChildren().begin();
		for (; it != node->GetChildren().end(); ++it)
			this->GetSubTreeByLength(length, *it, tree);
		this->ImageReconstruction(tree->m_img);
		//this->ImageReconstruction(tree->m_skeleton, true);
		this->UnMarkAll();
		return *tree;
	} //fi
	cLenght += node->GetEdge()->m_length;
	if (cLenght <= length)
	{
		node->Mark();
		Edge* edge = new Edge();
		*edge = *node->GetEdge();
		Node* target = new Node(node->GetCoords());
		edge->m_target = target;
		tree->Insert(edge);
		vec_nodes::const_iterator it = node->GetChildren().begin();
		for (; it != node->GetChildren().end(); ++it)
			this->GetSubTreeByLength(length, *it, tree, cLenght);
		if (node->GetChildren().size() == 1)
		{
			Node* child = *(node->GetChildren().begin());
			node->MergeNodes(child);
			const Edge* edge = (child)->GetEdge();
			//removing from nodeVector and edgeVector
			EdgeVector::iterator it2 = this->m_edges.begin();
			for (; it2 != this->m_edges.end(); ++it2)
				if (edge == *it2)
				{
					this->m_edges.erase(it2);
					break;
				} //if
			vec_nodes::iterator it3 = this->m_nodes.begin();
			for (; it3 != this->m_nodes.end(); ++it3)
				if (child == *it3)
				{
					this->m_nodes.erase(it3);
					break;
				} //if
		} //if
	} //if
	return *(new AirwaysTree());
}

AirwaysTree& AirwaysTree::GetSubTreeByRadius(const double& radius, Node* node,
		AirwaysTree* tree, double cRadius)
{
	if (tree == NULL)
	{
		node = this->m_root;
		tree = new AirwaysTree();
		Node* current = new Node(node->GetCoords());
		tree->SetRoot(current);
		node->Mark();
		vec_nodes::const_iterator it = node->GetChildren().begin();
		for (; it != node->GetChildren().end(); ++it)
			this->GetSubTreeByRadius(radius, *it, tree);
		this->ImageReconstruction(tree->m_img);
		//this->ImageReconstruction(tree->m_skeleton, true);
		this->UnMarkAll();
		return *tree;
	} //fi
	cRadius += node->GetEdge()->m_aRadius;
	if (cRadius >= radius)
	{
		node->Mark();
		Edge* edge = new Edge();
		*edge = *node->GetEdge();
		Node* target = new Node(node->GetCoords());
		edge->m_target = target;
		tree->Insert(edge);
		vec_nodes::const_iterator it = node->GetChildren().begin();
		for (; it != node->GetChildren().end(); ++it)
			this->GetSubTreeByRadius(radius, *it, tree, cRadius);
		if (node->GetChildren().size() == 1)
		{
			Node* child = *(node->GetChildren().begin());
			node->MergeNodes(child);
			const Edge* edge = (child)->GetEdge();
			//removing from nodeVector and edgeVector
			EdgeVector::iterator it2 = this->m_edges.begin();
			for (; it2 != this->m_edges.end(); ++it2)
				if (edge == *it2)
				{
					this->m_edges.erase(it2);
					break;
				} //if
			vec_nodes::iterator it3 = this->m_nodes.begin();
			for (; it3 != this->m_nodes.end(); ++it3)
				if (child == *it3)
				{
					this->m_nodes.erase(it3);
					break;
				} //if
		} //if
	} //if
	return *(new AirwaysTree());
}

void AirwaysTree::MarkLevels(const int& level, Node* currentNode)
{
	if (currentNode == NULL)
		currentNode = this->m_root;
	if (currentNode->GetLevel() <= level)
	{
		currentNode->Mark();
		vec_nodes::const_iterator it = currentNode->GetChildren().begin();
		for (; it != currentNode->GetChildren().end(); it++)
			this->MarkLevels(level, *it);
	} //if
}

bool AirwaysTree::IsEmpty() const
{
	return this->m_root == NULL;
}

bool AirwaysTree::IsNodeInsidePath(unsigned int idNode_starting, unsigned int  idNode_ending, unsigned int  idNode_searched)
{
	// Check if the starting point exist
	Node* node_starting = this->m_root->GetNodeById(idNode_starting);
	if(!node_starting)
		return false;

	// Check if the searched node exist in the subtree of the starting node
	Node* node_searched = node_starting->GetNodeById(idNode_searched);
	if(!node_searched)
		return false;

	// Check if the ending node exist in the subtree of the searched node
	if(!node_searched->HasNodeId(idNode_ending))
		return false;

	// If all conditions were true then the node is in the path
	return true;
}

void AirwaysTree::SetRoot(Node* root)
{
	this->m_root = root;
	this->m_root->SetLevel(0);
	this->m_nodes.push_back(this->m_root);
}

void AirwaysTree::SetImageName(const std::string& img_name)
{
	this->m_img_name = img_name;
}

void AirwaysTree::SetResultPath(const std::string& folderPath)
{
	this->m_result_path = folderPath;
}

bool AirwaysTree::Insert(Edge* edge)
{
	if (this->m_root == NULL)
		return false;
	// Search if the source or target coordinates exist in the tree
	Node* source = this->GetNode(edge->m_source->GetCoords());
	Node* target = this->GetNode(edge->m_target->GetCoords());

	// If the source and the target coordinates do not exist then the
	// edge can not be added, also if both exist in the tree.
	if ( ( source == NULL && target == NULL ) || ( source != NULL && target != NULL) )
		return false;
	else if (source == NULL && target != NULL) // If target was found
	{
		// Change the direction of the edge
		edge->m_target = edge->m_source;
		edge->m_source = target;
	}
	else
		edge->m_source = source;

	// Set the target and source nodes for the Edge
	edge->m_target->SetEdge(edge);
	edge->m_target->SetLevel(edge->m_source->GetLevel() + 1);
	edge->m_source->AddChild(edge->m_target);


	// Add the new child node
	this->m_nodes.push_back(edge->m_target);
	this->m_edges.push_back(edge);

	// Set the id node
	edge->m_target->SetId(this->m_nodes.size());
	return true;
}

void AirwaysTree::UnMarkAll(Node* currentNode)
{
	if (currentNode == NULL)
		currentNode = this->m_root;
	currentNode->UnMark();

	vec_nodes::const_iterator it = currentNode->GetChildren().begin();
	for (; it != currentNode->GetChildren().end(); ++it)
		this->UnMarkAll(*it);
}

void AirwaysTree::UpdateLevels(unsigned int levels)
{
	this->m_root->UpdateLevels(levels);
}

void AirwaysTree::CompareTreesOrkiszMorales(AirwaysTree& treeB, unsigned int Q, unsigned int F, vec_nodes& commonA, vec_nodes& commonB, vec_nodes& nonCommonA, vec_nodes& nonCommonB, std::map< unsigned int, std::vector<Node*> >& map_A_to_B, std::map< unsigned int, std::vector<Node*> >& map_B_to_A, std::vector< std::pair< std::pair<Node*, Node*>, std::pair<Node*, Node*> > >& vector_pair_edges_A_to_B)
{
	// 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 intemediary nodes do not have correpondece.
	//   2.6 Run the process (First step) using the found pair of nodes.

	if( !this->IsEmpty() && !treeB.IsEmpty() )
	{

		// ----------------------------
		// ---- Pre-Matching steps ----

		// Get the root nodes
		Node* root_A = this->m_root;
		Node* root_B = treeB.m_root;

		// Mark the roots
		root_A->Mark();
		root_B->Mark();

		// Add the root match
		map_A_to_B[root_A->GetId()].push_back(root_B);
		map_B_to_A[root_B->GetId()].push_back(root_A);

		// Add the root nodes match
		pair_nodes pair_edgeA(root_A, root_A);
		pair_nodes pair_edgeB(root_B, root_B);
		std::pair< pair_nodes, pair_nodes > pairRoorNodes_edge(pair_edgeA, pair_edgeB);
		vector_pair_edges_A_to_B.push_back(pairRoorNodes_edge);

		// ----------------------------
		// ----- Run the comparison----

		root_A->CompareSubTreesOrkiszMorales(root_B, Q, F, commonA, commonB, nonCommonA, nonCommonB, map_A_to_B, map_B_to_A, vector_pair_edges_A_to_B);

	}
	else
		std::cout << "Trees are empties." << std::endl;
}

void AirwaysTree::MarkPathFromNodeToNode(Node* node_begin, Node* node_end)
{
	if(this->m_root)
		this->m_root->MarkPathFromNodeToNode(node_begin, node_end);
}

void AirwaysTree::CompareTrees(AirwaysTree& treeB)
{
	// Compare all the "branches" (paths) in treeA with all branches in treeB
	Vector_Pair_PairNodes_Score vectorPairNodesScore = this->CompareAllBranches(treeB);

	std::cout << "Vector pairs size:" <<  vectorPairNodesScore.size() << std::endl;

	// Algorithm to find the comment branches (paths)
	// Until one of the trees is empty
	// 1. Find the pair, with at least one leaf in the pair, with the lowest score
	// 2. Compare the score of the father node of the leaf, or one of the leafs, with the correspondent pair leaf
	//   If the score is lower that the score of the actual pair
	//      - The compared leaf is not common. Mark the compared leaf, delete the compared leave and save the leaf as not common.
	//   If the score is greater
	//      - The leafs are common then mark both leafs as commons. Mark both leaves, delete them and save them as common leafs.

	// Variables
	//AirwaysTree* tree_copyA = this->GetCopy();
	//AirwaysTree* tree_copyB = treeB.GetCopy();

	//NodeVector nonCommonNodes_A;
	//NodeVector commonNodes_A;
	//NodeVector nonCommonNodes_B;
	//NodeVector commonNodes_B;

	std::map<unsigned int, Node*> nonCommonNodes_A;
	std::map<unsigned int, Node*> commonNodes_A;
	std::map<unsigned int, Node*> nonCommonNodes_B;
	std::map<unsigned int, Node*> commonNodes_B;

	// Until one of the trees is not empty
	while( this->GetWeight() > 1 && treeB.GetWeight() > 1 )
	{
		// Print the weight of the trees
		std::cout << "Weight [A,B]: [" << this->GetWeight() << "," << treeB.GetWeight() << "]" << std::endl;
		std::cout << "Leafs [A,B]: [" << this->GetNumberOfLeafs() << "," << treeB.GetNumberOfLeafs() << "]" << std::endl;
		std::cout << "Vector pairs size:" <<  vectorPairNodesScore.size() << std::endl;


		// 1. Find the pair, with at least one leaf in the pair, with the lowest score
		Pair_PairNodes_Score pair_nonMarked = this->GetBestLeaf(vectorPairNodesScore);
		Node* node_actualA = pair_nonMarked.first.first;
		Node* node_actualB = pair_nonMarked.first.second;
		double score_actual = pair_nonMarked.second;

		// 2. Take the leaf (L) and his father (F), the other node is called (O). Compare the score of the father node (F) with the correspondent pair leaf (O).
		if(node_actualA->IsLeaf())
		{
			//const Edge* edgeActual = node_actualA->GetEdge();
			//Node* targetActual = edgeActual->GetTarget();
			//const unsigned int id_father = targetActual->GetId();
			//double score_fatherA_to_B = this->GetPairNodes_Score(vectorPairNodesScore, 0, node_actualB->GetId());
			double score_fatherA_to_B = this->GetPairNodes_Score(vectorPairNodesScore, node_actualA->GetEdge()->GetTarget()->GetId(), node_actualB->GetId());
			//double score_fatherA_to_B =  pair_fatherA_to_B.second;
			// If the score is lower than the score of the actual pair
			if(score_fatherA_to_B < score_actual)
			{
				//- The compared leaf is not common. Mark the compared leaf, delete the compared leave and save the leaf as not common.
				node_actualA->Mark();
				std::map<unsigned int, Node*>::iterator it;
				it=nonCommonNodes_A.find(node_actualA->GetId());
				if(it==nonCommonNodes_A.end())
					nonCommonNodes_A[node_actualA->GetId()] = node_actualA;
				//nonCommonNodes_A.push_back(node_actualA);
			}
			// If the score is greater
			else
			{
				// - The leafs are common then mark both leafs as commons. Mark both leaves, delete them and save them as common leafs.
				node_actualA->Mark();
				node_actualB->Mark();

				std::map<unsigned int, Node*>::iterator it;
				it=commonNodes_B.find(node_actualB->GetId());
				if(it==commonNodes_B.end())
					commonNodes_B[node_actualB->GetId()] = node_actualB;

				it=commonNodes_A.find(node_actualA->GetId());
				if(it==commonNodes_A.end())
				{
					std::cout << "CommonNode A:" << node_actualA->GetId() << std::endl;
					commonNodes_A[node_actualA->GetId()] = node_actualA;
				}

				//commonNodes_B.push_back(node_actualB);
				//commonNodes_A.push_back(node_actualA);
			}
			this->DeleteNodeById(node_actualA->GetId());
			this->DeleteEntriesByIdNode(vectorPairNodesScore, 0, node_actualA->GetId());
		}
		else
		{
			double score_fatherB_to_A = this->GetPairNodes_Score(vectorPairNodesScore, node_actualA->GetId(), node_actualB->GetEdge()->GetTarget()->GetId());
			//double score_fatherB_to_A =  pair_fatherB_to_A.second;
			// If the score is lower that the score of the actual pair
			if(score_fatherB_to_A < score_actual)
			{
				//- The compared leaf is not common. Mark the compared leaf, delete the compared leave and save the leaf as not common.
				node_actualB->Mark();
				std::map<unsigned int, Node*>::iterator it;
				it=nonCommonNodes_B.find(node_actualB->GetId());
				if(it==nonCommonNodes_B.end())
					nonCommonNodes_B[node_actualB->GetId()] = node_actualB;
				//nonCommonNodes_B.push_back(node_actualB);
			}
			// If the score is greater
			else
			{
				// - The leafs are common then mark both leafs as commons. Mark both leaves, delete them and save them as common leafs.
				node_actualA->Mark();
				node_actualB->Mark();
				//commonNodes_B.push_back(node_actualB);
				//commonNodes_A.push_back(node_actualA);

				std::map<unsigned int, Node*>::iterator it;
				it=commonNodes_B.find(node_actualB->GetId());
				if(it==commonNodes_B.end())
					commonNodes_B[node_actualB->GetId()] = node_actualB;

				it=commonNodes_A.find(node_actualA->GetId());
				if(it==commonNodes_A.end())
					commonNodes_A[node_actualA->GetId()] = node_actualA;
			}
			treeB.DeleteNodeById(node_actualB->GetId());
			this->DeleteEntriesByIdNode(vectorPairNodesScore, 1, node_actualB->GetId());
		}
	}

	// Print the results
	std::cout << "Non-CommonNodes [A,B]" <<  nonCommonNodes_A.size() << ", " << nonCommonNodes_B.size() << std::endl;
	std::cout << "CommonNodes [A,B]" <<  commonNodes_A.size() << ", " << commonNodes_B.size() << std::endl;

	// Get the branches with the lowest difference
	/*bool first = true;
	double lowestValue = 0;
	Pair_Node_Node bestPairNodes;
	int iteration = 1;
	for(Vector_Pair_PairNodes_Score::iterator it = vectorPairNodesScore.begin(); it != vectorPairNodesScore.end(); ++it )
	{
		if(first)
		{
			lowestValue = (*it).second;
			bestPairNodes = (*it).first;
			first = false;
		}
		else if( (*it).second < lowestValue )
		{
			lowestValue = (*it).second;
			bestPairNodes = (*it).first;
		}
	}

	// Print the best pair of nodes
	if(vectorPairNodesScore.size() > 0)
		std::cout << "Best pair: Node A:" << (bestPairNodes).first->GetId() << ", Node B:" << (bestPairNodes).second->GetId() << std::endl;
	 */

	/*// AMP - This code was commented after a meeting with Leonardo Florez
	// We make the hypothesis that both roots are equal and
	// that the root has only one child.
	// The comparison of two branches implies the alignment
	// of the mother branches and then the comparison of the
	// daughters branches using intrinsic characteristics of
	// each branch and a distance function between both.

	if (this->IsEmpty() || treeB.IsEmpty())
	{
		std::cout << "One or both airways are empty." << std::endl;
		return;
	}

	Edge actualRootEdge = this->m_root->GetEdge();
	Edge comparedRootEdge = treeB.m_root->GetEdge();

	// By the hypothesis, both root edges are similar. So we have to compared
	// the edged of the root edges.
	actualRootEdge.compareChildEdges(comparedRootEdge);
	 */
}

Pair_PairNodes_Score AirwaysTree::GetBestLeaf(Vector_Pair_PairNodes_Score vectorPairNodesScore)
{
	// Variables
	Pair_PairNodes_Score pairNodesScore_final;
	bool first = true;

	for(Vector_Pair_PairNodes_Score::iterator it = vectorPairNodesScore.begin(); it != vectorPairNodesScore.end(); ++it)
	{
		// Check that one of the node is a leaf
		pair_nodes pair_actual = (*it).first;
		Node* a = pair_actual.first;
		Node* b = pair_actual.second;
		if( a->IsLeaf() || b->IsLeaf() )
		{
			if(first)
			{
				first = false;
				pairNodesScore_final = (*it);
			}
			else if((*it).second < pairNodesScore_final.second)
			{
				pairNodesScore_final = (*it);
			}
		}
	}

	if(first)
		std::cout << "Best leaf not found" << std::endl;

	return pairNodesScore_final;
}

double AirwaysTree::GetPairNodes_Score(Vector_Pair_PairNodes_Score vectorPairNodesScore, const unsigned int idA, const unsigned int idB)
{
	bool found = false;
	double score = -1;

	for(Vector_Pair_PairNodes_Score::iterator it = vectorPairNodesScore.begin(); it != vectorPairNodesScore.end(); ++it)
	{
		// Check that one of the node is a leaf
		pair_nodes pair_actual = (*it).first;
		Node* a = pair_actual.first;
		Node* b = pair_actual.second;
		if( a->GetId() == idA && b->GetId() == idB )
		{
			found = true;
			score = (*it).second;
		}
	}

	if(!found)
		std::cout << "Pair of Ids not found: " << idA << ", " << idB << std::endl;

	return score;
}

bool AirwaysTree::DeleteNodeById(unsigned int nId)
{
	bool deleted = false;
	if(m_root->GetId() == nId)
	{
		delete(m_root);
		m_root = NULL;
		return true;
	}

	return this->m_root->DeleteNodeById(nId);
}

unsigned int AirwaysTree::DeleteEntriesByIdNode(Vector_Pair_PairNodes_Score& vectorPairNodesScore, unsigned int component, unsigned int nId)
{
	unsigned int pairs_deleted = 0;

	Vector_Pair_PairNodes_Score::iterator it = vectorPairNodesScore.begin();
	while(it != vectorPairNodesScore.end())
	{
		// Check that one of the node is a leaf
		pair_nodes pair_actual = (*it).first;
		unsigned int id_actual = -1;
		if(component == 0)
			id_actual = pair_actual.first->GetId();
		else
			id_actual = pair_actual.second->GetId();

		if( id_actual == nId)
		{
			it = vectorPairNodesScore.erase(it);
			++pairs_deleted;
		}
		else
			++it;
	}

	std::cout << "Pairs deleted: " << pairs_deleted << std::endl;

	return pairs_deleted;
}

AirwaysTree* AirwaysTree::GetCopy()
{
	return NULL;
}

Vector_Pair_PairNodes_Score AirwaysTree::CompareAllBranches(AirwaysTree& treeB)
{
	std::cout << "Compare all branches  ..." << std::endl;
	// Variables
	int nodesA = this->m_nodes.size();
	int nodesB = treeB.m_nodes.size();

	Vector_Pair_PairNodes_Score vectorPairNodesScore;

	// For all the actual branches ( nodes different from the root)
	for(unsigned int idA = 2; idA <= nodesA; ++idA)
	{
		// Get the actual branch in A
		Edge* branchA = this->GetComposedEdge(idA);

		//Get the final actual node in A
		Node* node_actualA = this->GetNodeById(idA);

		std::cout << "IdA: " << idA << std::endl;

		// For all the branches in B tree
		for(unsigned int idB = 2; idB <= nodesB; ++idB)
		{
			// Get the actual branch in B
			Edge* branchB = treeB.GetComposedEdge(idB);

			//Get the final actual node in B
			Node* node_actualB = treeB.GetNodeById(idB);

			// Get the score from A to B and from B to A
			double scoreA2B = branchA->CompareWith(branchB);
			double scoreB2A = branchB->CompareWith(branchA);

			// Get the final score
			// Options:
			// A. Maximum score
			//double score = scoreB2A > scoreA2B ? scoreB2A : scoreA2B ;

			// B. Sum of scores
			double score = scoreB2A + scoreA2B;

			// Build the pair of nodes
			pair_nodes actualPairNodes(node_actualA, node_actualB);

			Pair_PairNodes_Score actualPairNodesScore(actualPairNodes, score);
			vectorPairNodesScore.push_back(actualPairNodesScore);

			//std::cout << "IdA: " << idA << ", IdB: " << idB << ", score: " << score << ", ... OK" << std::endl;
			delete(branchB);
		}
		delete(branchA);
	}

	std::cout << "Compare all branches  ... OK" << std::endl;
	return vectorPairNodesScore;
}

Edge* AirwaysTree::GetComposedEdge(unsigned int idNode)
{
	return this->m_root->GetComposedEdge(idNode);
}

AirwaysTree* AirwaysTree::GetSingleDetachedTreeNodeById(unsigned int nId)
{
	AirwaysTree* tree_detached = NULL;
	Node* node_searched = this->GetNodeById(nId);
	if(node_searched)
	{
		Node* node_root_detached = node_searched->GetDetachedRootCopy();
		tree_detached = new AirwaysTree();
		tree_detached->m_root = node_root_detached;
	}
	else
		std::cout << "Node not found" << std::endl;
	return tree_detached;
}

void AirwaysTree::SubIsomorphism(AirwaysTree& treeB)
{
	if (this->IsEmpty() || treeB.IsEmpty())
		return;
	//Marking the root node
	this->m_root->Mark();
	treeB.m_root->Mark();

	//iterating children - first step
	vec_nodes::const_iterator it = this->m_root->GetChildren().begin();
	for (; it != this->m_root->GetChildren().end(); ++it)
	{
		unsigned int totalCost = 0;
		Node* currentB = NULL;
		Node* currentA = *it;
		if (currentA->IsMarked())
			continue;
		vec_nodes::const_iterator it2 = treeB.m_root->GetChildren().begin();
		for (; it2 != treeB.m_root->GetChildren().end(); ++it2)
		{
			if ((*it2)->IsMarked())
				continue;
			unsigned int currentCost = (*it)->GetCost(*it2);
			if (totalCost < currentCost)
			{
				currentB = *it2;
				totalCost = currentCost;
			} //if
		} //for
		if (currentB != NULL)
			if (*currentA == *currentB)
				this->GetSubTreeByDetachingNode(currentA).SubIsomorphism(
						treeB.GetSubTreeByDetachingNode(currentB));
	} //for
	//end of first step
	//second step
	it = this->m_root->GetChildren().begin();
	for (; it != this->m_root->GetChildren().end(); ++it)
	{
		if ((*it)->IsMarked())
			continue;
		vec_nodes::const_iterator it2 = treeB.m_root->GetChildren().begin();
		for (; it2 != treeB.m_root->GetChildren().end(); ++it2)
		{
			Node* currentB = *it2;
			Node* currentA = *it;
			if (*currentB > *currentA)
				continue;
			if (currentA->FindSimilarEdgeByAccumulation(currentB))
			{
				AirwaysTree subTreeA = this->GetSubTreeByDetachingNode(
						currentA);
				AirwaysTree subTreeB = treeB.GetSubTreeByDetachingNode(
						currentB);
				subTreeB.UpdateLevels((*it2)->GetLevel());
				subTreeA.SubIsomorphism(subTreeB);
				treeB.m_root->UpdateLevels(treeB.m_root->GetLevel());
				currentA->Mark();
				if (!(*it2)->MarkPath(currentB))
					std::cout << "Error finding path" << std::endl;
				break;
			} //if
		} //for
	} //for
	vec_nodes::const_iterator it2 = treeB.m_root->GetChildren().begin();
	for (; it2 != treeB.m_root->GetChildren().end(); ++it2)
	{
		if ((*it2)->IsMarked())
			continue;
		it = this->m_root->GetChildren().begin();
		for (; it != this->m_root->GetChildren().end(); ++it)
		{
			Node* currentB = *it2;
			Node* currentA = *it;
			if (*currentA > *currentB)
				continue;
			if (currentB->FindSimilarEdgeByAccumulation(currentA))
			{
				AirwaysTree subTreeA = this->GetSubTreeByDetachingNode(
						currentA);
				AirwaysTree subTreeB = treeB.GetSubTreeByDetachingNode(
						currentB);
				subTreeA.UpdateLevels((*it)->GetLevel());
				subTreeA.SubIsomorphism(subTreeB);
				this->m_root->UpdateLevels(this->m_root->GetLevel());
				currentB->Mark();
				if (!(*it)->MarkPath(currentA))
					std::cout << "Error finding path" << std::endl;
				break;
			} //if
		} //for
	}
	//End of second step
}

bool AirwaysTree::Isomorphism(AirwaysTree& tree)
{
	if ((this->GetWeight() != tree.GetWeight())
			|| (this->GetHeight() != tree.GetHeight()))
		return false;
	return this->m_root->CompareNodes(tree.m_root);
}

void AirwaysTree::ImageReconstructionFromNode(TInputImage::Pointer& result,	Node* node, bool skeleton)
{
	if (node == NULL)
		return;
	//Finding the center of the sphere
	const Edge* edge = node->GetEdge();
	if (edge == NULL)
		return;
	//making a copy of the current image
	TInputImage::Pointer copy;
	DuplicatorType::Pointer duplicator = DuplicatorType::New();
	if (skeleton)
		duplicator->SetInputImage(this->m_skeleton);
	else
		duplicator->SetInputImage(this->m_img);
	duplicator->Update();
	copy = duplicator->GetOutput();
	//First remove the node brothers -- the following code in a way does not
	// make sense but I'll just comment it if it is necessary
	/*Node* parent = edge->m_source;
     NodeVector::iterator pIt = parent->m_children.begin();
     for (; pIt != parent->m_children.end(); ++pIt)
     if ((*pIt) != node)
     this->RemoveBranchFromImage(copy, node);
     //end of brother removal*/
	pair_posVox_rad center;
	vec_pair_posVox_rad::const_iterator it = edge->m_vec_pair_posVox_rad.begin();
	for (; it != edge->m_vec_pair_posVox_rad.end(); ++it)
		if (edge->m_source->GetCoords() == (*it).first)
			center = *it;
	//converting to index
	TInputImage::PointType point;
	point[0] = center.first[0];
	point[1] = center.first[1];
	point[2] = center.first[2];
	TInputImage::IndexType indCenter;
	if (skeleton)
		this->m_skeleton->TransformPhysicalPointToIndex(point, indCenter);
	else
		this->m_img->TransformPhysicalPointToIndex(point, indCenter);
	//creating itk sphere
	SphereType::Pointer sphere = SphereType::New();
	sphere->SetRadius(center.second * 4);
	SphereType::InputType sphereCenter;
	sphereCenter[0] = indCenter[0];
	sphereCenter[1] = indCenter[1];
	sphereCenter[2] = indCenter[2];
	sphere->SetCenter(sphereCenter);

	//testing code
	//	std::cout<<"center = "<< indCenter << std::endl;
	//std::cout << "Radio = " << center.second << std::endl;
	TInputImage::SizeType regionSize;
	regionSize[0] = 8 * center.second;
	regionSize[1] = 8 * center.second;
	regionSize[2] = 8 * center.second;
	//std::cout << "RegionSize = " << regionSize << std::endl;

	TInputImage::IndexType regionIndex;
	regionIndex[0] = indCenter[0] - center.second * 4;
	regionIndex[1] = indCenter[1] - center.second * 4;
	regionIndex[2] = indCenter[2] - center.second * 4;
	//	std::cout << "regionIndex = " << regionIndex << std::endl;

	TInputImage::RegionType region;
	region.SetSize(regionSize);
	region.SetIndex(regionIndex);
	RegionIterator it2(copy, region);
	while (!it2.IsAtEnd())
	{
		TInputImage::IndexType ind = it2.GetIndex();
		SphereType::InputType point;
		point[0] = ind[0];
		point[1] = ind[1];
		point[2] = ind[2];
		SphereType::OutputType output = sphere->Evaluate(point);
		if (output)
			it2.Set(0);
		++it2;
	}

	/*
	 * First region growing - Segmenting the wanted branch to be removed
	 * Removing the branch from the image
	 */
	if (!ComputeSimpleRegionGrowing(copy, this->m_root->GetCoords()))
		return;
	SubtractImageFilterType::Pointer subtractFilter =
			SubtractImageFilterType::New();
	if (skeleton)
		subtractFilter->SetInput1(this->m_skeleton);
	else
		subtractFilter->SetInput1(this->m_img);
	subtractFilter->SetInput2(copy);
	subtractFilter->Update();
	TInputImage::Pointer diff = subtractFilter->GetOutput();
	/*
	 * Second region growing - Cleaning the resulting image
	 */
	Node* leaf = this->GetLeaf(node);
	if (ComputeSimpleRegionGrowing(diff, leaf->GetCoords()))
		result = diff;
	else
		std::cout << "problemas" << std::endl;
	//cin.ignore().get(); //Pause Command for Linux Terminal

}

void AirwaysTree::ImageReconstruction(TInputImage::Pointer& result, bool skeleton, Node* node)
{
	if (node == NULL)
	{
		DuplicatorType::Pointer duplicator = DuplicatorType::New();
		if (skeleton)
			duplicator->SetInputImage(this->m_skeleton);
		else
			duplicator->SetInputImage(this->m_img);
		duplicator->Update();
		result = duplicator->GetOutput();
		node = this->m_root;
	} //if
	if (!node->IsMarked())
	{
		this->RemoveBranchFromImage(result, node);
		return;
	}
	vec_nodes::const_iterator it = node->GetChildren().begin();
	for (; it != node->GetChildren().end(); ++it)
		this->ImageReconstruction(result, skeleton, *it);

}

void AirwaysTree::ImageReconstructionBySpheres(TInputImage::Pointer& result, Node* node, bool useMarks)
{
	if (node == NULL)
	{
		node = this->m_root;
		DuplicatorType::Pointer duplicator = DuplicatorType::New();
		duplicator->SetInputImage(m_img);
		duplicator->Update();
		result = duplicator->GetOutput();
		//Removing white pixels -- cleaning image
		TInputImage::RegionType lRegion = result->GetLargestPossibleRegion();
		RegionIterator it(result, lRegion);
		while (!it.IsAtEnd())
		{
			it.Set(0);
			++it;
		} //while
	} //if
	if (node->IsMarked() || !useMarks)
	{
		this->CreateSpheres(result, node);
		vec_nodes::const_iterator it = node->GetChildren().begin();
		for (; it != node->GetChildren().end(); ++it)
			this->ImageReconstructionBySpheres(result, *it);
	} //if
}

Node* AirwaysTree::GetNode(const Vec3 nodeCoords, Node* current)
{
	if (current == NULL)
		current = this->m_root;
	if (current->GetCoords() == nodeCoords)
		return current;
	vec_nodes::const_iterator it = current->GetChildren().begin();
	for (; it != current->GetChildren().end(); ++it)
	{
		Node* child = GetNode(nodeCoords, *it);
		if (child != NULL)
			return child;
	} //rof
	return NULL;
}

Node* AirwaysTree::GetLeaf(Node* current)
{
	if (current == NULL)
		current = this->m_root;
	if (current->IsLeaf())
		return current;
	vec_nodes::const_iterator it = current->GetChildren().begin();
	Node* node = NULL;
	unsigned int maxHeight = 0;
	for (; it != current->GetChildren().end(); ++it)
	{
		unsigned int cHeight = this->GetHeight(*it);
		if (maxHeight <= cHeight)
		{
			maxHeight = cHeight;
			node = *it;
		} //if
	} //for
	if (node != NULL)
		return this->GetLeaf(node);
	return NULL;
}

void AirwaysTree::UpdateEdges(Node* node)
{
	if (node == NULL)
		node = this->m_root;

	if (node->GetEdge() != NULL)
	{
		Edge* edge = node->GetEdge();
		//edge->m_skInfoPairVector = this->GetSkeletonInfoFromEdge(edge->m_source->GetCoords(), edge->m_target->GetCoords());
		edge->SetSkeletonPairVector( this->GetSkeletonInfoFromEdge(edge->m_source->GetCoords(), edge->m_target->GetCoords()) );
		node->GetEdge()->UpdateEdgeInfo();
	}
	vec_nodes::const_iterator it = node->GetChildren().begin();
	for (; it != node->GetChildren().end(); ++it)
		this->UpdateEdges(*it);
}

AirwaysTree& AirwaysTree::GetSubTreeByDetachingNode(Node* node)
{
	AirwaysTree* tree = new AirwaysTree();
	tree->m_root = node;
	return *tree;
}

DiGraph_EdgePair AirwaysTree::AddBoostEdge(const pair_posVox_rad& source, const pair_posVox_rad& target, DiGraph& graph)
{
	bool vFound[2] = { false, false };
	DiGraph_VertexDescriptor vDescriptor[2];

	//Remember that DiGraph_VertexIteratorPair, first = begin, second = end
	DiGraph_VertexIndexMap index = get(boost::vertex_index, graph);

	//Search the vertex in the graph
	for (DiGraph_VertexIteratorPair vPair = boost::vertices(graph);	vPair.first != vPair.second && (!vFound[0] || !vFound[1]); ++vPair.first)
	{
		// Get the current information [vector, intensity]
		pair_posVox_rad current = graph[index[*vPair.first]];

		// Check if current is equal to the source or target vertex
		if (current.first == source.first)
		{
			vDescriptor[0] = index[*vPair.first];
			vFound[0] = true;
		} //if
		else if (current.first == target.first)
		{
			vDescriptor[1] = index[*vPair.first];
			vFound[1] = true;
		} //if
	} //for

	// If one of the vertices is not in the graph, adds the new vertex

	// Add the vertices if they are not in the graph
	if (!vFound[0])
		vDescriptor[0] = boost::add_vertex(source, graph);
	if (!vFound[1])
		vDescriptor[1] = boost::add_vertex(target, graph);

	// Check if the edge exist in the graph
	// boost::edge returns the pair<edge_descriptor, bool>, with bool=true is the edge exists
	DiGraph_EdgePair edgeC1 = boost::edge(vDescriptor[0], vDescriptor[1],graph);
	DiGraph_EdgePair edgeC2 = boost::edge(vDescriptor[1], vDescriptor[0],graph);

	// If the edge does not exist, then add it
	if (!edgeC1.second && !edgeC2.second)
	{
		DiGraph_EdgePair nEdge = boost::add_edge(vDescriptor[0], vDescriptor[1], graph);
		return nEdge;
	} //if

	// If an edge has not been added then -> second = false
	// Remove the added vertices because the edge already exists.
	if (!vFound[0])
		boost::remove_vertex(vDescriptor[0], graph);

	if (!vFound[1])
		boost::remove_vertex(vDescriptor[1], graph);

	edgeC1.second = false;
	edgeC1.second = false;

	return edgeC1;
}

bool AirwaysTree::FindNeighborhood(DiGraph& graph, Vec3Queue& queue, Vec3List& used, const Vec3& end, ConstNeighborhoodIterator& iterator)
{
	if (queue.empty())
		return false;

	// Get the next pixel in the queue
	Vec3 current = queue.front();

	// Put the pixel in the used list
	used.push_back(current);

	// Delete next pixel from the queu
	queue.pop();

	//AMP//iterator.GoToBegin();
	//AMP//while (!iterator.IsAtEnd())
	//AMP//{

	// AMP commented
	/*unsigned int centerIndex = iterator.GetCenterNeighborhoodIndex();
		TInputImage::IndexType index = iterator.GetIndex(centerIndex);
		SKPairInfo src(Vec3(index[0], index[1], index[2]),
				iterator.GetPixel(iterator.GetCenterNeighborhoodIndex()));
		if (src.first != current)
		{
			++iterator;
			continue;
		}	//if
	 */
	//PMA commented

	// AMP

	// Set the current index to the actual pixel
	TInputImage::IndexType indexActual;
	indexActual[0] = current[0];
	indexActual[1] = current[1];
	indexActual[2] = current[2];

	// Place the iterator in the actual index
	iterator.SetLocation(indexActual);

	// Create the pair [vector, intensity] for the actual index
	pair_posVox_rad src(current, iterator.GetPixel(iterator.GetCenterNeighborhoodIndex()));

	// PMA

	// Iterate over the neighbors
	for (unsigned int i = 0; i < iterator.Size(); i++)
	{
		//AMP//if (iterator.GetPixel(i) != 0 && i != centerIndex)

		// If the intensity is not zero and it's the center
		if (iterator.GetPixel(i) != 0 && i != iterator.GetCenterNeighborhoodIndex())
		{
			// Get the index of the iterator
			TInputImage::IndexType ind = iterator.GetIndex(i);

			// Create the target pair [vector, intensity] for the target
			pair_posVox_rad target(Vec3(ind[0], ind[1], ind[2]),	iterator.GetPixel(i));

			// Add the edge to the graph
			bool added = AddBoostEdge(src, target, graph).second;

			// If it could not be added then it is because it already exist, then go to next neighboor
			if (!added)
				continue;

			// The edge was added. Stop the adding process if the target vertex was reached.
			if (target.first == end)
				return true;

			// If it the edge was added and it is not the target then add it to the queue, if and only if
			// it was not already used.
			if (std::find(used.begin(), used.end(), target.first)== used.end())
				queue.push(target.first);
		}//if
	}//for
	//AMP//break;
	//AMP//}	//while
	return false;
}

DiGraph AirwaysTree::GetGraphShortestPath(const DiGraph& graph, const Vec3& begin, const Vec3& end)
{
	// Variables
	DiGraph ret; // Returning graph with the shortest path
	DiGraph_EdgeIterator ei, eo; // Initial and final iterators

	// Iterate over the edges of the graph
	boost::tie(ei, eo) = boost::edges(graph);
	for (; ei != eo; ++ei)
	{
		// Get the source of the actual edge
		pair_posVox_rad src = graph[boost::source(*ei, graph)];

		// If the source corresponds to the begin pixel
		if (src.first == begin)
		{
			// Get the target of the actual edge
			pair_posVox_rad target = graph[boost::target(*ei, graph)];

			// If the actual target corresponds to the end pixel
			if (target.first == end)
			{
				// Add the edge to the final path
				AddBoostEdge(src, target, ret);

				//AMP
				std::cout << "Add final target [src]-[ target]:[" << src.first << "]-[" << target.first << "], numVertices:" << boost::num_vertices(ret) << std::endl;
				//PMA

				// Return the path
				return ret;
			}

			// If the actual target does not correspond to the end pixel, then add it to the returning path
			// and run the shortest path with the target of the actual edge.
			ret = GetGraphShortestPath(graph, target.first, end);

			// If the path is empty then the iteration continues
			if (boost::num_vertices(ret) == 0)
				continue;

			// if not return ret + new edge
			// If the path was found then add the actual edge and return the path
			AddBoostEdge(src, target, ret);
			//AMP
			//std::cout << "Add found edge [src]-[ target]:[" << src.first << "]-[" << target.first << "], numVertices:" << boost::num_vertices(ret) << std::endl;
			//PMA
			return ret;

		}//fi
	}
	return ret;
}

vec_pair_posVox_rad AirwaysTree::GetGraphShortestPath_AMP(const DiGraph& graph, const Vec3& begin, const Vec3& end)
{
	//std::cout << "SKPairInfoVector AirwaysTree::GetGraphShortestPath_AMP(const DiGraph& graph, const Vec3& begin, const Vec3& end)" << std::endl;
	// Variables
	vec_pair_posVox_rad path;
	DiGraph ret; // Returning graph with the shortest path
	DiGraph_EdgeIterator ei, eo; // Initial and final iterators

	// Iterate over the edges of the graph
	boost::tie(ei, eo) = boost::edges(graph);
	for (; ei != eo; ++ei)
	{
		// Get the source of the actual edge
		pair_posVox_rad src = graph[boost::source(*ei, graph)];

		// If the source corresponds to the begin pixel
		if (src.first == begin)
		{
			// Get the target of the actual edge
			pair_posVox_rad target = graph[boost::target(*ei, graph)];

			// If the actual target corresponds to the end pixel
			if (target.first == end)
			{
				// Add the edge to the final path
				AddBoostEdge(src, target, ret);
				path.push_back(target);
				path.push_back(src);

				//AMP
				//std::cout << "Add final target [src]-[ target]:[" << src.first << "]-[" << target.first << "], numVertices:" << boost::num_vertices(ret) << ", path_size:" << path.size() << std::endl;
				//PMA

				// Return the path
				//std::cout << "SKPairInfoVector AirwaysTree::GetGraphShortestPath_AMP(const DiGraph& graph, const Vec3& begin, const Vec3& end) ... FIRST ... OK" << std::endl;
				return path;
			}

			//std::cout << "Not yet in the target voxel" << std::endl;
			// If the actual target does not correspond to the end pixel, then add it to the returning path
			// and run the shortest path with the target of the actual edge.
			path = GetGraphShortestPath_AMP(graph, target.first, end);
			//std::cout << "Path found, path size:" << path.size() << std::endl;

			// If the path is empty then the iteration continues
			if (path.size() == 0)
				continue;

			// if not return ret + new edge
			// If the path was found then add the actual edge and return the path
			AddBoostEdge(src, target, ret);
			path.push_back(src);
			//AMP
			//std::cout << "Add found edge [src]-[ target]:[" << src.first << "]-[" << target.first << "], numVertices:" << boost::num_vertices(ret)  << ", path_size:" << path.size() << std::endl;
			//PMA
			//std::cout << "SKPairInfoVector AirwaysTree::GetGraphShortestPath_AMP(const DiGraph& graph, const Vec3& begin, const Vec3& end) ... SECOND ... OK" << std::endl;
			return path;

		}//fi
	}

	//std::cout << "SKPairInfoVector AirwaysTree::GetGraphShortestPath_AMP(const DiGraph& graph, const Vec3& begin, const Vec3& end) ... END ... OK" << std::endl;
	return path;
}

vec_pair_posVox_rad AirwaysTree::GetSkeletonInfoFromEdge(const Vec3& source, const Vec3& target)
{
	//converting to pixel
	TInputImage::PointType pointSource;
	pointSource[0] = source[0];
	pointSource[1] = source[1];
	pointSource[2] = source[2];

	TInputImage::PointType pointTarget;
	pointTarget[0] = target[0];
	pointTarget[1] = target[1];
	pointTarget[2] = target[2];

	TInputImage::IndexType indexSource;
	TInputImage::IndexType indexTarget;

	this->m_skeleton->TransformPhysicalPointToIndex(pointSource, indexSource);
	this->m_skeleton->TransformPhysicalPointToIndex(pointTarget, indexTarget);
	//end of conversion

	// AMP // Print the initial and final voxels to verify their positions
	//std::cout << "Begin: [" << indexSource[0] << "," << indexSource[1] << "," << indexSource[2] << "] -- " << this->m_skeleton->GetPixel(indexSource) << std::endl;
	//std::cout << "End: [" << indexTarget[0] << "," << indexTarget[1] << "," << indexTarget[2] << "] -- " << this->m_skeleton->GetPixel(indexTarget) << std::endl;
	//std::cout << "BeginVoxel: [" << pointSource[0] << "," << pointSource[1] << "," << pointSource[2] << "]"<< std::endl;
	//std::cout << "EndVoxel: [" << pointTarget[0] << "," << pointTarget[1] << "," << pointTarget[2] << "]" << std::endl;
	// PMA //

	Vec3 sourcePxl(indexSource[0], indexSource[1], indexSource[2]);
	Vec3 targetPxl(indexTarget[0], indexTarget[1], indexTarget[2]);

	// AMP // Commented
	/*
	// Compute the distance in each direction and add "21" in each one
	float dstX = beginPxl.EculideanDistance(
			Vec3(endPxl[0], beginPxl[1], beginPxl[2])) + 21;
	float dstY = beginPxl.EculideanDistance(
			Vec3(beginPxl[0], endPxl[1], beginPxl[2])) + 21;
	float dstZ = beginPxl.EculideanDistance(
			Vec3(beginPxl[0], beginPxl[1], endPxl[2])) + 21;

	//start point
	TInputImage::IndexType index;
	index[0] = beginPxl[0] < endPxl[0] ? beginPxl[0] : endPxl[0];
	index[1] = beginPxl[1] < endPxl[1] ? beginPxl[1] : endPxl[1];
	index[2] = beginPxl[2] < endPxl[2] ? beginPxl[2] : endPxl[2];

	index[0] -= index[0] <= 10 ? 0 : 10;
	index[1] -= index[1] <= 10 ? 0 : 10;
	index[2] -= index[2] <= 10 ? 0 : 10;


	//size of region
	TInputImage::SizeType size;
	size[0] = ceil(dstX);
	size[1] = ceil(dstY);
	size[2] = ceil(dstZ);

	TInputImage::RegionType region;
	region.SetSize(size);
	region.SetIndex(index);

	//ConstNeighborhoodIterator iterator(radius, this->m_skeleton, region);
	// PMA // Commented
	 */

	TInputImage::SizeType radius;
	radius[0] = 1;
	radius[1] = 1;
	radius[2] = 1;
	ConstNeighborhoodIterator iterator(radius, this->m_skeleton, this->m_skeleton->GetLargestPossibleRegion());

	DiGraph graph;
	Vec3Queue queue;
	Vec3List used;

	// Set the first pixel as starting pixel
	queue.push(sourcePxl);

	//FindNeighborhood(graph, queue, used, endPxl, iterator);

	// Add to the graph all the connected voxel until the ending pixel is found
	bool endFound = false;
	while (!queue.empty() && !endFound)
		endFound = FindNeighborhood(graph, queue, used, targetPxl, iterator);

	if(!endFound)
		std::cout << "***************** END NOT FOUND!!!!" << std::endl;
	else
		std::cout << "***************** END FOUND!!!! - Graph Vertices:" << boost::num_vertices(graph) << ", Graph Edges:" << boost::num_edges(graph) << std::endl;

	// Get the shortest path between the begin and end pixels
	//std::cout << "GetGraphShortestPath [from]-[to]:[" << sourcePxl << "]-[" << targetPxl << "]" << std::endl;
	//DiGraph result = GetGraphShortestPath(graph, sourcePxl, targetPxl);
	vec_pair_posVox_rad path = GetGraphShortestPath_AMP(graph, sourcePxl, targetPxl);
	//std::cout << "GetGraphShortestPath ... OK , numVertices:" << boost::num_vertices(result) << std::endl;

	//AMP
	bool beginPixelFound = false;
	bool endPixelFound = false;
	vec_pair_posVox_rad vector;

	for(vec_pair_posVox_rad::reverse_iterator it_path = path.rbegin(); it_path != path.rend(); ++it_path)
	{
		TInputImage::IndexType ind;
		ind[0] = (*it_path).first[0];
		ind[1] = (*it_path).first[1];
		ind[2] = (*it_path).first[2];
		//std::cout << "["<< (*it_path).first << "], ";

		// To avoid precision problems
		if (indexSource == ind)
		{
			beginPixelFound = true;
			(*it_path).first = source;
		}
		else if (indexTarget == ind)
		{
			endPixelFound = true;
			(*it_path).first = target;
		}
		else
		{
			TInputImage::PointType pnt;
			this->m_skeleton->TransformIndexToPhysicalPoint(ind, pnt);
			(*it_path).first = Vec3(pnt[0], pnt[1], pnt[2]);
		}
		vector.push_back((*it_path));
	}
	//vector = path;
	//PMA

	// Variables to iterate over the shortest path and save the results
	/*// AMP
	DiGraph_VertexIterator i, e;
	SKPairInfoVector vector;

	// Iterate over the shortest path and add each vertex in real coordinates
	boost::tie(i, e) = boost::vertices(result);
	std::cout << "Inserting ... [begin,end]: [" << begin << "], [" << end << "]" << std::endl;
	for (; i != e; ++i)
	//for (; e != i; --e)
	{
		// Get the actual vertex
		SKPairInfo vertex = result[*i];
		//SKPairInfo vertex = result[*e];
		std::cout << "["<< vertex.first << "], ";

		// Get the index (voxel position) of the actual vertex
		TInputImage::IndexType ind;
		ind[0] = vertex.first[0];
		ind[1] = vertex.first[1];
		ind[2] = vertex.first[2];

		// To avoid precision problems
		if (indexBegin == ind)
		{
			beginPixelFound = true;
			vertex.first = begin;
		}
		else if (indexEnd == ind)
		{
			endPixelFound = true;
			vertex.first = end;
		}
		else
		{
			TInputImage::PointType pnt;
			this->m_skeleton->TransformIndexToPhysicalPoint(ind, pnt);
			vertex.first = Vec3(pnt[0], pnt[1], pnt[2]);
		}
		vector.push_back(vertex);
	}
	std::cout << "Inserting ... OK" << std::endl;


	 *///AMP


	if(!beginPixelFound)
		std::cout << "Begin not found: "<< source << std::endl;
	if(!endPixelFound)
		std::cout << "End not found:" << target << std::endl;

	return vector;
}

bool AirwaysTree::ComputeSimpleRegionGrowing(TInputImage::Pointer& img,
		const Vec3 seedPtn)
{
	TInputImage::PointType targetPtn;
	targetPtn[0] = seedPtn[0];
	targetPtn[1] = seedPtn[1];
	targetPtn[2] = seedPtn[2];
	TInputImage::IndexType seed;
	img->TransformPhysicalPointToIndex(targetPtn, seed);
	if (img->GetPixel(seed) == 0)
	{
		//std::cout << "seed is 0" << std::endl;
		return false;
	}
	ConnectedFilterType::Pointer neighborhoodConnected =
			ConnectedFilterType::New();
	neighborhoodConnected->SetLower(1);
	neighborhoodConnected->SetUpper(1);
	neighborhoodConnected->SetReplaceValue(1);
	neighborhoodConnected->SetSeed(seed);
	neighborhoodConnected->SetInput(img);
	neighborhoodConnected->Update();
	img = neighborhoodConnected->GetOutput();
	return true;
}

void AirwaysTree::RemoveBranchFromImage(TInputImage::Pointer& img, Node* node)
{
	//typedef itk::ImageFileWriter<TInputImage> WriterType;
	//Finding the center of the sphere
	const Edge* edge = node->GetEdge();
	if (edge == NULL)
		return;
	pair_posVox_rad center;
	vec_pair_posVox_rad::const_iterator it = edge->m_vec_pair_posVox_rad.begin();
	for (; it != edge->m_vec_pair_posVox_rad.end(); ++it)
		if (edge->m_source->GetCoords() == (*it).first)
			center = *it;
	//converting to index
	TInputImage::PointType point;
	point[0] = center.first[0];
	point[1] = center.first[1];
	point[2] = center.first[2];
	TInputImage::IndexType indCenter;
	img->TransformPhysicalPointToIndex(point, indCenter);
	double radius = center.second;
	//creating itk sphere
	SphereType::Pointer sphere = SphereType::New();
	SphereType::InputType sphereCenter;
	sphereCenter[0] = indCenter[0];
	sphereCenter[1] = indCenter[1];
	sphereCenter[2] = indCenter[2];
	sphere->SetCenter(sphereCenter);
	unsigned int objCount = 0;
	unsigned int obj3Count = 0; // to assure 3 Connected obj
	//Number of connected components, children_size + parent
	unsigned int nCConnex = edge->m_source->GetNumberOfChildren() + 1;
	TInputImage::Pointer copy;
	//std::cout << "entró" << std::endl;
	while (objCount < nCConnex || obj3Count <= nCConnex)
	{
		if (objCount >= 3)
			obj3Count++;
		radius += 1.0;
		sphere->SetRadius(radius);
		//making a copy of the current image
		DuplicatorType::Pointer duplicator = DuplicatorType::New();
		duplicator->SetInputImage(img);
		duplicator->Update();
		copy = duplicator->GetOutput();

		//testing code
		//	std::cout<<"center = "<< indCenter << std::endl;
		//std::cout << "Radio = " << center.second << std::endl;
		TInputImage::SizeType regionSize;
		regionSize[0] = 4 * radius;
		regionSize[1] = 4 * radius;
		regionSize[2] = 4 * radius;
		//std::cout << "RegionSize = " << regionSize << std::endl;
		TInputImage::SizeType size = img->GetLargestPossibleRegion().GetSize();
		TInputImage::IndexType regionIndex;
		regionIndex[0] = indCenter[0] - (radius * 2);
		if (regionIndex[0] < 0)
			regionIndex[0] = indCenter[0];
		else if (regionIndex[0] > (unsigned int) size[0])
			regionIndex[0] = size[0];
		regionIndex[1] = indCenter[1] - (radius * 2);
		if (regionIndex[1] < 0)
			regionIndex[1] = indCenter[1];
		else if (regionIndex[1] > (unsigned int) size[1])
			regionIndex[1] = size[1];
		regionIndex[2] = indCenter[2] - (radius * 2);
		if (regionIndex[2] < 0)
			regionIndex[2] = indCenter[2];
		else if (regionIndex[2] > (unsigned int) size[2])
			regionIndex[2] = size[2];
		int diff[3];
		diff[0] = size[0] - (regionSize[0] + regionIndex[0]);
		diff[1] = size[1] - (regionSize[1] + regionIndex[1]);
		diff[2] = size[2] - (regionSize[2] + regionIndex[2]);
		//std::cout << "(1) Region Index = " << regionIndex << std::endl;
		//std::cout << "(1) Region size = " << regionSize << std::endl;
		if (diff[0] < 0)
			regionSize[0] = size[0] - regionIndex[0];
		if (diff[1] < 0)
			regionSize[1] = size[1] - regionIndex[1];
		if (diff[2] < 0)
			regionSize[2] = size[2] - regionIndex[2];
		//	std::cout << "regionIndex = " << regionIndex << std::endl;
		//std::cout << "(2) Region Index = " << regionIndex << std::endl;
		//std::cout << "(2) Region size = " << regionSize << std::endl;
		TInputImage::RegionType region;
		region.SetSize(regionSize);
		region.SetIndex(regionIndex);
		RegionIterator it2(copy, region);
		while (!it2.IsAtEnd())
		{
			TInputImage::IndexType ind = it2.GetIndex();
			SphereType::InputType point;
			point[0] = ind[0];
			point[1] = ind[1];
			point[2] = ind[2];
			SphereType::OutputType output = sphere->Evaluate(point);
			if (output)
				it2.Set(0);
			++it2;
		}				//while
		if (node->IsLeaf())
		{
			TInputImage::PointType ptn;
			ptn[0] = node->GetCoords()[0];
			ptn[1] = node->GetCoords()[1];
			ptn[2] = node->GetCoords()[2];
			TInputImage::IndexType tgt;
			img->TransformPhysicalPointToIndex(ptn, tgt);
			if (img->GetPixel(tgt) == 0)
			{
				std::cout << "It entered here!!!!" << std::endl;
				return;
			}
		}				//if
		CastFilterType::Pointer castFilter = CastFilterType::New();
		castFilter->SetInput(copy);
		castFilter->Update();
		ConnectedComponentImageFilterType::Pointer connFilter =
				ConnectedComponentImageFilterType::New();
		connFilter->SetFullyConnected(true);
		connFilter->SetInput(castFilter->GetOutput());
		connFilter->Update();
		objCount = connFilter->GetObjectCount();
		//test
		/*WriterType::Pointer writer = WriterType::New();
         writer->SetInput(copy);
         writer->SetFileName("output_antes.mhd");
         writer->Update();*/
	}				//while
	//std::cout << "salio" << std::endl;
	/*
	 * First region growing - Segmenting the wanted branch to be removed
	 * Removing the branch from the image
	 */
	Node* leaf = this->GetLeaf(node);
	if (!ComputeSimpleRegionGrowing(copy, leaf->GetCoords()))
		return;
	SubtractImageFilterType::Pointer subtractFilter =
			SubtractImageFilterType::New();
	subtractFilter->SetInput1(img);
	subtractFilter->SetInput2(copy);
	subtractFilter->Update();
	TInputImage::Pointer diff = subtractFilter->GetOutput();
	/*
	 * Second region growing - Cleaning the resulting image
	 */
	if (ComputeSimpleRegionGrowing(diff, this->m_root->GetCoords()))
		img = diff;
	/*else
     std::cout << "problemas" << std::endl;*/
	//cin.ignore().get(); //Pause Command for Linux Terminal
}

//Metodo obsoleto, ya no nos interesa la reconstruccion por esferas
void AirwaysTree::CreateSpheres(TInputImage::Pointer& img, Node* node)
{
	if (node->GetEdge() == NULL)
		return;
	const Edge* edge = node->GetEdge();
	vec_pair_posVox_rad::const_iterator it = edge->m_vec_pair_posVox_rad.begin();
	for (; it != edge->m_vec_pair_posVox_rad.end(); ++it)
	{
		//converting to index
		TInputImage::PointType point;
		point[0] = (*it).first[0];
		point[1] = (*it).first[1];
		point[2] = (*it).first[2];
		TInputImage::IndexType indexPoint;
		img->TransformPhysicalPointToIndex(point, indexPoint);
		//creating itk sphere
		SphereType::Pointer sphere = SphereType::New();
		sphere->SetRadius((*it).second);
		SphereType::InputType sphereCenter;
		sphereCenter[0] = indexPoint[0];
		sphereCenter[1] = indexPoint[1];
		sphereCenter[2] = indexPoint[2];
		sphere->SetCenter(sphereCenter);
		//creating region
		TInputImage::SizeType regionSize;
		regionSize[0] = 4 * (*it).second;
		regionSize[1] = 4 * (*it).second;
		regionSize[2] = 4 * (*it).second;
		TInputImage::IndexType regionIndex;
		regionIndex[0] = indexPoint[0] - 2 * ceil((*it).second);
		regionIndex[1] = indexPoint[1] - 2 * ceil((*it).second);
		regionIndex[2] = indexPoint[2] - 2 * ceil((*it).second);
		TInputImage::RegionType region;
		region.SetSize(regionSize);
		region.SetIndex(regionIndex);
		RegionIterator it2(img, region);
		while (!it2.IsAtEnd())
		{
			TInputImage::IndexType ind = it2.GetIndex();
			SphereType::InputType point;
			point[0] = ind[0];
			point[1] = ind[1];
			point[2] = ind[2];
			SphereType::OutputType output = sphere->Evaluate(point);
			if (output)
				it2.Set(1);
			++it2;
		}		//while
	}		//for
}

void AirwaysTree::printUpToLevel(int level)
{
	cout << "Printing up to level: " << level <<endl;
	this->m_root->printUpToLevel(level);
	cout << "Printing done" <<endl;
}

void AirwaysTree::printNodeAndChildrenIds()
{
	cout << "Printing nodes and children: " <<endl;
	this->m_root->printNodeAndChildrenIds( );
	cout << "Printing nodes and children DONE" <<endl;
}

void AirwaysTree::createQuaternionsUpToLevel(int level)
{
	cout << "Creating quaternions up to level: " << level <<endl;
	if(level >= 2)
	{
		// Print header
		cout << "Level" << " " << "SonPositionX" << " " << "SonPositionY" << " " << "SonPositionZ" << " "
				"ParentVectorX" << " " << "ParentVectorY" << " " << "ParentVectorZ" << " " <<
				"ChildVectorX" << " " << "ChildVectorY" << " " << "ChildVectorZ"<< " " <<
				"Qr" << " " << "Qx" << " " << "Qy" " " << "Qz" " " << "Qtheta" << " " <<
				"Wx" << " " << "Wy" << " " << "Wz" << " " <<
				"EuclDistSonParent" << " " <<"EuclDistSonGrandson" << " " <<
				"RadMeanSonParent" << " " << "RadMeanSonGrandson" <<endl;

		this->m_root->createQuaternionsUpToLevel(level, level);
	}
	else
		cout << "Quaternions must be created from level 2." <<endl;
	cout << "Creating quaternions done" <<endl;
}

void AirwaysTree::getStatisticsBifurcations()
{
	cout << "Statistics bifurcations ... " <<endl;

	int p[6] = {0,0,0,0,0,0};

	this->m_root->getStasticsBifurcations(p);

	cout << "Bifurcations:" << endl;
	for(int i = 0; i < 6; i++)
	{
		cout << i+1 << "->" << p[i] << endl;
	}

	cout << "Statistics bifurcations done " <<endl;
}

void AirwaysTree::saveAirwayToImage(std::string filename, int dims[3], double spc[3], double nOrigin[3])
{
	TInputImage::Pointer imagePointer;
	imagePointer = TInputImage::New();

	TInputImage::IndexType start;
	start[0] = 0;
	start[1] = 0;
	start[2] = 0;

	TInputImage::SizeType size;
	size[0] = dims[0];
	size[1] = dims[1];
	size[2] = dims[2];

	TInputImage::RegionType region;
	region.SetSize( size );
	region.SetIndex( start );

	TInputImage::SpacingType spacing;
	spacing[0] = spc[0];
	spacing[1] = spc[1];
	spacing[2] = spc[2];

	TInputImage::PointType origin;
	origin[0] = nOrigin[0];
	origin[1] = nOrigin[1];
	origin[2] = nOrigin[2];

	imagePointer->SetRegions( region );
	imagePointer->SetSpacing( spacing );
	imagePointer->SetOrigin( origin );
	imagePointer->Allocate();

	//this->m_root->saveToImage(_imagePointer);
	vec_nodes nodes_children_root = this->m_root->GetChildren();
	vec_nodes::const_iterator it_children = nodes_children_root.begin();
	for( ; it_children != nodes_children_root.end(); ++it_children)
	{
		Vec3 coords_father = (*it_children)->GetEdge()->GetSource()->GetCoords();
		Vec3 coords_child = (*it_children)->GetCoords();
		drawLineInImage(coords_father, coords_child, imagePointer);
	}

	cout << "Writing ..." << endl;
	typedef itk::ImageFileWriter<TInputImage> WriterType;
	WriterType::Pointer writer = WriterType::New();
	writer->SetInput(imagePointer);
	writer->SetFileName(filename);
	writer->Update();
	cout << "Writing ... done" << endl;
}

void AirwaysTree::drawLineInImage(Vec3 initVoxel, Vec3 endVoxel, TInputImage::Pointer imagePointer)
{
	TInputImage::RegionType region = imagePointer->GetLargestPossibleRegion();

	TInputImage::SpacingType spacing = imagePointer->GetSpacing();

	TInputImage::PointType origin = imagePointer->GetOrigin();

	TInputImage::IndexType init;
	init[0]=(initVoxel[0]-origin[0])/spacing[0];
	init[1]=(initVoxel[1]-origin[1])/spacing[1];
	init[2]=(initVoxel[2]-origin[2])/spacing[2];

	TInputImage::IndexType end;
	end[0]=(endVoxel[0]-origin[0])/spacing[0];
	end[1]=(endVoxel[1]-origin[1])/spacing[1];
	end[2]=(endVoxel[2]-origin[2])/spacing[2];

	TPixel pixel;
	pixel = 1;

	itk::LineIterator<TInputImage> it(imagePointer, init, end);
	it.GoToBegin();
	while (!it.IsAtEnd())
	{
		it.Set(pixel);
		++it;
	}
}

//To store the graph in a vtk
void AirwaysTree::saveAirwayAsVTK(const std::string filename, bool common)
{
	vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
	colors->SetNumberOfComponents(3);
	colors->SetName("Colors");

	//pointer of lines and points
	vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
	vtkSmartPointer<vtkPoints> pts = vtkSmartPointer<vtkPoints>::New();

	Vec3 root = this->m_root->GetCoords();

	//UndirectedGraph

	srand(time(NULL));
	unsigned int id = 1;

	CalculateVTKLinesFromEdges(this->m_root, 0, id, pts, lines, colors, common);

	vtkSmartPointer<vtkPolyData> linesPolyData = vtkSmartPointer<vtkPolyData>::New();
	linesPolyData->SetPoints(pts);
	linesPolyData->SetLines(lines);

	linesPolyData->GetCellData()->SetScalars(colors);

	// Write the file
	vtkSmartPointer<vtkPolyDataWriter> writer = vtkSmartPointer<vtkPolyDataWriter>::New();
	writer->SetFileName(filename.c_str());
#if VTK_MAJOR_VERSION <= 5
	writer->SetInput(linesPolyData);
#else
	writer->SetInputData(linesPolyData);
#endif
	writer->Write();
}

//To store the graph in a vtk
void AirwaysTree::CalculateVTKLinesFromEdges(const Node* node, const unsigned int& parentId,
		unsigned int& cId, vtkSmartPointer<vtkPoints>& pts,
		vtkSmartPointer<vtkCellArray>& lines,
		vtkSmartPointer<vtkUnsignedCharArray>& colors, bool common)
{
	if (node == NULL)
		return;

	// Colors
	unsigned char red[3] = { 255, 0, 0 };
	unsigned char green[3] = { 0, 255, 0 };
	unsigned char blue[3] =	{ 0, 0, 255 };
	unsigned char yellow[3] = { 255, 255, 0 };

	const vec_nodes children = node->GetChildren();

	for(vec_nodes::const_iterator it = children.begin(); it != children.end(); ++it)
	{
		if (!(*it)->IsMarked() && common)
			continue;

		const Edge* edge = (*it)->GetEdge();
		//pts->InsertNextPoint(edge->GetSource()->GetCoords().GetVec3());
		//pts->InsertNextPoint(edge->GetTarget()->GetCoords().GetVec3());

		pts->InsertNextPoint(node->GetCoords().GetVec3());
		pts->InsertNextPoint((*it)->GetCoords().GetVec3());

		// Set color to be used
		int numColor = node->GetLevel() % 4;
		if(numColor == 0)
			colors->InsertNextTupleValue(green);
		else if(numColor == 1)
			colors->InsertNextTupleValue(red);
		else if(numColor == 2)
			colors->InsertNextTupleValue(blue);
		else
			colors->InsertNextTupleValue(yellow);

		vtkSmartPointer<vtkLine> line = vtkSmartPointer<vtkLine>::New();
		line->GetPointIds()->SetId(0, parentId);
		line->GetPointIds()->SetId(1, cId++);
		lines->InsertNextCell(line);
		unsigned int newParent = cId++;
		CalculateVTKLinesFromEdges(*it, newParent, cId, pts, lines, colors,	common);
	} //for
}


} /* namespace airways */


#endif