// asusmptions for bvh parser
/* assume max of MAX_CHILDREN joints from a root

   assumes 3 or 6 chan ordering as follows: 
   for 6 chan: (xt, yt, zt, zr, xr, yr);
   for 3 chan: (zr, xr, yr)
*/

// todo: make it more generic by recording # roots, # joints, #end_sites

#include "proj.h"
#include "RotConverter.h"
#include "MotionCapturer.h"
#include "Marker.h"


// reading hierchary tree
Status init_node(char *hdr_lineBuf, Node* &node);
	// alloc mem for a node and assign its type

Status fill_node(FILE *file, Node* node, int &rot_cnt, int &rot_off, int &ends_cnt);
	/* read in offset, chan, and children structural info for a node
		rot_off = 0, rot_cnt = 0, ends_cnt = 0
			before first called, inc after each call 
		fills in the offset embedded in hierarchy,
		and get rot_cnt (NOTE: end_site has no rotation, doesn't count as a node)
	*/

/* fill each node's parent, pickname (for 3d picking)
	 after the hierachy is built. */
void fill_info_for_child(Node *root, Node **pickarr, GLuint &cur_pickname);

Status get_offsets(Node *node, char *line );	
	// read in offsets

Status get_channels(Node *node, char *line );
	// syntax checking, not relevant


////// for reading motin data portion
Status	fill_transform(FILE *file, MotionCapturer *cap, FileType type);
	// alloc and fill in trans and e_rot array

Status	read_motion(FILE *file, MotionCapturer *cap, FileType type);
	/* read in the motion part of data, 
		fills cap's frame_cnt, frame_time, and trans/rot data.
	*/

Status fill_quat(double *euler, double* &quat, int rot_cnt);
	// allocate and fill a quat array for rot_cnt number of quaternions

Status fill_emap(double *quatern, double* &emap, double* &mqinterp, int rot_cnt);
	// allocate and fill a expmap array for rot_cnt number of quaternions

Status init_interps(MotionCapturer *cap);
/* alloc mem and init the interp arrays to 0 */


void disp_a_frame(Node *root, double* &t_ptr, double* &e_ptr, double* &q_ptr);



/* 	read in file and fill a MotionCapturer object w/ hierarchical and motion data info
	return SUCCESS if read file ok
	FAILURE otherwise
*/
Status read_bvh_file(FILE *file, MotionCapturer *motionCapturer, FileType type)
{
	char hdr_lineBuf[LINESIZE];	// where to read the line into
	char *tok;    // used to tokenize each line of the file.
	Status status;
		
	if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;
	tok = strtok(hdr_lineBuf, " \t\n");

	if( strcmp(tok, "HIERARCHY") != 0 )		return FAILURE;

	if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;	
	if ( init_node(hdr_lineBuf, motionCapturer->root) != SUCCESS ) return FAILURE;
	// alloc a node and assign its type and its offset to index into the rotation data.
	motionCapturer->root->e_off = 0;	// offset to euler array
	motionCapturer->root->e_off = 0;	// offset to quatern array

	motionCapturer->rot_cnt = 0;	
	motionCapturer->ends_cnt = 0;	
	int rot_off = 0;
		// rot offset keeps track of num nodes having rot off set, to be incr per recursion
	
	if ( fill_node(file, motionCapturer->root, motionCapturer->rot_cnt, rot_off, motionCapturer->ends_cnt) != SUCCESS ) return FAILURE;	
	// recurse. fill node's name, offset, chan, children info.

	motionCapturer->ends_cnt = 0;
	create_endsite_list(motionCapturer->endsites, motionCapturer->root, motionCapturer->ends_cnt);

	motionCapturer->pickname_cur = 1;	// 0 reserve for special
	int picksize = (motionCapturer->rot_cnt + motionCapturer->ends_cnt) * sizeof(Node *);
	motionCapturer->pickarr = (Node **)malloc(  picksize );
	memset(motionCapturer->pickarr, 0, picksize);
		
	fill_info_for_child(motionCapturer->root, motionCapturer->pickarr, motionCapturer->pickname_cur);

	printf("cur pickname=%d\n", motionCapturer->pickname_cur);

	motionCapturer->root->parent = NULL;
		/* root has no name, won't be loaded for IK end site picking */

	mdebug(DEBUG_READ, "Done parsing tree %d nodes.  Start reading motion data.\n",
			motionCapturer->rot_cnt);

	status = read_motion(file, motionCapturer, type);
		
	fclose(file);

	return status;

}



