// based on code by omen/Dimension^Platoon 

#include "PajaTypes.h"
#include "ContQuatC.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;


static
void
TRACE( const char* szFormat, ...  )
{
	char	szMsg[256];
	va_list	rList;
	va_start( rList, szFormat );
	_vsnprintf( szMsg, 255, szFormat, rList );
	va_end( rList );
	OutputDebugString( szMsg );
}


const uint32		CONTROLLER_VERSION = 1;

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

const PajaTypes::uint8		KEY_VERSION = 1;

//
//
// Key
//
//

KeyQuatC::KeyQuatC()
{
	// empty
}

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

const QuatC&
KeyQuatC::get_value() const
{
	return m_rVal;
}

void
KeyQuatC::set_value( const QuatC& rQuat )
{
	m_rVal = rQuat;
}

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

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

const QuatC&
KeyQuatC::get_in_tan() const
{
	return m_rInTan;
}

void
KeyQuatC::set_in_tan( const QuatC& rQuat )
{
	m_rInTan = rQuat;
}

const QuatC&
KeyQuatC::get_out_tan() const
{
	return m_rOutTan;
}

void
KeyQuatC::set_out_tan( const QuatC& rQuat )
{
	m_rOutTan = rQuat;
}

const Vector3C&
KeyQuatC::get_axis() const
{
	return m_rAxis;
}

void
KeyQuatC::set_axis( const PajaTypes::Vector3C& rAxis )
{
	m_rAxis = rAxis;
}

float32
KeyQuatC::get_angle() const
{
	return m_f32Angle;
}

void
KeyQuatC::set_angle( float32 f32Angle )
{
	m_f32Angle = f32Angle;
}

int32
KeyQuatC::get_spin() const
{
	return m_i32Spin;
}

void
KeyQuatC::set_spin( int32 i32Spin )
{
	m_i32Spin = i32Spin;
}

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

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

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

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

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

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

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

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

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

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


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

	uint8	ui8Version = KEY_VERSION;
	float32	f32Values[4];

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

	uint8	ui8Version;
	float32	f32Values[4];

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

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

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

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

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

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

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

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

KeyQuatC*
ContQuatC::add_key()
{
	KeyQuatC*	pKey = new KeyQuatC;
	m_rKeys.push_back( pKey );
	return pKey;
}


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

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

	return 1;
}

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



float32
ContQuatC::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
ContQuatC::comp_ab( KeyQuatC* pPrev, KeyQuatC* pCur, KeyQuatC* pNext )
{
	QuatC	v_in, v_out;
	float	log_angle;

	if( pPrev ) {
		if( pCur->get_spin() == 0 ) {
			log_angle = pCur->get_angle() * 0.5f;
			if( log_angle >= (M_PI * 0.5f) )
				log_angle -= (float32)M_PI;
		}
		else
			log_angle = 0.5f * (float32)M_PI;

		Vector3C	rAxis = pCur->get_axis();
		v_in = QuatC( rAxis[0] * log_angle, rAxis[1] * log_angle, rAxis[2] * log_angle, 0 );
	}

	if( pNext ) {
		if( pNext->get_spin() == 0 ) {
			log_angle = pNext->get_angle() * 0.5f;
			if( log_angle >= (M_PI * 0.5f) )
				log_angle -= (float32)M_PI;
		}
		else
			log_angle = 0.5f * (float32)M_PI;

		Vector3C	rAxis = pNext->get_axis();
		v_out = QuatC( rAxis[0] * log_angle, rAxis[1] * log_angle, rAxis[2] * log_angle, 0 );
	}

	if( !pPrev )
		v_in = v_out;
	
	if( !pNext )
		v_out = v_in;

	float32	fp = 1.0f;
	float32	fn = 1.0f;
	float32	cm = 1.0f - pCur->get_cont();

	if( pPrev && pNext ) {
		float32	dt = 2.0f / (float32)(pNext->get_time() - pPrev->get_time());
		fp = (float32)(pCur->get_time() - pPrev->get_time())*dt;
		fn = (float32)(pNext->get_time() - pCur->get_time())*dt;
		float32	c = (float32)fabs( pCur->get_cont() );
		fp = fp + c - c * fp;
		fn = fn + c - c * fn;
	}

	float tm = 0.5f * (1.0f - pCur->get_tens());
	float cp = 2.0f - cm;
	float bm = 1.0f - pCur->get_bias();
	float bp = 2.0f - bm;
	float tmcm = tm * cm;
	float tmcp = tm * cp;
	float ksm  = 1.0f - tmcm * bp * fp;
	float ksp  = -tmcp * bm * fp;
	float kdm  = tmcp * bp * fn;
	float kdp  = tmcm * bm * fn - 1.0f;

	QuatC	qa, qb;

	qa = (kdm * v_in + kdp * v_out) * 0.5f;
	qb = (ksm * v_in + ksp * v_out) * 0.5f;

	pCur->set_out_tan( pCur->get_value() * qa.exp() );
	pCur->set_in_tan( pCur->get_value() * qb.exp() );
}



