// copyright Min Zhong, cs838 proj2, April, 2000



// interpolation related routines

#include "RotConverter.h"
#include "viewer.h"
#include "MotionCapturer.h"
#include "vector.h"


/* slerp bw key pts p1 and p2, t=(0,1)
	res = result interpolated quat.
*/
void QuatSlerp(double *p1, double *p2, double t, double *res);


/* matrix to get cardinal param a, b, c, d
	-.5		1.5		 -1.5	.5
	1		-.25	2		-.5
	-.5		0		.5		0
	0		1		0		0 */
double mat_cardinal[16] = {-0.5, 1.5, -1.5, 0.5,  1, -2.5, 2, -0.5,  -0.5, 0, 0.5, 0,  0, 1, 0, 0};


// vector v2 = mat * v1
void mat44_mult_vec41(double *mat, double *v1, double *v2) 
{
	for (int i=0; i<4; i++) 
		v2[i] = mat[i*4]*v1[0] + mat[i*4+1]*v1[1] + mat[i*4+2]*v1[2] + mat[i*4+3]*v1[3];

}


/* returns a double[16] params
		[a1,b1,c1,d1; a2 b2 c2 d2; a3 b3 c3 d3; a4 b4 c4 d4] for calc interp vars 1,2,3,4 (e.g. x,y,z,w)
	in: double *p1, *p2;		// control points for interp on seg b/w t1 and t2
		tuple_size, offset to skip over each time step, 3 for euler/expmap, 4 for quat
	ASSUME caller alloc mem for params
  */
void calc_lin_curveParams(double *p1, double *p2, int t1, int t2, double *params)
{

	int dt = t2 - t1;

	memset(params, 0, 16 * sizeof(double) );	// zero out params


	// LINEAR:
	// [a b c d] = [0 0 m b], for y=mx+b, m=(y2-y1)/(x2-x1), b=(x2y1-x1y2)/(x2-x1)

	// row 1 params for z
	params[2] = (p2[0] - p1[0]) / dt;
	params[3] = (t2 * p1[0] - t1 * p2[0]) / dt;

	// row 2: params for x
	params[6] = (p2[1] - p1[1]) / dt;
	params[7] = (t2 * p1[1] - t1 * p2[1]) / dt;

	// row 3: params for y
	params[10] = (p2[2] - p1[2]) / dt;
	params[11] = (t2 * p1[2] - t1 * p2[2]) / dt;

	// row 4: don't care

}


// get the param matrix for cardinal interp 
void calc_cub_curveParams(double *p0, double *p1, double *p2, double *p3, double *params)
{

	double x1[4], x2[4], x3[4], x4[4];

	// params for var 1 
	x1[0] = p0[0]; 
	x1[1] = p1[0]; 
	x1[2] = p2[0]; 
	x1[3] = p3[0]; 
	mat44_mult_vec41(mat_cardinal, x1, params);

	x2[0] = p0[1]; 
	x2[1] = p1[1]; 
	x2[2] = p2[1]; 
	x2[3] = p3[1]; 
	mat44_mult_vec41(mat_cardinal, x2, params+4);

	x3[0] = p0[2]; 
	x3[1] = p1[2]; 
	x3[2] = p2[2]; 
	x3[3] = p3[2]; 
	mat44_mult_vec41(mat_cardinal, x3, params+8);

	x4[0] = p0[3]; 
	x4[1] = p1[3]; 
	x4[2] = p2[3]; 
	x4[3] = p3[3]; 
	mat44_mult_vec41(mat_cardinal, x4, params+12);

}



