
#include "PajaTypes.h"
#include "ContVector3C.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 ContVector3ChunksE {
	CHUNK_CONTROLLER_BASE = 0x1000,
	CHUNK_CONTROLLER_KEY = 0x2000,
};

const PajaTypes::uint8		KEY_VERSION = 1;

//
//
// Key
//
//

KeyVector3C::KeyVector3C()
{
	// empty
}

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

const Vector3C&
KeyVector3C::get_value() const
{
	return m_rVal;
}

void
KeyVector3C::set_value( const Vector3C& rVec )
{
	m_rVal = rVec;
}

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

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

const Vector3C&
KeyVector3C::get_in_tan() const
{
	return m_rInTan;
}

void
KeyVector3C::set_in_tan( const Vector3C& rVec )
{
	m_rInTan = rVec;
}

const Vector3C&
KeyVector3C::get_out_tan() const
{
	return m_rOutTan;
}

void
KeyVector3C::set_out_tan( const Vector3C& rVec )
{
	m_rOutTan = rVec;
}

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

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

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

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

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

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

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

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

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

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


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

	uint8	ui8Version = KEY_VERSION;
	float32	f32Values[3];

	// version
	ui32Error = pSave->write( &ui8Version, sizeof( ui8Version ) );
	// values
	f32Values[0] = m_rVal[0];
	f32Values[1] = m_rVal[1];
	f32Values[2] = m_rVal[2];
	ui32Error = pSave->write( f32Values, 3 * 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
KeyVector3C::load( LoadC* pLoad )
{
	uint32	ui32Error = IO_OK;

	uint8	ui8Version;
	float32	f32Values[3];

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

	if( ui8Version == KEY_VERSION ) {
		// values
		ui32Error = pLoad->read( f32Values, 3 * sizeof( float32 ) );
		m_rVal[0] = f32Values[0];
		m_rVal[1] = f32Values[1];
		m_rVal[2] = f32Values[2];
		// 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
//
//

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

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

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

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

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

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

KeyVector3C*
ContVector3C::add_key()
{
	KeyVector3C*	pKey = new KeyVector3C;
	m_rKeys.push_back( pKey );
	return pKey;
}


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

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

	return 1;
}

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



float32
ContVector3C::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
ContVector3C::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
ContVector3C::comp_first_deriv( uint32 ui32CurIndex, uint32 ui32NextIndex )
{
	float32		f32T = 0.5f * (1.0f - m_rKeys[ui32CurIndex]->get_tens());
	Vector3C	rValNext = m_rKeys[ui32NextIndex]->get_value();
	Vector3C	rValCur = m_rKeys[ui32CurIndex]->get_value();
	Vector3C	rInTanNext = m_rKeys[ui32NextIndex]->get_in_tan();

	m_rKeys[ui32CurIndex]->set_out_tan( f32T * (3.0f * (rValNext - rValCur) - rInTanNext) );
}

void
ContVector3C::comp_last_deriv( uint32 ui32CurIndex, uint32 ui32NextIndex )
{
	float32		f32T = 0.5f * (1.0f - m_rKeys[ui32NextIndex]->get_tens());
	Vector3C	rValNext = m_rKeys[ui32NextIndex]->get_value();
	Vector3C	rValCur = m_rKeys[ui32CurIndex]->get_value();
	Vector3C	rOutTanCur = m_rKeys[ui32CurIndex]->get_out_tan();

	m_rKeys[ui32NextIndex]->set_in_tan( -f32T * (3.0f * (rValCur - rValNext) + rOutTanCur) );
}

void
ContVector3C::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
ContVector3C::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;
	
	Vector3C	delm = m_rKeys[ui32Index]->get_value() - m_rKeys[ui32PrevIndex]->get_value();
	Vector3C	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
ContVector3C::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
ContVector3C::set_start_ort( uint32 ui32Ort )
{
	m_ui32StartOrt = ui32Ort;
}

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

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

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


Vector3C
ContVector3C::get_value( int32 i32Time ) const
{
	if( m_rKeys.size() == 0 )
		return Vector3C( 0, 0, 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() );

			Vector3C	rCurVal = m_rKeys[i]->get_value();
			Vector3C	rPrevVal = m_rKeys[i - 1]->get_value();

			// Calc value
			if( m_ui32Type == KEY_LINEAR ) {
				// A linear segment.
				return rPrevVal + (rCurVal - rPrevVal) * f32U;
			}
			else {
				// A Smooth segment
				Vector3C	rCurInTan = m_rKeys[i]->get_in_tan();
				Vector3C	rPrevOutTan = m_rKeys[i - 1]->get_out_tan();

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

				return rPrevVal * f32V[0] + rCurVal * f32V[1] + rPrevOutTan * f32V[2] + rCurInTan * f32V[3];
			}
		}
	}

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

	return Vector3C( 0, 0, 0 );
}


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

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

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

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


uint32
ContVector3C::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
ContVector3C::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 ) {
					KeyVector3C*	pKey = new KeyVector3C;
					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;
}

