////////////////////////////////////////////////////////////////////////////
//
//  Crytek Engine Source File.
//  Copyright (C), Crytek Studios, 2002.
// -------------------------------------------------------------------------
//  File name:   tcbspline.h
//  Version:     v1.00
//  Created:     12/04/2002 by Timur.
//  Compilers:   Visual Studio.NET
//  Description: 
// -------------------------------------------------------------------------
//  History:
//
////////////////////////////////////////////////////////////////////////////

#ifndef __tcbspline_h__
#define __tcbspline_h__
#pragma once

#include <ISplines.h>

namespace spline
{
	/****************************************************************************
	**                        TCBSpline class implementation									   **
	****************************************************************************/
	template <class T,class Key = TCBSplineKey<T> >
	class	TCBSpline : public TSpline< Key,HermitBasis >
	{
	public:
		virtual void comp_deriv();

	protected:
		virtual	void interp_keys( int key1,int key2,float u,T& val );
		float calc_ease( float t,float a,float b );

	private:
		void compMiddleDeriv( int curr );
		void compFirstDeriv();
		void compLastDeriv();
		void comp2KeyDeriv();
	};

	//////////////////////////////////////////////////////////////////////////
	template	<class T,class Key>
		inline	void TCBSpline<T,Key>::compMiddleDeriv( int curr )
	{
		float	dsA,dsB,ddA,ddB;
		float	A,B,cont1,cont2;
		int last = this->num_keys() - 1; 

		// dsAdjust,ddAdjust apply speed correction when continuity is 0.
		// Middle key.
		if (curr == 0)	{
			// First key.
			float dts = (this->GetRangeEnd() - this->time(last)) + (this->time(0) - this->GetRangeStart());
			float dt = 2.0f / (dts + this->time(1) - this->time(0));
			dsA = dt * dts;
			ddA = dt * (this->time(1) - this->time(0));
		}	else	{
			if (curr == last)
			{
				// Last key.
				float dts = (this->GetRangeEnd() - this->time(last)) + (this->time(0) - this->GetRangeStart());
				float dt = 2.0f / (dts + this->time(last) - this->time(last-1));
				dsA = dt * dts;
				ddA = dt * (this->time(last) - this->time(last-1));
			}	else	{
				// Middle key.
				float dt = 2.0f/(this->time(curr+1) - this->time(curr-1));
				dsA = dt * (this->time(curr) - this->time(curr-1));
				ddA = dt * (this->time(curr+1) - this->time(curr));
			}
		}
		typename TSpline<Key,HermitBasis >::key_type &k = this->key(curr);

		float c = (float)fabs(k.cont);
		float sa = dsA + c*(1.0f - dsA);
		float da = ddA + c*(1.0f - ddA);

		A = 0.5f * (1.0f - k.tens) * (1.0f + k.bias);
		B = 0.5f * (1.0f - k.tens) * (1.0f - k.bias);
		cont1 = (1.0f - k.cont);
		cont2 = (1.0f + k.cont);
		//dsA = dsA * A * cont1;
		//dsB = dsA * B * cont2;
		//ddA = ddA * A * cont2;
		//ddB = ddA * B * cont1;
		dsA = sa * A * cont1;
		dsB = sa * B * cont2;
		ddA = da * A * cont2;
		ddB = da * B * cont1;

		T qp,qn;
		if (curr > 0) qp = this->value(curr-1);	else qp = this->value(last);
		if (curr < last) qn = this->value(curr+1); else qn = this->value(0);
		k.ds = Concatenate(dsA*Subtract(k.value, qp) , dsB*Subtract(qn,k.value));
		k.dd = Concatenate(ddA*Subtract(k.value, qp) , ddB*Subtract(qn,k.value));
	}

	template	<class T,class Key>
		inline	void TCBSpline<T,Key>::compFirstDeriv()	
	{
		typename TSpline<Key,HermitBasis >::key_type &k = this->key(0);
		Zero(k.ds);
		k.dd = 0.5f*
			(1.0f - k.tens)*( 3.0f*
			Subtract(Subtract(this->value(1),k.value),this->ds(1)));
	}