/* read in the motion part of data, 
	fills cap's frame_cnt, frame_time, and trans/rot data.
*/
Status read_motion(FILE *file, MotionCapturer *cap, FileType type)
{
	Status status;
	char hdr_lineBuf[LINESIZE];	// where to read the line into
	char *tok;    // used to tokenize each line of the file.

	// get keyword "MOTION"
	if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;
	tok = strtok(hdr_lineBuf, " \t\n");
	if (tok == NULL) {	// skip blank line if htere is one
		if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;
		tok = strtok(hdr_lineBuf, " \t\n");
	}
	if( strcmp(tok, "MOTION") != 0 ) return FAILURE;

	// get keyword "Frames: #"
	if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;
	tok = strtok(hdr_lineBuf, " \t");
	if( strcmp(tok, "Frames:") != 0 )		return FAILURE;
	tok = strtok(NULL, " \t\n");
	cap->frame_cnt = atoi(tok);			// get frame cnt

	// get keyword "Frame Time: #"
	if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;
	tok = strtok(hdr_lineBuf, " \t");
	if( strcmp(tok, "Frame") != 0 )		return FAILURE;
	tok = strtok(NULL, " \t");
	if( strcmp(tok, "Time:") != 0 )		return FAILURE;
	tok = strtok(NULL, " \t\n");
	cap->frame_time = atof(tok);		// get frame time

	
	status = fill_transform(file, cap, type);
	
	if (type == BVH) {
		status = fill_quat(cap->euler, cap->quatern, cap->rot_cnt * cap->frame_cnt);
		if (status != SUCCESS) return status;
	}
	
	status = fill_emap(cap->quatern, cap->expmap, cap->mqinterp, cap->rot_cnt * cap->frame_cnt);
	if (status != SUCCESS) return status;

	status = init_interps(cap);

	return status;
}


/* alloc mem and init the interp arrays to 0
 */
Status init_interps(MotionCapturer *cap)
{
	int rot_tot = cap->rot_cnt * cap->frame_cnt;	// total number of rotations for all frames
	int e_size  = rot_tot * 3 * sizeof(double);		// 3 angles per euler rot per node
	int q_size  = rot_tot * 4 * sizeof(double);		// 4 param per rot per node

	cap->einterp = NULL;
	cap->einterp = (double*) malloc( e_size );
	if (cap->einterp == NULL) return FAILURE;
	memset(cap->einterp, 0, e_size);

	cap->qinterp = NULL;
	cap->qinterp = (double*) malloc( q_size );
	if (cap->qinterp == NULL) return FAILURE;
//	memcpy(cap->qinterp, cap->quatern, q_size);		// copy from quat
	memset(cap->qinterp, 0, q_size);		// copy from quat

	cap->minterp = NULL;
	cap->minterp = (double*) malloc( e_size );	// same size as euler
	if (cap->minterp == NULL) return FAILURE;
	memset(cap->minterp, 0, e_size);

	cap->mqinterp = NULL;
	cap->mqinterp = (double*) malloc( q_size );	// same size as quat
	if (cap->mqinterp == NULL) return FAILURE;
	memset(cap->mqinterp, 0, q_size);


	return SUCCESS;
}



