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


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


#define MAT_SIZE	16	// 4*4 mat


/* tries to find a node w/ specified name */
Status find_node_with_name(Node* root, char *node_name, Node* &node);

/* given m knows its parnet,find all his ancestors from the root down to parent */
void find_my_ancestors(Marker *m, Node *root);

/* get transform mat used to transform from root's coord to local coord of the last node in ancestors  */
void get_mat_down_ancestry(GLdouble *model_mat, Ancestry *ancestors, RotType rotType, double *trans, double* rot);


Marker::Marker(int f)
{

	int size_in_B;
	frame_cnt = f;
	next = NULL;
	ancestors = NULL;

	size_in_B = f*3*sizeof(double);
	trans = (double *)malloc( size_in_B );
	memset(trans, 0, size_in_B);
}



Marker::~Marker()
{
	Ancestry *del;
	while (ancestors != NULL) {
		del = ancestors;
		ancestors = ancestors->child;
		free(del);
	}
	ancestors = NULL;
	free(trans);
}


/* tries to find a node w/ specified name */
Status find_node_with_name(Node* root, char *node_name, Node* &node)
{
	int i;

	if (node_name == NULL) return FAILURE;

	if (strcmp(root->name, node_name) == 0) {
		node = root;
		return SUCCESS;
	}

	for (i=0; i<root->children_cnt; i++) {
		if (find_node_with_name(root->children[i], node_name, node) == SUCCESS)
			return SUCCESS;
	}

	return FAILURE;
}


/* given m knows its parnet,find all his ancestors from the root down to parent */
void find_my_ancestors(Marker *m, Node *root)
{
	Ancestry *prev;
	Ancestry *a = (Ancestry *)malloc(sizeof(Ancestry));
	a->node  = m->parent;
	a->child = NULL;

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

	m->ancestors = a;
}


/*	read a marker request file, alloc and init a link list of markers,
	headed by m_head 
	bad markers ignored (non-existing joints)
*/
Status read_marker_request_file(char *filename, Node *root, int f_cnt, Marker* &m_head)
{

	FILE	*file = NULL;
	char	hdr_lineBuf[LINESIZE];	// where to read the line into
	char	*tok;    // used to tokenize each line of the file.
	Marker	*m		= NULL;
	Marker	*m_prev	= NULL;
	Node	*parent = NULL;

	if ( (file = fopen(filename, "r")) == NULL) {
		fl_alert("Error opening marker file %s! File not loaded.", filename);
		return FAILURE;
	}

    mdebug(DEBUG_READ, "Reading marker config file %s.\n", filename);

	m_head = NULL;
	while (1) {

		if (!fgets(hdr_lineBuf, LINESIZE, file)) return FAILURE;
		tok = strtok(hdr_lineBuf, " \t\n");

		if (tok == NULL) return FAILURE;	// unexpected eof

		if ( strcmp(tok, "END")==0 ) break;	// done reading the file
		
		m = new Marker(f_cnt);

		memcpy(m->name, tok, strlen(tok)+1);

		parent = NULL;
		tok = strtok(NULL, " \t");
		if (tok == NULL || find_node_with_name(root, tok, parent) != SUCCESS) {
			fl_alert("Marker %s ignored -- parent %s NOT exist.", m->name, tok);
			delete(m);
			continue;
		}
		else {
			m->parent = parent;
		}

		// get offset
		tok = strtok(NULL, " \t");
		if (!tok) return FAILURE; 
		m->offset[0] = atof(tok);

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

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

		if (m_prev == NULL) {
			m_head = m;
			m_prev = m;
		}
		else {
			m_prev->next = m;
			m_prev = m;
		}

		find_my_ancestors(m, root);
	}
	
	return SUCCESS;
}

// create an end effector marker and add it to the list of end effector marker list
void add_endsite_marker(Node *parent, Node *root, double local_pos[3], int f_cnt, Marker* &m_head)
{
	Marker *m = new Marker(f_cnt);
	Marker *m_ptr;

	sprintf(m->name, "EndSiteOf%s", parent->name);
	v3_assign(m->offset, local_pos);	// get local offset
	m->parent = parent;
	find_my_ancestors(m, root);

	if (m_head == NULL)
		m_head = m;
	else {	// otherwise, insert new marker at the end
		m_ptr = m_head;
		while (m_ptr->next != NULL)
			m_ptr = m_ptr->next;

		m_ptr->next = m;
	}
}


