#include "tmachine.h"

#include "tcurves.h"

#include "tcurveutil.h"

#include "tmathutil.h"

#include "tbezier.h"

using namespace std;

//=============================================================================

ostream &operator<<(ostream &out, const TSegment &segment)

{

	return out << "S{" << segment.getP0() << ", " << segment.getP1() << "}";

}

//=============================================================================

void TCubic::split(double t, TCubic &first, TCubic &second) const

{

	double s = 1.0 - t;

	TPointD H = s * m_p1 + t * m_p2;

	first.m_p0 = m_p0;

	first.m_p1 = s * m_p0 + t * m_p1;

	first.m_p2 = s * first.m_p1 + t * H;

	second.m_p3 = m_p3;

	second.m_p2 = s * m_p2 + t * m_p3;

	second.m_p1 = s * H + t * second.m_p2;

	first.m_p3 = s * first.m_p2 + t * second.m_p1;

	second.m_p0 = first.m_p3;

}

double TCubic::getLength(double t0, double t1) const

{

	return -1;

}

//=============================================================================

TPointD TQuadratic::getPoint(double t) const

{

	double s = 1 - t;

	return m_p0 * s * s + 2 * t * s * m_p1 + t * t * m_p2;

}

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

double TQuadratic::getX(double t) const

{

	double s = 1 - t;

	return m_p0.x * s * s + 2 * t * s * m_p1.x + t * t * m_p2.x;

}

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

double TQuadratic::getY(double t) const

{

	double s = 1 - t;

	return m_p0.y * s * s + 2 * t * s * m_p1.y + t * t * m_p2.y;

}

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

double TQuadratic::getT(const TPointD &p) const

{

	// risolvo l'equazione  min|| b(t) - p ||

	// esprimo b in forma di polinomio  ed ottengo

	//

	//    ||  2                ||

	//min || a t + b t + c - p ||

	//    ||                   ||

	//

	// il tutto si riconduce a cercare le radici

	//  di un'equazione del tipo

	//    2  3          2               2

	// 2·a ·t  + 3·a·b·t  + t·(2·a·v + b ) + b·v

	// dove v e' pari a c - p

	vector<tpointd></tpointd>

		bez(3),

		poly(3);

	bez[0] = m_p0;

	bez[1] = m_p1;

	bez[2] = m_p2;

	bezier2poly(bez, poly);

	TPointD v = poly[0] - p;

	vector<double> toSolve(4);</double>

	vector<double> sol;</double>

	toSolve[3] = 2.0 * norm2(poly[2]);

	toSolve[2] = 3.0 * (poly[2].x * poly[1].x + poly[2].y * poly[1].y);

	toSolve[1] = 2.0 * (poly[2].x * v.x + poly[2].y * v.y) + norm2(poly[1]);

	toSolve[0] = (poly[1].x * v.x + poly[1].y * v.y);

	int nSol = rootFinding(toSolve, sol);

	if (-1 == nSol) // infinite soluzioni

		return 0;

	int minParameter = -1;

	double minDist = (std::numeric_limits<double>::max)();</double>

	for (int i = 0; i < nSol; ++i) {

		if (sol[i] < 0.0)

			sol[i] = 0.0;

		else if (sol[i] > 1.0)

			sol[i] = 1.0;

		double tmpDist = tdistance2(p, getPoint(sol[i]));

		if (tmpDist < minDist) {

			minDist = tmpDist;

			minParameter = i;

		}

	}

	if (minParameter != -1)

		return sol[minParameter];

	return tdistance2(m_p0, p) < tdistance2(m_p2, p) ? 0 : 1;

}

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

void TQuadratic::split(double t, TQuadratic &left, TQuadratic &right) const

{

	double dt;

	TPointD p;

	dt = 1.0 - t;

	left.m_p0 = m_p0;

	right.m_p2 = m_p2;

	left.m_p1 = dt * m_p0 + t * m_p1;

	right.m_p1 = dt * m_p1 + t * m_p2;

	p = dt * left.m_p1 + t * right.m_p1;

	left.m_p2 = right.m_p0 = p;

}

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

