

/**
@brief    BVH用 ツール
@file     BVHTool.cpp
@author   Fumi.Iseki (C)
*/


#include  "BVHTool.h"




using namespace jbxl;




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

CBVHTool::~CBVHTool(void)
{ 
	DEBUG_INFO("DESTRUCTOR: CBVHTool");

	free_data();
}




void  CBVHTool::init()
{
	// BVHData
	joint_num	= 0;	// 関節の数（End Site を含まない）
	frame_num	= 0;	// フレームの数
	frame_time	= 0.0;	// フレーム間の時間 s
	framepsec	= 0;	// FPS
	channels	= 0;	// 総チャンネル数．チャンネル数の合計．モーションデータの数．
	state		= 0;	// データの状態
	
	hierarchy	= NULL;	// 関節部のツリーデータへのポインタ．上記参照．
	motion		= NULL;	// MOTION データへのポインタ joint_num*frame_num
	offset		= NULL;	// 関節のOFFSET

	//
	channel_num	= NULL;
	channel_idx	= NULL;
	joint_name  = NULL;

	flex_joint	= init_Buffer();
}




void  CBVHTool::free_data(void)
{
	state	  	= 0;
	joint_num 	= 0;
	channels	= 0;
	frame_num 	= 0;
	frame_time 	= 0.0;
	framepsec	= 0;

	if (hierarchy!=NULL) {
		del_tTree(&hierarchy);
		hierarchy = NULL;
	}
	if (motion!=NULL) {
		free(motion);
		motion = NULL;
	}
	if (offset!=NULL) {
		free(offset);
		offset = NULL;
	}

	clear_data();
}




void  CBVHTool::clear_data(void)
{
	if (joint_name!=NULL) {
		for (int j=0; j<joint_num; j++) {
			free_Buffer(&joint_name[j]);
		}
		free(joint_name);
	}
	joint_name = NULL;

	if (channel_num!=NULL) {
		free(channel_num);
		channel_num = NULL;
	}

	if (channel_idx!=NULL) {
		free(channel_idx);
		channel_idx = NULL;
	}

	free_Buffer(&flex_joint);
}




void  CBVHTool::copy_bvh_data(BVHData* bvh)
{
	if (bvh==NULL) return;

	joint_num	= bvh->joint_num;
	frame_num	= bvh->frame_num;
	frame_time	= bvh->frame_time;
	framepsec	= bvh->framepsec;
	channels	= bvh->channels;
	state		= bvh->state;
	
	hierarchy	= bvh->hierarchy;
	motion		= bvh->motion;
	offset		= bvh->offset;
}




BVHData  CBVHTool::setback_bvh_data(void)
{
	BVHData bvh;

	set_offset();

	bvh.joint_num	= joint_num;
	bvh.frame_num	= frame_num;
	bvh.frame_time	= frame_time;
	bvh.framepsec	= framepsec;
	bvh.channels	= channels;
	bvh.state		= state;
	
	bvh.hierarchy	= hierarchy;
	bvh.motion		= motion;
	bvh.offset		= offset;

	return bvh;
}






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

BOOL  CBVHTool::readFile(char* fname)
{
	state = BVH_ERR_INVLD_ARGS;
	if (fname==NULL || fname[0]=='\0') return FALSE;

	unsigned long int sz = file_size(fname);
	if (sz==0) return FALSE;

	state = BVH_ERR_FAIL_FOPEN;
	FILE* fp = fopen(fname, "r");
	if (fp==NULL) return FALSE;

	BOOL ret = readBVH(fp);

	return ret;
}
	
	


BOOL  CBVHTool::readBVH(FILE* fp)
{
	state = BVH_ERR_INVLD_ARGS;
	if (fp==NULL) return FALSE;

	state = BVH_ERR_FAIL_OP;
	BVHData* ptr = bvh_read_data(fp);
	if (ptr==NULL) return FALSE;
	if (ptr->state!=0) {
		del_BVHData(&ptr);
		return FALSE;
	}
	
	//
	copy_bvh_data(ptr);
	get_bvh_params();

	return TRUE;
}




BOOL  CBVHTool::writeFile(char* fname)
{
	state = BVH_ERR_INVLD_ARGS;
	if (fname==NULL || fname[0]=='\0') return FALSE;

	state = 0;
	BVHData bvh_data = setback_bvh_data();
	state = bvh_write_file(fname, &bvh_data);
	if (state!=0) return FALSE;

	return TRUE;
}