	template	<class T,class Key>
		inline	void TCBSpline<T,Key>::compLastDeriv()
	{
		int last = this->num_keys() - 1;
		typename TSpline<Key,HermitBasis >::key_type &k = this->key(last);
		k.ds = -0.5f*(1.0f - 
			k.tens)*( 3.0f*
			Concatenate(Subtract(this->value(last-1),k.value) ,	this->dd(last-1) ) );
		Zero(k.dd);
	}

	template	<class T,class Key>
		inline	void TCBSpline<T,Key>::comp2KeyDeriv()
	{
		typename TSpline<Key,HermitBasis >::key_type &k1 = this->key(0);
		typename TSpline<Key,HermitBasis >::key_type &k2 = this->key(1);

		typename TSpline<Key,HermitBasis >::value_type val = Subtract(this->value(1),this->value(0));

		Zero(k1.ds);
		k1.dd = (1.0f - k1.tens)*val;
		k2.ds = (1.0f - k2.tens)*val;
		Zero(k2.dd);
	}

	template	<class T,class Key>
		inline	void	TCBSpline<T,Key>::comp_deriv()
	{
		if (this->num_keys() > 1)	{
			if ((this->num_keys() == 2) && !this->closed())	{
				comp2KeyDeriv();
				return;
			}
			if (this->closed()) {
				for (int i = 0; i < this->num_keys(); ++i)	{
					compMiddleDeriv( i );
				}
			}	else	{
				for (int i = 1; i < (this->num_keys()-1); ++i)
				{
					compMiddleDeriv( i );
				}
				compFirstDeriv();
				compLastDeriv();
			}
		}
		this->SetModified( false );
	}

	template	<class T,class Key>
		inline	float	TCBSpline<T,Key>::calc_ease( float t,float a,float b )
	{
		float k;
		float s = a+b;

		if (t == 0.0f || t == 1.0f) return t;
		if (s == 0.0f) return t;
		if (s > 1.0f) {
			k = 1.0f/s;
			a *= k;
			b *= k;
		}
		k = 1.0f/(2.0f-a-b);
		if (t < a)	{
			return ((k/a)*t*t);
		}	else	{
			if (t < 1.0f-b)	{
				return (k*(2.0f*t - a));
			}	else {
				t = 1.0f - t;
				return (1.0f - (k/b)*t*t);
			}
		}
	}

	template	<class T,class Key>
		inline	void	TCBSpline<T,Key>::interp_keys( int from,int to,float u,T& val )
	{
		u = calc_ease( u,this->key(from).easefrom,this->key(to).easeto );
		typename TSpline<Key,HermitBasis >::basis_type basis( u );
		val = Concatenate(
			Concatenate(
			Concatenate(
			(basis[0]*this->value(from)),(basis[1]*this->value(to))
			), 
			(basis[2]*this->dd(from))
			), 
			(basis[3]*this->ds(to))
			);
	}



	/****************************************************************************
	**                        TCBQuatSpline class implementation							 **
	****************************************************************************/
	//////////////////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////////////////
	class	TCBQuatSpline : public TCBSpline<Quat>
	{
	public:
		//void interpolate( float time,value_type& val );
		void comp_deriv();

	protected:
		void interp_keys( int key1,int key2,float u,value_type& val );

	private:
		void compKeyDeriv( int curr );

		// Loacal function to add quaternions.
		Quat AddQuat( const Quat &q1,const Quat &q2 )
		{
			return Quat( q1.w+q2.w, q1.v.x+q2.v.x, q1.v.y+q2.v.y, q1.v.z+q2.v.z );
		}
	};

	inline	void	TCBQuatSpline::interp_keys( int from,int to,float u,value_type& val )
	{
		u = calc_ease( u,key(from).easefrom,key(to).easeto );
		basis_type basis( u );
		//val = SquadRev( angle(to),axis(to), value(from), dd(from), ds(to), value(to), u );
		val = Quat::CreateSquad( value(from), dd(from), ds(to), value(to), u );
		val = (val).GetNormalized();	// Normalize quaternion.
	}


	inline	void TCBQuatSpline::comp_deriv()
	{
		if (num_keys() > 1)
		{
			for (int i = 0; i < num_keys(); ++i)	{
				compKeyDeriv( i );
			}
		}
		this->SetModified( false );
	}



#define	M_2PI		(2.0f*3.14159f - 0.00001f)

