#include "VMDTool.h"




using namespace jbxl;
using namespace jbxwl;



/*
VMD_PARENT		( 0)  => none
VMD_CENTER		( 1)  => NI_PELVIS	( 0)
VMD_LOWER		( 2)  => none
VMD_UPPER		( 3)  => NI_TORSO	( 1)	
VMD_UPPER2		( 4)  => NI_CHEST	( 3)
VMD_NECK		( 5)  => NI_NECK	( 4)
VMD_HEAD		( 6)  => NI_HEAD	( 5)
VMD_SKULL		( 7)  => NI_SKULL	( 6)	

VMD_EYES		( 8)  => none
VMD_L_EYE		( 9)  => NI_L_EYE	( 7)	
VMD_R_EYE		(10)  => NI_R_EYE	( 8)
VMD_L_BUST		(11)  => NI_L_BUST  ( 9)
VMD_R_BUST		(12)  => NI_R_BUST  (10)
					
VMD_L_SHLDR		(13)  => NI_L_COLLAR(11)
VMD_L_ARM		(14)  => NI_L_SHLDR (12)
VMD_L_ARM_TW	(15)  => none
VMD_L_ELBOW		(16)  => NI_R_ELBOW (13)
VMD_L_WRIST_TW	(17)  => none
VMD_L_WRIST		(18)  => NI_L_WRIST (14)
VMD_L_HAND		(19)  => NI_R_HAND	(15)
					
VMD_R_SHLDR		(20)  => NI_R_COLLAR(16)
VMD_R_ARM		(21)  => NI_R_SHLDR (17)
VMD_R_ARM_TW	(22)  => none
VMD_R_ELBOW		(23)  => NI_R_ELBOW (18)
VMD_R_WRIST_TW	(24)  => none
VMD_R_WRIST		(25)  => NI_R_WRIST (19)
VMD_R_HAND		(26)  => NI_R_HAND	(20)
					
VMD_L_HIP		(27)  => NI_L_HIP	(21)
VMD_L_KNEE		(28)  => NI_L_KNEE	(22)
VMD_L_ANKLE		(29)  => NI_L_ANKLE (23)
VMD_L_TOE		(30)  => NI_L_FOOT	(24)
					
VMD_R_HIP		(31)  => NI_R_HIP	(26)
VMD_R_KNEE		(32)  => NI_R_KNEE	(27)
VMD_R_ANKLE		(33)  => NI_R_ANKLE (28)
VMD_R_TOE		(34)  => NI_R_FOOT	(29)
*/


static std::string  _VMDJointName[] =		// MMD_MAX_JOINT_NUM
{
	// SJIS
	"全ての親", "センター", 
	"下半身", "上半身", "上半身2", "首", "頭", "頭先", 
	"両目", "左目", "右目", "左胸", "右胸",

	"左肩", "左腕", "左腕捩", "左ひじ", "左手捩", "左手首", "左手先",
	"右肩", "右腕", "右腕捩", "右ひじ", "右手捩", "右手首", "右手先",

	"左足", "左ひざ", "左足首", "左つま先",
	"右足", "右ひざ", "右足首", "右つま先"
};



static std::string  _VMDJointName_eng[] =		// MMD_MAX_JOINT_NUM
{
	"parent", "center", 
	"lower body", "upper body", "upper body2", "neck", "head", "skull", 
	"eyes", "eye_L", "eye_R", "bust_L", "bust_R",

	"shoulder_L", "arm_L", "arm twist_L", "elbow_L", "wrist twist_L", "wrist_L", "c wrist l",
	"shoulder_R", "arm_R", "arm twist_R", "elbow_R", "wrist twist_R", "wrist_R", "c wrist r",

	"leg_L", "knee_L", "ankle_L", "toe_L",
	"leg_R", "knee_R", "ankle_R", "toe_R"
};




std::string  jbxwl::VMDJointName(int n)
{
	std::string str = "";

	if (n>=0 && n<VMD_MAX_JOINT_NUM) {
		str = _VMDJointName[n];
	}

	return str; 
}



