

#include "PajaTypes.h"
#include "ContFloatC.h"
#include "DataBlockI.h"
#include "EditableI.h"
#include "KeyC.h"
#include <math.h>
#include <assert.h>
#include <stdlib.h>		//qsort
#include "FileIO.h"
#include "QuatC.h"

using namespace Composition;
using namespace PajaTypes;
using namespace FileIO;



const uint32		CONTROLLER_VERSION = 1;

enum ContFloatChunksE {
	CHUNK_CONTROLLER_BASE = 0x1000,
	CHUNK_CONTROLLER_KEY = 0x2000,
};

const PajaTypes::uint8		KEY_VERSION = 1;

//
//
// Key
//
//

KeyFloatC::KeyFloatC()
{
	// empty
}

KeyFloatC::~KeyFloatC()
{
	// empty
}

float32
KeyFloatC::get_value() const
{
	return m_f32Val;
}

void
KeyFloatC::set_value( float32 f32Val )
{
	m_f32Val = f32Val;
}

int32
KeyFloatC::get_time()
{
	return m_i32Time;
}

void
KeyFloatC::set_time( int32 i32Time )
{
	m_i32Time = i32Time;
}

float32
KeyFloatC::get_in_tan() const
{
	return m_f32InTan;
}

void
KeyFloatC::set_in_tan( float32 f32Val )
{
	m_f32InTan = f32Val;
}

float32
KeyFloatC::get_out_tan() const
{
	return m_f32OutTan;
}

void
KeyFloatC::set_out_tan( float32 f32Val )
{
	m_f32OutTan = f32Val;
}

float32
KeyFloatC::get_ease_in()
{
	return m_f32EaseIn;
}

float32
KeyFloatC::get_ease_out()
{
	return m_f32EaseOut;
}

float32
KeyFloatC::get_tens()
{
	return m_f32Tens;
}

float32
KeyFloatC::get_cont()
{
	return m_f32Cont;
}

float32
KeyFloatC::get_bias()
{
	return m_f32Bias;
}

void
KeyFloatC::set_ease_in( PajaTypes::float32 f32Val )
{
	m_f32EaseIn = f32Val;
}

void
KeyFloatC::set_ease_out( PajaTypes::float32 f32Val )
{
	m_f32EaseOut = f32Val;
}

void
KeyFloatC::set_tens( PajaTypes::float32 f32Val )
{
	m_f32Tens = f32Val;
}

void
KeyFloatC::set_cont( PajaTypes::float32 f32Val )
{
	m_f32Cont = f32Val;
}

void
KeyFloatC::set_bias( PajaTypes::float32 f32Val )
{
	m_f32Bias = f32Val;
}


uint32
KeyFloatC::save( SaveC* pSave )
{
	uint32	ui32Error = IO_OK;

	uint8	ui8Version = KEY_VERSION;

	// version
	ui32Error = pSave->write( &ui8Version, sizeof( ui8Version ) );
	// values
	ui32Error = pSave->write( &m_f32Val, sizeof( float32 ) );
	// time
	ui32Error = pSave->write( &m_i32Time, sizeof( m_i32Time ) );
	// ease in
	ui32Error = pSave->write( &m_f32EaseIn, sizeof( m_f32EaseIn ) );
	// ease out
	ui32Error = pSave->write( &m_f32EaseOut, sizeof( m_f32EaseOut ) );
	// tens
	ui32Error = pSave->write( &m_f32Tens, sizeof( m_f32Tens ) );
	// cont
	ui32Error = pSave->write( &m_f32Cont, sizeof( m_f32Cont ) );
	// bias
	ui32Error = pSave->write( &m_f32Bias, sizeof( m_f32Bias ) );

	return ui32Error;
}

