// ASF.cp
//
// parses BVH .ASF files to form articulation skeleton

//    R. Parent, August 2002
//

//=====================================================
#include "ASF.h"
#include <fstream.h>
#include <iomanip.h>
#include <iostream.h>
#include <stdlib.h>
#include <cstdlib>
#include "Point3.h"

// =============================================================
// GET ASF
int ASFskeleton:: getASF()
{
  char     string[100];
  char     letter;
  char     inputFilename[100];
  char     outputFilename[100];
  Bone     *newbone,*bptr;
  int      i;
  char	   command[100];
  
  strcpy(inputFilename,"abcASF.txt");

  ifstream inputFile(inputFilename, ios::in);
  if (!inputFile) {
  	cerr << "ERROR opening file: " << inputFilename << endl;
  	return -1;
  } 

  
  // --------------------------------------------------------
  // process ASF file
  else {
  	cout << "open ASF file successful..";
	bones = NULL;
  
	// -----------------------------------
	// find line that contains 'units'
	inputFile >> string;
    while (strcmp(string,":units")) {
		inputFile >> string;
    }

	// -----------------------------------
    // skip down to DOCUMENTATION line
    while (strcmp(string,":documentation")) {
		inputFile >> string;
    }
    
	// -----------------------------------
    // skip down to ROOT line
    while (strcmp(string,":root")) {
		inputFile >> string;
    }
	inputFile >> string;


	// -----------------------------------
    // skip down to BONEDATA line
    while (strcmp(string,":bonedata")) {
		inputFile >> string;
    }

    // found stuff to here at least
    cout << "process bones...";

	// -----------------------------------
    // PROCESS BONES
    numberOfBones = 0;
    while ( (inputFile >> string)         && 
	        (strcmp(string,":hierarchy"))    ) {

		if (!(strcmp(string,"begin"))) {      // new bone
			numberOfBones++;
			newbone = new Bone;
			strcpy(newbone->name,"");
			newbone->length = 1.0;
			newbone->direction[0] = 1.0;
			newbone->direction[1] = 1.0;
			newbone->direction[2] = 1.0;
			// initialize dofs
			strcpy(newbone->dof[0],"NULL");
			strcpy(newbone->dof[1],"NULL");
			strcpy(newbone->dof[2],"NULL");
			newbone->limit[0][0] = -360.0; newbone->limit[0][1] = 360.0; 
			newbone->limit[1][0] = -360.0; newbone->limit[1][1] = 360.0; 
			newbone->limit[2][0] = -360.0; newbone->limit[2][1] = 360.0; 
			// add to sequential list of bones
			newbone->next = bones;
			bones = newbone;
			// init outside of hierarchy
			newbone->child = NULL;
			newbone->sibling = NULL;
			// init global info
			newbone->position[0] = 0.0;
			newbone->position[1] = 0.0;
			newbone->position[2] = 0.0;
			newbone->tucs.setIdentity();
			newbone->numberOfDOFs = 0;
	        newbone->dofValues[0] = NULL;
	        newbone->dofValues[1] = NULL;
	        newbone->dofValues[2] = NULL;

			// process BONE attribute-value pairs
			while ( (inputFile >> string)   &&
			        (strcmp(string,"end"))     ) {
			        
				// ID
				if (!(strcmp(string,"id"))) {
					inputFile >> newbone->id;
				}

				// NAME
				if (!(strcmp(string,"name"))) {
					inputFile >> newbone->name;

				}
			  
			 	// DIRECTION
				if (!(strcmp(string,"direction"))) {
					inputFile >> newbone->direction[0];
					inputFile >> newbone->direction[1];
					inputFile >> newbone->direction[2];
				}
				
				// LENGTH
				if (!(strcmp(string,"length"))) {
					inputFile >> newbone->length;
				}

				// AXIS
				if (!(strcmp(string,"axis"))) {
					inputFile >> newbone->orientation[0];
					inputFile >> newbone->orientation[1];
					inputFile >> newbone->orientation[2];
					inputFile >> newbone->axisOrder;
				}
			  
				// DOFs
				if (!(strcmp(string,"dof"))) {        // got "dof"
					char  c;
					
					i = 0;
					c = inputFile.get();
					while ( (c != '\r') && (c != '\n') && (i<80) && (c != EOF) ) {
						string[i++] = c;
						c = inputFile.get();
					}
					string[i] = '\0';
					newbone->numberOfDOFs = sscanf(string,"%s %s %s", newbone->dof[0],newbone->dof[1],newbone->dof[2]);
				}

				// LIMITS
				// assumes if any limits are given, then all limits are given
				if (!(strcmp(string,"limits"))) {        // got "dof"
					char c;
					for (i=0; i<newbone->numberOfDOFs; i++) {
						while ( (c = inputFile.get()) != '(') ;
						inputFile >> newbone->limit[i][0] >> newbone->limit[i][1];
					}
					c = inputFile.get();
					while ( (c != '\r') && (c != '\n') && (i<80) && (c != EOF) ) {
						c = inputFile.get();
					}
				}
			}
			
			// END of this bone - clean up
			if (!(strcmp(string,"end"))) {
				// cout << "end" << endl;
			}
			
		}
	        
    }
    
    cout << "..done processing bones...";

    cout << "processing hierarchy...";
    
    
	// -----------------------------------
    // PROCESS HIERARCHY
    if (!(strcmp(string,":hierarchy"))) {
		Bone    *pptr,*cptr,*sptr;
		char     parent[20];
		char     child[20];
		char     rest[80];
		char     line[80];
      
		// search for 'begin'
		while ( (inputFile >> string)    &&
		        (strcmp(string,"begin"))    ) { ;}
		        
      
		// get 'parent' joint
		while ( (inputFile >> parent)   &&
		        (strcmp(parent,"end"))     ) { // not 'end'
	
			sptr = NULL;
			if (strcmp(parent,"root")) {		// if not root, search
				pptr = findBone(parent);
  			}
  			else pptr = NULL;
  			// if parent pointer is set or parent is 'root', continue
  			if ( (pptr != NULL) || (!(strcmp(parent,"root"))) ) { 

  				// get rest of line into 'line'
  				char c;
				i = 0;
				c = inputFile.get();
				while ( (c != '\r') && (c != '\n') && (i<80) && (c != EOF) ) {
					line[i++] = c;
					c = inputFile.get();
				}
				line[i] = '\0';
				if (pptr != NULL) {
					if (pptr->child != NULL) {
						sptr = pptr->child;
						while (sptr->sibling != NULL) sptr = sptr->sibling;
					}
				}
  				splitString(line,child,rest);
  				while (splitString(line,child,rest) > 0) {
					if ((cptr = findBone(child)) != NULL) {   // found the bone
						if (sptr == NULL) {            // if sibling pointer is NULL
		  					if (!(strcmp(parent,"root"))) {
								root.child = cptr;
							}
							else {
								pptr->child = cptr;
							}
						}
						else {
							sptr->sibling = cptr;
						}
						sptr = cptr;
					}
					else {
						return -1;
					}
					// copy rest to line
					i = 0;
					while (rest[i] != '\0') {line[i] = rest[i]; i++;}
					line[i] = '\0';

		 		}
			}
			
		} // end found
    } // end of hierarchy
    
    cout << "...done" << endl;
    return 1;
  }
}

