// parseBVH.c
//
// parses BVH .ASF files and .AMC files
// not robust or tested; pretty quick and dirty code
// assumes XYZ order for compound rotations
//
// includes basic matrix manipulation code
// compiled with: gcc parseBHV.c -lm
//
//    R. Parent, May 2002
//

//=====================================================
#include <stdio.h>
#include <math.h>

#define NULL 0
#define PI 3.141596536

//=====================================================
// typedefs

// -------------------------
// matrix structure
typedef float matrix[3][3];

// -------------------------
// bone structure
typedef struct bone_struct {
  int    id;                     // ID
  char   name[30];               // name 
  float  direction[3];           // direction to translate local origin
  float  length;                 // distance to translate local origin
  float  orientation[3];         // angles to rotate local coordinate system
  char   axisOrder[10];          // order of roations (ignored for now)
  int    numberOfDOFs;           // number of DOFs of joint
  char   dof[3][10];             // names of DOFs (ignored for now)
  float  dofValues[3];           // DOF values (from .AMC file)
  float  limit[3][3];            // limits on DOFs
  struct bone_struct   *next;    // linear linked list of bones
  struct bone_struct   *child;   // child of this bone
  struct bone_struct   *sibling; // sibling of this bone
  float  position[3];            // origin of local coordinate system
  matrix tucs;                   // (local) transformed unit coordinate system
} bone_td;

// -------------------------
// root structures
typedef struct root_struct {
  char   order[30];
  char   axisOrder[10];         
  float  position[3];
  float  orientation[3];
  bone_td *child;
} root_td;

// =================================================================
// function prototypes

bone_td *findBone(char *);

void printBones(void);
void printHierarchy(void);
void printSubTree(int, bone_td *,float *,matrix);

void formXrotMatrix(float,matrix);
void formYrotMatrix(float,matrix);
void formZrotMatrix(float,matrix);
void matrixMultiply(matrix,matrix,matrix);
void setIdentity(matrix);
void printMatrix(matrix);
void copyMatrix(matrix,matrix);

int getASF(void);
int getAMC(void);

// =================================================================
// globals
bone_td  *bones;
root_td  root;
FILE    *ASFfile;
int      numberOfBones;

// =============================================================
// =============================================================
// MAIN
main()
{
  if (getASF()) {
    getAMC();   
  }
  exit(0);
}

// =============================================================
// GET AMC
int getAMC()
{
  char     line[100];
  char     filename[100],name[30];
  bone_td *newbone;
  FILE    *AMCfile;
  int      frame;
  float    tx,ty,tz,rx,ry,rz;
  int      count;
  bone_td *bptr;
  int      i;
  float    dof[3];

  printf("input AMC file name: ");
  scanf("%s",filename);

  if ((AMCfile = fopen(filename,"r")) == NULL) {
    printf("ERROR opening file %s - TERMINATING\n",filename);
    return 0;
  } 
  else {

    // skip down to frame line
    fgets(line,80,AMCfile);                     // get a line from AMC file
    while (sscanf(line,"%d",&frame)==0) 
      fgets(line,80,AMCfile);

    while (!feof(AMCfile)) {
      printf("frame %d\n",frame);
      fgets(line,80,AMCfile);
      while ( (!feof(AMCfile)) && (sscanf(line,"%d",&frame)==0) ) {
        sscanf(line,"%s",name);
        if (!(strcmp(name,"root"))) {
          sscanf(line,"%f %f %f %f %f %f",
	       &(root.position[0]),
               &(root.position[1]),
	       &(root.position[2]),
	       &(root.orientation[0]),
	       &(root.orientation[1]),
	       &(root.orientation[2])       );
        }
        else {
	  bptr = findBone(name);
	  // printf("%s (%d)",bptr->name,bptr->numberOfDOFs);
	  sscanf(line,"%*s %f %f %f",&(dof[0]),&(dof[1]),&(dof[2]));
	  for (i=0; i<bptr->numberOfDOFs; i++) {
	    bptr->dofValues[i] = dof[i];
	    // printf(" %f",bptr->dofValues[i]);
	  }
	  // printf("\n");
	}
	fgets(line,80,AMCfile);
      }
      // printHierarchy();      // prints the info for EACH frame (verbose)
      // printf("next frame\n");
    }
    return 1;
  }
}