/**
BOOL  CBVHTool::writeMultiFile(char* fname, int sec, int space)

sec秒区切りで．BVHファイルを出力する．のりしろ(フレーム数)付き．

@param fname  出力ファイル名のベース．fname_00 〜 fname_99 のファイルが出力される．
@param sec    1ファイルの最大再生時間（秒）．
@param space  のりしろのフレーム数．
@retval TRUE  成功
@retval FALSE 失敗
*/
BOOL  CBVHTool::writeMultiFile(char* fname, int sec, int space)
{
	state = BVH_ERR_INVLD_ARGS;
	if (fname==NULL || fname[0]=='\0') return FALSE;

	int echfrm = (int)(sec/frame_time);			// 1ファイルのフレーム数
	int num = (frame_num + echfrm - 1)/echfrm;	// 出力ファイルの数
	if (num==0 || num>99) return FALSE;

	//
	char* fn = dup_str(fname);
	char* et = get_file_extension(fn);
	if (et!=NULL) et[-1] = '\0';

	Buffer format = make_Buffer_bystr(fn);
	cat_s2Buffer("_%02d", &format);
	if (et!=NULL) {
		cat_s2Buffer(".", &format);
		cat_s2Buffer(et,  &format);
	}
	::free(fn);

	//
	state = 0;
	BVHData bvh_data = setback_bvh_data();

	for (int i=0; i<num; i++) {
		//
		int outfrm = i*echfrm;							// 今まで出力した有効フレーム数

		if (i==num-1) {
			bvh_data.frame_num = frame_num - outfrm;	// 残りのフレーム数．のりしろは使用しない．
		}
		else if (i==num-2) {
			if ((num-1)*echfrm + space > frame_num) {	// のりしろが大きすぎる
				space = frame_num - (num-1)*echfrm;
			}
			bvh_data.frame_num = echfrm + space;
		}
		else {
			bvh_data.frame_num = echfrm + space;
		}

		if (i!=0 && space>0) {
			bvh_data.frame_num++;								//
			bvh_data.motion = &(motion[(outfrm-1)*channels]);	// -1: のりしろ
			if (channel_num[0]==6) {							// ROOT のチャンネル数
				bvh_data.motion[0] = motion[0];
				bvh_data.motion[1] = motion[1];
				bvh_data.motion[2] = motion[2];
				bvh_data.motion[3] = 0.0;
				bvh_data.motion[4] = 0.0;
				bvh_data.motion[5] = 0.0;
			}
		}
		else {
			bvh_data.motion = &(motion[outfrm*channels]);
		}
		
		//
		char* fn = numbering_name((char*)format.buf, i);
		state = bvh_write_file(fn, &bvh_data);
		free(fn);
		if (state!=0) break;
	}
	
	free_Buffer(&format);
	
	if (state!=0) return FALSE;
	return TRUE;
}




BOOL  CBVHTool::printBVH(FILE* fp)
{
	state = BVH_ERR_INVLD_ARGS;
	if (fp==NULL) return FALSE;

	state = 0;
	BVHData bvh_data = setback_bvh_data();
	state = bvh_print_data(fp, &bvh_data);
	if (state!=0) return FALSE;

	return TRUE;
}





void  CBVHTool::setHierarchy(tTree* hrchy)
{
	free_data();

	hierarchy = hrchy;
	joint_num = bvh_count_joints(hierarchy);
	channels  = bvh_count_channels(hierarchy);

	get_bvh_params();
}





void  CBVHTool::set_offset(void)
{
	if (offset==NULL) return;

	int jnum = 0;
	_set_offset(hierarchy, &jnum);
}




void  CBVHTool::_set_offset(tTree* tree, int* jnum)
{
   if (tree!=NULL) {
        while(tree->esis!=NULL) tree = tree->esis;
  
		do {
            tList_data* ld = &(tree->ldat);

			/////////////////////////////////////////
			if (ld->ptr==NULL) {
				ld->sz  = sizeof(vector);
				ld->ptr = (void*)malloc(ld->sz);
			}
			*((vector*)ld->ptr) = offset[*jnum];
			(*jnum)++;
			/////////////////////////////////////////

			if (tree->next!=NULL) _set_offset(tree->next, jnum);
            tree = tree->ysis;
			//
        } while(tree!=NULL);
    }

    return;


}







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