TRectD TQuadratic::getBBox() const

{

	TRectD bBox;

	if (m_p0.x < m_p2.x)

		bBox.x0 = m_p0.x, bBox.x1 = m_p2.x;

	else

		bBox.x0 = m_p2.x, bBox.x1 = m_p0.x;

	if (m_p0.y < m_p2.y)

		bBox.y0 = m_p0.y, bBox.y1 = m_p2.y;

	else

		bBox.y0 = m_p2.y, bBox.y1 = m_p0.y;

	TPointD denom = 2 * m_p1 - m_p0 - m_p2;

	if (denom.x != 0) {

		double tx = (m_p1.x - m_p0.x) / denom.x;

		if (tx >= 0 && tx <= 1) {

			double x = getPoint(tx).x;

			if (x < bBox.x0)

				bBox.x0 = x;

			else if (x > bBox.x1)

				bBox.x1 = x;

		}

	}

	if (denom.y != 0) {

		double ty = (m_p1.y - m_p0.y) / denom.y;

		if (ty >= 0 && ty <= 1) {

			double y = getPoint(ty).y;

			if (y < bBox.y0)

				bBox.y0 = y;

			else if (y > bBox.y1)

				bBox.y1 = y;

		}

	}

	return bBox;

}

/*!

Calcolo della curvatura per una Quadratica.

Vedi Farin pag.176 per la spiegazione della formula

usata.

*/

double TQuadratic::getCurvature(double t) const

{

	assert(0 <= t && t <= 1.0);

	TQuadratic q1, q2;

	split(t, q1, q2);

	double signum = 1.0;

	if (areAlmostEqual(t, 1.0)) {

		signum *= -1.0;

		std::swap(q1, q2);

		std::swap(q2.m_p0, q2.m_p2);

	}

	TPointD v_1_0(q2.m_p1 - q2.m_p0);

	double

		a = norm2(v_1_0);

	if (isAlmostZero(a))

		return (std::numeric_limits<double>::max)();</double>

	a = 1.0 / sqrt(a);

	double

		b = cross(v_1_0 * a, q2.m_p2 - q2.m_p0);

	return 0.5 * signum * b / a;

}

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

double TQuadratic::getLength(double t0, double t1) const

{

	TQuadraticLengthEvaluator lengthEval(*this);

	t0 = min(max(0.0, t0), 1.0); // backward compatibility

	t1 = min(max(0.0, t1), 1.0); // backward compatibility

	if (t0 > t1)

		std::swap(t0, t1);

	if (t0 > 0.0)

		return lengthEval.getLengthAt(t1) - lengthEval.getLengthAt(t0);

	return lengthEval.getLengthAt(t1);

}

double TQuadratic::getApproximateLength(double t0, double t1, double error) const

{

	if (t0 == t1)

		return 0;

	t0 = min(max(0.0, t0), 1.0);

	t1 = min(max(0.0, t1), 1.0);

	if (t0 > t1)

		std::swap(t0, t1);

	TQuadratic q;

	if (t0 == 0.0 && t1 == 1.0)

		q = *this;

	else {

		TQuadratic q1;

		split(t0, q, q1);

		assert(t0 != 1.0);

		double newPar = (t1 - t0) / (1.0 - t0);

		q1.split(newPar, q, q1);

	}

	double step = computeStep(q, error);

	double length = 0.0;

	TPointD p1 = q.getP0();

	TPointD p2;

	for (double t = step; t < 1.0; t += step) {

		p2 = q.getPoint(t);

		length += tdistance(p1, p2);

		p1 = p2;

	}

	length += tdistance(p1, q.getP2());

	return length;

}

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

int TQuadratic::getX(double y, double &x0, double &x1) const