// =============================================================
// GET ASF
int getASF()
{
  char     string[100];
  char     filename[100];
  bone_td *newbone;

  printf("input ASF file name: ");
  scanf("%s",filename);

  bones = NULL;
  if ((ASFfile = fopen(filename,"r")) == NULL) {
    printf("ERROR opening file %s - TERMINATING\n",filename);
    return 0;
  } 
  else {

    fscanf(ASFfile,"%s",string);
    while (strcmp(string,":units")) {
      fscanf(ASFfile,"%s",string);
    }
    // printf("%s\n",string);

    // skip down to DOCUMENTATION line
    while (strcmp(string,":documentation")) {
      fscanf(ASFfile,"%s",string);
    }
    // printf("%s\n",string);

    // skip down to ROOT line
    while (strcmp(string,":root")) {
      fscanf(ASFfile,"%s",string);
    }
    // printf(":root\n");
    fscanf(ASFfile,"%s",string);

    while ((!(strcmp(string,"axis")))        || 
	 (!(strcmp(string,"order")))       ||
	 (!(strcmp(string,"position")))    || 
	 (!(strcmp(string,"orientation")))    ) {
      if (!(strcmp(string,"axis"))) {
	fgets(string,80,ASFfile);                    // get rest of line
      }
      if (!(strcmp(string,"order"))) {
	fgets(string,80,ASFfile);                    // get rest of line
      }
      if (!(strcmp(string,"position"))) {
	fgets(string,80,ASFfile);                    // get rest of line
      }
      if (!(strcmp(string,"orientation"))) {
	fgets(string,80,ASFfile);                    // get rest of line
      }
      fscanf(ASFfile,"%s",string);
    }
    // printf("%s\n",string);

    // skip down to BONEDATA line
    while (strcmp(string,":bonedata")) {
      fscanf(ASFfile,"%s",string);
    }
    // printf("%s\n",string);            // print 'bone' line

    // PROCESS BONES
    numberOfBones = 0;
    while ( (EOF != fscanf(ASFfile,"%s",string)) && 
	  (strcmp(string,":hierarchy"))            ) {

      if (!(strcmp(string,"begin"))) {      // new bone
	numberOfBones++;
	newbone = (bone_td *)malloc(sizeof(bone_td));
	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[0][0] = 1.0;
	newbone->tucs[0][1] = 0.0;
	newbone->tucs[0][2] = 0.0;
	newbone->tucs[1][0] = 0.0;
	newbone->tucs[1][1] = 1.0;
	newbone->tucs[1][2] = 0.0;
	newbone->tucs[2][0] = 0.0;
	newbone->tucs[2][1] = 0.0;
	newbone->tucs[2][2] = 1.0;
	newbone->numberOfDOFs = 0;
        newbone->dofValues[0] = 0.0;
        newbone->dofValues[1] = 0.0;
        newbone->dofValues[2] = 0.0;
	
	while ( (EOF != fscanf(ASFfile,"%s",string)) &&
		(strcmp(string,"end"))                  ) {
	  if (!(strcmp(string,"id"))) {
	    fscanf(ASFfile,"%d",&(newbone->id));
	  }
	  
	  if (!(strcmp(string,"name"))) {
	    fscanf(ASFfile,"%s",newbone->name);
	  }
	  
	  if (!(strcmp(string,"direction"))) {
	    fscanf(ASFfile,"%f %f %f",
		   &(newbone->direction[0]),
		   &(newbone->direction[1]),
		   &(newbone->direction[2]));
	  }
	  
	  if (!(strcmp(string,"length"))) {
	    fscanf(ASFfile,"%f",&(newbone->length));
	  }
	  
	  if (!(strcmp(string,"axis"))) {
	    fscanf(ASFfile,"%f %f %f",
		   &(newbone->orientation[0]),
		   &(newbone->orientation[1]),
		   &(newbone->orientation[2]));
	    fscanf(ASFfile,"%s",newbone->axisOrder);
	  }
	  
	  if (!(strcmp(string,"dof"))) {        // got "dof"
	    fgets(string,80,ASFfile);
	    sscanf(string,"%s %s %s",
		   newbone->dof[0],newbone->dof[1],newbone->dof[2]);
	  }
	  
	  if (!(strcmp(string,"limits"))) {        // got "dof"
	    if (strcmp(newbone->dof[0],"NULL")) {
	      newbone->numberOfDOFs = 1;
	      fscanf(ASFfile," (%f %f)",
		     &(newbone->limit[0][0]),&(newbone->limit[0][1]));
	      if (strcmp(newbone->dof[1],"NULL")) {
		newbone->numberOfDOFs = 2;
		fscanf(ASFfile," (%f %f)",
		       &(newbone->limit[1][0]),&(newbone->limit[1][1]));
		if (strcmp(newbone->dof[2],"NULL")) {
		  newbone->numberOfDOFs = 3;
		  fscanf(ASFfile," (%f %f)",
			 &(newbone->limit[2][0]),&(newbone->limit[2][1]));
		  
		}
	      }
	    }
	  }
	}
	
	if (!(strcmp(string,"end"))) {        // finished with bone
	  // clean up bone
	  printf("\nbone id: %d\n",newbone->id);
	  printf("name: %s\n",newbone->name);
	  printf("direction: %f %f %f\n",
		 newbone->direction[0],
		 newbone->direction[1],
		 newbone->direction[2] );
	  /*
	    printf("magnitude: %f\n",sqrt(
	    newbone->direction[0]*newbone->direction[0]+
	    newbone->direction[1]*newbone->direction[1]+
	    newbone->direction[2]*newbone->direction[2]));
	  */
	  printf("length: %f\n",newbone->length);
	  printf("axis: %f %f %f\n",
		 newbone->orientation[0],
		 newbone->orientation[1],
		 newbone->orientation[2]);
	  printf("axis order: %s\n",newbone->axisOrder);
	  if (strcmp(newbone->dof[0],"NULL")) {
	    printf("DOFs\n   %s: (%f:%f)\n",
		   newbone->dof[0],newbone->limit[0][0],newbone->limit[0][1]);
	  }
	  if (strcmp(newbone->dof[1],"NULL")) {
	    printf("   %s: (%f:%f)\n",
		   newbone->dof[2],newbone->limit[1][0],newbone->limit[1][1]);
	  }
	  if (strcmp(newbone->dof[2],"NULL")) {
	    printf("   %s: (%f:%f)\n",
		   newbone->dof[2],newbone->limit[2][0],newbone->limit[2][1]);
	  }
	}
      }
      
    }
    
    if (!(strcmp(string,":hierarchy"))) {   // found HIERARCHY
      bone_td  *pptr,*cptr,*sptr;
      char     parent[20];
      char     child[20];
      char     rest[80];
      char     line[80];
      
      while ( (EOF != fscanf(ASFfile,"%s",string)) &&
	      (strcmp(string,"begin"))               ) { ;}
      // printf("hierarchy \n");
      
      
      while ( (EOF != fscanf(ASFfile,"%s",parent)) &&
	      (strcmp(string,"end"))                  ) { // not 'end'
	
	// printf("  parent:%s\n",parent);
	sptr = NULL;
	// scanf("%*d");
	if (strcmp(parent,"NULL")) {                 // found a link
	  if (strcmp(parent,"root")) {               // if not root, search
	    // printf("look for parent bone: %s\n",parent);
	    pptr = findBone(parent);
	  }
	  if ( (pptr != NULL) || (!(strcmp(parent,"root"))) ) {   
	    // printf("  found parent bone: %s\n",parent);
	    
	    // printf("  look for children\n");
	    while (fgetword(child) != 0) {
	      // printf("  found child name:%s;\n",child);
	      // scanf("%*d");
	      
	      // printf("  look for child bone: %s\n",child);
	      if ((cptr = findBone(child)) != NULL) {   // found the bone
		// printf("  found child bone: %s\n",child);
		// printf("   found:%s:%s:\n",parent,child);
		
		if (sptr == NULL) {
		  if (!(strcmp(parent,"root"))) {
		    root.child = cptr;
		  }
		  else {
		    pptr->child = cptr;
		  }
		}
		else {
		  sptr->sibling = cptr;
		}
		sptr = cptr;
	      }
	      else {
		printf(" ERROR: child bone not found\n");
		exit(0);
	      }
	    }
	    // printf("  no more children\n");
	  }
	  // else printf(" no parent found\n");
	}
	else {
	  // printf(" didn't find parent bone: %s\n",parent);
	}
      } // end found
    } // end of hierarchy
    
    printHierarchy();
    return 1;
  }
}

