#include  "..\stdafx.h"


#include  "NiFileTool.h"
#include  "MFCio.h"



using namespace jbxl;
using namespace jbxwl;





CNiFileTool::~CNiFileTool(void)
{ 
	close_wfile(); 
	close_rfile();

	freeFrameData();
}





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

void  CNiFileTool::clear_data(void)
{
	m_enable_wfile = FALSE;
	m_wfp		= NULL;
	m_rfp		= NULL;
	m_num		= 0;
	m_lib		= NiSDK_None;

	frame_data	= NULL;
	frame_start	= 0;
	frame_num	= 0;
	frame_ptr   = 0;

	frame_timer = 0;
	lap_timer	= 0;
	play_time	= 0.0f;
	time_scale	= 1.0f;

	memset(m_ni2sdkjoint, -1, sizeof(int)*NI_JOINT_NUM);
	clearStartPosition();
	clearJointsData();
}



void  CNiFileTool::init(NiSDK_Lib lib)
{
	m_lib = lib;

	m_num = 0;
	for (int j=0; j<NI_JOINT_NUM; j++) {
		m_ni2sdkjoint[j] = Ni2SDKPosJointNum(j, lib);
		if (m_ni2sdkjoint[j]>=0) m_num++;
	}
}




BOOL  CNiFileTool::open_wfile(CString fname)
{
	if (m_enable_wfile) return TRUE;

	if (m_wfp!=NULL) return FALSE;
	m_wfp = tfopen(fname, _T("w"));
	if (m_wfp==NULL) return FALSE;

	//
	fprintf(m_wfp, "%s\n", NI_FILE_HD_ID);

	m_enable_wfile = TRUE;
	return TRUE;
}



BOOL  CNiFileTool::open_rfile(CString fname)
{
	if (m_rfp!=NULL) return FALSE;
	m_rfp = tfopen(fname, _T("r"));
	if (m_rfp==NULL) return FALSE;

	return TRUE;
}



void  CNiFileTool::close_wfile(void)
{
	if (m_wfp!=NULL) {
		fclose(m_wfp);
		m_wfp = NULL;
		m_enable_wfile = FALSE;
	}
}



void  CNiFileTool::close_rfile(void)
{
	if (m_rfp!=NULL) {
		fclose(m_rfp);
		m_rfp = NULL;
	}
}





///////////////////////////////////////////////////////////////////////////////////////////////////
//
// I/O
//

void  CNiFileTool::writeJointsText(Vector<double>* pos, Quaternion* rot, BOOL mirror)
{
	if (m_wfp==NULL) return;

	fprintf(m_wfp, "%d\n", GetMsecondsTimer());

	for (int j=0; j<NI_JOINT_NUM; j++) {
		int n = m_ni2sdkjoint[j];
		if (n>=0) {
			std::string jn = NiSDK2JointName(j, NiSDK_None);
			if (mirror) { 
				n = NiSDK2MirrorJointNum(n, m_lib);
				fprintf(m_wfp, " %-10s %11.6f %11.6f %11.6f    ", jn.c_str(), pos[n].x, -pos[n].y, pos[n].z);
				fprintf(m_wfp, " %11.8f %11.8f %11.8f %11.8f\n",  -rot[n].x, rot[n].y, -rot[n].z, rot[n].s);
			}
			else {
				fprintf(m_wfp, " %-10s %11.6f %11.6f %11.6f    ", jn.c_str(), pos[n].x,pos[n].y, pos[n].z);
				fprintf(m_wfp, " %11.8f %11.8f %11.8f %11.8f\n",  rot[n].x, rot[n].y, rot[n].z, rot[n].s);
			}
		}
	}

	return;
}




BOOL  CNiFileTool::saveAsJointsText(CString fname, CString backup)
{
	return copyFileWithCounter(fname, backup);
}