{

	int ret = 0;

	double t;

	if (y > getBBox().y1 || y < getBBox().y0)

		return 0;

	double a = getP0().y - 2 * getP1().y + getP2().y;

	double half_b = getP1().y - getP0().y;

	double c = getP0().y - y;

	if (a == 0) //segment

	{

		if (half_b == 0) //horizontal segment, or point

		{

			if (c == 0) {

				x0 = getP0().x;

				x1 = getP2().x;

				return 2;

			} else

				return 0;

		} else {

			t = -c / (2 * half_b);

			if (t >= 0 && t <= 1) {

				x0 = getPoint(t).x;

				return 1;

			}

		}

	}

	double discr = half_b * half_b - a * c;

	if (discr < 0)

		return 0;

	double coeff = 1.0 / a;

	double coeff1 = -half_b * coeff;

	if (discr == 0) {

		t = coeff1;

		if (t >= 0 && t <= 1) {

			ret = 2;

			x0 = x1 = getPoint(t).x;

		}

	} else {

		discr = sqrt(discr) * coeff;

		t = coeff1 + discr;

		if (t >= 0 && t <= 1) {

			ret++;

			x0 = getPoint(t).x;

		}

		t = coeff1 - discr;

		if (t >= 0 && t <= 1) {

			ret++;

			if (ret == 2)

				x1 = getPoint(t).x;

			else

				x0 = getPoint(t).x;

		}

	}

	return ret;

}

int TQuadratic::getY(double y, double &y0, double &y1) const

{

	TQuadratic temp(*this);

	swap(temp.m_p0.x, temp.m_p0.y);

	swap(temp.m_p1.x, temp.m_p1.y);

	swap(temp.m_p2.x, temp.m_p2.y);

	return temp.getX(y, y0, y1);

}

//=============================================================================

TPointD TCubic::getPoint(double t) const

{

	double s = 1 - t;

	return m_p0 * s * s * s + 3 * t * s * (s * m_p1 + t * m_p2) + t * t * t * m_p3;

}

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

TPointD TCubic::getSpeed(double t) const

{

	double s = 1 - t;

	return 3.0 * ((m_p1 - m_p0) * s * s + 2 * (m_p2 - m_p0) * s * t + (m_p3 - m_p2) * t * t);

}

//=============================================================================

TThickQuadratic::TThickQuadratic()

	: TQuadratic(), m_thickP0(0), m_thickP1(0), m_thickP2(0)

{

}

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

TThickQuadratic::TThickQuadratic(const TQuadratic &q)

	: TQuadratic(q), m_thickP0(0.0), m_thickP1(0.0), m_thickP2(0.0)

{

}

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

TThickQuadratic::TThickQuadratic(const TPointD &p0, double thickP0,

								 const TPointD &p1, double thickP1,

								 const TPointD &p2, double thickP2)

	: TQuadratic(p0, p1, p2), m_thickP0(thickP0), m_thickP1(thickP1), m_thickP2(thickP2)

{

}

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

TThickQuadratic::TThickQuadratic(const TThickPoint &p0,

								 const TThickPoint &p1,

								 const TThickPoint &p2)

	: TQuadratic(TPointD(p0.x, p0.y), TPointD(p1.x, p1.y), TPointD(p2.x, p2.y)), m_thickP0(p0.thick), m_thickP1(p1.thick), m_thickP2(p2.thick)

{

}

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

TThickQuadratic::TThickQuadratic(const TThickQuadratic &thickQuadratic)

	: TQuadratic(thickQuadratic), m_thickP0(thickQuadratic.m_thickP0), m_thickP1(thickQuadratic.m_thickP1), m_thickP2(thickQuadratic.m_thickP2)

{

}

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

void TThickQuadratic::setThickP0(const TThickPoint &p)

{

	m_p0 = p;

	m_thickP0 = p.thick;

}

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

void TThickQuadratic::setThickP1(const TThickPoint &p)

{

	m_p1 = p;

	m_thickP1 = p.thick;

}

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

void TThickQuadratic::setThickP2(const TThickPoint &p)

{

	m_p2 = p;

	m_thickP2 = p.thick;

}

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