// =================================================================
// FIND BONE
bone_td *findBone(char *name) {
  bone_td  *bptr;

  // printf("  search nodes for: %s\n",name);
  bptr = bones;

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

  return bptr;  
}



// =================================================================
// PRINT HIERARCHY
void printHierarchy()
{
  bone_td *ptr;
  matrix   tucs,M;
  float    position[3];

  // initialize unit coordinate system
  setIdentity(tucs);
  
  // rotate unit coordinate system according to root orientation
  formXrotMatrix(root.orientation[0],M);
  matrixMultiply(tucs,M,tucs);
  formYrotMatrix(root.orientation[1],M);
  matrixMultiply(tucs,M,tucs);
  formZrotMatrix(root.orientation[2],M);
  matrixMultiply(tucs,M,tucs);

  // translate root position;
  position[0] = root.position[0];
  position[1] = root.position[1];
  position[2] = root.position[2];

  // print position and transformed unit coordinate system
  printf("root\n");
  printf("%f %f %f\n",position[0],position[1],position[2]);
  printMatrix(tucs);

  // recursively print tree of bones
  if ( (ptr=root.child) != NULL) {
    printSubTree(1,ptr,position,tucs);
    while ( (ptr=ptr->sibling) != NULL) {
      printSubTree(1,ptr,position,tucs);
    }
  }
}