// =============================================================
// GET AMC
int ASFskeleton:: getAMC()
{
  char     line[100];
  char     name[30];
  Bone    *newbone;
  int      frame,dummy;
  float    tx,ty,tz,rx,ry,rz;
  int      count;
  Bone *bptr;
  int      i;
  float    dof[3];
    char     inputFilename[100];


  strcpy(inputFilename,"inputAMC.txt");

  ifstream inputFile(inputFilename, ios::in);
  if (!inputFile) {
  	cerr << "ERROR opening file: " << inputFilename << endl;
  	return -1;
  } 

  
  // --------------------------------------------------------
  // process AMC file
  else {
	char  c;

	cout << endl;
  	cout << "open input file successful..";

    numberOfFrames = 0;
	
	// get the next line
	i = 0;
	c = inputFile.get();
	while ( (c != '\r') && (c != '\n') && (i<80) && (c != EOF) ) {
		line[i++] = c;
		c = inputFile.get();
	}
	line[i] = '\0';

    while (c != EOF) {
      if (sscanf(line,"%d",&frame)==1) numberOfFrames++;
		// get the next line
		i = 0;
		c = inputFile.get();
		while ( (c != '\r') && (c != '\n') && (i<80) && (c != EOF) ) {
			line[i++] = c;
			c = inputFile.get();
		}
		line[i] = '\0';
    }
    cout << "number of Frames: " << numberOfFrames;

	inputFile.close();
	ifstream inputFile(inputFilename, ios::in);
	
    // -------------------------
    // go through all bones and allocate float per frame for each DOF

    // first do the root
    root.position[0] = new float[numberOfFrames];
    root.position[1] = new float[numberOfFrames];
    root.position[2] = new float[numberOfFrames];
    root.orientation[0] = new float[numberOfFrames];
    root.orientation[1] = new float[numberOfFrames];
    root.orientation[2] = new float[numberOfFrames];

    // now do the bones
    bptr = bones;
    while (bptr != NULL) {
		for (i=0; i<bptr->numberOfDOFs; i++) {
			bptr->dofValues[i] = new float[numberOfFrames];
		}
		bptr = bptr->next;
    }
    
    // -------------------------
    // go through frame info and stick into DOFs

    frame = 0;
    
	// get the first line
	i = 0;
	c = inputFile.get();
	while ( (c != '\r') && (c != '\n') && (i<80) && (c != EOF) ) {
		line[i++] = c;
		c = inputFile.get();
	}
	line[i] = '\0';
	
	while (!inputFile.eof()) {

		// get lines until frame line is read in
	    while ((sscanf(line,"%d",&frame)!=1)) {
			// get the next line
			i = 0;
			c = inputFile.get();
			while ( (c != '\r') && (c != '\n') && (i<80) && (c != EOF) ) {
				line[i++] = c;
				c = inputFile.get();
			}
			line[i] = '\0';
	    }
	    
	    
		// get the next line
		i = 0;
		c = inputFile.get();
		while ( (c != '\r') && (c != '\n') && (i<80) && (c != EOF) ) {
			line[i++] = c;
			c = inputFile.get();
		}
		line[i] = '\0';
		while ((sscanf(line,"%d",&dummy) == 0)) {
			sscanf(line,"%s",name);
			if (!strcmp(name,"root")) { // DOF for root node
				sscanf(line,"%s %f %f %f %f %f %f",name,&tx,&ty,&tz,&rx,&ry,&rz);
				root.position[0][frame-1] = tx;
				root.position[1][frame-1] = ty;
				root.position[2][frame-1] = tz;
				root.orientation[0][frame-1] = rx;
				root.orientation[1][frame-1] = ry;
				root.orientation[2][frame-1] = rz;
			}
			else { // DOF for other node
				bptr = findBone(name);
				if (bptr == NULL) {
					cout << "ERROR in DOFs: bone '" << name << "' not found" << endl;
				}
				else {
					count = bptr->numberOfDOFs;
					if (count == 3) {
						sscanf(line,"%s %f %f %f",name,&rx,&ry,&rz);
						bptr->dofValues[0][frame-1] = rx;
						bptr->dofValues[1][frame-1] = ry;
						bptr->dofValues[2][frame-1] = rz;
					}
					else if (count == 2) {
						sscanf(line,"%s %f %f",name,&rx,&ry);
						bptr->dofValues[0][frame-1] = rx;
						bptr->dofValues[1][frame-1] = ry;
					}
					else if (count == 1) {
						sscanf(line,"%s %f",name,&rx);
						bptr->dofValues[0][frame-1] = rx;
					}
				}
			}
			// get the next line
			i = 0;
			c = inputFile.get();
			while ( (c != '\r') && (c != '\n') && (i<80) && (c != EOF) ) {
				line[i++] = c;
				c = inputFile.get();
			}
			line[i] = '\0';
		}
	}

     
    cout << "..number of Frames: " << numberOfFrames << endl;
    

  }
}