/* fills cap's trans array (root's translate) and 
	euler array (all nodes' euler rotation) 
*/
Status fill_transform(FILE *file, MotionCapturer *cap, FileType type)
{
	double *t_ptr;	// ptr to trans array
	double *r_ptr;	// ptr to euler array
	int i, j;
	int tup_size;
	int t_size	= 3 * cap->frame_cnt;	// TODO: frame_cnt * 3 * root_cnt
	int r_cnt_per_frame ;
	int r_size;

	if (type == BVH)
		tup_size = 3;
	else if (type == QBVH)
		tup_size = 4;

	r_cnt_per_frame = cap->rot_cnt * tup_size ;
	r_size = r_cnt_per_frame * cap->frame_cnt;	

	cap->trans = NULL;
	cap->trans = (double*) malloc( t_size * sizeof(double));	
	if (cap->trans == NULL) return FAILURE;
	memset(cap->trans, 0, t_size);

	cap->euler = NULL;
	cap->euler = (double*) malloc( r_size * sizeof(double));
	if (cap->euler == NULL) return FAILURE;
	memset(cap->euler, 0, r_size);
	r_ptr = cap->euler;

	if (type == QBVH) {
		cap->quatern = NULL;
		cap->quatern = (double*) malloc( r_size * sizeof(double));
		if (cap->quatern == NULL) return FAILURE;
		memset(cap->quatern, 0, r_size);
		r_ptr = cap->quatern;
	}

	t_ptr = cap->trans;
	/* note: in case of unexpected EOF, i still try to continue,
		will try to draw fig w/ missing data */
	for (i = 0; i < cap->frame_cnt; i++) {

		// store first 3 as root's transl
		if (fscanf(file, "%lf", t_ptr++ ) != 1) {
				printf("%c Warning: unexpected EOF reached.\n", SOUND_BELL);
				break;
		}
		if (fscanf(file, "%lf", t_ptr++ ) != 1) {
				printf("%c Warning: unexpected EOF reached.\n", SOUND_BELL);
				break;
		}
		if (fscanf(file, "%lf", t_ptr++ ) != 1) {
				printf("%c Warning: unexpected EOF reached.\n", SOUND_BELL);
				break;
		}
		
		// store each node's rot
		for (j=0; j < r_cnt_per_frame; j++) {
			
			if (fscanf(file, "%lf", r_ptr++) != 1) {
				printf("%c Warning: unexpected EOF reached.\n", SOUND_BELL);
				fflush(stdout);
				break;
			}
		}

	}

	return SUCCESS;
}
	

// allocate and fill a quat array for rot_cnt number of quaternions
Status fill_quat(double *euler, double* &quat, int rot_cnt)
{
	int i;
	int q_size = rot_cnt * 4 * sizeof(double);
	double *q_ptr, *e_ptr;

	quat = NULL;
	quat = (double *)malloc( q_size ) ;
	if (quat == NULL) return FAILURE;
	memset(quat, 0, q_size);

	q_ptr = quat;
	e_ptr = euler;

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

		// conv 3 euler angle around z, x, y to a quat
//		euler3ToQuatern(e_ptr[0], e_ptr[1], e_ptr[2], q_ptr);
		EulerToQuat(e_ptr[0], e_ptr[1], e_ptr[2], q_ptr);

		e_ptr += 3;
		q_ptr += 4;	// incr to next quaternion
	}

	return SUCCESS;
}


// allocate and fill a expmap array for rot_cnt number of quaternions
Status fill_emap(double *quatern, double* &emap, double* &mqinterp, int rot_cnt)
{
	int i;
	int m_size = rot_cnt * 3* sizeof(double);
	int q_size = rot_cnt * 4* sizeof(double);
	double *q_ptr, *m_ptr;

	emap = NULL;
	emap = (double *)malloc( m_size ) ;
	if (emap == NULL) return FAILURE;
	memset(emap, 0, m_size);

	q_ptr = quatern;
	m_ptr = emap;
	for (i=0; i<rot_cnt; i++) {

		quatToEmap(q_ptr, m_ptr);	// conv a q to an expmap
		q_ptr += 4;
		m_ptr += 3;		
	}

	return SUCCESS;
}




/* 
	a debug func to check if trans/rot data are filled in correctly.
	at first call of this func, each ptr is set to the corresponding frame start of data,
	e.g. e_ptr = frameIdx * rot_cnt * 3 before first call.
*/
void disp_a_frame(Node *root, double* &t_ptr, double* &e_ptr, double* &q_ptr)
{

	int i;

	if (root != NULL ) {

		if (root->type == ROOT) {
			printf("%s Trans (%2.2lf, %2.2lf, %2.2lf)	", root->name, t_ptr[0], t_ptr[1], t_ptr[2]);
			t_ptr += 3;
		}

		if (root->type != END_SITE)	{// end site doesn't have rot
			printf("%s Euler (%2.2lf, %2.2lf, %2.2lf)\n", root->name, e_ptr[0], e_ptr[1], e_ptr[2]);
			printf("%s Quatern (%2.2lf, %2.2lf, %2.2lf, %2.2lf) \n", 
				root->name, q_ptr[Scalar], q_ptr[XCoord], q_ptr[YCoord], q_ptr[ZCoord]);
			e_ptr += 3;
			q_ptr += 4;
		}

		// recurse to disp its children
		for (i = 0; i<root->children_cnt; i++)
			disp_a_frame(root->children[i], t_ptr, e_ptr, q_ptr);				

	}
	
}