/* fills in frames bw frame keyTimes[first_keyIdx] and frame keyTimes[first_keyIdx+1] inclusive
	rot holds the interpolated data interp from ref_rot
*/
void Gl_Win::interp_bw2(int first_keyIdx, InterpType interpType, double *ref_rot, double *rot, int tuple_size)
{

	int i, t;
	int t0, t1, t2, t3;		// key times
	double td;		// normalized t
	double params[16];
	double func_deg_vec[]={0,0,0,1};	// holds blend func: e.g. =[0 0 t 1] for lin, [t^3 t^2 t 1] for cubic
	double v[4];	// result vect at time t.
	double *f_intrp;
	double *ptr_per_frame;
	double *f0, *f1, *f2, *f3;	// ptr to keyed frames
	int		frame_size_inDouble, frame_size_inBytes;
	int		off_per_joint;
	int		off_f0, off_f1, off_f2, off_f3;	// offsets to get to key frames in rot or ref_rot arr.
	
	frame_size_inDouble = rot_cnt * tuple_size;
	frame_size_inBytes	= frame_size_inDouble * sizeof(double);

	// interp bw t1 and t2
	t1 = keyTimes[first_keyIdx];
	off_f1 = t1 * frame_size_inDouble;
	f1 = ref_rot + off_f1;

	f_intrp = rot + off_f1; //f1 + frame_size_inDouble;	//start interp on the frame after f1	

	// copy the frames after the last key, no interp.
	if (first_keyIdx == key_cnt-1) {
		memcpy(f_intrp, ref_rot + off_f1, (frame_cnt - t1) * frame_size_inBytes);
		return;
	}

	t2 = keyTimes[first_keyIdx + 1];
	off_f2 = t2 * frame_size_inDouble;
	f2 = ref_rot + off_f2;

	// for cardinal, first and last seg not interpolated
	if (interpType == CARDINAL &&
		(first_keyIdx==0 || first_keyIdx==key_cnt-2) ) {

		memcpy(f_intrp, ref_rot + off_f1, (t2 - t1 + 1) * frame_size_inBytes);
		return;
	}

	if (t1+1 == t2) return;	// conseq frames, no need to interp	

	// set up the other two ctrl pts for cub interp
	if (interpType == CARDINAL) {		
	
		t0 = keyTimes[first_keyIdx - 1];
		t3 = keyTimes[first_keyIdx + 2];

		off_f0 = t0 * frame_size_inDouble;
		off_f3 = t3 * frame_size_inDouble;

		f0 = ref_rot + off_f0;
		f3 = ref_rot + off_f3;

	}


	off_per_joint = 0;
	ptr_per_frame = f_intrp;	//start interp 

	for (i = 0; i<rot_cnt; i++) {

		if (interpType == LINEAR || interpType == EM_LIN)
			calc_lin_curveParams(f1+off_per_joint, f2+off_per_joint, t1, t2, params);
		else if (interpType == CARDINAL)
			calc_cub_curveParams(f0+off_per_joint, f1+off_per_joint, 
								f2+off_per_joint, f3+off_per_joint, params);
	
		for (t=t1; t<=t2; t++) {

			td = ((t-t1) / (double) (t2-t1));	// normalize it to (0,1)

			if (interpType == SLERP || interpType == EM_SLERP) {
				QuatSlerp(f1+off_per_joint, f2+off_per_joint, td, ptr_per_frame);
			}
			else {	// lin or cardinal
				if (interpType == LINEAR || interpType == EM_LIN )	// lin or cardinal
					func_deg_vec[2] = t;	// for linear: func_deg_vec=[0 0 t 1]

				if (interpType == CARDINAL) {// for cubic: [t^3 t^2 t 1] for cubic
					double td_sq = td * td;
					func_deg_vec[2] = td;
					func_deg_vec[1] = td_sq;
					func_deg_vec[0] = td_sq * td;
				}

				mat44_mult_vec41(params, func_deg_vec, v); 
				ptr_per_frame[0] = v[0];
				ptr_per_frame[1] = v[1];
				ptr_per_frame[2] = v[2];
			}

			ptr_per_frame += frame_size_inDouble;

		
		}

		off_per_joint += tuple_size;
		ptr_per_frame = f_intrp + off_per_joint;	// reset ptr_per_rot to that joint in first frame to interp.

	}

}


