#include "catmullrom.hpp"

CatmullRom::CatmullRom()
{
	points = 0;
	this->pcount = 0;
}

CatmullRom::CatmullRom(int pointcount)
{
	points = 0;
    if (pointcount >= 4)
    {
        this->pcount = pointcount;
        this->pointindex = 0;
        this->points = new Vector[pointcount];
		//int i; for(i=0; i<pcount; i++) points[i] = Vector();
    }
    else
    {
        points = 0;
		this->pcount = 0;
        dmsMsg("trying to create a CatmullRom-spline with less than 4 points!\n");
    }
}
CatmullRom::~CatmullRom()
{
    if (points != 0 || pcount!=0)
    {
//		delete [] this->points;
    }

}

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

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

Vector CatmullRom::getValue(float pos)
{

    //t = 0 -> index = 1
    //t = 1.0f -> index = count - 2

    if (pos > 1.000f)
    {
        return Vector(0, 0, 0);
    }
    if (pos < 0.000f)
    {
        return Vector(0, 0, 0);
    }

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

    float t = val - index;

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

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

    Vector result = ((p1*2) + (p2 - p0) * t + (p0*2 - p1 * 5 + p2*4 - p3)*t2 + (p1*3 - p0- p2*3 + p3) * t3)*0.5f;
    return result;

}

Vector CatmullRom::getTangent(float pos)
{
    
    return Vector(0, 0, 0);
}
