#include "Bezier.hpp"

Bezier::Bezier()
{
	points = 0;
	tangents = 0;
}

Bezier::Bezier(int pointcount)
{
	points = 0;
	tangents = 0;
    if (pointcount >= 2)
    {
        this->pcount = pointcount;
        this->pointindex = 0;
        points = new Vector[pointcount];
        tangents= new Vector[pointcount];
    }
    else
    {
        dmsMsg("trying to create a Bezier-spline with less than 2 points!\n");
    }
}
Bezier::~Bezier()
{
    if (points != 0)
    {
        delete [] points;
    }
    if (tangents != 0)
    {
        delete [] tangents;
    }

}

void Bezier::setPoint(int index, Vector &point, Vector &tangent)
{
	if(index >= pcount || index<0)
	{
		dmsMsg("Trying to add a point to a Bezier-spline in a position that is out of range\n");
	} else {
		points[index] = point;
		tangents[index] = point + tangent;
	}
}

void Bezier::addPoint(Vector &point, Vector &tangent)
{
	if(pointindex>=0 && pointindex < pcount) 
	{
		points[pointindex] = point;
		tangents[pointindex] = point + tangent;
		pointindex++;
	} else
		dmsMsg("Trying to add a point to a Bezier-spline that is full\n");
}

Vector Bezier::getValue(float pos)
{

//    float val = (pos * (pcount-3)) + 1;
//    const int index = ((int)floorf(pos * (pcount-3))) + 1;

    if (pos > 1.0000f)
    {
        return Vector(0, 0, 0);
    }
    float val = pos * (pcount-1);
    const int index = (int)floorf(pos * (pcount-1));
    
    float t = val - index;

    float t2 = t*t;
    float t3 = t2*t;

    Vector p0 = points[index];
    Vector p1 = tangents[index];
    Vector p2 = tangents[index + 1];
    Vector p3 = points[index + 1];

    Vector result = p0*(1-t)*(1-t)*(1-t) + p1*3*t*(1-t)*(1-t) + p2*3*t2*(1-t)+p3*t3; 
    return result;

}