uint32
KeyFloatC::load( LoadC* pLoad )
{
	uint32	ui32Error = IO_OK;

	uint8	ui8Version;

	// version
	ui32Error = pLoad->read( &ui8Version, sizeof( ui8Version ) );

	if( ui8Version == KEY_VERSION ) {
		// values
		ui32Error = pLoad->read( &m_f32Val, sizeof( float32 ) );
		// time
		ui32Error = pLoad->read( &m_i32Time, sizeof( m_i32Time ) );
		// ease in
		ui32Error = pLoad->read( &m_f32EaseIn, sizeof( m_f32EaseIn ) );
		// ease out
		ui32Error = pLoad->read( &m_f32EaseOut, sizeof( m_f32EaseOut ) );
		// tens
		ui32Error = pLoad->read( &m_f32Tens, sizeof( m_f32Tens ) );
		// cont
		ui32Error = pLoad->read( &m_f32Cont, sizeof( m_f32Cont ) );
		// bias
		ui32Error = pLoad->read( &m_f32Bias, sizeof( m_f32Bias ) );
	}

	return ui32Error;
}


//
//
// Controller
//
//

ContFloatC::ContFloatC( PajaTypes::uint32 ui32Type ) :
	m_ui32StartOrt( CONT_ORT_CONSTANT ),
	m_ui32EndOrt( CONT_ORT_CONSTANT ),
	m_ui32Type( ui32Type )
{
	// empty
}

ContFloatC::~ContFloatC()
{
	// delete keys
	for( uint32 i = 0; i < m_rKeys.size(); i++ )
		delete m_rKeys[i];
}

int32
ContFloatC::get_key_count()
{
	return m_rKeys.size();
}

KeyFloatC*
ContFloatC::get_key( int32 i32Index )
{
	if( i32Index >= 0 && i32Index < m_rKeys.size() )
		return m_rKeys[i32Index];
	return 0;
}

void
ContFloatC::del_key( int32 i32Index )
{
	assert( i32Index >= 0 && i32Index < m_rKeys.size() );

	KeyFloatC*	pKey = m_rKeys[i32Index];
	m_rKeys.erase( m_rKeys.begin() + i32Index );
}

KeyFloatC*
ContFloatC::add_key()
{
	KeyFloatC*	pKey = new KeyFloatC;
	m_rKeys.push_back( pKey );
	return pKey;
}


static
int
compare_func( const void* vpParam1, const void* vpParam2 )
{
	KeyFloatC*	pKey1 = *(KeyFloatC**)vpParam1;
	KeyFloatC*	pKey2 = *(KeyFloatC**)vpParam2;

	if( pKey1->get_time() < pKey2->get_time() )
		return -1;

	return 1;
}

void
ContFloatC::sort_keys()
{
	qsort( m_rKeys.begin(), m_rKeys.size(), sizeof( KeyFloatC* ), compare_func );
}



float32
ContFloatC::ease( float32 f32U, float32 f32A, float32 f32B ) const
{
	float32	f32K;
	float32	f32S = f32A + f32B;
	
	if( f32U == 0.0f || f32U == 1.0f ) return f32U;
	if( f32S == 0.0 ) return f32U;
	if( f32S > 1.0f ) {
		f32A = f32A / f32S;
		f32B = f32B / f32S;
	}
	f32K = 1.0f / (2.0f - f32A - f32B);
	if( f32U < f32A )
		return ((f32K / f32A) * f32U * f32U);
	else if( f32U < 1.0f - f32B )
		return (f32K * (2.0f * f32U - f32A));
	else {
		f32U = 1.0f - f32U;
		return (1.0f - (f32K / f32B) * f32U * f32U);
	}
}

void
ContFloatC::compute_hermite_basis( float f32U, float* pV ) const
{
	float32	f32U2, f32U3, f32A;
	
	f32U2 = f32U * f32U;
	f32U3 = f32U2 * f32U;
	f32A  = 2.0f * f32U3 - 3.0f * f32U2;
	pV[0] = 1.0f + f32A;
	pV[1] = -f32A;
	pV[2] = f32U - 2.0f * f32U2 + f32U3;
	pV[3] = -f32U2 + f32U3;
}