/**
CBVHTool::get_bvh_params()

既存の hierarchyデータから以下のデータを得る．
    channel_num, channel_idx, flex_joint, joint_name
*/
void  CBVHTool::get_bvh_params()
{
	size_t  len = sizeof(int)*joint_num;
	channel_num = (int*)malloc(len);
	channel_idx = (int*)malloc(len);
	//
	if (channel_num==NULL || channel_idx==NULL) {
		if (channel_num!=NULL) free(channel_num);
		if (channel_idx!=NULL) free(channel_idx);
		channel_num = NULL;
		channel_idx = NULL;
		return;
	}
	memset(channel_num, 0, sizeof((int)len));
	memset(channel_idx, 0, sizeof((int)len));

	//
	flex_joint = make_Buffer(joint_num*6);
	joint_name = (Buffer*)malloc(sizeof(Buffer)*joint_num);

	int* ch = channel_num;
	Buffer* jn = joint_name;

	_get_bvh_params(hierarchy, &ch, &jn);

	//
	channel_idx[0] = 0;
	for (int j=1; j<joint_num; j++) {
		channel_idx[j] = channel_idx[j-1] + channel_num[j-1];	
	}

}




void  CBVHTool::_get_bvh_params(tTree* tree, int** ch, Buffer** jn)
{
    if (tree!=NULL) {
        while(tree->esis!=NULL) tree = tree->esis;
  
		do {
            tList_data ld = tree->ldat;

			/////////////////////////////////////////
			if ((*ch)!=NULL) {
				**ch = ld.lv;
				(*ch)++;
			}
			cat_Buffer(&ld.val, &flex_joint);

			**jn = dup_Buffer(ld.key);
			(*jn)++;
			/////////////////////////////////////////


			if (tree->next!=NULL) {
				_get_bvh_params(tree->next, ch, jn);
			}
            tree = tree->ysis;
			//
        } while(tree!=NULL);
    }

    return;
}







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

Vector<double>*  CBVHTool::getPosData(int frame)
{
	if (frame<0 || frame>=frame_num) return NULL;

	size_t len = sizeof(Vector<double>)*joint_num;
	Vector<double>* vect = (Vector<double>*)malloc(len);
	if (vect==NULL) return NULL;
	memset(vect, 0, len);

	int frm = frame*channels;

	for (int j=0; j<joint_num; j++) {
		//
		BOOL  exstf = FALSE;
		int   chnum = channel_num[j];
		int   index = channel_idx[j];
		char* tpchr = (char*)&flex_joint.buf[index*2];


		for (int i=0; i<chnum; i++) {
			if (tpchr[i*2]=='P') {
				exstf = TRUE;
				index = frm + index + i;
				tpchr+= i*2;
				break;
			}
		}

		//
		vect[j].set(0.0, 0.0, 0.0);

		if (exstf) {
			for (int k=0; k<3; k++) {
				int k2 = k*2;
				if (tpchr[k2]=='P') {
					if      (tpchr[k2+1]=='X') vect[j].x = motion[index+k];
					else if (tpchr[k2+1]=='Y') vect[j].y = motion[index+k];
					else if (tpchr[k2+1]=='Z') vect[j].z = motion[index+k];
				}
			}
		}
	}

	return vect;
}





Quaternion*  CBVHTool::getQuaternion(int frame)
{
	if (frame<0 || frame>=frame_num) return NULL;

	size_t len = sizeof(Quaternion)*joint_num;
	Quaternion* quat = (Quaternion*)malloc(len);
	if (quat==NULL) return NULL;

	Vector<double> ex(1.0, 0.0, 0.0, 1.0);  
	Vector<double> ey(0.0, 1.0, 0.0, 1.0);  
	Vector<double> ez(0.0, 0.0, 1.0, 1.0);
	
	double d2r = PI/180.0;
	int frm = frame*channels;

	//
	for (int j=0; j<joint_num; j++) {
		//
		BOOL  exstf = FALSE;
		int   chnum = channel_num[j];
		int   index = channel_idx[j];
		char* tpchr = (char*)&flex_joint.buf[index*2];

		quat[j].set(1.0, 0.0, 0.0, 0.0, 1.0);

		//
		int chfwd = 0;
		for (int i=0; i<chnum; i++) {
			if (tpchr[i*2]=='R') {
				exstf = TRUE;				// 回転用データの存在
				index = frm + index + i;	// MOTION データの位置
				tpchr+= i*2;				// チャンネルインデックスの位置
				chfwd = i;					// 処理済チャンネル数
				break;
			}
		}

		//
		if (exstf) {
			Quaternion q1(1.0, 0.0, 0.0, 0.0, 1.0);
			Quaternion q2(1.0, 0.0, 0.0, 0.0, 1.0);
			Quaternion q3(1.0, 0.0, 0.0, 0.0, 1.0);
			
			// X->Y, Y->Z, Z->X
			double th = motion[index]*d2r;
			if      (tpchr[1]=='X') q3.setRotation(th, ey);	
			else if (tpchr[1]=='Y') q3.setRotation(th, ez);
			else if (tpchr[1]=='Z') q3.setRotation(th, ex);

			chnum = chnum - chfwd - 1;	// 残りの回転用チャンネル数
			if (chnum>0 && tpchr[2]=='R') {
				th = motion[index+1]*d2r;
				if      (tpchr[3]=='X') q2.setRotation(th, ey);
				else if (tpchr[3]=='Y') q2.setRotation(th, ez);
				else if (tpchr[3]=='Z') q2.setRotation(th, ex);

				chnum--;
				if (chnum>0 && tpchr[4]=='R') {
					th = motion[index+2]*d2r;
					if      (tpchr[5]=='X') q1.setRotation(th, ey);
					else if (tpchr[5]=='Y') q1.setRotation(th, ez);
					else if (tpchr[5]=='Z') q1.setRotation(th, ex);
				}
			}

			quat[j] = q3*q2*q1;
		}

	}

	return quat;
}





