#include "MotionCapturer.h"
#include "vector.h"
#include "viewer.h"

extern bool CLOSE_TO_ZERO( double a);
extern void rotToQuat(double *q, double aDeg, double x, double y, double z);
extern void get_lc_pos_from_q(double v4_pos[4], Bone *bone, double q[4]);

void init_dist_root2node(Node *node, double *qptr_curframe);

#define twistbend 1

static bool begins(const char* str1, const char* str2)
{
  const char* a = str1;
  const char* b = str2;

  // look at each character until we get to the end of B
  while(*b) {
	if (! *a) return false;		// A ends too early?
	if (toupper(*a) != toupper(*b)) return false;
	a++, b++;
  }
  return true;
}


// connect joints w/ bones recursively
void make_skeleton(Node* node)
{
  Node *child;
  Bone *bone;
  if (node == NULL) return;

  if (node->type == ROOT) {		// special bone for root
    bone = (Bone *)malloc(sizeof (Bone));
    memset(bone, 0, sizeof(Bone));
    bone->pnode = NULL;		// no parent
    bone->cnode = node;
    node->bone = bone;
  }

  for (int i=0; i<node->children_cnt; i++) {

    child = node->children[i];
    bone = (Bone *)malloc(sizeof (Bone));
    memset(bone, 0, sizeof(Bone));
    bone->pnode = node;
    bone->cnode = child;
    child->bone = bone;
    
    make_skeleton(node->children[i]);    
  }

}


void init_dist_root2node(Node *node, double *qptr_curframe)
{
	
	double v4_parent2node[4];

	double *parent_q;

	if (node->type != ROOT) {	
		parent_q = qptr_curframe + node->parent->q_off;

		node->bone->len = v3_magnitude(node->offset);

		node->dist_root2node_max = node->parent->dist_root2node_max + node->bone->len;
	
		get_lc_pos_from_q(v4_parent2node, node->bone, parent_q);	// the vec from parent to this node

		v3_add(node->parent->v4_dist_root2node_nat, v4_parent2node, node->v4_dist_root2node_nat);

		node->dist_root2node_nat = v3_magnitude(node->v4_dist_root2node_nat);	
/*
		printf(" %s to root = %2.2lf, max %2.2lf, <%2.2lf, %2.2lf, %2.2lf>\n",
			node->name, node->dist_root2node_nat, node->dist_root2node_max, 
			node->v4_dist_root2node_nat[X], node->v4_dist_root2node_nat[Y], node->v4_dist_root2node_nat[Z]);
*/	}

	for (int i=0; i<node->children_cnt; i++) 
		init_dist_root2node(node->children[i], qptr_curframe);
}


/* in case we don't have flex file, use default really flexible joint setting
  for each bone:  parent ctrl joint type, 
  child contains pos vects(nautual, central) and ang lim */