void
ContFloatC::comp_first_deriv( uint32 ui32CurIndex, uint32 ui32NextIndex )
{
	float32	f32T = 0.5f * (1.0f - m_rKeys[ui32CurIndex]->get_tens());
	float32	f32ValNext = m_rKeys[ui32NextIndex]->get_value();
	float32	f32ValCur = m_rKeys[ui32CurIndex]->get_value();
	float32	f32InTanNext = m_rKeys[ui32NextIndex]->get_in_tan();

	m_rKeys[ui32CurIndex]->set_out_tan( f32T * (3.0f * (f32ValNext - f32ValCur) - f32InTanNext) );
}

void
ContFloatC::comp_last_deriv( uint32 ui32CurIndex, uint32 ui32NextIndex )
{
	float32	f32T = 0.5f * (1.0f - m_rKeys[ui32NextIndex]->get_tens());
	float32	f32ValNext = m_rKeys[ui32NextIndex]->get_value();
	float32	f32ValCur = m_rKeys[ui32CurIndex]->get_value();
	float32	f32OutTanCur = m_rKeys[ui32CurIndex]->get_out_tan();

	m_rKeys[ui32NextIndex]->set_in_tan( -f32T * (3.0f * (f32ValCur - f32ValNext) + f32OutTanCur) );
}

void
ContFloatC::comp_2key_deriv( uint32 ui32CurIndex, uint32 ui32NextIndex )
{
	float32		f32Tens0 = 1.0f - m_rKeys[ui32CurIndex]->get_tens();
	float32		f32Tens1 = 1.0f - m_rKeys[ui32NextIndex]->get_tens();

	m_rKeys[ui32CurIndex]->set_out_tan( f32Tens0 * (m_rKeys[ui32NextIndex]->get_value() - m_rKeys[ui32CurIndex]->get_value()) );
	m_rKeys[ui32NextIndex]->set_in_tan( f32Tens1 * (m_rKeys[ui32NextIndex]->get_value() - m_rKeys[ui32CurIndex]->get_value()) );
}

void
ContFloatC::comp_middle_deriv( uint32 ui32PrevIndex, uint32 ui32Index, uint32 ui32NextIndex )
{
	float32	tm, cm, cp, bm, bp, tmcm, tmcp, ksm, ksp, kdm, kdp, c;
	float32	dt, fp, fn;

	int32	i32PrevTime = m_rKeys[ui32PrevIndex]->get_time();
	int32	i32Time = m_rKeys[ui32Index]->get_time();
	int32	i32NextTime = m_rKeys[ui32NextIndex]->get_time();

	// handle time in loop case
	if( i32NextTime <= i32PrevTime )
		i32NextTime += (m_rKeys[m_rKeys.size() - 1]->get_time() - m_rKeys[0]->get_time());
	if( i32PrevTime >= i32Time )
		i32PrevTime -= (m_rKeys[m_rKeys.size() - 1]->get_time() - m_rKeys[0]->get_time());

	// fp,fn apply speed correction when continuity is 0.0
	dt = 0.5f * (float32)(i32NextTime - i32PrevTime);
	fp = ((float32)(i32Time - i32PrevTime)) / dt;
	fn = ((float32)(i32NextTime - i32Time)) / dt;
	
	c  = (float32)fabs( m_rKeys[ui32Index]->get_cont() );
	fp = fp + c - c * fp;
	fn = fn + c - c * fn;
	cm = 1.0f - m_rKeys[ui32Index]->get_cont();
	tm = 0.5f * (1.0f - m_rKeys[ui32Index]->get_tens());
	cp = 2.0f - cm;
	bm = 1.0f - m_rKeys[ui32Index]->get_bias();
	bp = 2.0f - bm;
	tmcm = tm * cm;			tmcp = tm * cp;
	ksm = tmcm * bp * fp;	ksp = tmcp * bm * fp;
	kdm = tmcp * bp * fn; 	kdp = tmcm * bm * fn;
	
	float32	delm = m_rKeys[ui32Index]->get_value() - m_rKeys[ui32PrevIndex]->get_value();
	float32	delp = m_rKeys[ui32NextIndex]->get_value() - m_rKeys[ui32Index]->get_value();
	m_rKeys[ui32Index]->set_in_tan( ksm * delm + ksp * delp );
	m_rKeys[ui32Index]->set_out_tan( kdm * delm + kdp * delp );
}


