/******************************************************************************
	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"

static void skin_verts_pntb( const affine_t *infs, uint numInfs, uint maxInfsPerVert,
	const x42vertAnim_t * RESTRICT modVertPos, const x42vertTangent_t * RESTRICT modVertTan, uint numVerts,

	x42memStream_t *oPos, x42memStream_t *oNorm, x42memStream_t *oTan, x42memStream_t *oBin )
{
	uint i;

	REF_PARAM( numInfs );

	for( i = 0; i < numVerts; i++ )
	{
		uint j;

		const x42vertAnim_t * RESTRICT vp = modVertPos + i;
		const x42vertTangent_t * RESTRICT vt = modVertTan + i;
		
		affine_t m;

		vec3_t t, b;

		affine_scale( &m, infs + vp->idx[0], vp->wt[0] );
		for( j = 1; j < maxInfsPerVert; j++ )
			affine_mad( &m, infs + vp->idx[j], vp->wt[j], &m );

		affine_mul_pos( stream_currptr( oPos, float ), &m, vp->pos );
		stream_next( oPos );

		affine_mul_vec( t, &m, vt->tan );
		affine_mul_vec( b, &m, vt->bit );

		stream_emit3fv( oTan, t );
		stream_emit3fv( oBin, b );

		{
			float l, s;
			vec3_t n;

			vec3_cross( n, t, b );

			l = vec3_lensq( n );
			s = rsqrt( l );

			vec3_scale( stream_currptr( oNorm, float ), n, vt->nfac0 * s );
			stream_next( oNorm );
		}
	}
}

static void copy_verts_pntb( const x42vertAnim_t * RESTRICT modVertPos,
	const x42vertTangent_t * RESTRICT modVertTan, uint numVerts,

	x42memStream_t *oPos, x42memStream_t *oNorm, x42memStream_t *oTan, x42memStream_t *oBin )
{
	uint i;

	for( i = 0; i < numVerts; i++ )
	{
		const x42vertAnim_t * RESTRICT vp = modVertPos + i;
		const x42vertTangent_t * RESTRICT vt = modVertTan + i;
		
		stream_emit3fv( oPos, vp->pos );

		stream_emit3fv( oTan, vt->tan );
		stream_emit3fv( oBin, vt->bit );

		{
			float l, s;
			vec3_t n;

			vec3_cross( n, vt->tan, vt->bit );

			l = vec3_lensq( n );
			s = rsqrt( l );

			vec3_scale( stream_currptr( oNorm, float ), n, vt->nfac0 * s );
			stream_next( oNorm );
		}
	}
}

static void skin_verts_pn( const affine_t *infs, uint numInfs, uint maxInfsPerVert,
	const x42vertAnim_t * RESTRICT modVertPos, const x42vertNormal_t * RESTRICT modVertNorm, uint numVerts,
	
	x42memStream_t *oPos, x42memStream_t *oNorm )
{
	uint i;

	REF_PARAM( numInfs );

	for( i = 0; i < numVerts; i++ )
	{
		uint j;

		vec3_t n;
		float rLen;

#ifndef LIBX42_NO_INVERSE_TRANSPOSE
		float it[3][3], det;
#endif

		const x42vertAnim_t * RESTRICT vp = modVertPos + i;
		const x42vertNormal_t * RESTRICT vn = modVertNorm + i;
		
		affine_t m;
		affine_scale( &m, infs + vp->idx[0], vp->wt[0] );
		for( j = 1; j < maxInfsPerVert; j++ )
			affine_mad( &m, infs + vp->idx[j], vp->wt[j], &m );

		affine_mul_pos( stream_currptr( oPos, float ), &m, vp->pos );
		stream_next( oPos );

#ifdef LIBX42_NO_INVERSE_TRANSPOSE
		affine_mul_vec( n, &m, vn->n );
#else
		/*
			Compute the cofactor matrix into it.
		*/
		it[0][0] = m.c[1][1] * m.c[2][2] - m.c[1][2] * m.c[2][1];
		it[0][1] = m.c[1][2] * m.c[2][0] - m.c[1][0] * m.c[2][2];
		it[0][2] = m.c[1][0] * m.c[2][1] - m.c[1][1] * m.c[2][0];

		it[1][0] = m.c[2][1] * m.c[0][2] - m.c[2][2] * m.c[0][1];
		it[1][1] = m.c[2][2] * m.c[0][0] - m.c[2][0] * m.c[0][2];
		it[1][2] = m.c[2][0] * m.c[0][1] - m.c[2][1] * m.c[0][0];

		it[2][0] = m.c[0][1] * m.c[1][2] - m.c[0][2] * m.c[1][1];
		it[2][1] = m.c[0][2] * m.c[1][0] - m.c[0][0] * m.c[1][2];
		it[2][2] = m.c[0][0] * m.c[1][1] - m.c[0][1] * m.c[1][0];

		/*
			Compute the determinant.
		*/
		det = m.c[0][0] * it[0][0] + m.c[0][1] * it[0][1] + m.c[0][2] * it[0][2];

		/*
			Complete the inverse. Note that we're not taking the adjoint. This would be
			redundant since we're computing an inverse-transpose:

				adj( transpose( A ) ) = cof( transpose( transpose( A ) ) ) = cof( A )
		*/

		det = 1.0F / det;
		
		it[0][0] *= det;
		it[0][1] *= det;
		it[0][2] *= det;
		
		it[1][0] *= det;
		it[1][1] *= det;
		it[1][2] *= det;

		it[2][0] *= det;
		it[2][1] *= det;
		it[2][2] *= det;

		n[0] = it[0][0] * vn->norm[0] + it[1][0] * vn->norm[1] + it[2][0] * vn->norm[2];
		n[1] = it[0][1] * vn->norm[0] + it[1][1] * vn->norm[1] + it[2][1] * vn->norm[2];
		n[2] = it[0][2] * vn->norm[0] + it[1][2] * vn->norm[1] + it[2][2] * vn->norm[2];
