#ifndef BEZIER_H_
#define BEZIER_H_

#include "../typedefs.h"

//defines granularity of the bezier line - number of points per meter of length
//of each sides of the triangle (start point, peak, end point)
#define BEZIER_PTS_PER_METER 1.25

/** Computes one point on the Bezier line */
Vector3r pointOnCubicBezier(const Vector3r &startPoint, const Vector3r &startCtrlPoint, const Vector3r &endPoint, const Vector3r &endCtrlPoint, float t)
{
	float ax, bx, cx;
	float ay, by, cy;
	float tSquared, tCubed;
	Vector3r result;
	
	/* calculate the polynomial coefficients */
	cx = 3.0 * (startCtrlPoint.x - startPoint.x);
	bx = 3.0 * (endCtrlPoint.x - startCtrlPoint.x) - cx;
	ax = endPoint.x - startPoint.x - cx - bx;
	cy = 3.0 * (startCtrlPoint.y - startPoint.y);
	by = 3.0 * (endCtrlPoint.y - startCtrlPoint.y) - cy;
	ay = endPoint.y - startPoint.y - cy - by;
	
	/* calculate the curve point at parameter value t */
	tSquared = t * t;
	tCubed = tSquared * t;
	result.x = (ax * tCubed) + (bx * tSquared) + (cx * t) + startPoint.x;
	result.y = (ay * tCubed) + (by * tSquared) + (cy * t) + startPoint.y;
	result.z = 0;
	return result;
}

/** Computes points on Bezier line specified by parameters
 * @param points container to attach the points to
 * @param startPoint starting point of the line
 * @param startCtrlPoint first control point
 * @param endPoint ending point of the line
 * @param endCtrlPoint second control point
 * @param ratio drives the real position of the control points on the vectors (startPoint->startCtrlPoint) and (endPoint->endCtrlPoint)
 */
void computeBezier(Polygon &points, const Vector3r &startPoint, const Vector3r &startCtrlPoint, const Vector3r &endPoint, const Vector3r &endCtrlPoint, dReal ratio = 1)
{
	float dt;
	int i;
	
	Vector3r startPtGuid = startCtrlPoint - startPoint;
	Vector3r endPtGuid = endPoint - endCtrlPoint;
	dReal dev = acos(startPtGuid.dotProduct(endPtGuid)/(startPtGuid.length()*endPtGuid.length()));
	
	//adjust the length of control point vectors based on desired ratio
	if (ratio < 0) ratio = 0;
	if (ratio > 1) ratio = 1;
	Vector3r tmp1 = startPoint + (startPtGuid * ratio);
	Vector3r tmp2 = endPoint + (endPtGuid * -ratio);
	
	//estimate number of points
	dReal length = startPtGuid.length() + endPtGuid.length();
//	int numberOfPoints = (int)ceil((length * BEZIER_PTS_PER_METER) * dev / M_PI);
	int numberOfPoints = (int)ceil((length * BEZIER_PTS_PER_METER));
	//if two or less points should be computed, put in at least the start and end point and skip the whole computation
	if (numberOfPoints < 2) {
		points.push_back(startPoint);
		points.push_back(endPoint);
		return;
	}
	
//	_DEBUG("numberOfPoints=" << numberOfPoints);
	dt = 1.0 / (numberOfPoints - 1);
	for (i = 0; i < numberOfPoints; i++)
		points.push_back(pointOnCubicBezier(startPoint, tmp1, endPoint, tmp2, i * dt));
}

#endif /*BEZIER_H_*/