void
ContFloatC::prepare()
{
	if( m_rKeys.size() < 2 )
		return;

	if( m_ui32Type == KEY_SMOOTH ) {
		if( m_rKeys.size() == 2 ) {
			comp_2key_deriv( 0, 1 );
		}
		else {
			// comp middle derivates
			for( uint32 i = 1; i < m_rKeys.size() - 1; i++ ) {
				comp_middle_deriv( i - 1, i, i + 1 );
			}

			if( m_ui32StartOrt == CONT_ORT_LOOP )
				comp_middle_deriv( m_rKeys.size() - 2, 0, 1 );
			else
				comp_first_deriv( 0, 1 );

			if( m_ui32EndOrt == CONT_ORT_LOOP )
				comp_middle_deriv( m_rKeys.size() - 2, m_rKeys.size() - 1, 1 );
			else
				comp_last_deriv( m_rKeys.size() - 2, m_rKeys.size() - 1 );
		}
	}
}


void
ContFloatC::set_start_ort( uint32 ui32Ort )
{
	m_ui32StartOrt = ui32Ort;
}

void
ContFloatC::set_end_ort( uint32 ui32Ort )
{
	m_ui32EndOrt = ui32Ort;
}

uint32
ContFloatC::get_start_ort() const
{
	return m_ui32StartOrt;
}

uint32
ContFloatC::get_end_ort() const
{
	return m_ui32EndOrt;
}


float32
ContFloatC::get_value( int32 i32Time ) const
{
	if( m_rKeys.size() == 0 )
		return 0;

	if( m_rKeys.size() == 1 )
		return m_rKeys[0]->get_value();

	int32	i32StartTime = get_min_time();
	int32	i32EndTime = get_max_time();

	// Handle out of range
	if( i32Time < i32StartTime ) {
		if( m_ui32StartOrt == CONT_ORT_REPEAT || m_ui32StartOrt == CONT_ORT_LOOP )
			i32Time = i32EndTime + (i32Time - i32StartTime) % (i32EndTime - i32StartTime);
		else
			return m_rKeys[0]->get_value();
	}
	else if( i32Time >= i32EndTime ) {

		if( m_ui32EndOrt == CONT_ORT_REPEAT || m_ui32EndOrt == CONT_ORT_LOOP )
			i32Time = i32StartTime + (i32Time - i32StartTime) % (i32EndTime - i32StartTime);
		else
			return m_rKeys[m_rKeys.size() - 1]->get_value();
	}

	// Check if the value is out of range

	// Is the requested time before the first key?
	if( m_rKeys[0]->get_time() >= i32Time )
		return m_rKeys[0]->get_value();

	// Is the requested time after the last key?
	if( m_rKeys[m_rKeys.size() - 1]->get_time() <= i32Time )
		return m_rKeys[m_rKeys.size() - 1]->get_value();

	// interpolate
	for( uint32 i = 1; i < m_rKeys.size(); i++ ) {
		if( m_rKeys[i]->get_time() > i32Time ) {

			float32	f32U = float( i32Time - m_rKeys[i - 1]->get_time() ) /
							float( m_rKeys[i]->get_time() - m_rKeys[i - 1]->get_time() );

			f32U = ease( f32U, m_rKeys[i - 1]->get_ease_out(), m_rKeys[i]->get_ease_in() );

			float32	f32CurVal = m_rKeys[i]->get_value();
			float32	f32PrevVal = m_rKeys[i - 1]->get_value();

			// Calc value
			if( m_ui32Type == KEY_LINEAR ) {
				// A linear segment.
				return f32PrevVal + (f32CurVal - f32PrevVal) * f32U;
			}
			else {
				// A Smooth segment
				float32	f32CurInTan = m_rKeys[i]->get_in_tan();
				float32	f32PrevOutTan = m_rKeys[i - 1]->get_out_tan();

				float32	f32V[4];
				compute_hermite_basis( f32U, f32V );

				return f32PrevVal * f32V[0] + f32CurVal * f32V[1] + f32PrevOutTan * f32V[2] + f32CurInTan * f32V[3];
			}
		}
	}

	// we never should arrive here!
	assert( 0 );

	return 0;
}