// recursively find all end sites and make a marker list for it
void create_endsite_marker_list(Node *parent, Node *root, int f_cnt, 
								Marker* &m_head, int &m_cnt)
{

	int i;
	Node *child;

	if (parent == NULL ) return;

	for (i = 0; i<parent->children_cnt; i++) {
	
		child = parent->children[i];
		if (child->type == END_SITE) {
			add_endsite_marker(parent, root, child->offset, f_cnt, m_head);
			m_cnt++;
		}
		else {	// recurse
			create_endsite_marker_list(child, root, f_cnt, m_head, m_cnt); 
		}

	}

}





// assoc a bool w/ each marker in this marker list to decide which need to be drawn
void Gl_Win::init_marker_draw(RotType m_RotType, Node *root, Marker* m_head, 
							  Fl_Multi_Browser *marker_browser, 
								 MarkerDrawn* &m_drawlist, int m_cnt)
{

	int i;
	Marker *m;
	MarkerDrawn *md;

	if (marker_browser == NULL ) {
		fprintf(stderr, "ERROR: endsite browser not allocated.%c\n", SOUND_BELL);
		return;
	}

	m_drawlist = NULL;	
	if (m_cnt > 0) {
		m_drawlist = (MarkerDrawn *) malloc(m_cnt * sizeof(MarkerDrawn) );

		m = m_head;
		md = m_drawlist;
		for (i=0; i<m_cnt; i++) {
			m_drawlist[i].marker = m;
			m_drawlist[i].b_draw = false;	// default won't disp the marker

			marker_browser->add(m->name, 0);	// add it to browser

			m = m->next;
		}
	}

	calc_markers_pos(m_head, m_rotType);	// fill the trans array for each marker
}



/* reads in a marker request file,
	allocates markers.	
*/
Status Gl_Win::read_marker_request( Marker* &m_head )
{

	Status status;
	char *filename = NULL;
	Marker	*new_m_head = NULL;

	filename = fl_file_chooser("Enter bvh file name to load ", "*.mk", NULL);	// TODO: change .mk later.
	if (filename == NULL) {
		fl_alert("invalid file name!  File not loaded.");
		return FAILURE;
	}

	status	 = read_marker_request_file(filename, root, frame_cnt, new_m_head);		// read and parse file	

	if (status == SUCCESS && m_head != NULL) {	
		if (m_head != NULL)	// del old marks if exist
			del_markers(m_head);
		m_head = new_m_head;
	}
	return status;
}


/* get transform mat used to transform from root's coord to local coord of the last node in ancestors  */
void get_mat_down_ancestry(GLdouble *model_mat, Ancestry *ancestors, 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;
	Ancestry *a;
	Node	 *node;

	assert (ancestors != NULL);

	a = ancestors;
	do {	
		node = a->node;
		
		// do trans
		if (a == ancestors) {
			// 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 (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);
		}

		// keep going down the ancestry tree
		a = a->child;
		
	} while (a != NULL);

	glGetDoublev(GL_MODELVIEW_MATRIX, model_mat);

}
	

// draw trace of one marker
void draw_marker(Marker *m, int begin_frame, int end_frame)
{
	double *mptr;
	
	glBegin(GL_LINE_STRIP);

	mptr = m->trans + begin_frame * 3;
	for (int i=begin_frame; i<=end_frame; i++) {;

		glPushMatrix();				
			glVertex3d(mptr[0], mptr[1], mptr[2]);
		glPopMatrix();			

		mptr += 3;
	}
	glEnd();

}

// draw markers from frame begin to end
void Gl_Win::draw_markers(RotType m_rotType, Marker *m_head, int begin_frame, int end_frame)
{
	Marker *m;

	
	m = m_head;
	while (m != NULL) {

		draw_marker(m, begin_frame, end_frame);

		m = m->next;
	}

}


// draw selected markers
void Gl_Win::draw_sel_markers(MarkerDrawn *m_draw, int m_cnt, int begin_frame, int end_frame)
{

	for (int i=0; i < m_cnt; i++) {
		if (m_draw[i].b_draw) 
			draw_marker(m_draw[i].marker, begin_frame, end_frame);
	}

}

// for debug, make sure all marks got read in
void disp_markers(Marker *head)
{
	while (head != NULL) {
		printf("m %s, parent %s\n", head->name, head->parent->name);
		head = head->next;
	}
}


