#include  "stdafx.h"


#include  "ExNiFileDev.h"





void  CExNiFileDev::init(void)
{ 
	vect_fwrd.set(1.0, 0.0, 0.0, 1.0, 1.0);

	len_scale = 1.0f;
	init_pos  = FALSE;
	mirroring = TRUE;

	niExportData = NULL;

	setTempFilePath();
}




BOOL  CExNiFileDev::backupAsOrig(LPCTSTR backup)
{ 
	BOOL ret = FALSE;

	if (tempFilePath!=_T("") && backup!=NULL) {
		ret = saveAsJointsText((LPCTSTR)tempFilePath, backup);
	}

	return ret;
}




BOOL  CExNiFileDev::backupAsBVH(LPCTSTR backup)
{ 
	BOOL ret = FALSE;

	if (tempFilePath!=_T("") && backup!=NULL) {
		ret = saveAsBVH((LPCTSTR)tempFilePath, backup);
	}

	return ret;
}



//
// 戻り値：　正: 最後のフレームの次のフレームの番号, 負: エラー 次のフレームの番号x(-1)
//	
int  CExNiFileDev::convertJointsData(CNiNetwork* net, FileDevParam param)
{
	if (net==NULL || niExportData==NULL) return 0;
	if (param.controler!=NULL) {
		if (*param.controler!=NI_FILE_PLAYER_START && *param.controler!=NI_FILE_PLAYER_GO) return -1;
	}
	if (!startFrame(param.start_frame)) return 0;

	TCHAR buf[LNAME];

	//
	if (param.cm_unit) len_scale = 0.01f;
	else			   len_scale = 1.0f;
	mirroring  = param.mirroring;
	init_pos   = param.init_pos; 
	time_scale = param.time_scale;

	//
	int frame = frame_start;
	if (*param.controler==NI_FILE_PLAYER_START) clearStartPosition();

	while (frame<frame_num) {
		sntprintf(buf, LNAME, _T(" %d"), frame+1);
		param.counter->SetWindowText(buf);
		
		//
		clearJointsData();
		getFrameData(mirroring);
		correctPosVect(frame, *(param.controler));	// for start position
		if (param.calc_quat) calcRotQaut();

		niExportData->exportJointsData(posVect, rotQuat, net, NiSDK_None, NI_JOINT_NUM);
		if (param.controler!=NULL) {
			if (*param.controler!=NI_FILE_PLAYER_START && *param.controler!=NI_FILE_PLAYER_GO) {
				frame++;
				break;
			}
		}
		frame++;

		if (!nextFrame()) return -frame;
		DisPatcher();
	}

	if (frame==frame_num) frame = 0;
	return frame;
}




void  CExNiFileDev::correctPosVect(int frame, int cntrlr)
{
	if (posVect[NI_PELVIS].c<0.0) {
		posVect[NI_PELVIS] = (posVect[NI_R_HIP] + posVect[NI_L_HIP])/2.0;
	}
	if (posVect[NI_NECK].c<0.0) {
		posVect[NI_NECK] = (posVect[NI_R_SHLDR] + posVect[NI_L_SHLDR])/2.0;
	}
	if (posVect[NI_TORSO].c<0.0) {
		posVect[NI_TORSO] = (posVect[NI_PELVIS] + posVect[NI_NECK])/2.0;
	}

	if (cntrlr==NI_FILE_PLAYER_START || cntrlr==NI_FILE_PLAYER_GO) {// && init_pos) {
		if (frame==0) {
			if (init_pos) start_pos = posVect[NI_PELVIS]*len_scale;
			else          start_pos.set(0.0, 0.0, 0.0);
		}
	}
	for (int j=0; j<NI_JOINT_NUM; j++) {
		posVect[j] = posVect[j]*len_scale - start_pos;
	}
	return;
}	
	