int32
ContFloatC::get_min_time() const
{
	if( m_rKeys.size() )
		return m_rKeys[0]->get_time();
	return 0;
}

int32
ContFloatC::get_max_time() const
{
	if( m_rKeys.size() )
		return m_rKeys[m_rKeys.size() - 1]->get_time();
	return 0;
}

void
ContFloatC::set_type( uint32 ui32Type )
{
	m_ui32Type = ui32Type;
}

uint32
ContFloatC::get_type() const
{
	return m_ui32Type;
}


uint32
ContFloatC::save( SaveC* pSave )
{
	uint32	ui32Error = IO_OK;
	uint8	ui8Temp;

	pSave->begin_chunk( CHUNK_CONTROLLER_BASE, CONTROLLER_VERSION );
		// type
		ui8Temp = (uint8)m_ui32Type;
		ui32Error = pSave->write( &ui8Temp, sizeof( ui8Temp ) );
		// start ort
		ui8Temp = (uint8)m_ui32StartOrt;
		ui32Error = pSave->write( &ui8Temp, sizeof( ui8Temp ) );
		// end ort
		ui8Temp = (uint8)m_ui32EndOrt;
		ui32Error = pSave->write( &ui8Temp, sizeof( ui8Temp ) );
	pSave->end_chunk();

	// save keys
	for( uint32 i = 0; i < m_rKeys.size(); i++ ) {
		pSave->begin_chunk( CHUNK_CONTROLLER_KEY, CONTROLLER_VERSION );
		ui32Error = m_rKeys[i]->save( pSave );
		pSave->end_chunk();
	}

	return ui32Error;
}

uint32
ContFloatC::load( LoadC* pLoad )
{
	uint32	ui32Error = IO_OK;

	while( (ui32Error = pLoad->open_chunk()) == IO_OK ) {

		switch( pLoad->get_chunk_id() ) {
		case CHUNK_CONTROLLER_BASE:
			{
				if( pLoad->get_chunk_version() == CONTROLLER_VERSION ) {
					uint8	ui8Temp;
					// type
					ui32Error = pLoad->read( &ui8Temp, sizeof( ui8Temp ) );
					m_ui32Type = ui8Temp;
					// start ort
					ui32Error = pLoad->read( &ui8Temp, sizeof( ui8Temp ) );
					m_ui32StartOrt = ui8Temp;
					// end ort
					ui32Error = pLoad->read( &ui8Temp, sizeof( ui8Temp ) );
					m_ui32EndOrt = ui8Temp;
				}
			}
			break;
		case CHUNK_CONTROLLER_KEY:
			{
				if( pLoad->get_chunk_version() == CONTROLLER_VERSION ) {
					KeyFloatC*	pKey = new KeyFloatC;
					ui32Error = pKey->load( pLoad );
					m_rKeys.push_back( pKey );
				}
			}
			break;
		default:
			assert( 0 );
		}

		pLoad->close_chunk();

		if( ui32Error != IO_OK && ui32Error != IO_END )
			return ui32Error;
	}

	if( ui32Error != IO_OK && ui32Error != IO_END )
		return ui32Error;

	sort_keys();
	prepare();

	return IO_OK;
}