// =================================================================
// PRINT SUBTREE
void printSubTree(int level,bone_td *ptr,float *position,matrix tucs)
{
  int     i;
  matrix  M,T,TT;
  float   P[3],PP[3];

  if (ptr == NULL) return;

  // translate bone in bone direction for length of bone according to input tuc
  P[0] = position[0];
  P[1] = position[1];
  P[2] = position[2];

  P[0] += ptr->length*ptr->direction[0]*tucs[0][0];
  P[0] += ptr->length*ptr->direction[0]*tucs[1][0];
  P[0] += ptr->length*ptr->direction[0]*tucs[2][0];

  P[1] += ptr->length*ptr->direction[1]*tucs[0][1];
  P[1] += ptr->length*ptr->direction[1]*tucs[1][1];
  P[1] += ptr->length*ptr->direction[1]*tucs[2][1];

  P[2] += ptr->length*ptr->direction[2]*tucs[0][2];
  P[2] += ptr->length*ptr->direction[2]*tucs[1][2];
  P[2] += ptr->length*ptr->direction[2]*tucs[2][2];

  // rotate the input tuc according to the orientation angles of bone
  formXrotMatrix(ptr->orientation[0],M);
  matrixMultiply(T,M,tucs);
  formYrotMatrix(ptr->orientation[1],M);
  matrixMultiply(T,M,T);
  formZrotMatrix(ptr->orientation[2],M);
  matrixMultiply(T,M,T);

  // rotate the input tuc according to the articulation DOFs
  formXrotMatrix(ptr->dofValues[0],M);
  matrixMultiply(T,M,T);
  if (ptr->numberOfDOFs > 1) {
    formYrotMatrix(ptr->dofValues[1],M);
    matrixMultiply(T,M,T);
    if (ptr->numberOfDOFs > 2) {
      formZrotMatrix(ptr->dofValues[2],M);
      matrixMultiply(T,M,T);
    }
  }


  for (i=0; i<level; i++) printf("  ");
  printf("%s\n",ptr->name);
  printf("%f %f %f\n",P[0],P[1],P[2]);
  printMatrix(T);

  if ( (ptr=ptr->child) != NULL) {
    PP[0] = P[0]; PP[1] = P[1]; PP[2] = P[2];
    copyMatrix(TT,T);
    printSubTree(level+1,ptr,PP,TT);
    while ( (ptr=ptr->sibling) != NULL) {
      PP[0] = P[0]; PP[1] = P[1]; PP[2] = P[2];
      copyMatrix(TT,T);
      printSubTree(level+1,ptr,PP,TT);
    }
  }
}

// =================================================================
// PRINT BONES
void printBones()
{
  bone_td    *bone;

  printf("\nbones:");
  bone = bones;
  while (bone != NULL) {
    printf(" %d:%s",bone->id,bone->name);
    bone = bone->next;
  }

  printf("\n");
}

