// TnzCore includes
#include "tmathutil.h"
#include "tcurves.h"
#include "tbezier.h"
#include "tstrokedeformations.h"
#include "tstroke.h"
#include "tcurveutil.h"
#include "tcg_wrap.h"

// tcg includes
#include "tcg/tcg_poly_ops.h"

#define INCLUDE_HPP
#include "tcg/tcg_polylineops.h"
#include "tcg/tcg_cyclic.h"
#undef INCLUDE_HPP

#include "tstrokeutil.h"

//*********************************************************************************
//    Local namespace  stuff
//*********************************************************************************

namespace
{

typedef std::vector<TThickCubic *> TThickCubicArray;
typedef std::vector<TThickQuadratic *> QuadStrokeChunkArray;

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

int getControlPointIndex(const TStroke &stroke,
						 double w)
{
	TThickPoint p = stroke.getControlPointAtParameter(w);

	int i = 0;
	int controlPointCount = stroke.getControlPointCount();

	for (; i < controlPointCount; ++i)
		if (stroke.getControlPoint(i) == p)
			return i;

	return controlPointCount - 1;
}

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

double findMinimum(const TStrokeDeformation &def,
				   const TStroke &stroke,
				   double x1,
				   double x2,
				   double xacc,
				   double length = 0,
				   int max_iter = 100)

{
	int j;
	double dx, f, fmid, xmid, rtb;

	f = def.getDelta(stroke, x1) - length;
	fmid = def.getDelta(stroke, x2) - length;

	if (f == 0)
		return x1;
	if (fmid == 0)
		return x2;

	if (f * fmid > 0.0)
		return -1;

	rtb = f < 0.0 ? (dx = x2 - x1, x1) : (dx = x1 - x2, x2);

	for (j = 1; j <= max_iter; j++) {
		fmid = def.getDelta(stroke, xmid = rtb + (dx *= 0.5)) - length;
		if (fmid <= 0.0)
			rtb = xmid;
		if (fabs(dx) < xacc || fmid == 0.0)
			return rtb;
	}
	return -2;
}

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

/**
  * Rationale:
  *  Supponiamo di voler modellare un segmento (rappresentato da una stroke)
  *  in modo che assuma la forma di una parabola (caso abituale offerto dal modificatore).
  *  Poniamo il che:
  *   (o) i punti della stroke si trovino lungo l'asse y=-100;
  *   (o) le x che corrisponderanno siano x1=-10 e x2=+10 (ovvio dall'equazione).
  * 
  *  La parabola potrà essere rappresentata sul lato sx da una quadratica con 
  *  punti di controllo:
  *    P0=(-10,-100),
  *    P1=(-5,    0),
  *    P2=( 0,    0).
  *  Se conosciamo il numero di tratti lineari che rappresentano questa parabola,
  *  sappiamo anche quanti "campioni" sono richiesti per la sua linearizzazione.
  *  Questo parametro può essere utilizzato per stabilire in modo qualitativo
  *  il valore con cui campionare la stroke da testare; ci dovranno essere tanti
  *  punti da spostare per quanti campioni sono presenti nel riferimento.
  */
double
computeIncrement(double strokeLength,
				 double pixelSize)
{
	assert(pixelSize > 0 && "Pixel size is negative!!!");
	assert(strokeLength > 0 && "Stroke Length size is negative!!!");

	// altezza della parabola (va verso il basso)
	double
		height = 100;

	// suppongo di fare almeno un drag di 100 pixel
	assert(height >= 100.0);

	double
		x = sqrt(height);

	// il punto p1 dovra' essere all'intersezione
	//  tra le tangenti ai due estremi.
	//  La tangente del punto p2 e l'asse x,
	//  l'altra avra' versore dato dal gradiente in p0,
	//  cioe': grad(x,-2 x)
	//  e se y = m x + q
	//  m =
	double
		m = 2.0 * x;

	double
		q = m * x - height;

	double
		p1x = q / m;

	double
		scale = strokeLength / (2.0 * x);

	TScale
		scaleAffine(scale, scale);

	TPointD
		p0 = scaleAffine * TPointD(-x, -height),
		p1 = scaleAffine * TPointD(-p1x, 0.0),
		p2 = scaleAffine * TPointD(0.0, 0.0);

	TQuadratic
		quadratic(p0,
				  p1,
				  p2);

	double
		step = computeStep(quadratic,
						   pixelSize);

	//  giusto per aggiungere punti anche nel caso peggiore.
	if (step >= 1.0)
		step = 0.1;

	return step;
}

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

void detectEdges(const std::vector<TPointD> &pointArray, std::vector<UINT> &edgeIndexArray)
{
	// ASSUNZIONE: sharpPointArray non contiene punti coincidenti adiacenti

	int size = pointArray.size();
	// controllo che ci siano piu' di tre elementi
	if (size < 3)
		return;
	//  scorre pointArray e per ogni suo punto cerca di inscrivere triangoli (utilizzando i
	//  punti a sinistra e a destra) considerando potenziali corner quelli con lati l tale
	//  che dMin <= l <= dMax (in realta' alla prima volta che l > dMax: breack) e con apertura
	//  angolare alpha <= alphaMax. Poi cerca i max locali tra i potenziali corner in una
	//  finestra di semiampiezza dMax (al solito alla prima volta che si supera dMax: breack)

	//  valori di default: dMin = 7; dMax = dMin + 2; alphaMax = 2.6 (150°)

	const double dMin = 4;
	const double dMax = dMin + 3;
	const double alphaMax = 2.4; // ( 137.5°)
	const double dMin2 = dMin * dMin;
	const double dMax2 = dMax * dMax;
	std::vector<double> sharpnessArray;
	sharpnessArray.push_back(TConsts::pi); //  il primo punto e' un corner
	int nodeCount;
	for (nodeCount = 1; nodeCount < size - 1; ++nodeCount) { //  scorre la sharpPointArray escludendo gli estremi
		sharpnessArray.push_back(0);
		TPointD point(pointArray[nodeCount]);
		int leftCount;
		for (leftCount = nodeCount - 1; leftCount >= 0; --leftCount) { //  calcola i lati "left" dei triangoli inscritti...
			TPointD left = pointArray[leftCount];
			double dLeft2 = norm2(left - point);
			if (dLeft2 < dMin2)
				continue;
			else if (dLeft2 > dMax2)
				break;
			int rightCount;
			for (rightCount = nodeCount + 1; rightCount < size; ++rightCount) { //  calcola i lati "right" dei triangoli inscritti...
				TPointD right = pointArray[rightCount];
				double dRight2 = norm2(right - point);
				if (dRight2 < dMin2)
					continue;
				else if (dMax2 < dRight2)
					break;

				//  calcola i lati "center" dei triangoli inscritti
				double dCenter2 = norm2(left - right);
				assert(dLeft2 != 0.0 && dRight2 != 0.0);

				double cs = (dLeft2 + dRight2 - dCenter2) / (2 * sqrt(dLeft2 * dRight2));
				double alpha = acos(cs);
				if (alpha > alphaMax)
					continue;

				double sharpness = TConsts::pi - alpha;

				if (sharpnessArray[nodeCount] < sharpness)
					sharpnessArray[nodeCount] = sharpness;
			}
		}
	}

	edgeIndexArray.push_back(0); //  il primo punto e' un corner

	// trovo i massimi locali escludendo gli estremi
	for (nodeCount = 1; nodeCount < size - 1; ++nodeCount) { //  scorre la lista escludendo gli estremi
		bool isCorner = true;
		TPointD point(pointArray[nodeCount]);
		int leftCount;
		for (leftCount = nodeCount - 1; leftCount >= 0; --leftCount) { //  scorre la lista di sharpPoint a sinistra di node...
			TPointD left = pointArray[leftCount];
			double dLeft2 = norm2(left - point);
			if (dLeft2 > dMax2)
				break;
			if (sharpnessArray[leftCount] > sharpnessArray[nodeCount]) {
				isCorner = false;
				break;
			}
		}
		if (isCorner)
			continue;
		int rightCount;
		for (rightCount = nodeCount + 1; rightCount < size; ++rightCount) { //  scorre la lista di sharpPoint a destra di node..
			TPointD right = pointArray[rightCount];
			double dRight2 = norm2(right - point);
			if (dRight2 > dMax2)
				break;
			if (sharpnessArray[rightCount] > sharpnessArray[nodeCount]) {
				isCorner = false;
				break;
			}
		}
		if (isCorner)
			edgeIndexArray.push_back(nodeCount);
	}
	edgeIndexArray.push_back(size - 1); //  l'ultimo punto e' un corner
}

} // namespace

