#include "MotionData.h"

CKeyFrameMotion::CKeyFrameMotion()
{
	return;
}
CKeyFrameMotion::~CKeyFrameMotion()
{
	init();
}

void CKeyFrameMotion::init(void)
{
	qArray.clear();
	frameArray.clear();
	interTypeArray.clear();
	basePosArray.clear();
	objSwapEvents.clear();

	return;
}

fk_Vector CKeyFrameMotion::getInterVec(fk_Vector vecA, fk_Vector vecB, double t, fk_MotionInterType argType)
{
	static fk_Vector retVec;

	switch(argType) {
	case LINEAR:
		retVec = (1.0-t)*vecA + t*vecB;
		return retVec;
	case SPHERE:
		retVec = pow(cos(t*FK_PI/2.0), 2)*vecA + pow(sin(t*FK_PI/2.0), 2)*vecB;
		return retVec;
	case POWER:
		retVec = (1.0-pow(t, 3))*vecA + pow(t, 3)*vecB;
		return retVec;
	case REV_POWER:
		retVec = pow(1.0-t, 3)*vecA + (1.0-pow(1.0-t, 3))*vecB;
		return retVec;
	case FREEZE:
	case HIDE:		// <----------------------------------------------------オブジェクトのすげ替え対応のため追加
		return vecB;
	default:
		retVec.init();
		return retVec;
	}
}

fk_Quaternion CKeyFrameMotion::getInterQ(fk_Quaternion qA, fk_Quaternion qB, double t, fk_MotionInterType argType)
{
	static fk_Quaternion retQ;

	switch(argType) {
	case LINEAR:
		if(qA == qB) return qA;
		else return fk_Q_Inter_Linear(qA, qB, t);
	case SPHERE:
		if(qA == qB) return qA;
		else return fk_Q_Inter_Sphere(qA, qB, t);
	case POWER:
		retQ = (1.0-pow(t, 3))*qA + pow(t, 3)*qB;
		return retQ;
	case REV_POWER:
		retQ = pow(1.0-t, 3)*qA + (1.0-pow(1.0-t, 3))*qB;
		return retQ;
	case FREEZE:
	case HIDE:		// <----------------------------------------------------オブジェクトのすげ替え対応のため追加
		return qB;
	default:
		retQ.init();
		return retQ;
	}
}

void CKeyFrameMotion::pushKeyFrame(fk_Quaternion argQ, int argFrame, fk_MotionInterType argType, int argKeyPos)
{
	if(argKeyPos == -1) {
		qArray.push_back(argQ);
		frameArray.push_back(argFrame);
		interTypeArray.push_back(argType);
	} else if(argKeyPos < (int)qArray.size()){
		qArray.insert(qArray.begin()+argKeyPos, argQ);
		frameArray.insert(frameArray.begin()+argKeyPos, argFrame);
		interTypeArray.insert(interTypeArray.begin()+argKeyPos, argType);
	}

	return;
}
void CKeyFrameMotion::pushKeyFrame(fk_Quaternion argQ, fk_Vector argBasePos, int argFrame, fk_MotionInterType argType, int argKeyPos)
{
	if(argKeyPos == -1) {
		qArray.push_back(argQ);
		frameArray.push_back(argFrame);
		interTypeArray.push_back(argType);
		basePosArray.push_back(argBasePos);
	} else if(argKeyPos < (int)qArray.size()){
		qArray.insert(qArray.begin()+argKeyPos, argQ);
		frameArray.insert(frameArray.begin()+argKeyPos, argFrame);
		interTypeArray.insert(interTypeArray.begin()+argKeyPos, argType);
		basePosArray.insert(basePosArray.begin()+argKeyPos, argBasePos);
	}
	return;
}
bool CKeyFrameMotion::changeKeyFrame(int argKeyID, fk_Quaternion argQ, int argFrame, fk_MotionInterType argType)
{
	if(argKeyID >= getTotalKeyNum()) return false;

	qArray[argKeyID] = argQ;
	frameArray[argKeyID] = argFrame;
	interTypeArray[argKeyID] = argType;

	return true;
}
bool CKeyFrameMotion::changeKeyFrame(int argKeyID, fk_Quaternion argQ, fk_Vector argBasePos, int argFrame, fk_MotionInterType argType)
{
	if(argKeyID >= getTotalKeyNum()) return false;

	qArray[argKeyID] = argQ;
	frameArray[argKeyID] = argFrame;
	interTypeArray[argKeyID] = argType;
	basePosArray[argKeyID] = argBasePos;

	return true;
}
bool CKeyFrameMotion::deleteKeyFrame(int argKeyID)
{
	if(argKeyID >= getTotalKeyNum()) return false;

	qArray.erase(qArray.begin()+argKeyID);
	frameArray.erase(frameArray.begin()+argKeyID);
	interTypeArray.erase(interTypeArray.begin()+argKeyID);

	if(isBaseMotion()) basePosArray.erase(basePosArray.begin()+argKeyID);

	return true;
}

bool CKeyFrameMotion::isBaseMotion(void)
{
	if(basePosArray.size() != 0) return true;
	else return false;
}
int CKeyFrameMotion::getTotalKeyNum(void)
{
	return (int)qArray.size();
}

int CKeyFrameMotion::getTotalFrameNum(void)
{
	int retVal = 0;

	for(int i = 0; i < getTotalKeyNum(); i++) {
		retVal += frameArray[i];
	}

	return retVal;
}
bool CKeyFrameMotion::getFrameToKeyID(int argFrame, int *outID, int *outFrame)
{
	if(argFrame >= getTotalFrameNum()) return false;

	int	nowKey = 0, nowFrame = 0;
	for(int i = 0; i < getTotalKeyNum(); i++) {
		nowKey = i;
		nowFrame += frameArray[i];
		if(argFrame < nowFrame) break;
	}

	*outID = nowKey;
	*outFrame = argFrame-(nowFrame-frameArray[nowKey]);

	return true;
}

void CKeyFrameMotion::setFrameState(int argFrame, fk_Model *argModel)
{
	static int				nowKey = 0, stepFrame = 0;
	static fk_Quaternion	q;
	
	if(!getFrameToKeyID(argFrame, &nowKey, &stepFrame)) return;

	if(nowKey != 0 && stepFrame == 0) {
		argModel->glAngle(qArray[nowKey-1].getEuler());
		if(isBaseMotion()) {
			argModel->glMoveTo(basePosArray[nowKey-1]);//-basePosArray[0]);
		}
	} else if(nowKey != 0) {
       	q = getInterQ(qArray[nowKey-1], qArray[nowKey], (double)(stepFrame+1)/(double)frameArray[nowKey], interTypeArray[nowKey]);
		argModel->glAngle(q.getEuler());

		if(isBaseMotion()) {
			argModel->glMoveTo(getInterVec(basePosArray[nowKey-1], basePosArray[nowKey],
				(double)(stepFrame+1)/(double)frameArray[nowKey], interTypeArray[nowKey]));//-basePosArray[0]);
		}
	} else {
		static fk_Quaternion nowQ;
		nowQ.makeEuler(argModel->getAngle());

       	q = getInterQ(nowQ, qArray[nowKey], (double)(stepFrame+1)/(double)frameArray[nowKey]);
		argModel->glAngle(q.getEuler());

		if(isBaseMotion()) {
			argModel->glMoveTo(getInterVec(argModel->getPosition(), basePosArray[nowKey],
				(double)(stepFrame+1)/(double)frameArray[nowKey]));//-basePosArray[0]);
		}
	}

	return;
}
