#include "../config.h"

#include "trajectory.h"

Trajectory::Trajectory( )
{
	registration = 0;
    //accumulatedTime = 0.0f;
}


Trajectory::~Trajectory( )
{
	for( auto& p : ControlPoints )
		delete( p );
		
	ControlPoints.clear();
}


void Trajectory::AddPoint( Vector2D *p )
{
    ControlPoints.push_back( p );
}


void Trajectory::AddRegistry( void )
{
	registration++;
}

void Trajectory::DeleteRegistry( void )
{
	registration--;
}

void Trajectory::CalculatePosition( float *accumulatedTime, float time, uint16_t speed, bool looped, libnum::num *xreturn, libnum::num *yreturn )
{
    *accumulatedTime += speed * time / 2000000.0f;
	if (*accumulatedTime>ControlPoints.size()) *accumulatedTime-=ControlPoints.size();
	if (*accumulatedTime<0) *accumulatedTime+=ControlPoints.size();

    libnum::num t = libnum::num( *accumulatedTime - (int) *accumulatedTime );    

	int p0, p1, p2, p3;

	if (!looped)
	{
		p1 = (int) *accumulatedTime + 1;
		p2 = p1 + 1;
		p3 = p2 + 1;
		p0 = p1 - 1;
	}
	else
	{
		p1 = (int) *accumulatedTime;
		p2 = (p1 + 1) % ControlPoints.size();
		p3 = (p2 + 1) % ControlPoints.size();
		p0 = p1 >= 1 ? p1 - 1 : ControlPoints.size() - 1;
	}

	libnum::num tt = t * t;
	libnum::num ttt = tt * t;

	libnum::num q1 = -ttt + 2*tt - t;
	libnum::num q2 = 3*ttt - 5*tt + libnum::num( 2 );
	libnum::num q3 = -3*ttt + 4*tt + t;
	libnum::num q4 = ttt - tt;

	libnum::num tx = libnum::num( 0.5f ) * (ControlPoints[p0]->x * q1 + ControlPoints[p1]->x * q2 + ControlPoints[p2]->x * q3 + ControlPoints[p3]->x * q4);
	libnum::num ty = libnum::num( 0.5f ) * (ControlPoints[p0]->y * q1 + ControlPoints[p1]->y * q2 + ControlPoints[p2]->y * q3 + ControlPoints[p3]->y * q4);

	*xreturn = tx;
    *yreturn = ty;
}