/*
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at https://mozilla.org/MPL/2.0/.
 */

#pragma once

#include "Timestamp.hpp"
#include "Math.hpp"

namespace OpenVulkano::Math
{
	template<typename T, typename = std::enable_if<std::is_floating_point_v<T>>>
	class Pose
	{
		Quaternion<T> m_orientation;
		Vector3_SIMD<T> m_position;

	public:
		Pose() {}

		Pose(const Quaternion<T>& orientation, const Vector3_SIMD<T>& position)
			: m_orientation(orientation), m_position(position)
		{}

		Pose(const Math::Vector3_SIMD<T>& eulerAngle, const Vector3f_SIMD& position)
			: m_orientation(Math::Utils::qua(eulerAngle)), m_position(position)
		{}

		Pose(const Math::Matrix4<T> &matrix)
		{
			Math::Vector3_SIMD<T> scale;
			Math::Quaternion<T> rotation;
			Math::Vector3_SIMD<T> translation;
			Math::Vector3_SIMD<T> skew;
			Math::Vector4_SIMD<T> perspective;
		
			bool decomposed = Math::Utils::decompose(matrix, scale, rotation, translation, skew, perspective);
			if(decomposed)
			{
				m_orientation = rotation;
				m_position = translation;
			}
		}

		bool operator==(const Math::Pose<T> &otherPose) const
		{
			return (GetOrientation() == otherPose.GetOrientation()) && (GetPosition() == otherPose.GetPosition());
		}

		[[nodiscard]] const Quaternion<T>& GetOrientation() const { return m_orientation; }

		void SetOrientation(const Math::Quaternion<T>& orientation) { m_orientation = orientation; }

		void SetOrientation(const Math::Vector3_SIMD<T>& eulerAngle) { m_orientation = Math::Utils::qua(eulerAngle); }

		[[nodiscard]] const Vector3_SIMD<T>& GetPosition() const { return m_position; }

		void SetPosition(const Math::Vector3_SIMD<T>& pos) { m_position = pos; }

		[[nodiscard]] Pose<T> Interpolate(const Pose<T>& otherPose, T mixFactor) const
		{
			return Pose<T>(
						Utils::mix(m_orientation, otherPose.m_orientation, mixFactor),
						Utils::mix(m_position, otherPose.m_position, mixFactor)
					);
		}

		[[nodiscard]] Math::Matrix4<T> ToMatrix() const
		{
			Math::Matrix4<T> mat = Utils::toMat4(m_orientation);
			mat[3] = Math::Vector4<T>(m_position, 1);
			return mat;
		}
	};

	typedef Pose<float> PoseF;
	typedef Pose<double> PoseD;

	template<typename T, typename = std::enable_if<std::is_floating_point_v<T>>>
	class PoseWithTimestamp final : public Pose<T>
	{
		Timestamp m_timestamp;

	public:
		PoseWithTimestamp(Timestamp timestamp, Quaternion<T> orientation, Vector3_SIMD<T> position)
			: m_timestamp(timestamp), Pose<T>(orientation, position)
		{}

		PoseWithTimestamp(Timestamp timestamp, Math::Vector3_SIMD<T> eulerAngle, Vector3_SIMD<T> position)
			: m_timestamp(timestamp), Pose<T>(eulerAngle, position)
		{}

		PoseWithTimestamp(Timestamp timestamp, Pose<T> pose)
			: m_timestamp(timestamp), Pose<T>(pose)
		{}

		[[nodiscard]] PoseWithTimestamp<T> Interpolate(const PoseWithTimestamp& otherPose, Timestamp targetTime) const
		{
			PoseWithTimestamp* start = *this;
			PoseWithTimestamp* end = &otherPose;
			if (start->m_timestamp == end->m_timestamp) return *this; // Nothing to interpolate
			if (start->m_timestamp > end->m_timestamp)
			{
				std::swap(start, end);
			}
			if (targetTime <= start->m_timestamp) return *start;
			if (targetTime >= end->m_timestamp) return *end;
			T mixFactor = (targetTime - start->m_timestamp) / (end->m_timestamp - start->m_timestamp);
			return PoseWithTimestamp<T>(targetTime, start->Interpolate(*end, mixFactor));
		}

		[[nodiscard]] Timestamp GetTimestamp() { return m_timestamp; }
	};

	typedef PoseWithTimestamp<float> PoseWithTimestampF;
	typedef PoseWithTimestamp<double> PoseWithTimestampD;
}