// alloc mem for a node and assign its type
Status init_node(char *hdr_lineBuf, Node* &node)
{
	char *tok;

  	tok = strtok(hdr_lineBuf, " \t");

	// alloc and init node
	node = (Node *)malloc( NODE_SIZE );
	memset(node, 0, NODE_SIZE);
	
	if ( strcmp(tok, "ROOT") == 0 )
		node->type = ROOT;
	else if ( strcmp(tok, "JOINT") == 0 )
		node->type = JOINT;
	else if ( strcmp(tok, "End") == 0) {
		tok = strtok(NULL, " \t\n");
		if ( strcmp(tok, "Site") != 0 ) return FAILURE;
			node->type = END_SITE;
	}
	else {
		free(node);
		return FAILURE;
	}

	// get root/joint's name
	if (node->type != END_SITE) {
		tok = strtok(NULL, " \t\n");
		memcpy(node->name, tok, strlen(tok)+1);
	}

  return SUCCESS;

}


/* fill each node's parent after the hierachy is built. */
void fill_info_for_child(Node *root, Node **pickarr, GLuint &cur_pickname)
{

	Node *child;
	int i;

	if (root == NULL) return;

	for (i=0; i<root->children_cnt; i++) {
		child = root->children[i];
		child->parent = root;
		child->pickname = cur_pickname;
		
		pickarr[cur_pickname] = root->children[i];	// register it in pick arr

		// give endsites a name
		if (child->type == END_SITE)
			sprintf(child->name, "%s_EndSite", root->name);

		cur_pickname++;
		fill_info_for_child(root->children[i], pickarr, cur_pickname);
	}

}


/* read in offset, chan, and children structural info for a node
	rot_off = rot_cnt = ends_cnt =0 before first call, 
	and inc after each call.
	fills in the offset embedded in hierarchy,
	and get rot_cnt (NOTE: end_site has no rotation, doesn't count as a node)
*/
Status fill_node(FILE *file, Node* node, 
				 int &rot_cnt, int &rot_off, int &ends_cnt)
{
	Status status;
	int i;
	char hdr_lineBuf[LINESIZE];	// where to read the line into
	char line[LINESIZE];	// store a copy of hdr_lineBuf 

	char *tok;

  // skip "{"
  if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;
  tok = strtok(hdr_lineBuf, " \t\n");
  if ( strcmp(tok, "{") != 0) return FAILURE;

  // get offset
  if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;	
  if ( get_offsets(node, hdr_lineBuf) != SUCCESS) return FAILURE;
  
  if (node->type == END_SITE) {	// end_site only has offsets
	 // skip "}"
	if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;
	tok = strtok(hdr_lineBuf, " \t\n");
	if ( strcmp(tok, "}") != 0) return FAILURE;
	
	ends_cnt ++;
	return SUCCESS;
  }
	
  // get chans
	if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;
	if ( get_channels(node, hdr_lineBuf) != SUCCESS) return FAILURE;


	// calc offset to index into the rotation data for this child
	node->e_off = rot_off * 3;
	node->q_off = rot_off * 4;

	rot_off++;
	rot_cnt++;

	// get children
	for (i=0; i<MAX_CHILDREN; i++) {

		// check if there are children 
		if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;
			memcpy(line, hdr_lineBuf, strlen(hdr_lineBuf)+1);
		
		tok = strtok(hdr_lineBuf, " \t\n");
		if ( strcmp(tok, "}") == 0 ) {
/*
			mdebug(DEBUG_READ, "%s has %d chans, %d children\n", 
			  node->name, node->chan_cnt, node->children_cnt );
*/	
			return SUCCESS;		// "}" => done, no more children
		}
		else {	// else processing the child
			if ( init_node(line, node->children[i]) == FAILURE ) return FAILURE;

			status = fill_node(file, node->children[i], rot_cnt, rot_off, ends_cnt);	// recurse
			if (status == FAILURE) return FAILURE;
			node->children_cnt++;
		}

	}
		
	return SUCCESS;
}