// =================================================================
// FIND BONE
Bone * ASFskeleton:: findBone(char *name) 
{
  Bone  *bptr;

  bptr = bones;

  while ( (bptr != NULL) && (strcmp(bptr->name,name)) ) {
    bptr = bptr->next;
  }

  return bptr;  
}

// =================================================================
// GET NUMBER OF BONES
int ASFskeleton:: getNumberOfBones() 
{
	return numberOfBones;
}

// =================================================================
// DRAW STICK FIGURE
void ASFskeleton:: drawStickFigure(int frame,int inplace)
{
	Point3		P;
	Matrix		tucs;
	Matrix		M;
	Bone		*ptr;
	
	while (frame > numberOfFrames) frame -= numberOfFrames; 

	if ( (frame >= 1) && (frame <= numberOfFrames) ) {

		// initialize unit coordinate system
		tucs.setIdentity();

		// translate root position;
		if (inplace == 0) {
			// rotate unit coordinate system according to root orientation
			M.formXrotMatrix(root.orientation[0][frame-1]);
			tucs.matrixPreMultiply(M);
			M.formYrotMatrix(root.orientation[1][frame-1]);
			tucs.matrixPreMultiply(M);
			M.formZrotMatrix(root.orientation[2][frame-1]);
			tucs.matrixPreMultiply(M);

			P.x = root.position[0][frame-1];
			P.y = root.position[1][frame-1];
			P.z = root.position[2][frame-1];
		}
		else {
			P.x = 0.0;
			P.y = 0.0;
			P.z = 0.0;
		}


		// recursively traverse tree of bones
		if ( (ptr=root.child) != NULL) {
			drawStickSubFigure(1,ptr,P,tucs,frame);
			while ( (ptr=ptr->sibling) != NULL) {
				drawStickSubFigure(1,ptr,P,tucs,frame);
			}
		 }
	}
}