Vector<double>*  CBVHTool::getPosOffset(void)
{
	size_t len = sizeof(Vector<double>)*joint_num;
	Vector<double>* vect = (Vector<double>*)malloc(len);

	if (vect==NULL) return NULL;
	memset(vect, 0, len);

	for (int j=0; j<joint_num; j++) {
		vector vt = offset[j];
		vect[j].x = vt.x;
		vect[j].y = vt.y;
		vect[j].z = vt.z;
		vect[j].n = vt.n;
	}

	return vect;
}







////////////////////////////////////////////////////////////////////////////
// Pending

/*
Matrix<double>*  CBVHTool::getRotMatrix(int frame)
{
	if (frame<0 || frame>=frame_num) return NULL;

	size_t len = sizeof(Matrix<double>)*joint_num;
	Matrix<double>* mtrx = (Matrix<double>*)malloc(len);
	if (mtrx==NULL) return NULL;

	double d2r = PI/180.0;
	int frm = frame*bvh_data->channels;

	for (int j=0; j<joint_num; j++) {
		//
		BOOL  exstf = FALSE;
		int   chnum = channel_num[j];
		int   index = channel_idx[j];
		char* tpchr = (char*)&flex_joint.buf[index*2];

		for (int i=0; i<chnum; i++) {
			if (tpchr[i*2]=='R') {
				exstf = TRUE;
				index = frm + index + i;
				tpchr+= i*2;
				break;
			}
		}
		if (exstf) {
			if (tpchr[2]!='R' || tpchr[4]!='R') {
				exstf = FALSE;	// not supported
			}
		}

		//
		if (exstf) {
			//
			Vector<double> e;

			if (tpchr[1]=='X' && tpchr[3]=='Y' && tpchr[5]=='Z') {
				// ZYX -> XZY
				print_message("WARNING: CBVHTool::getRotMatrix: Not Implemeted Function is called. XYZ\n");
				mtrx[j].init(2, 3, 3);
			}

			else if (tpchr[1]=='X' && tpchr[3]=='Z' && tpchr[5]=='Y') {
				// YZX -> ZXY
				print_message("WARNING: CBVHTool::getRotMatrix: Not Implemeted Function is called. XZY\n");
				mtrx[j].init(2, 3, 3);
			}

			else if (tpchr[1]=='Y' && tpchr[3]=='X' && tpchr[5]=='Z') {
				// ZXY -> XYZ
				e.z = bvh_data->motion[index]  *d2r;
				e.y = bvh_data->motion[index+1]*d2r;
				e.x = bvh_data->motion[index+2]*d2r;
				mtrx[j] = EulerXYZ2RotMatrix(e);
			}

			else if (tpchr[1]=='Y' && tpchr[3]=='Z' && tpchr[5]=='X') {
				// XZY -> YXZ 
				print_message("WARNING: CBVHTool::getRotMatrix: Not Implemeted Function is called. YZX\n");
				mtrx[j].init(2, 3, 3);
			}

			else if (tpchr[1]=='Z' && tpchr[3]=='X' && tpchr[5]=='Y') {
				// YXZ -> ZYX 
				e.x = bvh_data->motion[index]  *d2r;
				e.y = bvh_data->motion[index+1]*d2r;
				e.z = bvh_data->motion[index+2]*d2r;
				mtrx[j] = EulerZYX2RotMatrix(e);
			}

			else if (tpchr[1]=='Z' && tpchr[3]=='Y' && tpchr[5]=='X') {
				// XYZ -> YZX
				print_message("WARNING: CBVHTool::getRotMatrix: Not Implemeted Function is called. ZYX\n");
				mtrx[j].init(2, 3, 3);
			}

			else {
				mtrx[j].init(2, 3, 3);		// 3x3行列
			}
		}
		else {
			mtrx[j].init(2, 3, 3);			// 3x3行列
		}
	}

	return mtrx;
}

*/