#endif
		
		//normalize and write the result
		rLen = rsqrt( vec3_lensq( n ) );

		vec3_scale( stream_currptr( oNorm, float ), n, rLen );
		stream_next( oNorm );
	}
}

static void copy_verts_pn( const x42vertAnim_t * RESTRICT modVertPos,
	const x42vertNormal_t * RESTRICT modVertNorm, uint numVerts,
	
	x42memStream_t *oPos, x42memStream_t *oNorm )
{
	uint i;

	for( i = 0; i < numVerts; i++ )
	{
		const x42vertAnim_t * RESTRICT vp = modVertPos + i;
		const x42vertNormal_t * RESTRICT vn = modVertNorm + i;
		
		stream_emit3fv( oPos, vp->pos );
		stream_emit3fv( oNorm, vn->norm );
	}
}

static void skin_verts_p( const affine_t *infs, uint numInfs, uint maxInfsPerVert,
	const x42vertAnim_t * RESTRICT modVertPos, uint numVerts,
	
	x42memStream_t *oPos )
{
	uint i;

	REF_PARAM( numInfs );

	for( i = 0; i < numVerts; i++ )
	{
		uint j;

		const x42vertAnim_t * RESTRICT vp = modVertPos + i;
		
		affine_t m;
		affine_scale( &m, infs + vp->idx[0], vp->wt[0] );
		for( j = 1; j < maxInfsPerVert; j++ )
			affine_mad( &m, infs + vp->idx[j], vp->wt[j], &m );

		affine_mul_pos( stream_currptr( oPos, float ), &m, vp->pos );
		stream_next( oPos );
	}
}

static void copy_verts_p( const x42vertAnim_t * RESTRICT modVertPos, uint numVerts,
	
	x42memStream_t *oPos )
{
	uint i;

	for( i = 0; i < numVerts; i++ )
	{
		const x42vertAnim_t * RESTRICT vp = modVertPos + i;
	
		stream_emit3fv( oPos, vp->pos );
	}
}

