#include "convexhull.h"


ConvexHull::ConvexHull()
{
}


// Adds two convex hulls together and returns the result
ConvexHull ConvexHull::add(const ConvexHull& ca, const ConvexHull& cb)
{
	ConvexHull ch;
	ch.updateHull(ca);
	ch.updateHull(cb);

	return ch;
}


// Adds one more element to the convex hull
// Change this for a more efficient implementation if you can
// For example, using std::list instead of std::vector
void ConvexHull::updateHull(const Geometry::Vector3Df& last)
{
	if (_hull.empty())
		_offset = last;
//	_sum += last;
//	_offset = _sum/_hull.size();

	Geometry::Vector3Df clast( center(last) );

	// Operate in the _centeredHull instead of _hull!
	_centeredHull.resize(_hull.size());
	for (unsigned i=0 ; i<_hull.size() ; i++)
		_centeredHull[i] = center(_hull[i]);

	unsigned i, i1;
	Geometry::Vector3Df v;

	if (_hull.size() < 2)
		{
			_hull.push_back(last);
		}
	else
		{
			// Search for the first voided segment of the convex hull
			int start = -1;
			for (i=0 ; i<_hull.size() ; i++)
				{
					i1 = (i+1)%_hull.size();
					v = (_centeredHull[i1] - _centeredHull[i]) ^ (clast-_centeredHull[i]);
					if (v.z() > 0)
						{
							start = i1;
							break;
						}
				}

			if (-1 != start)
				{
					bool still = true;
					i = start;
					std::vector<unsigned> discarded;
					for (i=start ; still ; i = (i+1) % _hull.size())
						{
							i1 = (i+1)%_hull.size();
							v = (_centeredHull[i1] - _centeredHull[i]) ^ (clast-_centeredHull[i]);
							if (v.z() > 0)
								{
									discarded.push_back(i);
								}
							else
								still = false;
						}

					while (!discarded.empty())
						{
							_hull.erase(_hull.begin()+discarded[discarded.size()-1]);
							discarded.pop_back();
						}

					_hull.insert(_hull.begin()+start, last);
				}
		}
}


// Puts two convex hulls together
void ConvexHull::updateHull(const ConvexHull& ch)
{
	std::vector<Geometry::Vector3Df>::const_iterator it;
	for (it=ch._hull.begin() ; it!=ch._hull.end() ; it++)
		updateHull(*it);
}


// Assignation operator
ConvexHull& ConvexHull::operator=(const ConvexHull& ch)
{
	_hull = ch._hull;
	_centeredHull = ch._centeredHull;
	_sum = ch._sum;
	_offset = ch._offset;

	return *this;
}


// Calculates the diameter of the convex hull using the
// Snyder and Tang Algorithm, as explained in
// <http://cgm.cs.mcgill.ca/~athens/cs507/Projects/2000/MS/diameter/node5.html>
float ConvexHull::diameter()
{
	// Operate in the _centeredHull instead of _hull!
	_centeredHull.resize(_hull.size());
//	std::cout << "offset = (" << _offset << ')' << std::endl;
	for (unsigned i=0 ; i<_hull.size() ; i++)
		{
			Geometry::Vector3Df v(center(_hull[i]));
			_centeredHull[i].setSpheric(1.0, v.x(), v.y());
//			std::cout << '(' << _hull[i] << ") => ";
//			std::cout << '(' << center(_hull[i]) << ") => ";
//			std::cout << '(' << _centeredHull[i] << ')' << std::endl;
		}

#if 0
	unsigned a, b, c, A, B;
	double diam = 0.0, tmp;
	A = 0;
	for (b=1 ; b<_centeredHull.size() ; b++)
		{
			tmp = _centeredHull[A].squaredDistance(_centeredHull[b]);
			if (tmp > diam)
				{
					B = b;
					diam = tmp;
				}
		}
	for (c=0 ; c<_centeredHull.size() ; c++)
		{
			tmp = _centeredHull[B].squaredDistance(_centeredHull[c]);
			diam = MAX(diam, tmp);
		}

	return sqrt(diam);
#else
	unsigned a, b;
	double diam = 0.0, tmp;
	for (a=0 ; a<_centeredHull.size() ; a++)
		for (b=a+1 ; b<_centeredHull.size() ; b++)
			{
				tmp = _centeredHull[a].angle(_centeredHull[b]);
				if (tmp > diam)
					diam = tmp;
			}

	return diam;
#endif
}


// Computes the perimeter of the convex hull
float ConvexHull::perimeter()
{
	// Operate in the _centeredHull instead of _hull!
	_centeredHull.resize(_hull.size());
	for (unsigned i=0 ; i<_hull.size() ; i++)
		{
			Geometry::Vector3Df v(center(_hull[i]));
			_centeredHull[i].setSpheric(1.0, v.x(), v.y());
		}

	unsigned a;
	double sum = 0.0;
	for (a=0 ; a<_centeredHull.size() ; a++)
		{
			const Geometry::Vector3Df& v0 = _centeredHull[a];
			const Geometry::Vector3Df& v1 = _centeredHull[(a+1)%_centeredHull.size()];
			sum += v0.angle(v1);
		}

	return sum;
}


// Corrects the position of the point to be centered around the offset
Geometry::Vector3Df ConvexHull::center(const Geometry::Vector3Df& point) const
{
	Geometry::Vector3Df tmp, tmp2;
	double theta, phi;

	tmp.setSpheric(1.0, point.x(), point.y());
	tmp.rotateZ(-_offset.x());
	tmp.rotateY(-_offset.y());

	tmp.getSpheric(theta, phi);
	tmp2.x(theta);
	tmp2.y(phi);

	return tmp2;

/*
//	static Geometry::Vector3Df middle(0.0, M_PI/2.0, 0.0);
	Geometry::Vector3Df tmp(_offset - point);

	tmp.x( normalize(-M_PI, M_PI, tmp.x()) );
	tmp.y( normalize(0, M_PI, tmp.y()) );

	return tmp;
*/
}