/******************************************************************************
	libx42 - skinned vertex animation library
	Copyright (C) 2007 HermitWorks Entertainment Corporation

	This program is free software; you can redistribute it and/or modify it
	under the terms of the GNU General Public License as published by the Free
	Software Foundation; either version 2 of the License, or (at your option)
	any later version.

	This program is distributed in the hope that it will be useful, but
	WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
	or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
	for more details.

	You should have received a copy of the GNU General Public License along
	with this program; if not, write to the
	
		Free Software Foundation, Inc.
		51 Franklin Street, Fifth Floor
		Boston, MA  02110-1301, USA.
******************************************************************************/

#include "local.h"

typedef struct x42ksBoneEntry_t
{
	u16					frame;
	u16					value;
} x42ksBoneEntry_t, x42ksBoneEntries_t[3][2]; //[type][min/max];

typedef struct x42ksState_t
{
	u16					minFrame, maxFrame;
	float				targetFrame;
	const x42keyStreamEntry_t	* RESTRICT ks;
} x42ksState_t;

struct x42animFrames_t
{
	x42ksState_t		* RESTRICT frames;		//one per animGroup
	x42ksBoneEntries_t	* RESTRICT kpairs;		//one per bone
};

static void init_anim_frames( x42animFrames_t *anim, const x42data_t *x42 );
static void scan_anim_frames( x42animFrames_t *anim, const x42data_t *x42 );

X42_EXPORT size_t X42_CALL x42_GetAnimFramesSize( const x42data_t *x42 )
{
#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rz( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rz( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
#endif

	return sizeof( x42animFrames_t ) +
		sizeof( x42ksState_t ) * x42->header.numAnimGroups +
		sizeof( x42ksBoneEntries_t ) * x42->header.numBones;
}