//*******************************************************************************
//    API  functions
//*******************************************************************************

bool increaseControlPoints(TStroke &stroke,
						   const TStrokeDeformation &deformer,
						   double pixelSize)
{

	if (isAlmostZero(stroke.getLength())) {
		return norm2(deformer.getDisplacement(stroke, 0.0)) > 0;
	}

	// step 1:
	// It's possible to have control point at not null potential
	//  but with delta equal 0 (equipotential control point)
	bool notVoidPotential = false;

	for (int i = 0; i < stroke.getControlPointCount(); ++i) {
		double par = stroke.getParameterAtControlPoint(i);
		if (deformer.getDisplacement(stroke, par) != TThickPoint()) {
			notVoidPotential = true;
			break;
		}
	}

	// step 2:
	//  increase control point checking delta of deformer
	double maxDifference = deformer.getMaxDiff(); //sopra questo valore di delta, si aggiungono punti

	int strokeControlPoint = stroke.getControlPointCount();

	// pixelSize = sq( pixelSize );

	if (pixelSize < TConsts::epsilon)
		pixelSize = TConsts::epsilon;

	double
		length = stroke.getLength(),
		// set the step function of length
		//    step = length > 1.0 ?  pixelSize * 15.0/ length : length,
		//step = 0.01,
		w = 0.0;

	double
		step = computeIncrement(length,
								pixelSize);

	double x1, x2, d1, d2, diff, offset, minimum, incr;

	incr = step;

	while (w + incr < 1.0) {
		d1 = deformer.getDelta(stroke, w);
		d2 = deformer.getDelta(stroke, w + incr);

		diff = d2 - d1;

		if (fabs(diff) >= maxDifference) // if there is a step of potential
		{
			if (tsign(diff) > 0) {
				x1 = w;
				x2 = w + incr;
			} else {
				x1 = w + incr;
				x2 = w;
			}

			offset = (d1 + d2) * 0.5;

			// find the position of step
			minimum = findMinimum(deformer, stroke, x1, x2, TConsts::epsilon, offset, 20); //tra x1 e x2 va messo un nuovo punto di controllo. dove?
			//questa funzione trova il punto in cui si supera il valore maxdifference

			// if minimum is not found or is equal to previous value
			//  use an euristic...
			if (minimum < 0 || w == minimum) {
				minimum = w + incr * 0.5;
				w += step;
			}

			//... else insert a control point in minimum
			w = minimum; //la scansione riprende dal nuovo punto, in questo modo si infittisce...
			stroke.insertControlPoints(minimum);

			// update of step
			incr = step;
		} else
			incr += step;
	}

	// return true if control point are increased
	return (stroke.getControlPointCount() > strokeControlPoint) || notVoidPotential;
}

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