//
// 位置データからクォータニオンを計算
//
void  CExNiFileDev::calcRotQaut(void)
{
	//
	Vector<double> torso_down  = posVect[NI_PELVIS]  - posVect[NI_TORSO];
	Vector<double> shldr_left  = posVect[NI_L_SHLDR] - posVect[NI_R_SHLDR];
	Vector<double> torso_up	   = - torso_down;
	Vector<double> shldr_right = - shldr_left;

	// PELVIS YZ平面の回転
	Vector<double> vect = NewellMethod4(posVect[NI_R_HIP], posVect[NI_PELVIS], posVect[NI_L_HIP], posVect[NI_TORSO]);
	rotQuat[NI_PELVIS] = V2VQuaternion(vect_fwrd, vect);

	// X 軸回転
	Quaternion quat_lhip = ~rotQuat[NI_PELVIS]*posVect[NI_L_HIP]*rotQuat[NI_PELVIS];
	Quaternion quat_rhip = ~rotQuat[NI_PELVIS]*posVect[NI_R_HIP]*rotQuat[NI_PELVIS];

	Vector<double> hip_left  = quat_lhip.getVector() - quat_rhip.getVector(); 
	Vector<double> hip_right = - hip_left;
	double thx = atan2(hip_left.z, hip_left.y);
	Quaternion xrot;
	xrot.setRotation(thx, 1.0, 0.0, 0.0, 1.0);

	//
	rotQuat[NI_PELVIS] = rotQuat[NI_PELVIS]*xrot;
	rotQuat[NI_TORSO]  = VPPQuaternion(torso_up, posVect[NI_TORSO], posVect[NI_NECK]);
	rotQuat[NI_NECK]   = PPPQuaternion(posVect[NI_TORSO],  posVect[NI_NECK],  posVect[NI_HEAD]);

	//
	rotQuat[NI_L_SHLDR] = VPPQuaternion(shldr_left,    posVect[NI_L_SHLDR], posVect[NI_L_ELBOW]);
	rotQuat[NI_R_SHLDR] = VPPQuaternion(shldr_right,   posVect[NI_R_SHLDR], posVect[NI_R_ELBOW]);
	rotQuat[NI_L_HIP]   = VPPQuaternion(torso_down,    posVect[NI_L_HIP],   posVect[NI_L_KNEE]);
	rotQuat[NI_R_HIP]   = VPPQuaternion(torso_down,    posVect[NI_R_HIP],   posVect[NI_R_KNEE]);
//	rotQuat[NI_L_ANKLE] = VPPQuaternion(vect_ft_left,  posVect[NI_L_ANKLE], posVect[NI_L_FOOT]);
//	rotQuat[NI_R_ANKLE] = VPPQuaternion(vect_ft_right, posVect[NI_R_ANKLE], posVect[NI_R_FOOT]);

	//
	if (posVect[NI_L_WRIST].c>0.0) {
		rotQuat[NI_L_ELBOW] = PPPQuaternion(posVect[NI_L_SHLDR], posVect[NI_L_ELBOW], posVect[NI_L_WRIST]);
		rotQuat[NI_L_WRIST] = PPPQuaternion(posVect[NI_L_ELBOW], posVect[NI_L_WRIST], posVect[NI_L_HAND]);
	}
	else {
		rotQuat[NI_L_ELBOW] = PPPQuaternion(posVect[NI_L_SHLDR], posVect[NI_L_ELBOW], posVect[NI_L_HAND]);
	}
	if (posVect[NI_R_WRIST].c>0.0) {
		rotQuat[NI_R_ELBOW] = PPPQuaternion(posVect[NI_R_SHLDR], posVect[NI_R_ELBOW], posVect[NI_R_WRIST]);
		rotQuat[NI_R_WRIST] = PPPQuaternion(posVect[NI_R_ELBOW], posVect[NI_R_WRIST], posVect[NI_R_HAND]);
	}
	else {
		rotQuat[NI_R_ELBOW] = PPPQuaternion(posVect[NI_R_SHLDR], posVect[NI_R_ELBOW], posVect[NI_R_HAND]);
	}

	//
	if (posVect[NI_L_ANKLE].c>0.0) {
		rotQuat[NI_L_KNEE]  = PPPQuaternion(posVect[NI_L_HIP],   posVect[NI_L_KNEE],  posVect[NI_L_ANKLE]);
	}
	else{
		rotQuat[NI_L_KNEE]  = PPPQuaternion(posVect[NI_L_HIP],   posVect[NI_L_KNEE],  posVect[NI_L_FOOT]);
	}
	if (posVect[NI_R_ANKLE].c>0.0) {
		rotQuat[NI_R_KNEE]  = PPPQuaternion(posVect[NI_R_HIP],   posVect[NI_R_KNEE],  posVect[NI_R_ANKLE]);
	}
	else{
		rotQuat[NI_R_KNEE]  = PPPQuaternion(posVect[NI_R_HIP],   posVect[NI_R_KNEE],  posVect[NI_R_FOOT]);
	}


	// To Avatar Coordinate
	rotQuat[NI_L_WRIST] = ~rotQuat[NI_PELVIS]*rotQuat[NI_L_WRIST]*rotQuat[NI_PELVIS];
	rotQuat[NI_R_WRIST] = ~rotQuat[NI_PELVIS]*rotQuat[NI_R_WRIST]*rotQuat[NI_PELVIS];
	rotQuat[NI_L_ELBOW] = ~rotQuat[NI_PELVIS]*rotQuat[NI_L_ELBOW]*rotQuat[NI_PELVIS];
	rotQuat[NI_R_ELBOW] = ~rotQuat[NI_PELVIS]*rotQuat[NI_R_ELBOW]*rotQuat[NI_PELVIS];
	rotQuat[NI_L_SHLDR] = ~rotQuat[NI_PELVIS]*rotQuat[NI_L_SHLDR]*rotQuat[NI_PELVIS];
	rotQuat[NI_R_SHLDR] = ~rotQuat[NI_PELVIS]*rotQuat[NI_R_SHLDR]*rotQuat[NI_PELVIS];
//	rotQuat[NI_L_ANKLE] = ~rotQuat[NI_PELVIS]*rotQuat[NI_L_ANKLE]*rotQuat[NI_PELVIS];
//	rotQuat[NI_R_ANKLE] = ~rotQuat[NI_PELVIS]*rotQuat[NI_R_ANKLE]*rotQuat[NI_PELVIS];
	rotQuat[NI_L_KNEE]  = ~rotQuat[NI_PELVIS]*rotQuat[NI_L_KNEE] *rotQuat[NI_PELVIS];
	rotQuat[NI_R_KNEE]  = ~rotQuat[NI_PELVIS]*rotQuat[NI_R_KNEE] *rotQuat[NI_PELVIS];
	rotQuat[NI_L_HIP]   = ~rotQuat[NI_PELVIS]*rotQuat[NI_L_HIP]  *rotQuat[NI_PELVIS];
	rotQuat[NI_R_HIP]   = ~rotQuat[NI_PELVIS]*rotQuat[NI_R_HIP]  *rotQuat[NI_PELVIS];
	rotQuat[NI_NECK]    = ~rotQuat[NI_PELVIS]*rotQuat[NI_NECK]   *rotQuat[NI_PELVIS];
	rotQuat[NI_TORSO]   = ~rotQuat[NI_PELVIS]*rotQuat[NI_TORSO]  *rotQuat[NI_PELVIS];

	return;
}