	inline	void TCBQuatSpline::compKeyDeriv( int curr )
	{
		Quat  qp,qm;	
		float fp,fn;
		int last = num_keys() - 1;

		if (curr > 0 || closed())
		{
			int prev = (curr != 0) ? curr - 1 : last;
			qm = value(prev);
			if ( (qm|(value(curr))) < 0.0f) qm = -qm;
			qm = LnDif( qm,value(curr) );
		}

		if (curr < last || closed())
		{
			int next = (curr != last) ? curr + 1 : 0;
			Quat qnext = value(next);
			if ( (qnext|(value(curr))) < 0.0f) qnext = -qnext;
			qp = value(curr);
			qp = LnDif( qp,qnext );
		}

		if (curr == 0 && !closed())   qm = qp;
		if (curr == last && !closed()) qp = qm;

		key_type &k = key(curr);
		float c = (float)fabs(k.cont);

		fp = fn = 1.0f;
		if ((curr > 0 && curr < last) || closed())
		{
			if (curr == 0)	{
				// First key.
				float dts = (this->GetRangeEnd() - this->time(last)) + (this->time(0) - this->GetRangeStart());
				float dt = 2.0f / (dts + this->time(1) - this->time(0));
				fp = dt * dts;
				fn = dt * (this->time(1) - this->time(0));
			}	else	{
				if (curr == last)	{
					// Last key.
					float dts = (this->GetRangeEnd() - this->time(last)) + (this->time(0) - this->GetRangeStart());
					float dt = 2.0f / (dts + this->time(last) - this->time(last-1));
					fp = dt * dts;
					fn = dt * (this->time(last) - this->time(last-1));
				}	else	{
					// Middle key.
					float dt = 2.0f/(this->time(curr+1) - this->time(curr-1));
					fp = dt * (this->time(curr) - this->time(curr-1));
					fn = dt * (this->time(curr+1) - this->time(curr));
				}
			}
			fp += c - c*fp;
			fn += c - c*fn;
		}

		float tm,cm,cp,bm,bp,tmcm,tmcp,ksm,ksp,kdm,kdp;

		cm = 1.0f - k.cont;
		tm = 0.5f*(1.0f - k.tens);
		cp = 2.0f - cm;
		bm = 1.0f - k.bias;
		bp = 2.0f - bm;
		tmcm = tm * cm;
		tmcp = tm * cp;
		ksm  = 1.0f - tmcm * bp * fp;
		ksp  = -tmcp * bm * fp;
		kdm  = tmcp * bp * fn;
		kdp  = tmcm * bm * fn - 1.0f;

		Quat qa = 0.5f * AddQuat(kdm*qm,kdp*qp);
		Quat qb = 0.5f * AddQuat(ksm*qm,ksp*qp);
		qa = exp(qa.v);
		qb = exp(qb.v);

		// ds = qb, dd = qa.
		k.ds = value(curr) * qb;
		k.dd = value(curr) * qa;
	}

	/****************************************************************************
	**                    TCBAngleAxisSpline class implementation							 **
	****************************************************************************/
	///////////////////////////////////////////////////////////////////////////////
	//
	// TCBAngleAxisSpline takes as input relative Angle-Axis values.
	// Interpolated result is returned as Normalized quaternion.
	//
	//////////////////////////////////////////////////////////////////////////
	struct SAngleAxis
	{
		float angle;
		Vec3 axis;
	};


	class	TCBAngleAxisSpline : public TCBSpline<Quat,TCBAngAxisKey>
	{
	public:
		//void interpolate( float time,value_type& val );
		void comp_deriv();

		// Angle axis used for quaternion.
		float&	angle( int i )	{ return key(i).angle; };
		Vec3& axis( int i )	{ return key(i).axis; };

	protected:
		void interp_keys( int key1,int key2,float u,value_type& val );

	private:
		virtual	void compKeyDeriv( int curr );

		// Loacal function to add quaternions.
		Quat AddQuat( const Quat &q1,const Quat &q2 )
		{
			return Quat( q1.w+q2.w,q1.v.x+q2.v.x,q1.v.y+q2.v.y,q1.v.z+q2.v.z );
		}
	};

	//////////////////////////////////////////////////////////////////////////
	inline	void	TCBAngleAxisSpline::interp_keys( int from,int to,float u,value_type& val )
	{
		u = calc_ease( u,key(from).easefrom,key(to).easeto );
		basis_type basis( u );
		val = Quat::CreateSquadRev( angle(to),axis(to), value(from), dd(from), ds(to), value(to), u );
		val = (val).GetNormalized();	// Normalize quaternion.
	}