void Gl_Win::interp( InterpType interpType ) 
{

	int i = 0; 
	int tuple_size;
	double *rot;
	double *ref_rot;
	int t0;		// first key time
	int frame_size_inBytes;


	switch (interpType) {
	case LINEAR:
	case CARDINAL:

		tuple_size = 3;
		ref_rot = cap->euler;
		rot = cap->einterp;		
		break;

	case SLERP:
	case Q_BERZIER:

		tuple_size = 4;
		ref_rot = cap->quatern;
		rot = cap->qinterp;	
		
		mdebug(DEBUG_Q, "interp q\n");
		break;
	case EM_SLERP:
		tuple_size = 4;
		ref_rot = cap->quatern;
		rot = cap->mqinterp;
		break;
	case EM_LIN:
		tuple_size = 3;
		ref_rot = cap->expmap;
		rot = cap->minterp;
		break;

	default:
		return;
	}

	// init the interp array
	frame_size_inBytes = tuple_size * rot_cnt * sizeof(double) ;
	memset( rot, 0, frame_cnt * frame_size_inBytes);

	t0 = keyTimes[0];
	if (t0 > 0)		// just copy, no interpolate before first key
		memcpy(rot, ref_rot, t0 * frame_size_inBytes);

	if (interpType == Q_BERZIER) {

		int pseudo_kcnt; // smallest number >= key_cnt that's mod 3 = 1.
		// need this so i+=3 won't skip last few frames

		// inc every 3 keys + one at the end
		// so perfect case: (key_cnt % 3 == 1)
		pseudo_kcnt = key_cnt;
		
		if (key_cnt % 3 == 0)
			pseudo_kcnt += 1;
		else if (key_cnt % 3 == 1)
			pseudo_kcnt += 2;
		
		for (i=0; i<pseudo_kcnt; i+=3)
			interp_bw4_qSpline(i, ref_rot, rot,  tuple_size);

		return;
	}


	// other interp types
	for (i=0; i<key_cnt; i++) {

		interp_bw2(i, interpType, ref_rot, rot,  tuple_size);	
	}


	// conv back to emap
	if (interpType == EM_SLERP) {
		
		double tot_rot	= rot_cnt * frame_cnt;
		double *q_ptr = cap->mqinterp;
		double *m_ptr = cap->minterp;

		for (i=0; i< tot_rot; i++) {
			quatToEmap(q_ptr, m_ptr);
			q_ptr += 4;
			m_ptr += 3;
		}
	}

}


/* referred to 
	http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
*/
void QuatSlerp(double *p1, double *p2, double t, double *res)
{
	double tmp1[4];
	double tmp2[4];
	double omega, cosom, sinom, s1, s2;

	// calc cosine
	cosom = p1[XCoord] * p2[XCoord] + p1[YCoord] * p2[YCoord] + p1[ZCoord] * p2[ZCoord]
						   + p1[Scalar] * p2[Scalar];

	// adjust signs (if necessary)
	if ( cosom <0.0 ){ 
		
		double v4_zero[4] = {0, 0, 0, 0};
		
		cosom = -cosom;				
		v4_sub(v4_zero, p2, tmp2);	// negate to get smaller rot when 2 rots are possible

	} else  {

		v4_assign(tmp2, p2);
	}


	// calculate coefficients

	if ( (1.0 - cosom) > NEAR_ZERO ) {
			// standard case (slerp)
			omega = acos(cosom);
			sinom = sin(omega);
			s1 = sin((1.0 - t) * omega) / sinom;
			s2 = sin(t * omega) / sinom;

	} else { 
	// "p1" and "p2" quaternions are very close 
		//  ... so we can do a linear interpolation
			s1 = 1.0 - t;
			s2 = t;
	}
	// calculate final values
	v4_scale(tmp1, s1, tmp1);
	v4_scale(tmp2, s2, tmp2);
	v4_add(tmp1, tmp2, res);	// slerp = q1*s1 + q2*s2
	
}