int   jbxwl::VMDJointNum(char* name)
{
	for (int i=0; i<VMD_MAX_JOINT_NUM; i++) {
		if (!strcmp(name, _VMDJointName[i].c_str()) || !strcmp(name, _VMDJointName_eng[i].c_str())) {
			return i;
		}
	}
	return -1;
}



//
// VMDのジョイント番号から，共通ジョイントの番号を得る
//
static  int  _VMD2NiJointNum[] =		// VMD_MAX_JOINT_NUM (35)
{
	-1,  0, 
	 2,  1,  3,  4,  5,  6,
	-1,  7,  8,  9, 10,
	11, 12, -1, 13, -1, 14, 15,
	16, 17, -1, 18, -1, 19, 20,
	21, 22, 23, 24,
	26, 27, 28, 29 
};




int  jbxwl::VMD2NiJointNum(int joint)
{
	if (joint<0 || joint>=VMD_MAX_JOINT_NUM) return -1;

	return _VMD2NiJointNum[joint];
}





//////////////////////////////////////////////////////////////////////////////////////////////////////
//
// CVMDTool Class
//

void  CVMDTool::init(void)
{
	memset(&vmd_header, 0, sizeof(VMDFileHeader));
	vmd_motion = NULL;
	vmd_datnum = 0;
	vmd_joints = NULL;
	vmd_frmnum = 0;
	dmy_joints = NULL;
	dmy_frmnum = 0;

	frameData  = frame_data;
	rate_frame = VMD_FARME_RATE/30.;

	A2TPose.setRotation(37./180.*PI, 1.0, 0.0, 0.0, 1.0);
	clearJointsData();
}




void  CVMDTool::clearData(void)
{
	if (vmd_motion!=NULL) ::free(vmd_motion);
	if (vmd_joints!=NULL) ::freeFrameData(vmd_joints, vmd_frmnum);
	if (dmy_joints!=NULL) ::freeFrameData(dmy_joints, dmy_frmnum);

	vmd_motion = NULL;
	vmd_datnum = 0;
	vmd_joints = NULL;
	vmd_frmnum = 0;
	dmy_joints = NULL;
	dmy_frmnum = 0;
}





void  CVMDTool::clearJointsData(double val)
{
//	clearStartPosition();

	for (int i=0; i<VMD_MAX_JOINT_NUM; i++) {
		posVect[i].init(val);
		rotQuat[i].init(val);
	}
}






/////////////////////////////////////////////////////////////////////////////////////
//
//
//


BOOL  CVMDTool::readFile(FILE* fp)
{
	if (fp==NULL) return FALSE;

	vmd_datnum = 0;
	vmd_header = readFileHeader(fp);
	if (vmd_header.data_num==0) return FALSE;

	vmd_motion = readJointsData(fp, vmd_header.data_num);
	if (vmd_motion==NULL) return FALSE;
	vmd_datnum = vmd_header.data_num;

	return TRUE;
}




VMDFileHeader  CVMDTool::readFileHeader(FILE* fp)
{
	VMDFileHeader fhd;

	fread(fhd.header,   30, 1, fp);
	fread(fhd.name,     20, 1, fp);
	fread(&fhd.data_num, 4, 1, fp);
	
	if (strcmp(VMD_FILE_HD_ID2, fhd.header)) {
		fhd.data_num = 0;
	}

	return fhd;
}





VMDJointData  CVMDTool::readJointData(FILE* fp)
{
	VMDJointData frame;

	fread(frame.name,     15, 1, fp);
	fread(&frame.frm_num, 96, 1, fp);

	return frame;
}




VMDJointData*  CVMDTool::readJointsData(FILE* fp, unsigned int& frmnum)
{
	VMDJointData* motion_data = (VMDJointData*)malloc(frmnum*sizeof(VMDJointData));
	if (motion_data==NULL) return NULL;
	memset(motion_data, 0, frmnum*sizeof(VMDJointData));

	//
	unsigned int num = 0;
	while(num<frmnum) {
		motion_data[num] = readJointData(fp);
		if (feof(fp)) break;
		num++;	
	}

	frmnum = num;
	if (frmnum==0) {
		::free(motion_data);
		motion_data = NULL;
	}

	return motion_data;
}