int  CNiFileTool::readJointsTextFile(CString fname)
{
	if (fname==_T("")) return -1;
	
	BOOL ret = open_rfile(fname);
	if (!ret) return -2;

	if (frame_data!=NULL) freeFrameData();
	Buffer buf = make_Buffer(LDATA);	// Line 読み込み用バッファ
	

	// ファイルヘッダのチェック
	fgets_Buffer(&buf, m_rfp);
	if (strcmp(NI_FILE_HD_ID, (char*)buf.buf)) {
		close_rfile();
		free_Buffer(&buf);
		return -3;
	}


	// フレーム数を数える
	int frame = 0;
	fgets_Buffer(&buf, m_rfp);
	while(!feof(m_rfp)) {
		if (buf.buf[0]>='0' && buf.buf[0]<='9') frame++; // 行頭が数字の場合はフレーム時間
		fgets_Buffer(&buf, m_rfp);
	}
	if (frame==0) {
		close_rfile();
		free_Buffer(&buf);
		return -4;
	}


	// 各フレームのジョイントの数を調べる
	int* frame_time = (int*)malloc(frame*sizeof(int));
	int* joints_num = (int*)malloc(frame*sizeof(int));
	memset(frame_time, 0, frame);
	memset(joints_num, 0, frame);

	fseek(m_rfp, 0, SEEK_SET);
	fgets_Buffer(&buf, m_rfp);
	// 最初のフレーム時間行まで移動
	while (buf.buf[0]<'0' || buf.buf[0]>'9') fgets_Buffer(&buf, m_rfp);

	for (int i=0; i<frame; i++) {
		frame_time[i] = atoi((char*)buf.buf);

		int joints = 0;
		fgets_Buffer(&buf, m_rfp);
		while (!feof(m_rfp) && (buf.buf[0]<'0' || buf.buf[0]>'9')) {
			if (buf.buf[0]==' ') joints++;
			fgets_Buffer(&buf, m_rfp);
		}
		joints_num[i] = joints;
	}


	// Frame Data の格納先を作る
	ret = makeFrameData(frame, frame_time, joints_num);
	::free(frame_time);
	::free(joints_num);
	if (!ret) {
		close_rfile();
		free_Buffer(&buf);
		return -5;
	}


	//
	// データの読み込み
	//
	int prvms = 0;
	fseek(m_rfp, 0, SEEK_SET);
	fgets_Buffer(&buf, m_rfp);
	// 最初のフレーム時間行まで移動
	while (buf.buf[0]<'0' || buf.buf[0]>'9') fgets_Buffer(&buf, m_rfp);

	char jname[L_ID];
	memset(jname, 0, L_ID);

	for (int i=0; i<frame_num; i++) {
		int nowms = frame_data[i].msec;
		if (i==0) prvms = nowms;
		int msec = nowms - prvms;
		if (msec<0) msec += 60000;	// + 60sec
		prvms = nowms;

		if (i==0) frame_data[0].msec = 0;
		else      frame_data[i].msec = frame_data[i-1].msec + msec;

		//
		NiJointData* jdat = frame_data[i].joint_data;
		for (int j=0; j<frame_data[i].num; j++) {
			fgets_Buffer(&buf, m_rfp);
			while (buf.buf[0]!=' ') fgets_Buffer(&buf, m_rfp);

			sscanf((char*)buf.buf, "%s %f %f %f %f %f %f %f", 
								jname, &jdat[j].px, &jdat[j].py, &jdat[j].pz,
									   &jdat[j].qx, &jdat[j].qy, &jdat[j].qz, &jdat[j].qs);
			jdat[j].joint = JointName2NiSDK(jname, NiSDK_None);
		}

		//DEBUG_ERR("frame = %d, joints = %d, time = %d", i, frame_data[i].num, frame_data[i].msec);
	}

 	close_rfile();
	free_Buffer(&buf);

	play_time = (frame_data[frame_num-1].msec - frame_data[0].msec)/1000.f;

	return frame_num;
}






///////////////////////////////////////////////////////////////////////////////////////////////////
//
// BVH
//

BOOL  CNiFileTool::saveAsBVH(CString fname, CString backup)
{

	return TRUE;
}








///////////////////////////////////////////////////////////////////////////////////////////////////
//
// Frame
//


void  CNiFileTool::setFrame(int frame) 
{ 
	frame_start = 0;
	if (frame_num!=0) {
		frame_start = frame%frame_num;
	}

	frame_ptr = frame_start;
}



//
// Start 位置の設定とフレームデータの準備
//
BOOL  CNiFileTool::startFrame(int start)
{
	if (frame_num==0 || start<0 || start>=frame_num) return FALSE;

	lap_timer	= GetMsecondsTimer();
	frame_timer	= (int)(frame_data[start].msec*time_scale);
	setFrame(start);

	return TRUE;
}



