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


#include "viewer.h"
#include "vector.h"


int n_choose_k(int n, int k)
{

	int top = 1;
	int i;

	if (n == k) return 1;	// note: 0c0 = 1

	if (n == 0) return 0;

	for (i=k+1; i <= n; i++)
		top *= i;

	for (i = 2; i <= n-k; i++)
		top /= i;

	return top;
}


void map_R4ToS3(double q_R4[4], double q_S3[4])
{

	double scale = 1 / (double) (q_R4[0]*q_R4[0] + q_R4[1]*q_R4[1] 
						+ q_R4[2]*q_R4[2] + q_R4[3]*q_R4[3]);

	q_S3[0] = scale * (q_R4[0]*q_R4[0] + q_R4[1]*q_R4[1] 
						+ q_R4[2]*q_R4[2] - q_R4[3]*q_R4[3]) ;

	q_S3[1] = scale * 2 * q_R4[0]*q_R4[3] ; 

	q_S3[2] = scale * 2 * q_R4[1]*q_R4[3] ; 

	q_S3[3] = scale * 2 * q_R4[2]*q_R4[3] ; 

}


// s3->R4
void inv_map_s3ToR4(double q[4], double t, double qres[4])
{

	double x0 = q[0];
	double x1 = q[1];
	double x2 = q[2];
	double x3 = q[3];

	if ( q[0]==1 && q[1]==0 && q[2]==0 && q[3]==0 ) {
		
		// set arbitrary hyperplane H (x4=0) - origin
		qres[0] = 1;
		qres[1] = 1;
		qres[2] = 1;
		qres[3] = 0;
	}
	else {
		qres[0] = t * x1;
		qres[1] = t * x2;
		qres[2] = t * x3;
		qres[3] = t * (1 - x0);
	}

}


/* fills nth degree poly coef array coef[n+1] 
   ASSUME coef[n+1] mem alloc by caller
*/
void bernstein_coef(int n, int *coef)
{
	int k;

	for (k=0; k<=n; k++) {
		coef[k] = n_choose_k(n, k);
	}

}


/* u= the parametrized var 
   get the b poly(blend func) for nth degree bezier curve at time u
*/
double bernstein_poly(double u, int n, int k, int coef_k)
{
	double poly;

	poly = coef_k * pow(u, k) * pow((1-u), (n-k));

	return poly;
}



/* find a pt on r4 nth deg bezier, and map back to s3 */
void pt_on_R4berzier_to_S3(int n, int *coef, Point4 ctrls[4], double u, double pt[4])
{
	int i, j, k;
	double choose;	// the product of the choose terms = (nhalf C i) * (nhalf C j) / (n C k)
	double wt;	// wt to normalize a 4-vector in s3.
	double wt_inner, wt_tot;  // tot wt 
	Point4 c;	// ctrls in s3
	Point4 c_inner, c_tot;	// intermediate result
	double blend;	// bernstein poly
	double cosm;	// cos or magn of hte mapped s3 quat
	Point4 v4_zero = {0, 0, 0, 0};

	int half_n = n / 2;
	
	v4_set(c_tot, 0);	// init vect to 0
	wt_tot = 0;

	for (k=0; k<=n; k++) {

		v4_set(c_inner, 0);
		wt_inner = 0;

		for (i=0; i<=half_n; i++) {
			for (j=0; j<=half_n; j++) {
//			j = k - i;
//			if (j>=0 && j<=half_n) {
				if (k != i + j) continue;  // paper says only sum when k=i+j 

				choose = n_choose_k(half_n, i) * n_choose_k(half_n, j) / (double) n_choose_k(n, k);

				c[0] = ctrls[i][0]*ctrls[j][0] + ctrls[i][1]*ctrls[j][1] 
						+ ctrls[i][2]*ctrls[j][2] - ctrls[i][3]*ctrls[j][3] ;

				c[1] = 2 * ctrls[i][0]*ctrls[j][3] ;

				c[2] = 2 * ctrls[i][1]*ctrls[j][3] ;

				c[3] = 2 * ctrls[i][2]*ctrls[j][3] ;

				wt = ctrls[i][0]*ctrls[j][0] + ctrls[i][1]*ctrls[j][1] 
						+ ctrls[i][2]*ctrls[j][2] + ctrls[i][3]*ctrls[j][3] ;

				v4_scale(c, choose, c );	// mult by choose
				wt *= choose;

				v4_add(c_inner, c, c_inner);	// inner loops summation
				wt_inner += wt;

			}	// for j
		}	// for i

		blend = bernstein_poly(u, n, k, coef[k]);

		v4_scale(c_inner, blend, c_inner);
		wt_inner *= blend;

		v4_add(c_inner, c_tot, c_tot);		// outer loop summation
		wt_tot += wt_inner;

	}	// for k

	v4_scale(c_tot, (double)1/wt_tot, pt);	// normalize by wt
	
	cosm = pt[0]*pt[0] + pt[1]*pt[1] + pt[2]*pt[2] + pt[3]*pt[3] ;

	v4_scale(pt, cosm, pt);	// normalize to unit q

}