Status Gl_Win::calc_markers_pos(Marker *m_head, RotType rotType)
{
	GLdouble model_mat_at_joint[MAT_SIZE];
	Marker	*m;
	double	*mptr;	// index to each frame of the m->trans
	double	*t_ptr;	
	double	*r_ptr;
	double	v_off[] = {0,0,0,1};
	double	v_pos[4];
	int		f;
	int		frame_size_inDouble;

	if (rotType == EULER) {
		if (e_interpType == NONE || e_interpType == HIDE)
			r_ptr = cap->euler;
		else 
			r_ptr = cap->einterp;

		frame_size_inDouble = 3 * rot_cnt ;

		mdebug(DEBUG_INTP, "recalc markers from euler\n");
	}
	else if (rotType == QUAT) {
		if (q_interpType == NONE || q_interpType == HIDE)
			r_ptr = cap->quatern;
		else 
			r_ptr = cap->qinterp ;

		frame_size_inDouble = 4 * rot_cnt ;

		mdebug(DEBUG_INTP, "recalc markers from quat\n");
	}
	else if (rotType == EXPMAP) {
		if (m_interpType == NONE || m_interpType == HIDE)
			r_ptr = cap->expmap ;
		else 
			r_ptr = cap->minterp ;

		frame_size_inDouble = 3 * rot_cnt ;

		mdebug(DEBUG_INTP, "recalc markers from emap\n");

	}


	t_ptr = trans + 3 * begin;	// frame offset into the trans array
	r_ptr += frame_size_inDouble * begin;

	for (f=0; f< frame_cnt; f++) {

		m = m_head;
		while (m != NULL) {

			mptr = m->trans + f * 3;

			glPushMatrix();
			glLoadIdentity();
		
			get_mat_down_ancestry(model_mat_at_joint, m->ancestors, rotType, t_ptr, r_ptr);
			
			glPopMatrix();
						
			v_off[0] = m->offset[0];
			v_off[1] = m->offset[1];
			v_off[2] = m->offset[2];

			glmat44_mult_vec41(model_mat_at_joint, v_off, v_pos);

			mptr[0] = v_pos[0];
			mptr[1] = v_pos[1];
			mptr[2] = v_pos[2];
			
			m = m->next;
		}
		
		t_ptr += 3;
		r_ptr += frame_size_inDouble;
	}

	// disp_markers(m_head);

	return SUCCESS;
}


void Gl_Win::del_markers( Marker* &m_head)
{

	Marker *del;

	while (m_head != NULL) {
		del = m_head;
		m_head = m_head->next;
		delete(del);
	}

	m_head = NULL;
}


/* export marker's global pos at each frame (begin to end) to file */
Status Gl_Win::export_markers( Marker *m_head)
{

	char *outname = NULL;
	FILE *fout;
	Marker *m;
	double ktime;
	int		t_off;

	outname = fl_file_chooser("Enter file name to save to ", "*", NULL);
	if (outname == NULL) {
		fl_alert("invalid file name!  File not saved.");
		return FAILURE;
	}

	if ( (fout = fopen(outname, "w")) == NULL) {
		fl_alert("Error opening file! File not saved.");
		return FAILURE;
	}

	fprintf(fout, "KSample,KTime,");
	if (m_head == NULL) {
		fclose(fout);
		return SUCCESS;
	}

	// for loop to print the marker names post-fix w/ x, y, z
	m = m_head;
	while (m != NULL && m->next != NULL) {
		fprintf(fout, "%s-X, %s-Y, %s-Z,", m->name, m->name, m->name);
		m = m->next;
	}
	if (m != NULL)  // print last one's name
		fprintf(fout, "%s-X, %s-Y, %s-Z\n", m->name, m->name, m->name);


	// print each marker's each frame info (index into maker's trans array
	ktime = begin * frame_time;
	t_off = begin * 3;
	
	for (int f=begin; f<=end; f++) {
		fprintf(fout, "%d,  %2.3lf, ", f, ktime);
		m = m_head;
		while (m != NULL && m->next != NULL) {
			fprintf(fout, "%2.3lf, %2.3lf, %2.3lf, ", 
				m->trans[t_off+X], m->trans[t_off+Y], m->trans[t_off+Z]);
			m = m->next;
		}
		if (m!= NULL) {
			fprintf(fout, "%2.3lf, %2.3lf, %2.3lf\n", 
				m->trans[t_off+X], m->trans[t_off+Y], m->trans[t_off+Z]);
		}

		t_off += 3;
		ktime += frame_time;
	}


	fclose(fout);

	return SUCCESS;
}