/******************************************************************************
 *
 * geomhelp.cpp
 *
 * (c) Leonid (Lodrion) Taycher 2000
 *
 ******************************************************************************/


#include <list>
#include <vector>
#include <aigeom.h>
#include <geomhelp.h>
#include <aiiutils.h>

aic2Vect aifTriangulate2D(const ais2TriagData& p1, 
								const ais2TriagData& p2, const ais2TriagData& p3)
{
	aic2Vect p[3];
	double d[3];
	
	
	// Check for the well-definedness of the problem and 
	// find the best ordering of checks.
	{
		double dd[3];
		
		dd[0] = (p1.p - p2.p).r();
		dd[1] = (p2.p - p3.p).r();
		dd[2] = (p3.p - p1.p).r();
		
		// Check for too close reference points
		if (dd[0] < 0.0001 || dd[1] < 0.0001 || dd[2] < 0.0001)
			throw 0;
		
		if (dd[0] < (p1.dist + p2.dist))
		{
			p[0] = p1.p;
			p[1] = p2.p;
			p[2] = p3.p;
			d[0] = p1.dist;
			d[1] = p2.dist;
			d[2] = p3.dist;
		}
		else if (dd[1] < (p2.dist + p3.dist))
		{
			p[0] = p2.p;
			p[1] = p3.p;
			p[2] = p1.p;
			d[0] = p2.dist;
			d[1] = p3.dist;
			d[2] = p1.dist;
		}
		else if (dd[2] < (p3.dist + p1.dist))
		{
			p[0] = p3.p;
			p[1] = p1.p;
			p[2] = p2.p;
			d[0] = p3.dist;
			d[1] = p1.dist;
			d[2] = p2.dist;
		}
		else
			throw 0;
	}
	
	// Compute the triangulation. See the notebook for the derivation
	double l = (p[1] - p[0]).r(); // The distance between the base points
	aic2Vect lU = (p[1] - p[0]) / l;	// Unit difference direction
	
	double a = (l + (Sqr(d[0]) - Sqr(d[1])) / l) / 2.0;

	// The projection point 
	aic2Vect ctr = p[0] + lU * a; 
	double nrmlLen = sqrt(Sqr(d[0]) - Sqr(a));
	aic2Vect nrml (lU.y(), -lU.x()); // The normal to the difference direction
	
	
	// There are two points corresponding to (+-)nrml * nrmlLen
	{
		aic2Vect p1 = ctr - nrml * nrmlLen;
		if (((p1 - p[2]).r() -d[2]) < 0.001)
			return p1;
	}
	{
		aic2Vect p1 = ctr + nrml * nrmlLen;
		if (((p1 - p[2]).r() - d[2])< 0.001)
			return p1;
	}
		
	throw 0;
	
	return aic2Vect();
}


aic2Vect aifTriangulate2D(const vector<ais2TriagData>& pnts)
{
	if (pnts.size() < 3)
		throw 0;

	// For now just use first 3 points	
	return aifTriangulate2D(pnts[0], pnts[1], pnts[2]);
}