#include "MotionData.h"


CKeyFrameMotion::CKeyFrameMotion()
{
	init();

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

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

	return;
}

fk_Vector CKeyFrameMotion::getInterVec(const fk_Vector &vecA, const fk_Vector &vecB, double t, fk_MotionInterType argType)
{
	if(vecA == vecB) return vecA;

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

	return fk_Vector();
}

fk_Quaternion CKeyFrameMotion::getInterQ(const fk_Quaternion &qA, const fk_Quaternion &qB, double t, fk_MotionInterType argType)
{
	if(qA == qB) return qA;

	switch(argType) {
	case LINEAR:
	case SPHERE:
		return fk_Math::quatInterSphere(qA, qB, t);
	case POWER:
		return fk_Math::quatInterSphere(qA, qB, pow(t, 3));
	case REV_POWER:
		return fk_Math::quatInterSphere(qA, qB, (1.0-pow(1.0-t, 3)));
	case FREEZE:
	case HIDE:		// <----------------------------------------------------オブジェクトのすげ替え対応のため追加
		return qB;
	default:
		return fk_Quaternion();
	}
}

// スケールなし
void CKeyFrameMotion::pushKeyFrame(const fk_Quaternion &argQ, const fk_Vector &argBasePos, int argFrame, fk_MotionInterType argType, int argKeyPos)
{
	pushKeyFrame(argQ, argBasePos, fk_Vector(1.0, 1.0, 1.0), argFrame, argType, argKeyPos);
}
bool CKeyFrameMotion::changeKeyFrame(int argKeyID, const fk_Quaternion &argQ, const fk_Vector &argBasePos, int argFrame, fk_MotionInterType argType)
{
	return changeKeyFrame(argKeyID, argQ, argBasePos, fk_Vector(1.0, 1.0, 1.0), argFrame, argType);
}
// スケールあり
void CKeyFrameMotion::pushKeyFrame(const fk_Quaternion &argQ, const fk_Vector &argBasePos, const fk_Vector &argScale,
								   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);
		scaleArray.push_back(argScale);
	} 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);
		scaleArray.insert(scaleArray.begin()+argKeyPos, argScale);
	}
	return;
}
bool CKeyFrameMotion::changeKeyFrame(int argKeyID, const fk_Quaternion &argQ, const fk_Vector &argBasePos, const fk_Vector &argScale,
									 int argFrame, fk_MotionInterType argType)
{
	if(argKeyID >= getTotalKeyNum()) return false;

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

	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);
	scaleArray.erase(scaleArray.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(int argKeyID)
{
	int retVal = 0;

	if(argKeyID != -1) {
		for(int i = 0; i <= argKeyID; i++) {
			retVal += frameArray[i];
		}
	} else {
		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;
	if(getTotalFrameNum() == 0) 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;
}

int CKeyFrameMotion::setFrameState(int argFrame, fk_Model *argModel)
{
	static int				nowKey = 0, stepFrame = 0;
	static fk_Quaternion	q;
	static fk_Vector		sc;
	const fk_Vector			dirV(0.0, 0.0, -1.0);
	const fk_Vector			upV(0.0, 1.0, 0.0);
	
	if(!getFrameToKeyID(argFrame, &nowKey, &stepFrame)) return 0;

	if(nowKey != 0 && stepFrame == 0) {
		argModel->glVec(qArray[nowKey-1]*dirV);
		argModel->glUpvec(qArray[nowKey-1]*upV);
		if(isBaseMotion()) {
			argModel->glMoveTo(basePosArray[nowKey-1]);//-basePosArray[0]);
		}
		argModel->setScale(scaleArray[nowKey-1].x, scaleArray[nowKey-1].y, scaleArray[nowKey-1].z);
	} else if(nowKey != 0) {
       	q = getInterQ(qArray[nowKey-1], qArray[nowKey], (double)(stepFrame+1)/(double)frameArray[nowKey], interTypeArray[nowKey]);
		argModel->glVec(q*dirV);
		argModel->glUpvec(q*upV);
		if(isBaseMotion()) {
			argModel->glMoveTo(getInterVec(basePosArray[nowKey-1], basePosArray[nowKey],
				(double)(stepFrame+1)/(double)frameArray[nowKey], interTypeArray[nowKey]));//-basePosArray[0]);
		}
		sc = getInterVec(scaleArray[nowKey-1], scaleArray[nowKey], (double)(stepFrame)/(double)frameArray[nowKey], interTypeArray[nowKey]);
		argModel->setScale(sc.x, sc.y, sc.z);
	} else {
		static fk_Quaternion	nowQ;
		static fk_Vector		nowSc;
		nowQ.makeEuler(argModel->getAngle());
		nowSc.set(argModel->getScale(fk_X), argModel->getScale(fk_Y), argModel->getScale(fk_Z));

       	q = getInterQ(nowQ, qArray[nowKey], (double)(stepFrame+1)/(double)frameArray[nowKey]);
		argModel->glVec(q*dirV);
		argModel->glUpvec(q*upV);
		if(isBaseMotion()) {
			argModel->glMoveTo(getInterVec(argModel->getPosition(), basePosArray[nowKey],
				(double)(stepFrame+1)/(double)frameArray[nowKey]));//-basePosArray[0]);
		}
		sc = getInterVec(nowSc, scaleArray[nowKey], (double)(stepFrame)/(double)frameArray[nowKey], interTypeArray[nowKey]);
		argModel->setScale(sc.x, sc.y, sc.z);
	}

	if(interTypeArray[nowKey] == HIDE) {
		return -1;
	}

	return 1;
}