X42_EXPORT bool X42_CALL x42_SkinGroupVerts( x42memStream_t *oPos, x42memStream_t *oNorm, x42memStream_t *oTan, x42memStream_t *oBin,
	const x42data_t *x42, uint groupNum, const affine_t *boneMats, x42opts_t *opts )
{
	bool copyVerts;
	const x42group_t *g;

#ifndef LIBX42_NO_PARAM_VALIDATION
	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( groupNum < x42->header.numGroups, X42_ERR_OUTOFRANGE, "group number out of range" );
	demand_rf( boneMats != NULL, X42_ERR_BADPTR, "boneMats is null" );
	demand_rf( oPos != NULL && oPos->pStreamZero != NULL, X42_ERR_BADPTR, "oPos/oPos->pStreamZero is null" );

	if( oNorm )
	{
		demand_rf( oNorm->pStreamZero != NULL, X42_ERR_BADPTR, "oNorm->pStreamZero is null" );	
		demand_rf( x42->vertNorm != NULL || x42->vertTan != NULL, X42_ERR_INVALIDOP, "oNorm != NULL but model was not loaded with normals or tangents" );
	}
	
	if( oTan )
	{
		demand_rf( oTan->pStreamZero != NULL, X42_ERR_BADPTR, "oTan->pStreamZero is null" );
		demand_rf( x42->vertTan != NULL, X42_ERR_INVALIDOP, "oTan != NULL but model was not loaded with tangents" );
	}

	if( oBin )
	{
		demand_rf( oBin->pStreamZero != NULL, X42_ERR_BADPTR, "oTan->pStreamZero is null" );		
		demand_rf( x42->vertTan != NULL, X42_ERR_INVALIDOP, "oBin != NULL but model was not loaded with tangents" );
	}
#endif

	g = x42->groups + groupNum;

#ifndef LIBX42_NO_PARAM_VALIDATION
	demand_rf( g->maxVertInfluences >= 0 && g->maxVertInfluences <= 4, X42_ERR_BADDATA, "invalid group max influence count" );
#endif

	if( opts )
	{
		bool applied_opts = x42_ApplyOpts( opts );
		bool did_skin = opt_SkinGroupVerts( oPos, oNorm, oTan, oBin, x42, groupNum, boneMats, opts );

		if( applied_opts )
			x42_RemoveOpts( opts );

		if( did_skin )
			return true;
	}

	copyVerts = g->maxVertInfluences == 0;

	{
		uint i;
		
		affine_t infs[X42_MAX_INFLUENCES_PER_BATCH_V5];

		for( i = 0; i < g->numInfluences; i++ )
		{
			const x42influence_t *inf = x42->influences + g->influences[i];

			if( inf->bone != X42_MODEL_BONE )
				affine_mul( infs + i, boneMats + inf->bone, &inf->meshToBone );
			else
				affine_cpy( infs + i, &inf->meshToBone );
		}

		if( oTan || oBin || !x42->vertNorm )
		{
			byte scratch[32];
			x42memStream_t nullStream;

			nullStream.pStreamZero = align_ptr( scratch, 16 );
			nullStream.stride = 0;
			nullStream.scratchBytes = scratch + sizeof( scratch ) - (byte*)nullStream.pStreamZero;

			if( !oTan )
				oTan = &nullStream;
			if( !oBin )
				oTan = &nullStream;
			if( !oNorm )
				oTan = &nullStream;

			if( copyVerts )
			{
				copy_verts_pntb( x42->vertPos + g->firstVert, x42->vertTan + g->firstVert, g->numVerts,
					oPos, oNorm, oTan, oBin );
			}
			else
			{
				skin_verts_pntb( infs, g->numInfluences, g->maxVertInfluences, x42->vertPos + g->firstVert,
					x42->vertTan + g->firstVert, g->numVerts, oPos, oNorm, oTan, oBin );
			}
		}
		else if( oNorm )
		{
			if( copyVerts )
			{
				copy_verts_pn( x42->vertPos + g->firstVert, x42->vertNorm + g->firstVert, g->numVerts,
					oPos, oNorm );
			}
			else
			{
				skin_verts_pn( infs, g->numInfluences, g->maxVertInfluences, x42->vertPos + g->firstVert,
					x42->vertNorm + g->firstVert, g->numVerts, oPos, oNorm );
			}
		}
		else
		{
			if( copyVerts )
			{
				copy_verts_p( x42->vertPos + g->firstVert, g->numVerts, oPos );
			}
			else
			{
				skin_verts_p( infs, g->numInfluences, g->maxVertInfluences, x42->vertPos + g->firstVert, g->numVerts, oPos );
			}
		}
	}

	return true;
}