// read in offsets
Status get_offsets(Node *node, char *line )
{
  char *tok;

	tok = strtok(line, " \t");
	if ( strcmp(tok, "OFFSET") != 0 ) return FAILURE;
	
	tok = strtok(NULL, " \t");
	if (!tok) return FAILURE; 
	node->offset[0] = atof(tok);

	tok = strtok(NULL, " \t");
	if (!tok) return FAILURE; 
	node->offset[1] = atof(tok);

	tok = strtok(NULL, " \t");
	if (!tok) return FAILURE; 
	node->offset[2] = atof(tok);

	node->offset[3] = 1;	// w=1
	return SUCCESS;
}



Status get_channels(Node *node, char *line )
{
	char *tok;

	tok = strtok(line, " \t");
	if ( strcmp(tok, "CHANNELS") != 0 ) return FAILURE;

	tok = strtok(NULL, " \t");
	if (!tok) return FAILURE; 
	node->chan_cnt = atoi(tok);
	if (node->chan_cnt != 3 && node->chan_cnt != 6) {
	  fprintf(stderr, "invalid channel cnt for node %s\n", node->name);
	  return FAILURE;
	}

	// below is to verify valid chan layout e.g. 3 transl chan are together
	tok = strtok(NULL, " \t");
	if (!tok) return FAILURE; 	  
	if ( strcmp(tok, "Xposition") == 0 ) {

	  node->b_first3trans = true; // check trans channels

	  tok = strtok(NULL, " \t");
	  if (!tok) return FAILURE; 	  
	  if ( strcmp(tok, "Yposition") != 0 ) return FAILURE;
	  
	  if (node->chan_cnt == 3)
		tok = strtok(NULL, " \t\n");
	  else
		tok = strtok(NULL, " \t");
	  if (!tok) return FAILURE; 	  
	  if ( strcmp(tok, "Zposition") != 0 ) return FAILURE;
	}
	else if (strcmp(tok, "Zrotation") == 0) {

	  node->b_first3trans = false; // check rot channels

	  tok = strtok(NULL, " \t");
	  if (!tok) return FAILURE; 	  
	  if ( strcmp(tok, "Xrotation") != 0 ) return FAILURE;
	  
	  if (node->chan_cnt == 3)
		tok = strtok(NULL, " \t\n");
	  else
		tok = strtok(NULL, " \t");
	  if (!tok) return FAILURE; 	  
	  if ( strcmp(tok, "Yrotation") != 0 ) return FAILURE;
	}
	else 
	  return FAILURE;

	if (node->chan_cnt == 6) { // check the next 3
	  if (node->b_first3trans == true) { // verfiy next 3 are rot
	    tok = strtok(NULL, " \t");
	    if (!tok) return FAILURE; 	  	    
		if ( strcmp(tok, "Zrotation") != 0 ) return FAILURE;
	    
	    tok = strtok(NULL, " \t");
	    if (!tok) return FAILURE; 	  
	    if ( strcmp(tok, "Xrotation") != 0 ) return FAILURE;
	  
	    tok = strtok(NULL, " \t\n");
	    if (!tok) return FAILURE;	
		if ( strcmp(tok, "Yrotation") != 0 ) return FAILURE;
	  }
	  else { // verify next 3 are trans
	    tok = strtok(NULL, " \t");
	    if (!tok) return FAILURE; 	  	    
	    if ( strcmp(tok, "Xposition") != 0 ) return FAILURE;

	    tok = strtok(NULL, " \t");
	    if (!tok) return FAILURE; 	  
	    if ( strcmp(tok, "Yposition") != 0 ) return FAILURE;

	    tok = strtok(NULL, " \t\n");
	    if (!tok) return FAILURE; 	  
	    if ( strcmp(tok, "Zposition") != 0 ) return FAILURE;
	  }
	}

	return SUCCESS;
}