//
// 次のフレームデータの準備
//
BOOL  CNiFileTool::nextFrame(BOOL next)		// next==TRUE フレームを進める
{
	if (next) {
		frame_ptr++;
		if (frame_ptr==frame_num) return TRUE;	// END
		else if (frame_ptr>frame_num) {
			// not reachable
			DEBUG_ERR("CNiFileTool::nextFrame: frame ptr = %d, frame num = %d", frame_ptr, frame_num); 
			return FALSE;
		}
	}
	int msec = (int)(frame_data[frame_ptr].msec*time_scale);

	unsigned short ctime;
	frame_timer += GetMsecondsLapTimer(lap_timer, &ctime);
	
	if (frame_timer>=msec) {
		lap_timer = ctime;
		return TRUE;
	}
	else if (msec>frame_timer+5000) {
		DEBUG_ERR("CNiFileTool::nextFrame: next frame = %d, current frame = %d", msec, frame_timer);
		return FALSE;	// 5sec 進?
	}
	
	// Wait
	do {
		int slptm = Min(msec-frame_timer, 100);
		if (slptm>0) ::Sleep(slptm);
		frame_timer += GetMsecondsLapTimer(lap_timer, &ctime);
	} while (msec>frame_timer);
	
	lap_timer = ctime;
	return TRUE;
}



//
// フレームデータの格納先メモリを作成，初期化
//
BOOL  CNiFileTool::makeFrameData(int frame, int* frame_time, int* joints_num)
{
	frame_ptr  = 0;
	frame_num  = 0;
	frame_data = (NiJointsFrameData*)malloc(frame*sizeof(NiJointsFrameData));
	if (frame_data==NULL) return FALSE;
	memset(frame_data, 0, frame*sizeof(NiJointsFrameData));

	for (int i=0; i<frame; i++) {
		frame_data[i].msec = frame_time[i];
		frame_data[i].num  = joints_num[i];
		if (frame_data[i].num>0) {
			frame_data[i].joint_data = (NiJointData*)malloc(frame_data[i].num*sizeof(NiJointData));
			if (frame_data[i].joint_data==NULL) {
				freeFrameData();
				return FALSE;
			}
			memset(frame_data[i].joint_data, 0, frame_data[i].num*sizeof(NiJointData));
		}
	}

	frame_num = frame;
	return TRUE;
}




void  CNiFileTool::freeFrameData(void)
{
	if (frame_data==NULL) return;

	for (int i=0; i<frame_num  ; i++) {
		if (frame_data[i].joint_data!=NULL) {
			::free(frame_data[i].joint_data);
			frame_data[i].joint_data = NULL;
		}
	}

	::free(frame_data);
	frame_data = NULL;
	frame_num  = 0;
	return;
}





///////////////////////////////////////////////////////////////////////////////////////////////////
//
// joints
//

void  CNiFileTool::clearJointsData(void)
{
	for (int i=0; i<NI_JOINT_NUM; i++) {
		rotQuat[i].init(-1.0);
		posVect[i].init(-1.0);
	}
}



void  CNiFileTool::getPosVect(BOOL mirror, int frame)
{
	if (frame>=0) setFrame(frame);

	for (int j=0; j<frame_data[frame_ptr].num; j++) {
		int n = frame_data[frame_ptr].joint_data[j].joint;
		if (n>=0 && n<NI_JOINT_NUM) {
			if (mirror) {
				n = NiSDK2MirrorJointNum(n, NiSDK_None);
				posVect[n].y = - (double)frame_data[frame_ptr].joint_data[j].py;
			}
			else {
				posVect[n].y = (double)frame_data[frame_ptr].joint_data[j].py;
			}

			posVect[n].x = (double)frame_data[frame_ptr].joint_data[j].px;
			posVect[n].z = (double)frame_data[frame_ptr].joint_data[j].pz;
			posVect[n].c = 1.0;
		}
	}
	return;
}



void  CNiFileTool::getRotQuat(BOOL mirror, int frame)
{
	if (frame>=0) setFrame(frame);

	for (int j=0; j<frame_data[frame_ptr].num; j++) {
		int n = frame_data[frame_ptr].joint_data[j].joint;
		if (n>=0 && n<NI_JOINT_NUM) {
			if (mirror) {
				n = NiSDK2MirrorJointNum(n, NiSDK_None);
				rotQuat[n].x = - (double)frame_data[frame_ptr].joint_data[j].qx;
				rotQuat[n].z = - (double)frame_data[frame_ptr].joint_data[j].qz;
			}
			else {
				rotQuat[n].x = (double)frame_data[frame_ptr].joint_data[j].qx;
				rotQuat[n].z = (double)frame_data[frame_ptr].joint_data[j].qz;
			}

			rotQuat[n].y = (double)frame_data[frame_ptr].joint_data[j].qy;
			rotQuat[n].s = (double)frame_data[frame_ptr].joint_data[j].qs;
			rotQuat[n].c = 1.0;
		}
	}
	return;
}