// =================================================================
// DRAW STICK FIGURE
void	ASFskeleton:: drawStickSubFigure(int level, Bone* ptr ,Point3 P ,Matrix tucs,int frame)
{
	int     i;
	Matrix  M,T,TT,Tdof,Torient,TorientInv;
	Point3	PP;

	if (ptr == NULL) return;

	// for rotation matrix from DOFs
	Tdof.setIdentity();
	if (ptr->numberOfDOFs > 0) {
		M.formXrotMatrix(ptr->dofValues[0][frame-1]);
		Tdof.matrixPreMultiply(M);
		if (ptr->numberOfDOFs > 1) {
			M.formYrotMatrix(ptr->dofValues[1][frame-1]);
			Tdof.matrixPreMultiply(M);
			if (ptr->numberOfDOFs > 2) {
				M.formZrotMatrix(ptr->dofValues[2][frame-1]);
				Tdof.matrixPreMultiply(M);
			}
		}
	}

	// form rotation matrix from axis orientation
	Torient.setIdentity();
	M.formXrotMatrix(ptr->orientation[0]);
	Torient.matrixPreMultiply(M);
	M.formYrotMatrix(ptr->orientation[1]);
	Torient.matrixPreMultiply(M);
	M.formZrotMatrix(ptr->orientation[2]);
	Torient.matrixPreMultiply(M);

	// for inverse rotation matrix from axis orientation
	TorientInv.setIdentity();
	M.formZrotMatrix(-ptr->orientation[2]);
	TorientInv.matrixPreMultiply(M);
	M.formYrotMatrix(-ptr->orientation[1]);
	TorientInv.matrixPreMultiply(M);
	M.formXrotMatrix(-ptr->orientation[0]);
	TorientInv.matrixPreMultiply(M);

		
	// transform the 'tucs' and apply to the origin of the next bone
	T.copyMatrix(tucs);
	T.matrixPostMultiply(Torient);
	T.matrixPostMultiply(Tdof);
	T.matrixPostMultiply(TorientInv);

	// translate PP in the local x direction (defined by tucs) for 'length' distance
	PP = P; 
	PP.x += ptr->length*ptr->direction[0]*T.m[0][0];
	PP.x += ptr->length*ptr->direction[1]*T.m[0][1];
	PP.x += ptr->length*ptr->direction[2]*T.m[0][2];

	// translate P in the local y direction (defined by tucs) for 'length' distance
	PP.y += ptr->length*ptr->direction[0]*T.m[1][0];
	PP.y += ptr->length*ptr->direction[1]*T.m[1][1];
	PP.y += ptr->length*ptr->direction[2]*T.m[1][2];

	// translate P in the local z direction (defined by tucs) for 'length' distance
	PP.z += ptr->length*ptr->direction[0]*T.m[2][0];
	PP.z += ptr->length*ptr->direction[1]*T.m[2][1];
	PP.z += ptr->length*ptr->direction[2]*T.m[2][2];

	T.copyMatrix(tucs);
	T.matrixPostMultiply(Torient);
	T.matrixPostMultiply(Tdof);
	T.matrixPostMultiply(TorientInv);
	
	// draw from P to PP
	glBegin(GL_LINES);
		glVertex3f(P.x,P.y,P.z);
		glVertex3f(PP.x,PP.y,PP.z);
	glEnd();


	if ( (ptr=ptr->child) != NULL) {
		drawStickSubFigure(level+1,ptr,PP,T,frame);
		while ( (ptr=ptr->sibling) != NULL) {
			drawStickSubFigure(level+1,ptr,PP,T,frame);
		}
	}
	return;
}



// =================================================================
// SPLIT STRING
int ASFskeleton:: splitString(char *string,char *word,char *rest)
{
  int    i,j,k;
  
  // skip whitespace
  i=0;
  while ( ((string[i] == ' ') || (string[i] == '\t')) && (string[i] != '\0') ) i++;
  
  // copy word to 'word'
  j=0;
  while ( (string[i] != ' ') && (string[i] != '\t') && (string[i] != '\0') ) {
  	word[j++] = string[i++];
  }
  word[j] = '\0';
  
  // copy rest to 'rest'
  k = 0;
  while (string[i] != '\0') rest[k++] = string[i++];
  rest[k] = '\0';
  
  return j;
}