TThickPoint TThickQuadratic::getThickPoint(double t) const

{

	double s = 1 - t;

	return TThickPoint(m_p0 * s * s + 2 * t * s * m_p1 + t * t * m_p2, m_thickP0 * s * s + 2 * t * s * m_thickP1 + t * t * m_thickP2);

}

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

void TThickQuadratic::split(double t, TThickQuadratic &left, TThickQuadratic &right) const

{

	double dt;

	TPointD p;

	dt = 1.0 - t;

	// control points

	left.m_p0 = m_p0;

	right.m_p2 = m_p2;

	left.m_p1 = dt * m_p0 + t * m_p1;

	right.m_p1 = dt * m_p1 + t * m_p2;

	p = dt * left.m_p1 + t * right.m_p1;

	left.m_p2 = right.m_p0 = p;

	// thick points

	left.m_thickP0 = m_thickP0;

	right.m_thickP2 = m_thickP2;

	left.m_thickP1 = dt * m_thickP0 + t * m_thickP1;

	right.m_thickP1 = dt * m_thickP1 + t * m_thickP2;

	// store thickness of intermediary point

	p.x = dt * left.m_thickP1 + t * right.m_thickP1;

	left.m_thickP2 = right.m_thickP0 = p.x;

}

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

TRectD TThickQuadratic::getBBox() const

{

	TRectD bBox = TQuadratic::getBBox();

	double maxRadius = tmax(m_thickP0, m_thickP1, m_thickP2);

	if (maxRadius > 0) {

		//  bBox.enlarge(maxRadius) si comporta male nel caso bBox.isEmpty()

		bBox.x0 -= maxRadius;

		bBox.y0 -= maxRadius;

		bBox.x1 += maxRadius;

		bBox.y1 += maxRadius;

	}

	return bBox;

}

// ============================================================================

// Methods of the class TThickCubic

// ============================================================================

TThickCubic::TThickCubic()

	: TCubic(), m_thickP0(0), m_thickP1(0), m_thickP2(0), m_thickP3(0)

{

}

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

TThickCubic::TThickCubic(const TPointD &p0, double thickP0,

						 const TPointD &p1, double thickP1,

						 const TPointD &p2, double thickP2,

						 const TPointD &p3, double thickP3)

	: TCubic(p0, p1, p2, p3), m_thickP0(thickP0), m_thickP1(thickP1), m_thickP2(thickP2), m_thickP3(thickP3)

{

}

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

TThickCubic::TThickCubic(const TThickPoint &p0,

						 const TThickPoint &p1,

						 const TThickPoint &p2,

						 const TThickPoint &p3)

	: TCubic(TPointD(p0.x, p0.y), TPointD(p1.x, p1.y), TPointD(p2.x, p2.y), TPointD(p3.x, p3.y)), m_thickP0(p0.thick), m_thickP1(p1.thick), m_thickP2(p2.thick), m_thickP3(p3.thick)

{

}

//  tonino ***************************************************************

TThickCubic::TThickCubic(const T3DPointD &p0,

						 const T3DPointD &p1,

						 const T3DPointD &p2,

						 const T3DPointD &p3)

	: TCubic(TPointD(p0.x, p0.y), TPointD(p1.x, p1.y), TPointD(p2.x, p2.y), TPointD(p3.x, p3.y)), m_thickP0(p0.z), m_thickP1(p1.z), m_thickP2(p2.z), m_thickP3(p3.z)

{

}

//  tonino ***************************************************************

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

TThickCubic::TThickCubic(const TThickCubic &thickCubic)

	: TCubic(thickCubic), m_thickP0(thickCubic.m_thickP0), m_thickP1(thickCubic.m_thickP1), m_thickP2(thickCubic.m_thickP2), m_thickP3(thickCubic.m_thickP3)

{

}

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

void TThickCubic::setThickP0(const TThickPoint &p)

{

	m_p0.x = p.x;

	m_p0.y = p.y;

	m_thickP0 = p.thick;

}

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