void
ContQuatC::prepare()
{
	uint32	i;

	// make sure the angle between rotations is positive
	for( i = 0; i < m_rKeys.size(); i++ ) {
		if( m_rKeys[i]->get_angle() < 0.0f ) {
			m_rKeys[i]->set_angle( -m_rKeys[i]->get_angle() );
			m_rKeys[i]->set_axis( -m_rKeys[i]->get_axis() );
		}
	}

	// conversion from angle/axis representation to quaternions
	QuatC	rQuat;
	rQuat.from_axis_angle( m_rKeys[0]->get_axis(), m_rKeys[0]->get_angle() );
	m_rKeys[0]->set_value( rQuat );

	for( i = 1; i < m_rKeys.size(); i++ ) {
		// calculate spin value and modify the angle so it feets
		// <0,2*PI) boundaries
		int32	i32Spin;
		float32	f32Angle = m_rKeys[i]->get_angle();

		i32Spin = (int32)floor( f32Angle / (M_PI * 2.0f) );
		f32Angle -= i32Spin * M_PI * 2.0f;

		m_rKeys[i]->set_spin( i32Spin );
		m_rKeys[i]->set_angle( f32Angle );

		// convert it to quaternion...
		rQuat.from_axis_angle( m_rKeys[i]->get_axis(), m_rKeys[i]->get_angle() );

		// ...and now to absolute rotation
		rQuat = m_rKeys[i - 1]->get_value() * rQuat;

		m_rKeys[i]->set_value( rQuat );
	}

	if( m_rKeys.size() > 1 ) {

		// middle-keys tangents calculation
		for( i = 1; i < m_rKeys.size() - 1; i++ ) {
			KeyQuatC*	pPrev = m_rKeys[i - 1];
			KeyQuatC*	pCur = m_rKeys[i];
			KeyQuatC*	pNext = m_rKeys[i + 1];
			comp_ab( pPrev, pCur, pNext );
		}

		if( m_ui32StartOrt == CONT_ORT_LOOP && m_rKeys.size() > 2 ) {
			KeyQuatC*	pPrev = m_rKeys[m_rKeys.size() - 1];
			KeyQuatC*	pCur = m_rKeys[0];
			KeyQuatC*	pNext = m_rKeys[1];
			comp_ab( pPrev, pCur, pNext );
		}
		else {
			KeyQuatC*	pCur = m_rKeys[0];
			KeyQuatC*	pNext = m_rKeys[1];
			comp_ab( 0, pCur, pNext );
		}

		if( m_ui32EndOrt == CONT_ORT_LOOP && m_rKeys.size() > 2 ) {
			KeyQuatC*	pPrev = m_rKeys[m_rKeys.size() - 2];
			KeyQuatC*	pCur = m_rKeys[m_rKeys.size() - 1];
			KeyQuatC*	pNext = m_rKeys[0];
			comp_ab( pPrev, pCur, pNext );
		}
		else {
			KeyQuatC*	pPrev = m_rKeys[m_rKeys.size() - 2];
			KeyQuatC*	pCur = m_rKeys[m_rKeys.size() - 1];
			comp_ab( pPrev, pCur, 0 );
		}
	}

}


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

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

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

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


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

			// Calc value
			if( m_ui32Type == KEY_LINEAR ) {
				// A linear segment.
				QuatC	rCurVal = m_rKeys[i]->get_value();
				QuatC	rPrevVal = m_rKeys[i - 1]->get_value();
				return QuatC::slerp( f32U, rPrevVal, rCurVal );
			}
			else {

				KeyQuatC*	pCur = m_rKeys[i - 1];
				KeyQuatC*	pNext = m_rKeys[i];
				QuatC		p1, q1;

				float32	angle = pNext->get_angle() + ((float)pNext->get_spin()) * M_PI * 2.0f;

				if( !pNext->get_spin() ) {
					p1.from_axis_angle( pNext->get_axis(), f32U * angle );
					p1 = pCur->get_value() * p1;
					q1 = QuatC::slerp( f32U, pCur->get_out_tan(), pNext->get_in_tan() );
				}
				else {
					float32	_p0 = M_PI / angle;
					float32	_p1 = 1.0f - _p0;

					if( f32U < _p0 ) {
						QuatC	s0;
						s0.from_axis_angle( pNext->get_axis(), _p0 * angle );
						s0 = pCur->get_value() * s0;

						f32U *= 1.0f / _p0;

						p1 = QuatC::slerp( f32U, pCur->get_value(), s0 );
						q1 = QuatC::slerp( f32U, pCur->get_out_tan(), s0 );
					}
					else if( f32U <= _p1 ) {
						f32U -= _p0;
						f32U /= _p1 - _p0;

						p1.from_axis_angle( pNext->get_axis(), (float32)fmod( M_PI * f32U * (angle - M_PI * 2.0f), M_PI  *2.0f ) );

						return pCur->get_value() * p1;
					}
					else {
						QuatC	s1;

						s1.from_axis_angle( pNext->get_axis(), _p1 * angle );
						s1 = pCur->get_value() * s1;

						if( pNext->get_spin() & 1 )
							s1 = s1 * -1.0f;
						f32U -= _p1;
						f32U *= 1.0f / _p0;
						p1 = QuatC::slerp( f32U, s1, pNext->get_value() );
						q1 = QuatC::slerp( f32U, s1, pNext->get_in_tan() );
					}
				}

				return QuatC::slerp( 2.0f * f32U * (1.0f - f32U), p1, q1 );
			}
		}
	}

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

	return QuatC( 1, 0, 0, 0 );
}


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

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

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

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


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