/* interp cubic deg bezier quat spline seg 
	bw 4 ctrl pts keyTimes[first_keyIdx], keyTimes[first_keyIdx+3]*/
void Gl_Win::interp_bw4_qSpline(int first_keyIdx, double *ref_rot, double *rot, int tuple_size)
{

	int i, t;
	int t0, t1, t2, t3;		// key times
	int t_tot;	// total num frames in range of [t0,t3]
	double td;		// normalized t

	double *ptr_per_frame;
	double *f0, *f1, *f2, *f3;	// ptr to keyed frames
	double *f0_ptr, *f1_ptr, *f2_ptr, *f3_ptr;		// Pointers to travers f0-f3
	double *f_intrp;	// ptr to where to interp

	int		coef[7];	// for sixtic berzier in s3 (cub in r4)
	Point4	ctrls[4];	// 4 ctrl ptrs
	int		frame_size_inDouble, frame_size_inBytes;
	int		off_per_joint;	// incr by 4
	int		off_f0;	// offsets to get to f0 in rot or ref_rot arr.
	
	frame_size_inDouble = rot_cnt * tuple_size;
	frame_size_inBytes	= frame_size_inDouble * sizeof(double);

	t0 = keyTimes[first_keyIdx];
	off_f0 = t0 * frame_size_inDouble;
	f0		= ref_rot + off_f0;
	f_intrp = rot + off_f0;

	// can't interp the last seg if <4 ctrl pts there, just copy orig motion
	if ( first_keyIdx > key_cnt-4) {
		int bytes_to_copy = (frame_cnt - t0) * frame_size_inBytes;
		memcpy(f_intrp, f0, bytes_to_copy);
		return;
	}

	t1 = keyTimes[first_keyIdx + 1];
	t2 = keyTimes[first_keyIdx + 2];
	t3 = keyTimes[first_keyIdx + 3];

	f1 = ref_rot + t1 * frame_size_inDouble;
	f2 = ref_rot + t2 * frame_size_inDouble;
	f3 = ref_rot + t3 * frame_size_inDouble;

	f0_ptr = f0;
	f1_ptr = f1;
	f2_ptr = f2;
	f3_ptr = f3;

	// fill up the coef for nth deg berzier
	bernstein_coef( 6, coef );	

	t_tot = t3 - t0;	// total num frames in range of [t0,t3] inclusive
	off_per_joint = 0;
	ptr_per_frame = f_intrp;	//start interp 

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

		// fill up ctrls
		v4_assign(ctrls[0], f0_ptr);
		v4_assign(ctrls[1], f1_ptr);
		v4_assign(ctrls[2], f2_ptr);
		v4_assign(ctrls[3], f3_ptr);

		double t_fac = (double) (rand() % 32767) / 16383 - 1.0;
		inv_map_s3ToR4(ctrls[0], t_fac, ctrls[0]);		// s3 -> r4
		inv_map_s3ToR4(ctrls[1], t_fac, ctrls[1]);
		inv_map_s3ToR4(ctrls[2], t_fac, ctrls[2]);
		inv_map_s3ToR4(ctrls[3], t_fac, ctrls[3]);

		for (t=t0; t<=t3; t++) {
			td = ((t-t0) / (double) (t_tot));	// normalize it to [0,1)

			// construc map in r4 then map back to s3
			pt_on_R4berzier_to_S3(6, coef, ctrls, td, ptr_per_frame);	

			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.

		f0_ptr += tuple_size;	// move to next joint key
		f1_ptr += tuple_size;
		f2_ptr += tuple_size;
		f3_ptr += tuple_size;
	}

}




/// Below NOT USED ////////these are for regular bezier ////////////

/* returns a pt[4] on nth deg berzier at time u, 
   given n+1 ctrl pts
   ASSUME pt mem alloc by caller, 
   ctrls, coef set up by caller
 */
void pt_on_berzier_at_time(double u, int n, int *coef, 
			   Point4 *ctrls, Point4 pt)
{
	double tmp4[4];

	double blend;
	pt[0] = 0;
	pt[1] = 0;
	pt[2] = 0;
	pt[3] = 0;

	for (int k = 0; k <= n; k++) {
	blend = bernstein_poly(u, n, k, coef[k]);
	v4_scale(ctrls[k], blend, tmp4);
	v4_add( tmp4, pt, pt);
	}

}




/* pt_cnt many pts on  nth deg berzier given 
   given n+1 ctrl pts.
   ASSUMEs pts, coef mem alloc by caller,
   ctrls set up by caller
*/

void nth_berzier(int n, int *coef, 
		 Point4 *ctrls, int pt_cnt, Point4 *pts)
{
  int i;
  double u;

  // computes the b coef
  bernstein_coef(n, coef);

  for (i = 0; i < pt_cnt; i++) {
    u = i / (double) pt_cnt;  // normalize to [0,1]
    pt_on_berzier_at_time(u, n, coef, ctrls, pts[i]);
  }

}