#include "SplineCamSystem.h"

SplineCamSystem::SplineCamSystem(PScene scene)
	: System::ComponentSystem<Component::Camera, Component::Transform, PathFollow>(scene)
{}

SplineCamSystem::~SplineCamSystem()
{}

void SplineCamSystem::update(Component::Camera& cam, Component::Transform& transform, PathFollow& follow)
{
	if (follow.path == nullptr)
		return;
	Path& path = follow.path->accessComponent<Path>();

	//for (size_t i = 0; i < path.points.size() - 1; ++i)
	//{
	//	addDebugVertex(DebugVertex{
	//		.position = path.points[i],
	//		.color = Vector(0, 1, 0)
	//		});
	//	addDebugVertex(DebugVertex{
	//		.position = path.points[i + 1],
	//		.color = Vector(0, 1, 0)
	//		});
	//}

	updateFollowDistance(follow, path.getPathLength());
	std::tuple<int, float> pt = findP0AndTForDistance(path, follow.distance);
	auto [p0Index, t] = pt;
	if (p0Index < 0 || (path.points.size() <= p0Index + 3 && !path.looping))
		// Not enough points to interpolate...
		return;

	Vector p0 = path.points[p0Index];
	Vector p1 = path.points[(p0Index + 1) % path.points.size()];
	Vector p2 = path.points[(p0Index + 2) % path.points.size()];
	Vector p3 = path.points[(p0Index + 3) % path.points.size()];

	Vector oldPos = transform.getPosition() - follow.pathOffset;
	Vector newPos = path.catmullrom(t, p0, p1, p2, p3, path.tension);
	transform.setPosition(newPos + follow.pathOffset);

	if ((oldPos - follow.lastPos).length() > Path::LAST_POS_EPSILON)
	{
		follow.lastPos = oldPos;
	}
	Vector tangentVector = glm::normalize(follow.lastPos - newPos);
	Quaternion newRot = glm::quatLookAt(tangentVector, Vector(0.0f, 1.0f, 0.0f));
	transform.setRotation(newRot);
}

std::tuple<int, float> SplineCamSystem::findP0AndTForDistance(const Path& path, float distance)
{
	if (path.arcLengthTable.size() == 0)
		return std::tuple<int, float>(-1, 0.0f);

	int p0 = -1;
	float t = 0.0f;

	int pIndexToCheck = 1;//path.pointOffset + 1;

	//if (path.looping)
	//{
	//	// If looping, subtract loop distance until we are in the final "lap"
	//	float lapDistance = path.arcLengthTable[path.arcLengthTable.size() - 1][path.arcLengthTable[path.arcLengthTable.size() - 1].size() - 1];
	//	while (distance >= lapDistance)
	//	{
	//		distance -= lapDistance;
	//	}
	//
	//	// If we are in a new loop, the path.pointOffset might not be valid anymore
	//	if (path.arcLengthTable[path.pointOffset][0] > distance)
	//	{
	//		pIndexToCheck = 1;
	//	}
	//}
	float lapDistance = path.arcLengthTable[path.arcLengthTable.size() - 1][path.arcLengthTable[path.arcLengthTable.size() - 1].size() - 1];
	if (distance >= lapDistance)
	{
		getGlobals().running = false;
	}

	while (pIndexToCheck < path.arcLengthTable.size()
		&& path.arcLengthTable[pIndexToCheck][0] < distance)
	{
		pIndexToCheck++;
	}
	p0 = pIndexToCheck - 1;

	int arcTableIndex = 1;
	float additionalT = 0.0f;
	// TODO this could be improved e.g. by using binary search
	while (arcTableIndex < path.arcLengthTable[p0].size()
		&& path.arcLengthTable[p0][arcTableIndex] < distance)
	{
		arcTableIndex++;
	}
	arcTableIndex--;

	// Interpolate between arc length table entries to find additionalT
	if (arcTableIndex + 1 < path.arcLengthTable[p0].size())
	{
		float distanceA = path.arcLengthTable[p0][arcTableIndex];
		float distanceB = path.arcLengthTable[p0][arcTableIndex + 1];

		float abDistance = distanceB - distanceA;
		float requiredDistanceAfterA = distance - distanceA;
		additionalT = Path::ARC_LENGTH_TABLE_DISTANCE * (requiredDistanceAfterA / abDistance);
	}

	t = (arcTableIndex * Path::ARC_LENGTH_TABLE_DISTANCE) + additionalT;

	assert(p0 < path.points.size());
	assert(t >= 0.0f && t <= 1.0f);
	return std::tuple<int, float>(p0, t);
}

void SplineCamSystem::updateFollowDistance(PathFollow& follow, float pathLength)
{
	follow.time = follow.time + deltaTime;
	switch (follow.speedControl)
	{
	case PathFollow::SpeedControl::Linear:
		// We increase distance instead of setting it based on overall time, because Linear SpeedControl allows for traversalSpeed change
		follow.distance += (deltaTime * follow.traversalSpeed);
		break;
	case PathFollow::SpeedControl::EaseInEaseOut:
	{
		float timeNeededForPath = pathLength / follow.traversalSpeed;
		float t = follow.time / timeNeededForPath;
		int laps = (int)t;
		t = t - laps;
		float s = (glm::sin(t * glm::pi<float>() - glm::pi<float>() / 2.f) + 1) / 2.f;
		follow.distance = ((laps + s) * pathLength);
		break;
	}
	default:
		assert(false && "Unknown SpeedControl");
	}
}