void TThickCubic::setThickP1(const TThickPoint &p)

{

	m_p1.x = p.x;

	m_p1.y = p.y;

	m_thickP1 = p.thick;

}

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

void TThickCubic::setThickP2(const TThickPoint &p)

{

	m_p2.x = p.x;

	m_p2.y = p.y;

	m_thickP2 = p.thick;

}

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

void TThickCubic::setThickP3(const TThickPoint &p)

{

	m_p3.x = p.x;

	m_p3.y = p.y;

	m_thickP3 = p.thick;

}

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

TThickPoint TThickCubic::getThickPoint(double t) const

{

	double

		thick_l1,

		thick_h,

		thick_r3;

	double s = 1.0 - t;

	TPointD l1(m_p0 * s + m_p1 * t);

	thick_l1 = m_thickP0 * s + m_thickP1 * t;

	TPointD h(m_p1 * s + m_p2 * t);

	thick_h = m_thickP1 * s + m_thickP2 * t;

	TPointD r3(m_p2 * s + m_p3 * t);

	thick_r3 = m_thickP2 * s + m_thickP3 * t;

	// adesso riutilizzo le variabili gia' utilizzate

	// l2

	l1 = l1 * s + h * t;

	thick_l1 = thick_l1 * s + thick_h * t;

	// r1

	r3 = h * s + r3 * t;

	thick_r3 = thick_h * s + thick_r3 * t;

	// l3-r0

	h = l1 * s + r3 * t;

	thick_h = thick_l1 * s + thick_r3 * t;

	return TThickPoint(h, thick_h);

}

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

void TThickCubic::split(double t, TThickCubic &first, TThickCubic &second) const

{

	double s = 1.0 - t;

	TPointD H(m_p1 * s + m_p2 * t);

	double thick_h = m_thickP1 * s + m_thickP2 * t;

	first.m_p0 = m_p0;

	first.m_thickP0 = m_thickP0;

	first.m_p1 = m_p0 * s + m_p1 * t;

	first.m_thickP1 = m_thickP0 * s + m_thickP1 * t;

	first.m_p2 = first.m_p1 * s + H * t;

	first.m_thickP2 = first.m_thickP1 * s + thick_h * t;

	second.m_p3 = m_p3;

	second.m_thickP3 = m_thickP3;

	second.m_p2 = m_p2 * s + m_p3 * t;

	second.m_thickP2 = m_thickP2 * s + m_thickP3 * t;

	second.m_p1 = H * s + second.m_p2 * t;

	second.m_thickP1 = thick_h * s + second.m_thickP2 * t;

	first.m_p3 = first.m_p2 * s + second.m_p1 * t;

	first.m_thickP3 = first.m_thickP2 * s + second.m_thickP1 * t;

	second.m_p0 = first.m_p3;

	second.m_thickP0 = first.m_thickP3;

}

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

ostream &operator<<(ostream &out, const TQuadratic &curve)

{

	return out << "Q{" << curve.getP0() << ", " << curve.getP1() << ", "

			   << curve.getP2() << "}";

}

ostream &operator<<(ostream &out, const TCubic &curve)

{

	return out << "C{" << curve.getP0() << ", " << curve.getP1() << ", "

			   << curve.getP2() << ", " << curve.getP3() << "}";

}

ostream &operator<<(ostream &out, const TThickSegment &segment)

{

	return out << "TS{" << segment.getThickP0() << ", " << segment.getThickP1() << "}";

}

ostream &operator<<(ostream &out, const TThickQuadratic &tq)

{

	return out << "TQ{" << tq.getThickP0() << ", " << tq.getThickP1() << ", " << tq.getThickP2() << "}";

}

ostream &operator<<(ostream &out, const TThickCubic &tc)

{

	return out << "TC{" << tc.getThickP0() << ", "

			   << tc.getThickP1() << ", "

			   << tc.getThickP2() << ", "

			   << tc.getThickP3() << "}";

}

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

//  End Of File

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