#include "proj.h"
#include "MotionCapturer.h"
#include "vector.h"
#include "rotConverter.h"

MotionCapturer::MotionCapturer()
{
	root = NULL;
	frame_cnt = 0;
	pickname_cur = 1;		// for 3d picking, 0 reserve for special
	b_read_flex_success = false;

	trans = NULL;
	euler = NULL;
	quatern = NULL;
	expmap = NULL;
	einterp = NULL;
	qinterp = NULL;
	minterp = NULL;
	mqinterp = NULL;
	pickarr = NULL;

}


void del_node(Node* &node)
{
	int i;

	if (node != NULL) {
	  
	  free(node->bone);
		for (i = 0; i < node->children_cnt; i++) {		  
		  del_node(node->children[i]);
		}

		if (node->ancestors != NULL) 
		  del_ancestry(node->ancestors);
		free(node);
		
	}

	node = NULL;

}


MotionCapturer::~MotionCapturer()
{

	del_node(root);

	if (trans!=NULL) free(trans);
	if (euler!=NULL) free(euler);
	if (quatern!=NULL) free(quatern);
	if (expmap!=NULL) free(expmap);

	if (einterp!=NULL) free(einterp);
	if (qinterp!=NULL) free(qinterp);
	if (minterp!=NULL) free(minterp);
	if (mqinterp!=NULL) free(mqinterp);

	if (pickarr!=NULL) free(pickarr);

}



/* recursively find all end sites and fill the array of endsites
	init es_cnt to 0 before 1st call*/
void create_endsite_list(Node **endsites, Node *node, int &es_cnt)
{

	int i;
	Node *child;

	if (node == NULL ) return;

	for (i = 0; i<node->children_cnt; i++) {
	
		child = node->children[i];
		if (child->type == END_SITE) {
			endsites[es_cnt] = child;
			es_cnt++;
		}
		else {	// recurse
			create_endsite_list(endsites, child, es_cnt); 
		}

	}

}













/* returns a node's pos in a more global/upstream node's coord,
   returns global pos if ancestral_node=NULL */
void lc_to_ac_nodePos( double return_val[4],
		      Node *ancestral_node, Node *selNode,
		      RotType rotType, double *trans, double *rot )
{
  int		r_off;	// rotation offset stored at node, = root->q_off or e_off depending on rotType
  double	transX, transY, transZ, rotatX, rotatY, rotatZ;
  double	aDeg, x, y, z;
  Point4D local_pos = {0, 0, 0, 1};	// node at center of its coord
  double	model_mat[MAT44_SIZE];		// modelview mat
  Ancestry *a;
  Node *node;

  if (selNode->ancestors == NULL)
	  selNode->ancestors = find_my_ancestors(selNode);

  a = selNode->ancestors;
  if (ancestral_node != NULL) {	  
    // tranform starts at ancestral_node
    while (ancestral_node != a->node)
      a = a->child;
    
    assert(ancestral_node == a->node);

    a = a->child; // to get to this node's coord, don't transf on it.
  } 
  // otherwise transform starts from root if ancestral_node not specif

  glPushMatrix();
  glLoadIdentity();


  while (1) {

    node = a->node;

    if (node->type == ROOT) {	// root trans 
      
      transX	= trans[0];
      transY	= trans[1];
      transZ	= trans[2];
    }
    else {
      transX	= node->offset[0];
      transY	= node->offset[1];
      transZ	= node->offset[2];
    }
    
    glTranslated(transX, transY, transZ);

    // do rotate 
    if (node->type != END_SITE) {
    	// do rotate 
		if (rotType == EULER) {
			// rot = (Z,X,Y)`
			r_off = node->e_off;
			rotatZ = rot[r_off];
			rotatX = rot[r_off + 1];
			rotatY = rot[r_off + 2];

			glRotated(rotatZ, 0, 0, 1);
			glRotated(rotatX, 1, 0, 0);
			glRotated(rotatY, 0, 1, 0);
		}
		else {
			if (rotType == QUAT) {
				r_off = node->q_off;
				quaternToRot(rot + r_off, aDeg, x, y, z);
			}
			else {// rotType = EXPMAP
				r_off = node->e_off;
				emapToRot(rot + r_off, aDeg, x, y, z);
			}

			if (aDeg != 0)
				glRotated(aDeg, x, y, z);
		}
	}

    if (node == selNode) break; // check before inc a,so get teh last transform
    a = a->child;
  }    

  // get the node's cur global pos
  glGetDoublev(GL_MODELVIEW_MATRIX, model_mat);  
  glmat44_mult_vec41(model_mat, local_pos, return_val);

  glPopMatrix();

}






/* find all ancestors from the root down to this node, 
	set its child to NULL in ancestrial chain.
	return the ancestry chain start from root to this node (inclusive */
Ancestry *find_my_ancestors(Node *node)
{
	Ancestry *prev;
	Ancestry *a = (Ancestry *)malloc(sizeof(Ancestry));
	a->node  = node;
	a->child = NULL;		

	while (a->node->parent != NULL) {
		prev		= (Ancestry *)malloc(sizeof(Ancestry));
		prev->child = a;
		prev->node	= a->node->parent;
		a	= prev;
	}

	return a;
}


void del_ancestry(Ancestry* &ancestors)
{
	Ancestry *del;
	while (ancestors != NULL) {
		del = ancestors;
		ancestors = ancestors->child;
		free(del);
	}

	ancestors = NULL;

}