#include "system/xstddef.h"
#include "system/xstdio.h"
#include "system/xstring.h"
#include "system/xmath.h"
#include "misc/spline.h"

/*
##############################################################################

t = [0..1]

                  [  2 -2  1  1 ]
[ t^3 t^2 t 1 ] * [ -3  3 -2 -1 ] = [ P1 P4 R1 R4 ]
                  [  0  0  1  0 ]
                  [  1  0  0  0 ]

P1 = 2t^3 - 3t^2 + 1
P4 = -2t^3 + 3t^2
R1 = t^3 - 2t^2 + t
R4 = t^3 - t^2

Tangent ber„kning:

S(i)   = ((1 - a)(1 + b)(1 + c)/2)*(P(i) - P(i-1))
       + ((1 - a)(1 + b)(1 - c)/2)*(P(i+2) - P(i))

##############################################################################
*/

static void hermite( float t, float *p1, float *p4, float *r1, float *r4)
{
	float t2 = t * t;
	float t3 = t2 * t;
	float tt = 2*t3 - 3*t2; //Speed up

//	*p1 = 2*t3 - 3*t2 + 1;
//	*p4 = -2*t3 + 3*t2;
	*p1 = tt +1; //Speed up
	*p4 = -tt; //Speed up
	*r1 = t3 - 2*t2 + t;
	*r4 = t3 - t2;
}

/* ###########################################################################

name : spline_createanim()
does : Creates an anim with one (required) key.
wants: The number of values the spline is going to interpolate
gives: ANIM * if succesful, else NULL

########################################################################### */

ANIM *spline_createanim( int values)
{
  ANIM *anim;

	if ( (values <= 0) || (values > MAXVALUES))
		return( NULL);

  if ( (anim = (ANIM *)xmalloc( sizeof(ANIM))) == NULL)
		return( NULL);
  memset( anim, 0, sizeof( ANIM));

	anim->frames = 0;
	anim->keys = 1;
	anim->values = values;

  if ( (anim->first = (KEY *)xmalloc( sizeof(KEY))) == NULL)
	{
    xfree( anim);
		return (NULL);
	}
	memset( anim->first, 0, sizeof( KEY));

  anim->first->prev = anim->first;
  anim->first->next = anim->first;

	return( anim);
}

/* ###########################################################################

name : spline_freeanim()
does : Deallocates animation and all of it's keys.
wants: The animation
gives: nothing.

########################################################################### */

void spline_freeanim( ANIM *anim)
{
	KEY *key,*next;
	int i;

	key = anim->first;
	for( i=0; i<anim->keys; i++)
	{
		next = key->next;
    xfree( key);
		key = next;
	}

  xfree( anim);
}

/* ###########################################################################

name : spline_addkey()
does : Adds a key to the linked list of keys.
wants: The animation and the frame to put the key
gives: KEY * if succesul else NULL

########################################################################### */

KEY *spline_addkey( ANIM *anim, float frame)
{
	KEY *key,*newkey = NULL;

	key = anim->first;
	while ( (frame > key->frame) && (key->next != key))
	  key = key->next;

	if( key->frame == frame)
    return( key);

  if ( (newkey = (KEY *)xmalloc( sizeof( KEY))) == NULL)
		return( NULL);
	memset( newkey, 0, sizeof( KEY));

	if ( key->next == key) // Last key (Special Case)
	{
		newkey->prev = key;
		newkey->next = newkey;
		newkey->prev->next = newkey;
	}
	else // Normal operation
	{
		newkey->prev = key;
		newkey->next = key->next;
		newkey->prev->next = newkey;
		newkey->next->prev = newkey;
	}
	newkey->frame = frame;

	anim->keys++;
	if( frame > anim->frames)
		anim->frames = frame;

	return( newkey);
}

/* ###########################################################################

name : spline_calcframe()
does : Calculates the values for current frame.
       (NOTE: Frame doesn't have be a integer, can be any value [0..frames].
wants: The animation and the frame to calculate values for.
gives: TRUE if succesful, else FALSE

########################################################################### */

int spline_calcframe( ANIM *anim, float frame)
{
	/* key0 = previous, key1 = start, key2 = end, key3 = next */
	KEY *key0, *key1, *key2, *key3;
	float h1, h2, h3, h4;
	float t;
  float delta;
	float dd0a, dd0b, ds1a, ds1b;
	float adj0, adj1, dd0, ds1;
	float tlength;
	int i;

  if( (anim->keys == 1) || (frame < 0))
  {
    for ( i=0; i<anim->values; i++)
      anim->value[i] = anim->first->value[i];
    return( TRUE);
  }
  else if( frame > anim->frames)
  {
    key0 = anim->first;
    for ( i=0; i<anim->keys - 1; i++)
      key0 = key0->next;
    for ( i=0; i<anim->values; i++)
      anim->value[i] = key0->value[i];
    return( TRUE);
  }

	// Find current key, then set prev,next,nextnext key

  key1 = anim->first;
  while ( frame > key1->next->frame)
    key1 = key1->next;

  key0 = key1->prev;
  key2 = key1->next;
  key3 = key1->next->next;
	frame -= key1->frame;

	// Get t constant , 0 <= t <= 1
	tlength = key2->frame - key1->frame;
	t = frame / tlength;

	// Calculate spline coefficients
	hermite( t, &h1, &h2, &h3, &h4);

	//  Kochanek&Bartels tangent extraction.
	dd0a = (1.0 - key1->tension) * (1.0 + key1->contunity) * (1.0 + key1->bias);
	dd0b = (1.0 - key1->tension) * (1.0 - key1->contunity) * (1.0 - key1->bias);
	ds1a = (1.0 - key2->tension) * (1.0 - key2->contunity) * (1.0 + key2->bias);
	ds1b = (1.0 - key2->tension) * (1.0 + key2->contunity) * (1.0 - key2->bias);

	adj0 = tlength / (key2->frame - key0->frame);
	adj1 = tlength / (key3->frame - key1->frame);

	for ( i=0; i<anim->values; i++)
	{
		delta = key2->value[i] - key1->value[i];

		dd0 = adj0 * (dd0a * (key1->value[i] - key0->value[i]) + dd0b * delta);
		ds1 = adj1 * (ds1a * delta + ds1b * (key3->value[i] - key2->value[i]));

		anim->value[i] = key1->value[i] * h1 + key2->value[i] * h2 + dd0 * h3 + ds1 * h4;
	}

	return( TRUE);
}