// =================================================================
// FGETWORD
int fgetword(char *word)
{
  static char   c = '\0';
  int    i;

  if (c == '\n') { c = '\0'; return 0; }

  i = 0;
  // skip over white spaces up until newline
  c = fgetc(ASFfile);
  while( (c != '\n')  && ((c == ' ') || (c == '\t'))  ) 
    c = fgetc(ASFfile);

  // record non-white spaces
  while( (c != '\n')  && (c != ' ') && (c != '\t') ) {
    word[i++] = c;
    c = fgetc(ASFfile);
  }

  word[i] = '\0';
  return i;
}

// =================================================================
// FORM XROT MATRIX
void formXrotMatrix(float degrees,matrix A)
{
  float   angle;

  angle = PI*degrees/180;

  A[0][0] = 1.0; A[0][1] =        0.0; A[0][2] = -0.0;
  A[1][0] = 0.0; A[1][1] = cos(angle); A[1][2] = -sin(angle);
  A[2][0] = 0.0; A[2][1] = sin(angle); A[2][2] =  cos(angle);
}

// =================================================================
// FORM YROT MATRIX
void formYrotMatrix(float degrees,matrix A)
{
  float   angle;

  angle = PI*degrees/180;

  A[0][0] =  cos(angle); A[0][1] = 0.0; A[0][2] =  sin(angle);
  A[1][0] =         0.0; A[1][1] = 1.0; A[1][2] =         0.0;
  A[2][0] = -sin(angle); A[2][1] = 0.0; A[2][2] =  cos(angle);
}

// =================================================================
// FORM ZROT MATRIX
void formZrotMatrix(float degrees,matrix A)
{
  float   angle;

  angle = PI*degrees/180;

  A[0][0] =  cos(angle); A[0][1] = -sin(angle); A[0][2] =  0.0;
  A[1][0] =  sin(angle); A[1][1] =  cos(angle); A[1][2] =  0.0;
  A[2][0] =         0.0; A[2][1] =         0.0; A[2][2] =  1.0;
}

// =================================================================
// SET IDENTITY
void setIdentity(matrix A)
{
  A[0][0] =  1.0; A[0][1] = 0.0; A[0][2] =  0.0;
  A[1][0] =  0.0; A[1][1] = 1.0; A[1][2] =  0.0;
  A[2][0] =  0.0; A[2][1] = 0.0; A[2][2] =  1.0;
}

// =================================================================
// COPY MATRIX A = B
void copyMatrix(matrix A,matrix B)
{
  A[0][0] =  B[0][0]; A[0][1] = B[0][1]; A[0][2] =  B[0][2];
  A[1][0] =  B[1][0]; A[1][1] = B[1][1]; A[1][2] =  B[1][2];
  A[2][0] =  B[2][0]; A[2][1] = B[2][1]; A[2][2] =  B[2][2];
}

// =================================================================
// MATRIX MULTIPLY A = B x C
void matrixMultiply(matrix A, matrix B, matrix C)
{
  int  i,j,k;
  matrix T;

  for (i=0; i<3; i++) {
    for (j=0; j<3; j++) {
      T[i][j] = 0.0;
      for (k=0; k<3; k++) T[i][j] += B[i][k]*C[k][j];
    }
  }

  A[0][0] =  T[0][0]; A[0][1] = T[0][1]; A[0][2] =  T[0][2];
  A[1][0] =  T[1][0]; A[1][1] = T[1][1]; A[1][2] =  T[1][2];
  A[2][0] =  T[2][0]; A[2][1] = T[2][1]; A[2][2] =  T[2][2];
}

// =================================================================
// PRINT MATRIX
void printMatrix(matrix A)
{
  float s[3];
  int   i;

  printf("%f %f %f\n",A[0][0],A[0][1],A[0][2]);
  printf("%f %f %f\n",A[1][0],A[1][1],A[1][2]);
  printf("%f %f %f\n",A[2][0],A[2][1],A[2][2]);

  /*
  for (i=0; i<3; i++) {
    s[i] = sqrt(A[i][0]*A[i][0] + A[i][1]*A[i][1] + A[i][2]*A[i][2]);
  }
  printf("%f %f %f\n",s[0],s[1],s[2]);
  for (i=0; i<3; i++) {
    s[i] = sqrt(A[0][i]*A[0][i] + A[1][i]*A[1][i] + A[2][i]*A[2][i]);
  }
  printf("%f %f %f\n",s[0],s[1],s[2]);
  */
}