NiFrameData*  CVMDTool::getJointsFrame(void)
{
	//
	if (vmd_joints==NULL) {
		vmd_frmnum = vmd_datnum;
		vmd_joints = convert2FrameData(vmd_motion, vmd_frmnum);
	}

	/**
	if (vmd_joints!=NULL) {
		FILE* fp = fopen("D:\\VMD_Joint1.txt", "w");
		for (unsigned int i=0; i<vmd_frmnum; i++) {
			fprintf(fp, "%d: %d (%d)\n", i, vmd_joints[i].frmn, vmd_joints[i].msec);
			for (int j=0; j<vmd_joints[i].jnum; j++) {
				NiJointData* jdat = vmd_joints[i].jdat;
				fprintf(fp, "=> %d, %d, %s, %f, %f, %f, %f, %f, %f, %f\n", jdat[j].joint,jdat[j].index, VMDJointName(jdat[j].joint).c_str(), 
					jdat[j].vect.x, jdat[j].vect.y, jdat[j].vect.z, jdat[j].quat.x, jdat[j].quat.y, jdat[j].quat.z, jdat[j].quat.s);
			}
		}
		fclose(fp);
	}
	/**/


	calcJointRotation();


	/**
	if (vmd_joints!=NULL) {
		FILE* fp = fopen("D:\\VMD_Joint2.txt", "w");
		for (unsigned int i=0; i<vmd_frmnum; i++) {
			fprintf(fp, "%d: %d (%d)\n", i, vmd_joints[i].frmn, vmd_joints[i].msec);
			for (int j=0; j<vmd_joints[i].jnum; j++) {
				NiJointData* jdat = vmd_joints[i].jdat;
				fprintf(fp, "=> %d, %d, %s, %f, %f, %f, %f, %f, %f, %f\n", jdat[j].joint, jdat[j].index, VMDJointName(jdat[j].joint).c_str(), 
					jdat[j].vect.x, jdat[j].vect.y, jdat[j].vect.z, jdat[j].quat.x, jdat[j].quat.y, jdat[j].quat.z, jdat[j].quat.s);
			}
		}
		fclose(fp);
	}
	/**/

	// ジョイント番号の書き換え
	for (unsigned int i=0; i<vmd_frmnum; i++) {
		//
		for (int j=0; j<vmd_joints[i].jnum; j++) {
			int n = VMD2NiJointNum(vmd_joints[i].jdat[j].joint);
			vmd_joints[i].jdat[j].joint = n;
		}
	}


	/**/
	dmy_frmnum = (int)(vmd_joints[vmd_frmnum-1].frmn*rate_frame);
	dmy_joints = makeFixFrameData(dmy_frmnum, 0);
	if (dmy_joints==NULL) {
		dmy_frmnum = vmd_frmnum;
		return vmd_joints;
	}

	for (unsigned int i=0; i<dmy_frmnum; i++) {
		dmy_joints[i].jnum = NI_JOINT_NUM;
		dmy_joints[i].msec = (int)(i*(100.0/(rate_frame*3.0)));
	}
	if (rate_frame!=1.0) {
		for (unsigned int i=0; i<vmd_frmnum; i++) {
			vmd_joints[i].frmn = (int)(vmd_joints[i].frmn*rate_frame);
			vmd_joints[i].msec = (int)(vmd_joints[i].msec/rate_frame);
		}
	}
		
	return dmy_joints;
}




