/*
 * airwaysEdge.cxx
 *
 *  Created on: May 12, 2014
 *      Author: caceres
 */

#include "airwaysEdge.h"
#include <iostream>

namespace airways
{

Edge::Edge() : m_id(0), m_source(NULL), m_target(NULL), m_mark(false), m_angle(0), m_length(
		0), m_eDistance(0), m_aRadius(0), m_minRadius(0), m_maxRadius(0)
{

}

Edge::Edge(Edge* edge) :  m_id(0), m_source(NULL), m_target(NULL), m_mark(false), m_angle(0), m_length(
		0), m_eDistance(0), m_aRadius(0), m_minRadius(0), m_maxRadius(0)
{
	this->m_id = edge->m_id;
	this->m_source = edge->m_source;
	this->m_target = edge->m_target;
	this->m_angle = edge->m_angle;
	this->m_length = edge->m_length;
	this->m_eDistance = edge->m_eDistance;
	this->m_vec_pair_posVox_rad = edge->m_vec_pair_posVox_rad;
	this->m_aRadius = edge->m_aRadius;
	this->m_minRadius = edge->m_minRadius;
	this->m_maxRadius = edge->m_maxRadius;
}

Edge::~Edge()
{
}

int  Edge::GetAngle() const
{
	return this->m_angle;
}

double  Edge::GetARadius() const
{
	return this->m_aRadius;
}

double  Edge::GetEDistance() const
{
	return this->m_eDistance;
}

double  Edge::GetLength() const
{
	return this->m_length;
}

double  Edge::GetMaxRadius() const
{
	return this->m_maxRadius;
}

double  Edge::GetMinRadius() const
{
	return this->m_minRadius;
}

Node*  Edge::GetSource() const
{
	return this->m_source;
}

Node*  Edge::GetTarget() const
{
	return this->m_target;
}

const vec_pair_posVox_rad&  Edge::GetEdgeInfo() const
{
	return m_vec_pair_posVox_rad;
}

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

bool Edge::IsPointInfluencedByEdge(float point_x, float point_y, float point_z) const
{
	// Variables
	float minDistance = -1.0;
	bool first = true;
	float distanceTemp = 0.0;
	bool influenced = false;
	Vec3 vector(point_x, point_y, point_z);

	// Iterate over all voxel in the edge
	vec_pair_posVox_rad::const_iterator it = this->m_vec_pair_posVox_rad.begin();
	for (; it != this->m_vec_pair_posVox_rad.end() && !influenced; ++it)
	{
		// Get the vector from the compared source to each voxel
		Vec3 voxel_actual = ((*it).first);
		//Vec3 source_actual = this->m_source->GetCoords( );
		//Vec3 vector_actual(voxel_actual - source_actual);

		// Get the difference vector
		Vec3 vector_diff = vector - voxel_actual;

		// Get the distance
		distanceTemp = vector_diff.Norm();

		// Get the minimum
		if(first)
		{
			minDistance = distanceTemp;
			first = false;
			if(minDistance < ((*it).second*1.8))
				influenced = true;
		}
		else if(distanceTemp < minDistance)
		{
			minDistance = distanceTemp;
			if(minDistance < ((*it).second)*1.8)
				influenced = true;
		}
	}

	return influenced;
}

void  Edge::SetAngle(const double& angle)
{
	this->m_angle = angle;
}

void  Edge::SetARadius(const double& aRadius)
{
	this->m_aRadius = aRadius;
}

void  Edge::SetEDistance(const double& eDistance)
{
	this->m_eDistance = eDistance;
}

void  Edge::SetLength(const double& length)
{
	this->m_length = length;
}

void  Edge::SetMaxRadius(const unsigned int& maxRadius)
{
	this->m_maxRadius = maxRadius;
}

void  Edge::SetMinRadius(const unsigned int& minRadius)
{
	this->m_minRadius = minRadius;
}

void  Edge::SetSource(Node* source)
{
	this->m_source = source;
}

void  Edge::SetTarget(Node* target)
{
	this->m_target = target;
}

void  Edge::AddSkeletonPairInfo(const pair_posVox_rad& skPairInfo)
{
	this->m_vec_pair_posVox_rad.push_back(skPairInfo);
}

void  Edge::SetSkeletonPairVector(const vec_pair_posVox_rad& skPInfoVector)
{
	this->m_vec_pair_posVox_rad = skPInfoVector;
	UpdateEdgeInfo();
}

void  Edge::UpdateEdgeInfo()
{
	if (this->m_vec_pair_posVox_rad.empty())
	{
		std::cout << "   ------------------------ This edge has no information" << std::endl;
		std::cout << "Source:" << this->m_source->GetCoords() << std::endl;
		std::cout << "Target:" << this->m_target->GetCoords() << std::endl;
		return;
	}
	double min = std::numeric_limits<unsigned int>::max();
	double max = std::numeric_limits<unsigned int>::min();
	double average = 0;
	vec_pair_posVox_rad::iterator it = this->m_vec_pair_posVox_rad.begin();
	for (; it != this->m_vec_pair_posVox_rad.end(); ++it)
	{
		pair_posVox_rad info = *it;
		average += info.second;
		if (info.second < min)
			min = info.second;
		else if (info.second > max)
			max = info.second;
	}
	this->m_aRadius = average / this->m_vec_pair_posVox_rad.size();
	this->m_minRadius = min;
	this->m_maxRadius = max;
}

double Edge::CompareWith(Edge* edge)
{
	//std::cout << "Comparing ... " << std::endl;
	// To compared the edges, the distance in each component
	// is calculated and a distance function between edges is calculated.
	// The components are:
	// - Each component of the quaternion (son-parent quaterion). (4 components)
	// - Edge distance
	// - Edge average radius

	// Difference in each quaternion component
	//                  p
	//                  |
	//                  |
	//                  |
	//                  s
	//                 /
	//				/
	//               /
	//              g
	/*const Edge* edgeActualParent = this->m_source->GetEdge( );
	Vec3 sourceActualParent = edgeActualParent->m_source->GetCoords( );
	Vec3 targetActualParent = edgeActualParent->m_target->GetCoords( );
	Vec3 sourceActual = this->m_source->GetCoords( );
	Vec3 targetActual = this->m_target->GetCoords( );

	Vec3 sp_actual( sourceActualParent - targetActualParent );
	Vec3 sg_actual( targetActual - sourceActual );

	const Edge* edgeComparedParent = edge.m_source->GetEdge( );
	Vec3 sourceComparedParent = edgeComparedParent->m_source->GetCoords( );
	Vec3 targetComparedParent = edgeComparedParent->m_target->GetCoords( );
	Vec3 sourceCompared = edge.m_source->GetCoords( );
	Vec3 targetCompared = edge.m_target->GetCoords( );

	Vec3 sp_compared( sourceComparedParent - targetComparedParent );
	Vec3 sg_compared( targetCompared - sourceCompared );

	Quaternion q_actual(sp_actual, sg_actual);
	Quaternion q_compared(sp_compared, sg_compared);

	float dif_r = q_actual.getR() - q_compared.getR();
	float dif_i = q_actual.getI() - q_compared.getI();
	float dif_j = q_actual.getJ() - q_compared.getJ();
	float dif_k = q_actual.getK() - q_compared.getK();
	float dif_distance = this->m_length - edge.m_length;
	float dif_radius = this->m_aRadius - edge.m_aRadius;
	 */
	double distanceEdge = GetDistanceToEdge(edge);

	//std::cout << "Comparing ... OK" << std::endl;

	return distanceEdge;
}

float Edge::GetDistanceToEdge(Edge* edge)
{
	// a. The parent edges must be aligned before calculate the distance.
	// b. Parent end-points are aligned also.
	// Steps a and b represent a rotation and a translation respectively.
	// c. For each point in the actual Edge (this) the closest point in the
	// compared edge is found. This distance is calculated and then averaged
	// for all the points. Must be done in both ways?
	// We can take the maximum of both distances max(D(a,b),D(b,a))

	// Variables
	float averageDistance = 0.0;
	float numPoints = 0;

	// Get the parent Edges
	/*const Edge* edgeActualParent = this->m_source->GetEdge( );
	Vec3 sourceActualParent = edgeActualParent->m_source->GetCoords( );
	Vec3 targetActualParent = edgeActualParent->m_target->GetCoords( );
	Vec3 sourceActual = this->m_source->GetCoords( );
	Vec3 targetActual = this->m_target->GetCoords( );

	Vec3 sp_actual( sourceActualParent - targetActualParent );
	Vec3 sg_actual( targetActual - sourceActual );

	const Edge* edgeComparedParent = edge.m_source->GetEdge( );
	Vec3 sourceComparedParent = edgeComparedParent->m_source->GetCoords( );
	Vec3 targetComparedParent = edgeComparedParent->m_target->GetCoords( );
	Vec3 sourceCompared = edge.m_source->GetCoords( );
	Vec3 targetCompared = edge.m_target->GetCoords( );

	Vec3 sp_compared( sourceComparedParent - targetComparedParent );
	Vec3 sg_compared( targetCompared - sourceCompared );

	//Get the quaternion between parents
	Quaternion q_parent(sp_actual, sp_compared);

	// Get the translation vector to translate the compared parent to the actual parent
	const Vec3 transParents( targetActualParent - targetComparedParent);
	 */

	// Iterate over the voxels in the compared edge
	for(vec_pair_posVox_rad::iterator it = edge->m_vec_pair_posVox_rad.begin(); it != edge->m_vec_pair_posVox_rad.end(); ++it)
	{
		// Get the vector from the compared source to each voxel in the compared edge
		Vec3 voxel_actual = (*it).first;
		/*Vec3 vector_compared(voxel_actual - sourceCompared);

		// Rotate the vector using the parental quaternion
		Vec3 vector_compared_rotated = q_parent.rotateVector(vector_compared);
		 */

		// Find the distance to the closest point in the actual edge
		//averageDistance += getDistanceToClosestPoint(sourceActual+vector_compared_rotated);
		averageDistance += this->GetSmallestDistanceToPoint(voxel_actual);

		++numPoints;
	}

	// Get the average value
	if(numPoints > 0)
		averageDistance = averageDistance / numPoints;

	return averageDistance;
}

float Edge::GetDistanceToTranslatedEdge(Edge* edge, Vec3 vector_translation)
{
	// a. The parent edges must be aligned before calculate the distance.
	// b. Parent end-points are aligned also.
	// Steps a and b represent a rotation and a translation respectively.
	// c. For each point in the actual Edge (this) the closest point in the
	// compared edge is found. This distance is calculated and then averaged
	// for all the points. Must be done in both ways?
	// We can take the maximum of both distances max(D(a,b),D(b,a))

	// Variables
	float averageDistance = 0.0;
	float numPoints = 0;

	// Iterate over the voxels in the compared edge
	for(vec_pair_posVox_rad::iterator it = edge->m_vec_pair_posVox_rad.begin(); it != edge->m_vec_pair_posVox_rad.end(); ++it)
	{
		// Get the vector from the compared source to each voxel in the compared edge
		Vec3 voxel_actual = (*it).first;

		// Translate the voxel
		Vec3 voxel_translated = voxel_actual + vector_translation;

		// Find the distance to the closest point in the actual edge
		averageDistance += this->GetSmallestDistanceToPoint(voxel_translated);

		++numPoints;
	}

	// Get the average value
	if(numPoints > 0)
		averageDistance = averageDistance / numPoints;

	return averageDistance;
}

float Edge::GetDistanceWeigthedToTranslatedEdge(Edge* edge, Vec3 vector_translation)
{
	// a. The parent edges must be aligned before calculate the distance.
	// b. Parent end-points are aligned also.
	// Steps a and b represent a rotation and a translation respectively.
	// c. For each point in the actual Edge (this) the closest point in the
	// compared edge is found. This distance is calculated and then averaged
	// for all the points. Must be done in both ways?
	// We can take the maximum of both distances max(D(a,b),D(b,a))

	// Variables
	double alfa = 4; // Weight for duplicated closest points
	float distance_average = 0.0;
	float distance_actual = 0.0;
	float numPoints = 0;
	std::map<Vec3, vec_pair_posVox_rad > map_vector_pair_vector_distance; // Map to save the correspondences between the given edge voxels and this voxels, and the distance between them.

	// Iterate over the voxels in the compared edge
	for(vec_pair_posVox_rad::iterator it = edge->m_vec_pair_posVox_rad.begin(); it != edge->m_vec_pair_posVox_rad.end(); ++it)
	{
		// Get the vector from the compared source to each voxel in the compared edge
		Vec3 voxel_actual = (*it).first;

		// Translate the voxel
		Vec3 voxel_translated = voxel_actual + vector_translation;

		// Find the distance to the closest point in the actual edge
		Vec3 point_closest = this->GetClosestPoint(voxel_translated, distance_actual);
		distance_average += distance_actual;

		// Add the link
		pair_posVox_rad pair_vector_distance(voxel_actual, distance_actual);
		map_vector_pair_vector_distance[point_closest].push_back(pair_vector_distance);

		++numPoints;
	}

	// Add the weight to shared closest points
	float distance_weighted = 0.0;
	for(std::map<Vec3, vec_pair_posVox_rad>::iterator it = map_vector_pair_vector_distance.begin(); it != map_vector_pair_vector_distance.end(); ++it)
	{
		vec_pair_posVox_rad vector_pair_vector_distance = (*it).second;
		if(vector_pair_vector_distance.size() > 1)
		{
			for(vec_pair_posVox_rad::iterator it_vector = vector_pair_vector_distance.begin(); it_vector != vector_pair_vector_distance.end(); ++it_vector)
				distance_weighted += (*it_vector).second * alfa;
		}
		else
		{
			for(vec_pair_posVox_rad::iterator it_vector = vector_pair_vector_distance.begin(); it_vector != vector_pair_vector_distance.end(); ++it_vector)
				distance_weighted += (*it_vector).second;
		}
	}

	// Get the average value
	if(numPoints > 0)
	{
		distance_average = distance_average / numPoints;
		distance_weighted = distance_weighted / numPoints;
	}

	//std::cout << "[DistanceW;DistAvg][" << distance_weighted << ";" << distance_average << "];;";
	//return distance_average;
	return distance_weighted;
}

float Edge::GetDistanceToPoint(Vec3 point)
{
	// Variables
	bool first = true;
	float distance = 0.0;

	// Iterate over all voxel in the edge
	vec_pair_posVox_rad::iterator it = this->m_vec_pair_posVox_rad.begin();
	for (; it != this->m_vec_pair_posVox_rad.end(); ++it)
	{
		// Get the vector from the compared source to each voxel
		Vec3 voxel_actual = ((*it).first);

		// Get the difference vector
		Vec3 vector_diff = point - voxel_actual;

		// Get the distance
		distance += vector_diff.Norm();
	}

	return distance;
}


float Edge::GetSmallestDistanceToPoint(Vec3 vector)
{
	// Variables
	float minDistance = -1.0;
	bool first = true;
	float distanceTemp = 0.0;

	// Iterate over all voxel in the edge
	vec_pair_posVox_rad::iterator it = this->m_vec_pair_posVox_rad.begin();
	for (; it != this->m_vec_pair_posVox_rad.end(); ++it)
	{
		// Get the vector from the compared source to each voxel
		Vec3 voxel_actual = ((*it).first);
		//Vec3 source_actual = this->m_source->GetCoords( );
		//Vec3 vector_actual(voxel_actual - source_actual);

		// Get the difference vector
		Vec3 vector_diff = vector - voxel_actual;

		// Get the distance
		distanceTemp = vector_diff.Norm();

		// Get the minimum
		if(first)
		{
			minDistance = distanceTemp;
			first = false;
		}
		else if(distanceTemp < minDistance)
			minDistance = distanceTemp;
	}

	return minDistance;
}

Vec3 Edge::GetClosestPoint(Vec3 vector, float& distance)
{
	// Variables
	Vec3 point_closest;
	bool first = true;
	float distanceTemp = 0.0;

	// Iterate over all voxel in the edge
	vec_pair_posVox_rad::iterator it = this->m_vec_pair_posVox_rad.begin();
	for (; it != this->m_vec_pair_posVox_rad.end(); ++it)
	{
		// Get the vector from the compared source to each voxel
		Vec3 voxel_actual = ((*it).first);
		//Vec3 source_actual = this->m_source->GetCoords( );
		//Vec3 vector_actual(voxel_actual - source_actual);

		// Get the difference vector
		Vec3 vector_diff = vector - voxel_actual;

		// Get the distance
		distanceTemp = vector_diff.Norm();

		// Get the minimum
		if(first)
		{
			point_closest = voxel_actual;
			distance = distanceTemp;
			first = false;
		}
		else if(distanceTemp < distance)
		{
			point_closest = voxel_actual;
			distance = distanceTemp;
		}
	}

	return point_closest;
}

Edge* Edge::ConcatenateToSuperior(Edge* superior)
{
	if(superior == NULL)
		return new Edge(this);

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

	// Check concatenation condition in the edge information
	vec_pair_posVox_rad vectorInfo_thisEdge = this->GetEdgeInfo(); // Actual edge information
	vec_pair_posVox_rad vectorInfo_superiorEdge = superior->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 << "Edge::ConcatenateToSuperior - superior.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
	superior->SetTarget(this->GetTarget());
	superior->SetLength(superior->GetLength()+this->GetLength()-1);
	superior->SetEDistance(superior->GetEDistance()+this->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
	superior->SetSkeletonPairVector(vectorInfo_superiorEdge);

	return superior;

}

Edge&  Edge::operator=(Edge& edge)
{
	this->m_mark = edge.m_mark;
	this->m_angle = edge.m_angle;
	this->m_length = edge.m_length;
	this->m_eDistance = edge.m_eDistance;
	this->m_aRadius = edge.m_aRadius;
	this->m_minRadius = edge.m_minRadius;
	this->m_maxRadius = edge.m_maxRadius;
	this->m_source = edge.m_source;
	this->m_target = edge.m_target;
	this->m_vec_pair_posVox_rad = edge.m_vec_pair_posVox_rad;
	return *this;
}

const Edge&  Edge::operator=(const Edge& edge)
{
	this->m_mark = edge.m_mark;
	this->m_angle = edge.m_angle;
	this->m_length = edge.m_length;
	this->m_eDistance = edge.m_eDistance;
	this->m_aRadius = edge.m_aRadius;
	this->m_minRadius = edge.m_minRadius;
	this->m_maxRadius = edge.m_maxRadius;
	this->m_source = edge.m_source;
	this->m_target = edge.m_target;
	this->m_vec_pair_posVox_rad = edge.m_vec_pair_posVox_rad;
	return *this;
}

bool  Edge::operator==(const Edge& edge)
														  {
	bool cmp1 = true;
	bool cmp2 = true;
	bool cmp3 = true;
	bool cmp4 = true;
	bool cmp5 = true;
	bool cmp6 = true;
	/*if (this->m_angle != edge.m_angle)
     {
     double max =
     this->m_angle >= edge.m_angle ? this->m_angle : edge.m_angle;
     double tol = 0.3 * max;
     double diff = this->m_angle - edge.m_angle;
     if (!((-tol <= diff) && (diff <= tol)))
     cmp1 = false;
     }*/
	if (this->m_length != edge.m_length)
	{
		double max =
				this->m_length >= edge.m_length ? this->m_length : edge.m_length;
		double tol = 0.3 * max;
		double diff = abs(this->m_length - edge.m_length);
		if (diff > tol)
			cmp2 = false;
	}
	if (this->m_eDistance != edge.m_eDistance)
	{
		double max =
				this->m_eDistance >= edge.m_eDistance ?
						this->m_eDistance : edge.m_eDistance;
		double tol = 0.3 * max;
		double diff = abs(this->m_eDistance - edge.m_eDistance);
		if (diff > tol)
			cmp3 = false;
	}
	/*if (this->m_aRadius != edge.m_aRadius)
     {
     double max =
     this->m_aRadius >= edge.m_aRadius ?
     this->m_aRadius : edge.m_aRadius;
     double tol = 0.3 * max;
     double diff = abs(this->m_aRadius - edge.m_aRadius);
     if (diff > tol)
     cmp4 = false;
     }
     if (this->m_minRadius != edge.m_minRadius)
     {
     double max =
     this->m_minRadius >= edge.m_minRadius ?
     this->m_minRadius : edge.m_minRadius;
     double tol = 0.3 * max;
     double diff = abs(this->m_minRadius - edge.m_minRadius);
     if (diff > tol)
     cmp5 = false;
     }
     if (this->m_maxRadius != edge.m_maxRadius)
     {
     double max =
     this->m_maxRadius >= edge.m_maxRadius ?
     this->m_maxRadius : edge.m_maxRadius;
     double tol = 0.3 * max;
     double diff = abs(this->m_maxRadius - edge.m_maxRadius);
     if (diff > tol)
     cmp6 = false;
     }*/
	return (cmp1 && cmp2 && cmp3 && cmp4 && cmp5 && cmp6);
														  }

bool  Edge::operator!=(const Edge& edge)
														  {
	return !(*this == edge);
														  }

bool  Edge::operator<(const Edge& edge)
{
	bool cmp1 = true;
	bool cmp2 = true;
	if (this->m_length != edge.m_length)
	{
		double max =
				this->m_length >= edge.m_length ? this->m_length : edge.m_length;
		double tol = 0.3 * max;
		double diff = abs(this->m_length - edge.m_length);
		if (diff > tol)
			cmp2 = false;
	}
	if (this->m_eDistance != edge.m_eDistance)
	{
		double max =
				this->m_eDistance >= edge.m_eDistance ?
						this->m_eDistance : edge.m_eDistance;
		double tol = 0.3 * max;
		double diff = abs(this->m_eDistance - edge.m_eDistance);
		if (diff > tol)
			cmp2 = false;
	}
	if (cmp1 && cmp2)
		return false;
	if ((this->m_length < edge.m_length)
			|| (this->m_eDistance < edge.m_eDistance))
		return true;
	return false;

}

bool  Edge::operator>(const Edge& edge)
{
	bool cmp1 = true;
	bool cmp2 = true;
	if (this->m_length != edge.m_length)
	{
		double max =
				this->m_length >= edge.m_length ? this->m_length : edge.m_length;
		double tol = 0.3 * max;
		double diff = abs(this->m_length - edge.m_length);
		if (diff > tol)
			cmp2 = false;
	}
	if (this->m_eDistance != edge.m_eDistance)
	{
		double max =
				this->m_eDistance >= edge.m_eDistance ?
						this->m_eDistance : edge.m_eDistance;
		double tol = 0.3 * max;
		double diff = abs(this->m_eDistance - edge.m_eDistance);
		if (diff > tol)
			cmp2 = false;
	}
	if (cmp1 && cmp2)
		return false;
	if ((this->m_length > edge.m_length)
			|| (this->m_eDistance > edge.m_eDistance))
		return true;
	return false;
}

} /* namespace airways */