X42_EXPORT x42animFrames_t* X42_CALL x42_CreateAnimFrames( void *out_buffer, const x42data_t *x42 )
{
	uint i;
	x42animFrames_t *ret;

#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rn( out_buffer != NULL, X42_ERR_BADPTR, "out_buffer is NULL" );
	demand_rn( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rn( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
#endif

	ret = (x42animFrames_t*)out_buffer;

	if( !x42->header.numBones )
	{
		//this is pretty much a nop

		memset( ret, 0, sizeof( x42animFrames_t ) );
		return ret;
	}

	ret->frames = (x42ksState_t*)(ret + 1);
	ret->kpairs = (x42ksBoneEntries_t*)(ret->frames + x42->header.numAnimGroups);

	//init the cache structs
	for( i = 0; i < x42->header.numAnimGroups; i++ )
	{
		ret->frames[i].minFrame = 0;
		ret->frames[i].maxFrame = 0;
		ret->frames[i].targetFrame = 0;
		ret->frames[i].ks = NULL;
	}

	//initialize the keys
	memset( ret->kpairs, 0, sizeof( x42ksBoneEntries_t ) * x42->header.numBones );
	init_anim_frames( ret, x42 );

	return ret;
}

X42_EXPORT bool X42_CALL x42_UpdateAnimFrames( x42animFrames_t *anim, const x42data_t *x42,
	const float *times, x42opts_t *opts )
{
	uint i;

	REF_PARAM( opts );

#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rf( anim != NULL, X42_ERR_BADPTR, "anim is NULL" );
	demand_rf( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rf( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
#endif

	if( !x42->header.numBones )
		//nop!
		return true;

#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rf( times != NULL, X42_ERR_BADPTR, "times is NULL" );

	for( i = 0; i < x42->header.numAnimGroups; i++ )
	{
		float t = times[i] - (int)x42->header.baseFrame;

		demand_rf( t >= 0 && t <= x42->header.numFrames - 1,
			X42_ERR_BADPARAMS, "target frame time out of range" );
	}
#endif

	for( i = 0; i < x42->header.numAnimGroups; i++ )
		anim->frames[i].targetFrame = times[i] - (int)x42->header.baseFrame;

	init_anim_frames( anim, x42 );
	scan_anim_frames( anim, x42 );

	return true;
}

static void init_anim_frames( x42animFrames_t *anim, const x42data_t *x42 )
{
	uint i, j;
	const x42keyStreamEntry_t * RESTRICT ks, * ksEnd;

	ksEnd = x42->keyStream + x42->header.keyStreamLength;

	for( i = 0; i < x42->header.numAnimGroups; i++ )
	{
		uint numBones;
		x42animGroup_t * RESTRICT g;
		x42ksState_t * RESTRICT s = anim->frames + i;

		if( s->ks && s->minFrame <= s->targetFrame )
			continue;

		g = x42->animGroups + i;
		numBones = g->endBone - g->beginBone;

		s->minFrame = 0;
		s->maxFrame = x42->header.numFrames;
		s->ks = ksEnd;

		ks = x42->keyStream + (g->beginBone * 6);
		for( j = 0; j < numBones * 3; j++ )
		{
			//this chunk of ksEntries is sorted such that
			//ks->bone, ks->type are in kpairs order

			anim->kpairs[ks->bone][ks->type][0].frame = ks[0].frame;
			anim->kpairs[ks->bone][ks->type][0].value = ks[0].value;
			anim->kpairs[ks->bone][ks->type][1].frame = ks[1].frame;
			anim->kpairs[ks->bone][ks->type][1].value = ks[1].value;
			
			ks += 2;
		}

		for( ks = x42->keyStream + x42->header.numBones * 6; ks < ksEnd; ks++ )
		{
			if( ks->bone < g->beginBone || ks->bone >= g->endBone )
				continue;

			s->maxFrame = anim->kpairs[ks->bone][ks->type][1].frame;
			s->ks = ks;
			break;
		}
	}
}

static void scan_anim_frames( x42animFrames_t *anim, const x42data_t *x42 )
{
	uint i;
	u32 gMask;

	const x42keyStreamEntry_t * RESTRICT ks, *ksEnd;

	gMask = 0;
	ks = NULL;

	ksEnd = x42->keyStream + x42->header.keyStreamLength;

	for( i = 0; i < x42->header.numAnimGroups; i++ )
	{
		if( anim->frames[i].maxFrame >= anim->frames[i].targetFrame )
			continue;

		gMask |= 1 << i;

		if( !ks )
		{
			ks = anim->frames[i].ks;
		}
		else if( anim->frames[i].ks < ks )
		{
			ks = anim->frames[i].ks;
		}
	}

	if( !gMask )
		return;

	for( ; ks < ksEnd; ks++ )
	{
		uint group;
		x42ksBoneEntry_t * RESTRICT dst;
		x42ksState_t * RESTRICT s;

		group = x42->boneGroups[ks->bone];

		if( !(gMask & (1 << group)) )
			//done with this group
			continue;

		s = anim->frames + group;

		if( ks < s->ks )
			//group is already advanced past this ks position
			continue;

		dst = anim->kpairs[ks->bone][ks->type];

		if( dst[1].frame >= s->targetFrame )
		{
			//would push back a value that puts the min up too high, stop now
			s->maxFrame = dst[1].frame;
			s->ks = ks;
			gMask &= ~(1 << group);

			if( !gMask )
				break;
		}
		else
		{
			if( dst[1].frame > s->minFrame )
				s->minFrame = dst[1].frame;

			dst[0] = dst[1];
			dst[1].frame = ks->frame;
			dst[1].value = ks->value;
		}
	}

	if( gMask )
	{
		for( i = 0; i < x42->header.numAnimGroups; i++ )
		{
			if( gMask & (1 << i) )
			{
				anim->frames[i].maxFrame = x42->header.numFrames;
				anim->frames[i].ks = ksEnd;
			}
		}
	}
}

/**********************************************************
	Lerp and blend code here.
*/

X42_EXPORT size_t X42_CALL x42_GetAnimLerpSize( const x42data_t *x42 )
{
	size_t ret;

#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rz( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rz( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
#endif

	ret = sizeof( x42animLerp_t );
	ret += sizeof( vec3_t ) * x42->header.numBones; //pos
	ret += sizeof( vec3_t ) * x42->header.numBones; //scale
	ret += sizeof( quat_t ) * x42->header.numBones; //rot

	return ret;
}

X42_EXPORT x42animLerp_t* X42_CALL x42_CreateAnimLerp( void *out_buffer, const x42data_t *x42 )
{
	x42animLerp_t *ret;

#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rn( out_buffer != NULL, X42_ERR_BADPTR, "out_buffer is NULL" );
	demand_rn( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rn( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
#endif

	ret = (x42animLerp_t*)out_buffer;
	ret->p = (vec3_t*)(ret + 1);
	ret->s = (vec3_t*)(ret->p + x42->header.numBones);
	ret->r = (quat_t*)(ret->s + x42->header.numBones);

	return ret;
}

X42_EXPORT bool X42_CALL x42_LerpAnims( x42animLerp_t *lerp,
	const x42data_t *x42, const x42animFrames_t *anim, x42opts_t *opts )
{
	uint i, j;
	
	const vec3_t * RESTRICT pv;
	const vec3_t * RESTRICT sv;
	const quat_t * RESTRICT rv;

	vec3_t * RESTRICT po;
	vec3_t * RESTRICT so;
	quat_t * RESTRICT ro;

	REF_PARAM( opts );

#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rf( lerp != NULL, X42_ERR_BADPTR, "lerp is NULL" );
	demand_rf( anim != NULL, X42_ERR_BADPTR, "anim is NULL" );
	demand_rf( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rf( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
#endif

	pv = x42->posValues;
	sv = x42->scaleValues;
	rv = x42->rotValues;

	po = lerp->p;
	so = lerp->s;
	ro = lerp->r;

	for( i = 0; i < x42->header.numAnimGroups; i++ )
	{
		const x42ksBoneEntry_t * RESTRICT p;

		uint beginBone = x42->animGroups[i].beginBone;
		uint endBone = x42->animGroups[i].endBone;

		float target = anim->frames[i].targetFrame;

		for( j = beginBone; j < endBone; j++ )
		{
			p = anim->kpairs[j][X42_KT_POSITION];

			if( p[0].value == p[1].value )
				vec3_cpy( po[j], pv[p[0].value] );
			else
			{
				float s = (float)(int)p[0].frame;
				float e = (float)(int)p[1].frame;

				float t = (target - s) / (e - s);

				vec3_lerp( po[j], pv[p[0].value], pv[p[1].value], t );
			}

			p = anim->kpairs[j][X42_KT_SCALE];

			if( p[0].value == p[1].value )
				vec3_cpy( so[j], sv[p[0].value] );
			else
			{
				float s = (float)(int)p[0].frame;
				float e = (float)(int)p[1].frame;

				float t = (target - s) / (e - s);

				vec3_lerp( so[j], sv[p[0].value], sv[p[1].value], t );
			}

			p = anim->kpairs[j][X42_KT_ROTATION];

			if( p[0].value == p[1].value )
				quat_cpy( ro[j], rv[p[0].value] );
			else
			{
				float s = (float)(int)p[0].frame;
				float e = (float)(int)p[1].frame;

				float t = (target - s) / (e - s);

				quat_interp( ro[j], rv[p[0].value], rv[p[1].value], t );
			}
		}
	}

	return true;
}

X42_EXPORT bool X42_CALL x42_ApplyBoneOffsets( x42animLerp_t *lerp, const x42data_t *x42,
	const x42boneOffset_t *offsets, uint numOffsets, x42opts_t *opts )
{
	uint i, j;

	REF_PARAM( opts );

#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rf( lerp != NULL, X42_ERR_BADPTR, "lerp is NULL" );
	demand_rf( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rf( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
	demand_rf( offsets != NULL, X42_ERR_BADPTR, "offsets is NULL" );
#endif

	for( i = 0; i < numOffsets; i++ )
	{
		quat_t tmp;

		for( j = 0; j < x42->header.numBones; j++ )
		{
			if( x42name_eq2( x42, x42->bones[j].name, offsets[i].boneName ) )
				break;
		}

		if( j == x42->header.numBones )
			continue;

		vec3_add( lerp->p[j], lerp->p[j], offsets[i].pos_offset );
		vec3_add( lerp->s[j], lerp->s[j], offsets[i].scale_offset );

		quat_mul( tmp, offsets[i].rot_offset, lerp->s[j] );
		quat_cpy( lerp->s[j], tmp );
	}

	return true;
}

X42_EXPORT x42animLerp_t* X42_CALL x42_BlendAnimations( void *out_buffer, const x42data_t *x42,
	const x42animLerp_t *lerp0, const x42animLerp_t *lerp1, const float *blendValues, x42opts_t *opts )
{
	uint i, j;

	x42animLerp_t *ret;

	const vec3_t * RESTRICT pv0, * RESTRICT pv1;
	const vec3_t * RESTRICT sv0, * RESTRICT sv1;
	const quat_t * RESTRICT rv0, * RESTRICT rv1;

	vec3_t * RESTRICT po;
	vec3_t * RESTRICT so;
	quat_t * RESTRICT ro;

	REF_PARAM( opts );

#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rn( out_buffer != NULL, X42_ERR_BADPTR, "out_buffer is NULL" );
	demand_rn( lerp0 != NULL, X42_ERR_BADPTR, "lerp0 is NULL" );
	demand_rn( lerp1 != NULL, X42_ERR_BADPTR, "lerp1 is NULL" );
	demand_rn( blendValues != NULL, X42_ERR_BADPTR, "blendValues is NULL" );
	demand_rn( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rn( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
#endif

	ret = (x42animLerp_t*)out_buffer;

	pv0 = lerp0->p;
	sv0 = lerp0->s;
	rv0 = lerp0->r;

	pv1 = lerp1->p;
	sv1 = lerp1->s;
	rv1 = lerp1->r;

	po = ret->p;
	so = ret->s;
	ro = ret->r;

	for( i = 0; i < x42->header.numAnimGroups; i++ )
	{
		uint beginBone = x42->animGroups[i].beginBone;
		uint endBone = x42->animGroups[i].endBone;

		float lerp = blendValues[i];

		for( j = beginBone; j < endBone; j++ )
		{
			vec3_lerp( po[j], pv0[j], pv1[j], lerp );
		}

		for( j = beginBone; j < endBone; j++ )
		{
			vec3_lerp( so[j], sv0[j], sv1[j], lerp );
		}

		for( j = beginBone; j < endBone; j++ )
		{
			quat_interp( ro[j], rv0[j], rv1[j], lerp );
		}
	}

	return ret;
}

/************************************************************
	Matrix generation.
*/

X42_EXPORT size_t X42_CALL x42_GetAnimBoneMatricesSize( const x42data_t *x42 )
{
#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rz( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rz( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
#endif

	return sizeof( affine_t ) * x42->header.numBones;
}

X42_EXPORT affine_t* X42_CALL x42_GetAnimBoneMatrices( void *out_buffer, const x42data_t *x42,
	const x42animLerp_t *lerp, x42opts_t *opts )
{
	uint i;

	affine_t * RESTRICT ret;

	const vec3_t * RESTRICT pv;
	const vec3_t * RESTRICT sv;
	const quat_t * RESTRICT rv;

	REF_PARAM( opts );

#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rn( out_buffer != NULL, X42_ERR_BADPTR, "out_buffer is NULL" );
	demand_rn( lerp != NULL, X42_ERR_BADPTR, "lerp is NULL" );
	demand_rn( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rn( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
#endif

	pv = lerp->p;
	sv = lerp->s;
	rv = lerp->r;

	ret = (affine_t*)out_buffer;

	for( i = 0; i < x42->header.numBones; i++ )
	{
		affine_t * RESTRICT o = ret + i;

		const x42bone_t * RESTRICT b = x42->bones + i;

		if( b->parentIdx != X42_MODEL_BONE && (b->flags & X42_BF_USE_INV_PARENT_SCALE) )
		{
			vec3_t ips;
			affine_t rs;

			quat_to_affine( &rs, rv[i] );

			vec3_scale( rs.c[0], rs.c[0], sv[i][0] );
			vec3_scale( rs.c[1], rs.c[1], sv[i][1] );
			vec3_scale( rs.c[2], rs.c[2], sv[i][2] );

			ips[0] = 1.0F / sv[b->parentIdx][0];
			ips[1] = 1.0F / sv[b->parentIdx][1];
			ips[2] = 1.0F / sv[b->parentIdx][2];

			vec3_mul( o->c[0], rs.c[0], ips );
			vec3_mul( o->c[1], rs.c[1], ips );
			vec3_mul( o->c[2], rs.c[2], ips );
		}
		else
		{
			quat_to_affine( o, rv[i] );

			vec3_scale( o->c[0], o->c[0], sv[i][0] );
			vec3_scale( o->c[1], o->c[1], sv[i][1] );
			vec3_scale( o->c[2], o->c[2], sv[i][2] );
		}

		vec3_cpy( o->c[3], pv[i] );
	}

	for( i = 0; i < x42->header.numBones; i++ )
	{
		const x42bone_t * RESTRICT b = x42->bones + i;

		if( b->parentIdx != X42_MODEL_BONE )
			affine_mul( ret + i, ret + b->parentIdx, ret + i );
	}

	return ret;
}

X42_EXPORT size_t X42_CALL x42_GetAnimTagMatricesSize( const x42data_t *x42 )
{
#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rz( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rz( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
#endif

	return sizeof( affine_t ) * x42->header.numTags;
}

X42_EXPORT affine_t* X42_CALL x42_GetAnimTagMatrices( void *out_buffer, const x42data_t *x42,
	const affine_t *boneMats, x42opts_t *opts )
{
	uint i;

	affine_t * RESTRICT ret;

	const affine_t *bm;
	const x42tag_t * RESTRICT t;

	REF_PARAM( opts );

#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rn( out_buffer != NULL, X42_ERR_BADPTR, "out_buffer is NULL" );
	demand_rn( boneMats != NULL, X42_ERR_BADPTR, "boneMats is NULL" );
	demand_rn( x42 != NULL, X42_ERR_BADPTR, "x42 is NULL" );
	demand_rn( x42_ValidateHeader( &x42->header ), X42_ERR_BADDATA, "invalid x42 header data" );
#endif

	bm = boneMats;
	ret = (affine_t*)out_buffer;

	for( i = 0; i < x42->header.numTags; i++ )
	{
		affine_t * RESTRICT o = ret + i;

		t = x42->tags + i;

		if( t->bone != X42_MODEL_BONE )
			affine_mul( o, bm + t->bone, &t->tagMatrix );
		else
			affine_cpy( o, &t->tagMatrix );
	}

	return ret;
}