NiFrameData*  CVMDTool::convert2FrameData(VMDJointData* motion_data, unsigned int& datanum)
{
	if (motion_data==NULL || datanum<=0) return NULL;

	// VMDのデータをフレーム番号でソート
	VMDJointData swap_motion;
	unsigned int k = datanum - 1;
	while (k>0) {
		unsigned int j = 0;
		for (unsigned int i=0; i<k; i++) {
			if (motion_data[i].frm_num>motion_data[i+1].frm_num) {
				swap_motion      = motion_data[i];
				motion_data[i]   = motion_data[i+1];
				motion_data[i+1] = swap_motion;
				j = i;
			}
		}
		k = j;
	}

	// 一意的なフレーム数を数える
	unsigned int uniq_frame = 1;

	unsigned int frm_num = motion_data[0].frm_num;
	for (unsigned int i=1; i<datanum; i++) {
		if (frm_num!=motion_data[i].frm_num) {
			uniq_frame++;
			frm_num = motion_data[i].frm_num;
		}
	}

	NiFrameData* ni_joints = makeFixFrameData((int)uniq_frame, VMD_MAX_JOINT_NUM);
	if (ni_joints==NULL) return NULL;


	unsigned int datacnt = 0;
	for (unsigned int i=0; i<uniq_frame; i++) {
		frm_num = motion_data[datacnt].frm_num;
		ni_joints[i].frmn = frm_num;
		ni_joints[i].msec = (int)(frm_num*33.3333333333333);		// msec
		//
		while (frm_num==motion_data[datacnt].frm_num) {
			int joint = VMDJointNum(motion_data[datacnt].name);
			if (joint>=0) {
				ni_joints[i].jdat[joint].joint  = joint;
				ni_joints[i].jdat[joint].index  = i;
				ni_joints[i].jdat[joint].vect.x = -motion_data[datacnt].posz*VMD_GRID_UNIT;
				ni_joints[i].jdat[joint].vect.y =  motion_data[datacnt].posx*VMD_GRID_UNIT;
				ni_joints[i].jdat[joint].vect.z =  motion_data[datacnt].posy*VMD_GRID_UNIT;
				ni_joints[i].jdat[joint].quat.x =  motion_data[datacnt].qutz;
				ni_joints[i].jdat[joint].quat.y = -motion_data[datacnt].qutx;
				ni_joints[i].jdat[joint].quat.z = -motion_data[datacnt].quty;
				ni_joints[i].jdat[joint].quat.s =  motion_data[datacnt].qutw;
				if (ni_joints[i].jdat[joint].quat.s<0.0) ni_joints[i].jdat[joint].quat = - ni_joints[i].jdat[joint].quat;
				ni_joints[i].jdat[joint].quat.normalize();
			}
			datacnt++;
			if (datacnt==datanum) break;
		}
	}

	//
	datanum = uniq_frame;
	return ni_joints;
}





void  CVMDTool::calcJointRotation(void)
{
	if (vmd_joints==NULL || vmd_frmnum<=0) return;

	clearJointsData();
	NiJointData	 prevJoint[VMD_MAX_JOINT_NUM];
	for (int j=0; j<VMD_MAX_JOINT_NUM; j++) {
		prevJoint[j].joint = -1;
		prevJoint[j].index = -1;
		prevJoint[j].vect.init();
		prevJoint[j].quat.init();
	}

	// 関節の変換
	for (unsigned int i=0; i<vmd_frmnum; i++) {

		NiJointData* jdata = vmd_joints[i].jdat;

		for (int j=0; j<vmd_joints[i].jnum; j++) {
			//
			if (jdata[j].joint>=0) {
				posVect[j]   = jdata[j].vect;
				rotQuat[j]   = jdata[j].quat;				
				prevJoint[j] = jdata[j];
			}
			else {
				posVect[j]   = prevJoint[j].vect;
				rotQuat[j]   = prevJoint[j].quat;
			}
		}
		 
		//
		rotQuat[VMD_R_WRIST] = rotQuat[VMD_R_WRIST]*rotQuat[VMD_R_WRIST_TW];
		rotQuat[VMD_R_ELBOW] = rotQuat[VMD_R_ELBOW]*rotQuat[VMD_R_ARM_TW];
		rotQuat[VMD_L_WRIST] = rotQuat[VMD_L_WRIST]*rotQuat[VMD_L_WRIST_TW];
		rotQuat[VMD_L_ELBOW] = rotQuat[VMD_L_ELBOW]*rotQuat[VMD_L_ARM_TW];

		rotQuat[VMD_R_EYE]	 = rotQuat[VMD_R_EYE]*rotQuat[VMD_EYES];
		rotQuat[VMD_L_EYE]	 = rotQuat[VMD_L_EYE]*rotQuat[VMD_EYES];

		/////// A -> T
		rotQuat[VMD_R_WRIST] = ~A2TPose*rotQuat[VMD_R_WRIST]*A2TPose;
		rotQuat[VMD_R_ELBOW] = ~A2TPose*rotQuat[VMD_R_ELBOW]*A2TPose;
		rotQuat[VMD_R_ARM]	 =  rotQuat[VMD_R_ARM]*A2TPose;
		
		rotQuat[VMD_L_WRIST] =  A2TPose*rotQuat[VMD_L_WRIST]*~A2TPose;
		rotQuat[VMD_L_ELBOW] =  A2TPose*rotQuat[VMD_L_ELBOW]*~A2TPose;		
		rotQuat[VMD_L_ARM]	 =  rotQuat[VMD_L_ARM]*~A2TPose;

		//
		//rotQuat[VMD_CENTER]	= rotQuat[VMD_CENTER]*~rotQuat[VMD_PARENT];
		//posVect[VMD_CENTER]	= posVect[VMD_CENTER] + posVect[VMD_PARENT];
		
		
		//
		for (int j=0; j<vmd_joints[i].jnum; j++) {
			jdata[j].joint = prevJoint[j].joint;
			jdata[j].index = prevJoint[j].index;
			jdata[j].vect  = posVect[j];
			jdata[j].quat  = rotQuat[j];
		}

	}
	
	return;
}