void modifyControlPoints(TStroke &stroke,
						 const TStrokeDeformation &deformer)
{
	int cpCount = stroke.getControlPointCount();

	TThickPoint newP;

	for (int i = 0; i < cpCount; ++i) {
		newP = stroke.getControlPoint(i) + deformer.getDisplacementForControlPoint(stroke, i);
		if (isAlmostZero(newP.thick, 0.005))
			newP.thick = 0;
		stroke.setControlPoint(i, newP);
	}
}

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

void modifyControlPoints(TStroke &stroke,
						 const TStrokeDeformation &deformer, std::vector<double> &controlPointLen)
{
	UINT cpCount = stroke.getControlPointCount();

	TThickPoint newP;

#ifdef _DEBUG
	UINT debugVariable = controlPointLen.size();
#endif
	assert(controlPointLen.size() == cpCount);

	for (UINT i = 0; i < cpCount; ++i) {
		newP = stroke.getControlPoint(i) + deformer.getDisplacementForControlPointLen(stroke, controlPointLen[i]);
		if (isAlmostZero(newP.thick, 0.005))
			newP.thick = 0;
		stroke.setControlPoint(i, newP);
	}
}

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

void modifyThickness(TStroke &stroke, const TStrokeDeformation &deformer,
					 std::vector<double> &controlPointLen, bool exponentially)
{
	UINT cpCount = stroke.getControlPointCount();
	assert(controlPointLen.size() == cpCount);

	double disp;
	double thick;

	for (UINT i = 0; i < cpCount; ++i) {
		disp = (deformer.getDisplacementForControlPointLen(stroke, controlPointLen[i])).thick;

		thick = stroke.getControlPoint(i).thick;

		//The additive version is straightforward.
		//The exponential version is devised to keep derivative 1 at disp == 0;
		//it is typically used when the thickness decreases.

		thick = (exponentially && thick >= 0.005) ? thick * exp(disp / thick) : thick + disp;

		if (thick < 0.005)
			thick = 0.0;

		stroke.setControlPoint(i, TThickPoint(stroke.getControlPoint(i), thick));
	}
}

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

