#include "MotionPath.h"
#include "Matrix4x4.h"


namespace CHCDemoEngine 
{


MotionPath::MotionPath(const VertexArray &vertices):
mVertices(vertices), mCurrentVertexIdx(0), mT(.0f)
{
}


void MotionPath::Move(float velocity)
{
	mT += velocity;

	if (mT >= 1.0f)
	{
		mCurrentVertexIdx = (mCurrentVertexIdx + 1) % (int)mVertices.size();
		mT -= 1.0f;
	}
}


Vector3 MotionPath::GetCurrentPosition() const
{
	return (1.0f - mT) * mVertices[mCurrentVertexIdx] + mT * mVertices[(mCurrentVertexIdx + 1) % mVertices.size()];
}


Vector3 MotionPath::GetCurrentDirection() const
{
	return Vector3(1, 0, 0);
}


void MotionPath::Reset()
{
	mCurrentVertexIdx = 0;
	mT = 0;
}


Matrix4x4 MotionPath::GetOrientation()
{
	Vector3 dir = mVertices[(mCurrentVertexIdx + 1) % mVertices.size()] - mVertices[mCurrentVertexIdx];
	Vector3 ndir = -Normalize(dir);

	float pitch = -atan2(ndir.x, ndir.y);
	float yaw = atan2(ndir.z, sqrt((ndir.x * ndir.x) + (ndir.y * ndir.y)));

	
	Matrix4x4 roty = RotationYMatrix(pitch);
	Matrix4x4 rotx = RotationXMatrix(yaw);

	return roty * rotx;
}


}