void Gl_Win::init_flex(MotionCapturer *cap)
{

  Node *node;
  Bone *bone;
  
  // init all dist from root to each joint
  init_dist_root2node(root, cur_qptr);

  for (unsigned int i = 1; i < cap->pickname_cur; i++) {
    node = cap->pickarr[i];
    bone = node->bone;

	bone->joint_type = J_SOCK;
    bone->ang_twist_lim = 0;	// default no twist joint
	bone->ang_twist = 0;
    
    if (node->parent == NULL) {
      bone->joint_type = J_NONE; // special bone, to be able to move root
      continue;
    }

    if (strcmp(node->parent->name, "Hips")==0) {	  
      bone->joint_type = J_SOCK;
      if (strcmp(node->name, "LeftHip") == 0) {
		v4_assign(bone->v4_natural,  3.43, 0, 0, 1);
		v4_assign(bone->v4_central,  1, 0, 0, 1);
		bone->j_ang_lim = 10;
	
      } else if (strcmp(node->name, "RightHip") == 0) {
		v4_assign(bone->v4_natural,  -3.43, 0, 0, 1);
		v4_assign(bone->v4_central,  -1, 0, 0, 1);
		bone->j_ang_lim = 10;
      } else if (strcmp(node->name, "Chest") == 0) {
		v4_assign(bone->v4_natural,  0, 1, 0, 1);
		v4_assign(bone->v4_central,  0, 1, 1, 1);
		bone->j_ang_lim = 60;
		bone->ang_twist_lim = 20;
      }	
    }

	// the middle branch
	if (strcmp(node->parent->name, "Chest")==0) {	
		if (strcmp(node->name, "Neck")==0) {
			v4_assign(bone->v4_natural, 0, 1, 0, 1);
			v4_assign(bone->v4_central, 0, 1, 0.7, 1);
			bone->j_ang_lim = 60;		
		} 
		else if (strcmp(node->name, "RightCollar") == 0) {			  
		  v4_assign(bone->v4_natural, -1.06, 15.33, 1.76, 1); // some tiny close to vertical angle
		  v4_assign(bone->v4_central, -1.06, 15.33, 1.76, 1);
		  bone->j_ang_lim = 5;
		}
		else if (strcmp(node->name, "LeftCollar") == 0) {			  
		  v4_assign(bone->v4_natural, 1.06, 15.33, 1.76, 1); // some tiny close to vertical angle
		  v4_assign(bone->v4_central, 1.06, 15.33, 1.76, 1);
		  bone->j_ang_lim = 5;		  
		}
	} else if (begins(node->parent->name, "Neck")) {
		if (strcmp(node->name, "Head")==0) {			
			v4_assign(bone->v4_natural, 0, 1, 0, 1);
			v4_assign(bone->v4_central, 0, 1, 0, 1);
			bone->j_ang_lim = 40;
			bone->ang_twist_lim = 80;
		}		
	} else if (begins(node->parent->name, "Head")) {
		if (strcmp(node->name, "Head_EndSite")==0) {
			v4_assign(bone->v4_natural, 0, 1, 0, 1);
			v4_assign(bone->v4_central, 0, 1, 0, 1);
			bone->j_ang_lim = 30;
		}
	}


	else if (begins(node->name, "Left")) {
      
      if (strcmp(node->parent->name, "LeftHip") == 0) {
		if (strcmp(node->name, "LeftKnee") == 0) {
		  v4_assign(bone->v4_natural,  0.258819, -0.965926, 0.08748866, 1);	// 15 deg outx and 5 frontz
		  v4_assign(bone->v4_central,  1, -1, 1, 1); // upper leg = 45 deg up and out
		  bone->j_ang_lim = 60;
#ifdef twistbened
		  bone->ang_twist_lim = 150;
#endif
		}
	
      }  else if (strcmp(node->parent->name, "LeftKnee") == 0) {	  
		if (strcmp(node->name, "LeftAnkle") == 0) {
			bone->joint_type = J_BEND;	  
		  v4_assign(bone->v4_natural,  0, -1, -0.1, 1);
		  v4_assign(bone->v4_central,  0, -0.173648178, -0.984807753, 1); // lower leg  bend back 80
		  bone->j_ang_lim = 80;		  
		}
	  
      } else if (strcmp(node->parent->name, "LeftAnkle") == 0) {

		bone->ang_twist_lim = 5;
		if (strcmp(node->name, "LeftHeel") == 0) {
		  v4_assign(bone->v4_natural,  0, -3.196399, 0, 1);
		  v4_assign(bone->v4_central,  0, -0.984808, -0.173648, 1); // heel 10 deg back
		  bone->j_ang_lim = 20;
		} 
		else if (strcmp(node->name, "LeftAnkle_EndSite") == 0) {
					// is heel
		  v4_assign(bone->v4_natural,  0, -3.250000, 0, 1);
		  v4_assign(bone->v4_central,  0, -0.984808, -0.173648, 1); // 10 deg back
		  bone->j_ang_lim = 20;
		}
	  
      } else if (strcmp(node->parent->name, "LeftHeel") == 0) {	  
		if (strcmp(node->name, "LeftHeel_EndSite") == 0) {
		  // toe
			bone->joint_type = J_BEND;
		  v4_assign(bone->v4_natural,  0, 0, 9.64661, 1);
		  v4_assign(bone->v4_central,  0,  -1, 0.707107, 1); // = 45 deg up
		  bone->j_ang_lim = 45;
		}
      } 
	  
	  else if (strcmp(node->parent->name, "LeftCollar") == 0) {
		if (strcmp(node->name, "LeftShoulder") == 0) {
		  v4_assign(bone->v4_natural, 1, 0, 0, 1);
		  v4_assign(bone->v4_central, 1, 0, 0, 1);
		  bone->j_ang_lim = 10;
		}
	  } else if (strcmp(node->parent->name, "LeftShoulder") == 0) {		
		if (strcmp(node->name, "LeftElbow") == 0) {
		  bone->joint_type = J_SOCK; 
		  v4_assign(bone->v4_natural, 0, -1, 0, 1);
		  v4_assign(bone->v4_central, 1, 0, 1, 1); // horiz
		  bone->j_ang_lim = 135;
#ifdef twistbened
		  bone->ang_twist_lim = 90;
#endif
		}
      } else if (strcmp(node->parent->name, "LeftElbow") == 0) {
		if (strcmp(node->name, "LeftWrist") == 0) {
		  bone->joint_type = J_BEND; 
		  v4_assign(bone->v4_natural, 0, -1, 0, 1);
		  v4_assign(bone->v4_central, -1, 0, 0, 1); // horiz
		  bone->j_ang_lim = 90;
		}
      } else if (strcmp(node->parent->name, "LeftWrist") == 0) {
		if (strcmp(node->name, "LeftWrist_EndSite") == 0) {
		  bone->joint_type = J_SOCK; 
		  v4_assign(bone->v4_natural, 0, -1, 0, 1);
		  v4_assign(bone->v4_central, -0.2, -1, 0, 1); // down and slightly in
		  bone->j_ang_lim = 120;
		  bone->ang_twist_lim = 80;
		}
      }
    }       // done w/ left side

    else if (begins(node->name, "Right")) {
      
		if (strcmp(node->parent->name, "RightHip") == 0) {
		bone->joint_type = J_SOCK;	  
		if (strcmp(node->name, "RightKnee") == 0) {
		  v4_assign(bone->v4_natural,  -0.258819, -0.965926, 0.08748866, 1);	// 15 deg outx and 5 frontz
		  v4_assign(bone->v4_central,  -1, -1, 1, 1); // upper leg = 45 deg up and out
		  bone->j_ang_lim = 90;	  
		}
		
      }  else if (strcmp(node->parent->name, "RightKnee") == 0) {	  
		bone->joint_type = J_BEND;
		if (strcmp(node->name, "RightAnkle") == 0) {
		  v4_assign(bone->v4_natural,  0, -1, -0.1, 1);
		  v4_assign(bone->v4_central,  0, -0.173648178, -0.984807753, 1); // lower leg  bend back 80
		  bone->j_ang_lim = 80;
#ifdef twistbened
		  bone->ang_twist_lim = 90;
#endif
		}
		
      } else if (strcmp(node->parent->name, "RightAnkle") == 0) {
		bone->joint_type = J_SOCK;
		bone->ang_twist_lim = 5;
		if (strcmp(node->name, "RightHeel") == 0) {
		  v4_assign(bone->v4_natural,  0, -3.196399, 0, 1);
		  v4_assign(bone->v4_central,  0, -0.984808, -0.173648, 1); // heel 10 deg back
		  bone->j_ang_lim = 20;
		} else if (strcmp(node->name, "RightAnkle_EndSite") == 0) {
					// is heel
		  v4_assign(bone->v4_natural,  0, -3.250000, 0, 1);
		  v4_assign(bone->v4_central,  0, -0.984808, -0.173648, 1); // 10 deg back
		  bone->j_ang_lim = 20;
		}
	
      }else if (strcmp(node->parent->name, "RightHeel") == 0) {	  
		bone->joint_type = J_BEND;	  
		if (strcmp(node->name, "RightHeel_EndSite") == 0) {
		  // toe
		  v4_assign(bone->v4_natural,  0, 0, 9.64661, 1);
		  v4_assign(bone->v4_central,  0,  -1, 0.707107, 1); // = 45 deg up
		  bone->j_ang_lim = 45;
		}
      }

	  else if (strcmp(node->parent->name, "RightCollar") == 0) {
		if (strcmp(node->name, "RightShoulder") == 0) {
		  bone->joint_type = J_SOCK; 
		  v4_assign(bone->v4_natural, -1, 0, 0, 1);
		  v4_assign(bone->v4_central, -1, 0, 0, 1);
		  bone->j_ang_lim = 10;
		}
      } else if (strcmp(node->parent->name, "RightShoulder") == 0) {
		if (strcmp(node->name, "RightElbow") == 0) {
		  bone->joint_type = J_SOCK; 
		  v4_assign(bone->v4_natural, 0, -1, 0, 1);
		  v4_assign(bone->v4_central, -1, 0, 0, 1); // horiz
		  bone->j_ang_lim = 90;
#ifdef twistbened
		  bone->ang_twist_lim = 90;
#endif
		}
      } else if (strcmp(node->parent->name, "RightElbow") == 0) {
		if (strcmp(node->name, "RightWrist") == 0) {
		  bone->joint_type = J_BEND; 
		  v4_assign(bone->v4_natural, 0, -1, 0, 1);
		  v4_assign(bone->v4_central, -1, 0, 0, 1); // horiz
		  bone->j_ang_lim = 90;
		}
      } else if (strcmp(node->parent->name, "RightWrist") == 0) {
		if (strcmp(node->name, "RightWrist_EndSite") == 0) {
		  bone->joint_type = J_SOCK; 
		  v4_assign(bone->v4_natural, 0, -1, 0, 1);
		  v4_assign(bone->v4_central, 0.2, -1, 0, 1); // down and slightly in
		  bone->j_ang_lim = 120;
		  bone->ang_twist_lim = 80;
		}
      }

    } // if right side

	v3_normalize(bone->v4_central, bone->v4_central);
	v3_normalize(bone->v4_natural, bone->v4_natural);

	// find hte quat to go from natural pos to central
	//calc_q_v1tov2(bone->q_nat2cen, bone->v4_natural, bone->v4_central);

  } // for
  
}



// not used
// set natural pos, etc
void calc_bone_central(Bone *bone)
{
  Node *pn = bone->pnode;
  Node *cn = bone->cnode;
  Node *ppn = pn->parent;
  Bone *pb = bone->pnode->bone;
  
  double v3_zero[3] = {0, 0, 0};
  double v3_pb[3]; // parent bone vec
  double *v3_b;  // this bone vec
  double rot_axis[3];
  double model_mat[MAT44_SIZE];

  // assuming not rotation, this conv parent bone to loc coord
  v3_sub(v3_zero, pb->v4_natural, v3_pb);
  v3_b = bone->v4_natural;

  v3_crossp(v3_pb, v3_b, rot_axis);

  if (rot_axis[X] == 0 && rot_axis[Y] == 0 && rot_axis[Z] == 0 ) {
    // central vec in dir of natural
    v4_assign(bone->v4_central, bone->v4_natural);
  }
  else {
    glPushMatrix();
    glRotated(bone->jAxis_ang, rot_axis[X], rot_axis[Y], rot_axis[Z]);
    
    glGetDoublev(GL_MODELVIEW_MATRIX, model_mat);
    glmat44_mult_vec41(model_mat, bone->v4_natural, bone->v4_central);
    
    glPopMatrix();
  }

}