void transform_thickness(TStroke &stroke, const double poly[], int deg)
{
	int cp, cpCount = stroke.getControlPointCount();
	for (cp = 0; cp != cpCount; ++cp) {
		TThickPoint cpPoint = stroke.getControlPoint(cp);
		cpPoint.thick = std::max(
			tcg::poly_ops::evaluate(poly, deg, cpPoint.thick),
			0.0);

		stroke.setControlPoint(cp, cpPoint);
	}
}

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

TStroke *Toonz::merge(const std::vector<TStroke *> &strokes)
{
	if (strokes.empty())
		return 0;

	std::vector<TThickPoint>
		new_stroke_cp;

	int
		size_stroke_array = strokes.size();

	int
		size_cp;

	const TStroke *
		ref;

	TThickPoint
		last = TConsts::natp;

	if (!strokes[0])
		return 0;

	new_stroke_cp.push_back(strokes[0]->getControlPoint(0));
	int i, j;
	for (i = 0;
		 i < size_stroke_array;
		 i++) {
		ref = strokes[i];
		if (!ref)
			return 0;

		size_cp = ref->getControlPointCount();
		for (j = 0;
			 j < size_cp - 1;
			 j++) {
			const TThickPoint &
				pnt = ref->getControlPoint(j);

			if (last != TConsts::natp &&
				j == 0) {
				//new_stroke_cp.push_back( (last+pnt)*0.5 );
				new_stroke_cp.push_back(last);
			}

			if (j > 0)
				new_stroke_cp.push_back(pnt);
		}
		// last point needs to be merged
		last = ref->getControlPoint(size_cp - 1);
	}

	new_stroke_cp.push_back(ref->getControlPoint(size_cp - 1));

	TStroke *out = new TStroke(new_stroke_cp);
	return out;
}

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

namespace
{

class CpsReader
{
	std::vector<TThickPoint> &m_cps;

public:
	typedef TPointD value_type;

public:
	CpsReader(std::vector<TThickPoint> &cps) : m_cps(cps) {}

	void openContainer(const TPointD &point) { addElement(point); }
	void addElement(const TPointD &point) { m_cps.push_back(TThickPoint(point, 0.0)); }
	void closeContainer() {}
};

//===========================================================
//    Triplet to Quadratics
//===========================================================

template <typename iter_type>
double buildLength(const iter_type &begin, const iter_type &end, double tol)
{
	//Build direction
	iter_type it = begin, jt;
	++it;

	const TPointD &a = *begin, &b = *it;

	TPointD dir(normalize(b - a)), segDir;
	double dist;

	for (jt = it, ++it; it != end; jt = it, ++it) {
		segDir = *it - *jt;
		if (dir * segDir < 0)
			break;

		dist = tcg::point_ops::lineSignedDist(*it, a, dir);
		if (fabs(dist) > tol) {
			double s, t;
			if (dist > 0) {
				tcg::point_ops::intersectionCoords(*jt, segDir,
												   a + tol * tcg::point_ops::ortLeft(dir), dir, s, t);
			} else {
				tcg::point_ops::intersectionCoords(*jt, segDir,
												   a + tol * tcg::point_ops::ortRight(dir), dir, s, t);
			}

			s = tcrop(s, 0.0, 1.0);
			return (*jt + s * segDir - a) * dir;
		}
	}

	return (*jt - a) * dir;
}

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

/*
  Converts the specified points triplet into a sequence of quadratics' CPs (point
  a is not included, whereas c is).

  Conversion takes 4 parameters:

   - Adherence:     How much quadratics bend toward corners
   - Angle:         Inner product of corner's edges - full corners threshold
   - Relative:      Curvature radius/edge length    - full corners threshold
   - RelativeDist:  Tolerance about edge length build-ups

  See below for extended explanation.
*/

class TripletsConverter
{
	typedef std::vector<TPointD>::const_iterator iter_type;
	typedef std::reverse_iterator<iter_type> riter_type;
	typedef tcg::cyclic_iterator<iter_type> cyclic_iter_type;
	typedef std::reverse_iterator<cyclic_iter_type> rcyclic_iter_type;

