/*
 * 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 "Math.hpp"
#include "CoordinateSystem.hpp"

namespace OpenVulkano::Math
{
	class CoordinateSystemConverter
	{
		inline static Matrix3i ROT_XP90 = { 1, 0, 0, 0, 0, 1, 0, -1, 0 };
		inline static Matrix3i ROT_XN90 = { 1, 0, 0, 0, 0, -1, 0, 1, 0 };
		CoordinateSystem from, to;
	
	public:
		constexpr CoordinateSystemConverter(CoordinateSystem from, CoordinateSystem to) : from(from), to(to) {}
		
		template<typename T, glm::qualifier Q>
		[[nodiscard]] Vector<3, T, Q> Convert(const Vector<3, T, Q>& v) const
		{
			if (from == to) return v;
			return ConvertHandedness(ConvertUp(v));
		}
		
		template<typename T, glm::qualifier Q>
		[[nodiscard]] Vector<4, T, Q> Convert(const Vector<4, T, Q>& v) const
		{
			if (from == to) return v;
			return { ConvertHandedness(ConvertUp(reinterpret_cast<const glm::vec<3,T,Q>&>(v))), v.w };
		}

		template<typename T, glm::qualifier Q>
		[[nodiscard]] Matrix<3, T, Q> Convert(const Matrix<3, T, Q>& mat) const
		{
			if (from == to) return mat;
			return ConvertHandedness(ConvertUp(mat));
		}

		template<typename T, glm::qualifier Q>
		[[nodiscard]] Matrix<4, T, Q> Convert(const Matrix<4, T, Q>& mat) const
		{
			if (from == to) return mat;
			return ConvertHandedness(ConvertUp(mat));
		}
		
	private:
		// Conversion functions
		template<typename T, glm::qualifier Q>
		[[nodiscard]] Vector<3, T, Q> ConvertHandedness(const Vector<3, T, Q>& v) const
		{
			if (from.GetHandedness() == to.GetHandedness()) return v;
			if (to == CoordinateSystem::UpAxis::Y)
				return { v.x, v.y, -v.z };
			return { v.x, -v.y, v.z };
		}

		template<typename T, glm::qualifier Q>
		[[nodiscard]] Vector<3, T, Q> ConvertUp(const Vector<3, T, Q>& v) const
		{
			if (from.GetUpAxis() == to.GetUpAxis()) return v;
			if (from.GetUpAxis() == CoordinateSystem::UpAxis::Y)
				return { v.x,  -v.z, v.y };
			return { v.x, v.z,  -v.y };
		}

		template<int S, typename T, glm::qualifier Q>
		[[nodiscard]] Matrix<S, T, Q> ConvertHandedness(Matrix<S, T, Q> mat) const
		{
			constexpr int AXIS_Y = 1;
			constexpr int AXIS_Z = 2;
			if (from.GetHandedness() == to.GetHandedness()) return mat;
			if (to == CoordinateSystem::UpAxis::Y)
				for(int i = 0; i < S; i++) mat[i][AXIS_Z] = -mat[i][AXIS_Z];
			else
				for(int i = 0; i < S; i++) mat[i][AXIS_Y] = -mat[i][AXIS_Y];
			return mat;
		}

		template<int S, typename T, glm::qualifier Q>
		[[nodiscard]] Matrix<S, T, Q> ConvertUp(const Matrix<S, T, Q>& mat) const
		{
			if (from.GetUpAxis() == to.GetUpAxis()) return mat;
			Matrix<3, T, Q> conv((from.GetUpAxis() == CoordinateSystem::UpAxis::Y) ? ROT_XP90 : ROT_XN90);

			Matrix<S, T, Q> ret(conv * Matrix<3, T, Q>(mat));
			if constexpr (S == 4)
			{
				ret[3] = { ConvertUp(Vector<3, T, Q>(mat[3])), 1 };
			}
			return ret;
		}
	};
}