	//////////////////////////////////////////////////////////////////////////
	inline	void TCBAngleAxisSpline::comp_deriv()
	{
		// Convert from relative angle-axis to absolute quaternion.
		Quat q,lastq; 
		lastq.SetIdentity();
		for (int i = 0; i < num_keys(); ++i)
		{
			key_type &k = key(i);
			q.SetRotationAA( angle(i),axis(i) );
			q.Normalize(); // Normalize quaternion
			q = lastq * q;
			lastq = q;
			value(i) = q;
		}

		if (num_keys() > 1)
		{
			for (int i = 0; i < num_keys(); ++i)
			{
				compKeyDeriv( i );
			}
		}
		this->SetModified( false );
	}

	//////////////////////////////////////////////////////////////////////////
	inline	void TCBAngleAxisSpline::compKeyDeriv( int curr )
	{
		Quat  qp,qm;	
		float fp,fn;
		int last = num_keys() - 1;

		if (curr > 0 || closed())
		{
			int prev = (curr != 0) ? curr - 1 : last;
			if (angle(curr) > M_2PI) {
				Vec3 a = axis(curr);
				qm = Quat(0,log( Quat(0,a.x,a.y,a.z) ));
			} else {
				qm = value(prev);
				if ( (qm|(value(curr))) < 0.0f)
					qm = -qm;
				qm = LnDif( qm,value(curr) );
			}
		}

		if (curr < last || closed())
		{
			int next = (curr != last) ? curr + 1 : 0;
			if (angle(next) > M_2PI) {
				Vec3 a = axis(next);
				qp = Quat(0,log(Quat(0,a.x,a.y,a.z)));
			} else {
				Quat qnext = value(next);
				if ((qnext|(value(curr))) < 0.0f)
					qnext = -qnext;
				qp = value(curr);
				qp = LnDif( qp,qnext );
			}
		}

		if (curr == 0 && !closed())
			qm = qp;
		if (curr == last && !closed())
			qp = qm;

		key_type &k = key(curr);
		float c = (float)fabs(k.cont);

		fp = fn = 1.0f;
		if ((curr > 0 && curr < last) || closed())
		{
			if (curr == 0)	{
				// First key.
				float dts = (this->GetRangeEnd() - this->time(last)) + (this->time(0) - this->GetRangeStart());
				float dt = 2.0f / (dts + this->time(1) - this->time(0));
				fp = dt * dts;
				fn = dt * (this->time(1) - this->time(0));
			}	else	{
				if (curr == last)	{
					// Last key.
					float dts = (this->GetRangeEnd() - this->time(last)) + (this->time(0) - this->GetRangeStart());
					float dt = 2.0f / (dts + this->time(last) - this->time(last-1));
					fp = dt * dts;
					fn = dt * (this->time(last) - this->time(last-1));
				}	else	{
					// Middle key.
					float dt = 2.0f/(this->time(curr+1) - this->time(curr-1));
					fp = dt * (this->time(curr) - this->time(curr-1));
					fn = dt * (this->time(curr+1) - this->time(curr));
				}
			}
			fp += c - c*fp;
			fn += c - c*fn;
		}

		float tm,cm,cp,bm,bp,tmcm,tmcp,ksm,ksp,kdm,kdp;

		cm = 1.0f - k.cont;
		tm = 0.5f*(1.0f - k.tens);
		cp = 2.0f - cm;
		bm = 1.0f - k.bias;
		bp = 2.0f - bm;
		tmcm = tm * cm;
		tmcp = tm * cp;
		ksm  = 1.0f - tmcm * bp * fp;
		ksp  = -tmcp * bm * fp;
		kdm  = tmcp * bp * fn;
		kdp  = tmcm * bm * fn - 1.0f;

		Quat qa = 0.5f * AddQuat(kdm*qm,kdp*qp);
		Quat qb = 0.5f * AddQuat(ksm*qm,ksp*qp);
		qa = exp(qa.v);
		qb = exp(qb.v);

		// ds = qb, dd = qa.
		k.ds = value(curr) * qb;
		k.dd = value(curr) * qa;
	}
}; // namespace spline

#endif // __tcbspline_h__