	bool m_circular;
	iter_type m_first, m_end, m_last;
	double m_adherenceTol, m_angleTol, m_relativeTol, m_relativeDistTol;

public:
	TripletsConverter(const iter_type &begin, const iter_type &end,
					  double adherenceTol, double angleTol,
					  double relativeTol, double relativeDistTol)
		: m_circular(*begin == *(end - 1)), m_first(m_circular ? begin + 1 : begin), m_end(end), m_adherenceTol(adherenceTol), m_angleTol(angleTol), m_relativeTol(relativeTol), m_relativeDistTol(relativeDistTol) {}

	//Using bisector to convert a triplet
	void operator()(const TPointD &a, const iter_type &bt, const TPointD &c,
					tcg::sequential_reader<std::vector<TPointD>> &output)
	{
		const TPointD &b = *bt;

		double prod = tcg::point_ops::direction(b, a) * tcg::point_ops::direction(b, c);

		if (prod > m_angleTol) {
			//Full corner
			output.addElement(0.5 * (a + b));
			output.addElement(b);
			output.addElement(0.5 * (b + c));
		} else {
			//Build the angle bisector
			TPointD a_b(a - b);
			TPointD c_b(c - b);

			double norm_a_b = norm(a_b);
			double norm_c_b = norm(c_b);

			a_b = a_b * (1.0 / norm_a_b);
			c_b = c_b * (1.0 / norm_c_b);

			TPointD v(tcg::point_ops::normalized(a_b + c_b));
			double cos_v_dir = fabs(a_b * v);

			double t1 = tcrop(m_adherenceTol / (cos_v_dir * norm_a_b), 0.0, 0.5);
			double t2 = tcrop(m_adherenceTol / (cos_v_dir * norm_c_b), 0.0, 0.5);

			if (t1 == 0.5 && t2 == 0.5) {
				//Direct conversion
				output.addElement(b);
			} else {
				//Build the quadratic split
				TPointD d(b + t1 * (a - b)), f(b + t2 * (c - b)), e(0.5 * (d + f));

				//Build curvature radiuses at the corner

				//NOTE: Both speed and acceleration would hold 2.0 as multiplier, which
				//is calculated implicitly.

				TPointD speed(f - d);

				double num = norm(speed);
				if (num <= TConsts::epsilon) {
					//Curvature radius is 0 - full corner
					output.addElement(0.5 * (a + b));
					output.addElement(b);
					output.addElement(0.5 * (b + c));
				} else {
					num = 2.0 * num * num * num; // would be * 8 = 2^3, divided by the 4 below

					double den1 = fabs(cross(speed, a - d)); // * 4, from both args of the cross
					double den2 = fabs(cross(speed, c - f));

					double radius1 = (den1 == 0.0) ? 0.0 : num / den1;
					double radius2 = (den1 == 0.0) ? 0.0 : num / den2;

					//Build edges length
					double length1, length2;
					if (m_circular) {
						cyclic_iter_type it(bt, m_first, m_end, 0);
						cyclic_iter_type it1(bt, m_first, m_end, 1);
						cyclic_iter_type it_1(bt, m_first, m_end, -1);
						rcyclic_iter_type rit(it + 1), rit1(it_1 + 1);

						length1 = buildLength(rit, rit1, 0.25);
						length2 = buildLength(it, it1, 0.25);
					} else {
						riter_type rit(bt + 1), rend(m_first);

						length1 = buildLength(rit, rend, m_relativeDistTol);
						length2 = buildLength(bt, m_end, m_relativeDistTol);
					}

					//Test curvature radiuses against edge length
					if (radius1 / length1 < m_relativeTol && // both must hold
						radius2 / length2 < m_relativeTol) {
						//Full corner
						output.addElement(0.5 * (a + b));
						output.addElement(b);
						output.addElement(0.5 * (b + c));
					} else {
						//Quadratic split
						output.addElement(d);
						output.addElement(e);
						output.addElement(f);
					}
				}
			}
		}

		output.addElement(c);
	}
};

} //namespace

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

void polylineToQuadratics(const std::vector<TPointD> &polyline,
						  std::vector<TThickPoint> &cps,
						  double adherenceTol, double angleTol,
						  double relativeTol, double relativeDistTol,
						  double mergeTol)
{
	CpsReader cpsReader(cps);
	TripletsConverter op(polyline.begin(), polyline.end(),
						 adherenceTol, angleTol, relativeTol, relativeDistTol);
	tcg::polyline_ops::toQuadratics(polyline.begin(), polyline.end(), cpsReader, op, mergeTol);
}