NiJointData*  CVMDTool::getFrameData(int frmnum)
{
	//
	for (int j=0; j<VMD_MAX_JOINT_NUM; j++) {
		frameData[j].joint = -1;
		frameData[j].index = -1;
		frameData[j].vect.init(-1.0);
		frameData[j].quat.init(-1.0);
	}

	//
	for (int j=0; j<VMD_MAX_JOINT_NUM; j++) {
		//
		int n = vmd_joints[0].jdat[j].joint;
		if (n>=0) {
			unsigned int st_index = 0;
			unsigned int en_index = 0;

			while (en_index<vmd_frmnum && vmd_joints[en_index].frmn<frmnum) en_index++;
			if (en_index==vmd_frmnum) break;

			if (en_index==0 || (vmd_joints[en_index].frmn==frmnum && en_index==vmd_joints[en_index].jdat[j].index)) {
				frameData[n].vect = vmd_joints[en_index].jdat[j].vect;
				frameData[n].quat = vmd_joints[en_index].jdat[j].quat;
			}
			else {
				st_index = vmd_joints[en_index-1].jdat[j].index;
				while (en_index<vmd_frmnum && en_index!=vmd_joints[en_index].jdat[j].index) en_index++;

				if (en_index==vmd_frmnum) {
					frameData[n].vect = vmd_joints[st_index].jdat[j].vect;
					frameData[n].quat = vmd_joints[st_index].jdat[j].quat;
				}
				else {
					NiJointData st_joint = vmd_joints[st_index].jdat[j];
					NiJointData en_joint = vmd_joints[en_index].jdat[j];
					double tparam = ((double)(frmnum - vmd_joints[st_index].frmn))/(vmd_joints[en_index].frmn - vmd_joints[st_index].frmn);
				
					frameData[n].vect = (1.0-tparam)*st_joint.vect + tparam*en_joint.vect;
					frameData[n].quat = SlerpQuaternion(st_joint.quat, en_joint.quat, tparam);
				}
			}

			frameData[n].joint  = n;
			frameData[n].index  = frmnum;
			frameData[n].vect.c = 1.0;
			frameData[n].quat.c = 1.0;
		}
	}

	//if (frmnum>514 && frmnum<545) {
	//	Quaternion q = frameData[NI_PELVIS].quat;
	//	DEBUG_ERR("%d (%f,%f,%f), %f", frmnum, q.x, q.y, q.z, q.s);
	//}

	return frameData;
}




/*

Quaternion  interQuatBezier(Quaternion a, Quaternion b, double t, unsigned char* param)
{
	double t2 = t*t;
	double t3 = t2*t;
	double p0 = 1.0 - 3.0*t + 3.0*t2 - t3;
	double p1 = 3.0*t - 6.0*t2 + 3.0*t3;
	double p2 = 3.0*t2 - 3.0*t3;
//	double p3 = t3;
	
	Quaternion c = p0*a + p1* + p2* + t3*b;

	return c;
}
*/