// =====================================================================================================================
//  fennec, a free and open source game engine
//  Copyright © 2025 - 2026  Medusa Slockbower
//
//  This program is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <https://www.gnu.org/licenses/>.
// =====================================================================================================================

#ifndef FENNEC_MATH_EXT_TRANSFORM_H
#define FENNEC_MATH_EXT_TRANSFORM_H

#include <fennec/math/matrix.h>
#include <fennec/math/trigonometric.h>
#include <fennec/math/ext/constants.h>
#include <fennec/math/ext/quaternion.h>

namespace fennec
{

///
/// \brief Create a 2d translation matrix
/// \param x a vector containing the position
/// \returns An identity matrix with the last column set to x
template<typename genType>
constexpr mat<genType, 3, 3> translation(const vector<genType, 0, 1>& x) {
	return mat<genType, 3, 3>(
		1, 0, 0,
		0, 1, 0,
		x,    1
	);
}

///
/// \brief Create a 3d translation matrix
/// \param x a vector containing the position
/// \returns An identity matrix with the last column set to x
template<typename genType>
constexpr mat<genType, 4, 4> translation(const vector<genType, 0, 1, 2>& x) {
	return mat<genType, 4, 4>(
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		x,       1
	);
}

///
/// \brief Create a 2d scaling matrix
/// \param x a vector containing the scale for each axis
/// \returns A diagonal matrix with the terms of x
template<typename genType>
constexpr mat<genType, 3, 3> scaling(const vector<genType, 0, 1>& x) {
	return mat<genType, 3, 3>(
		x.x, 0,   0,
		0,   x.y, 0,
		0,   0,   1
	);
}

///
/// \brief Create a 3d scaling matrix
/// \param x a vector containing the scale for each axis
/// \returns A diagonal matrix with the terms of x
template<typename genType>
constexpr mat<genType, 4, 4> scaling(const vector<genType, 0, 1, 2>& x) {
	return mat<genType, 4, 4>(
		x.x, 0, 0, 0,
		0, x.y, 0, 0,
		0, 0, x.z, 0,
		0, 0, 0,   1
	);
}

template<typename genType>
constexpr mat<genType, 3, 3> shear(const vector<genType, 0, 1>& x) {
	return mat<genType, 3, 3>(
		1,   x.x, 0,
		x.y, 1,   0,
		0,   0,   1
	);
}

///
/// \brief Create a 2d rotation matrix
/// \param a the angle of the rotation in radians
/// \returns A rotation matrix that rotates about the "z-axis"
template<typename genType>
constexpr mat<genType, 3, 3> rotation(const genType a) {

	// Calculate sin and cos terms
	const genType c = fennec::cos(a);
	const genType s = fennec::sin(a);

	// Calculate the matrix
	return mat<genType, 3, 3>(
		c, -s, 0,
		s,  c, 0,
		0,  0, 1
	);
}

///
/// \brief Create a 3d rotation matrix from axis-angle
/// \param A The axis vector
/// \param a The rotation about the axis in radians
/// \returns A rotation matrix about A
template<typename genType>
constexpr mat<genType, 4, 4> rotation(const vector<genType, 0, 1, 2>& A, genType a) {
	// https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle

	// Calculate sin and cos terms
	const genType c = fennec::cos(a);
	const genType s = fennec::sin(a);

	// Calculate axis term
	const vector<genType, 0, 1, 2> u = (fennec::one<genType>() - c) * A;
	const vector<genType, 0, 1, 2> v = s * A;

	// Calculate the Matrix
	return mat<genType, 4, 4>(
		A.x * u.x + c,   A.x * u.y + v.z, A.x * u.z - v.y, 0,
		A.x * u.y - v.z, A.y * u.y + c,   A.y * u.z + v.x, 0,
		A.x * u.z + v.y, A.y * u.z - v.x, A.z * u.z + c,   0,
		0,               0,               0,               1
	);
}

// template<typename genType>
// constexpr qua<genType> rotation(const vector<genType, 0, 1, 2>& axis, genType angle) {
// 	vector<genType, 0, 1, 2> a = fennec::normalize(axis);
// 	const genType   s = fennec::sin(angle);
//
// 	return qua<genType>(fennec::cos(angle * genType(0.5)), s * a);
// }

/// \brief enum to denote the unit-axis of rotation
enum rotation_
{
	rotation_x = 0
,	rotation_y
,	rotation_z
};

///
/// \brief Create a 3d rotation matrix for an angle about a cartesian unit axis
/// \tparam axis the unit axis to rotate around
/// \param a the angle, in radians
/// \returns a matrix that rotates vectors around a unit axis
template<typename genType, rotation_ axis>
constexpr mat<genType, 4, 4> rotation(genType a) {
	// https://en.wikipedia.org/wiki/Rotation_matrix#Basic_3D_rotations

	// Calculate sin and cos terms
	const genType c = fennec::cos(a);
	const genType s = fennec::sin(a);

	// Calculate the matrix
	if constexpr(axis == rotation_x) {
		return mat<genType, 4, 4>(
			1, 0,  0, 0,
			0, c, -s, 0,
			0, s,  c, 0,
			0, 0,  0, 1
		);
	}
	else if constexpr(axis == rotation_y) {
		return mat<genType, 4, 4>(
			 c, 0, s, 0,
			 0, 0, 0, 0,
			-s, 0, c, 0,
			 0, 0, 0, 1
		);
	}
	else if constexpr(axis == rotation_z) {
		return mat<genType, 4, 4>(
			c, -s, 0, 0,
			s,  c, 0, 0,
			0,  0, 1, 0,
			0,  0, 0, 1
		);
	}

	return mat<genType, 4, 4>();
}

///
/// \brief enum to denote the order rotations are applied in
enum order_
{
	order_xyz
,	order_xzy
,	order_yxz
,	order_yzx
,	order_zxy
,	order_zyx
};

///
/// \brief Create 3d rotation matrix from euler angles
/// \tparam order the order in which to apply the rotations
/// \param E the euler angles, taken as tait-bryan (pitch, yaw, roll)
/// \returns a matrix that rotates vectors
template<typename genType, order_ order = order_zxy>
constexpr mat<genType, 4, 4> rotation(const vector<genType, 0, 1, 2>& E) {
	// expanded using a CAS

	// Calculate sin and cos terms
	const genType ca = fennec::cos(E.x);
	const genType sa = fennec::sin(E.x);
	const genType cb = fennec::cos(E.y);
	const genType sb = fennec::sin(E.y);
	const genType cg = fennec::cos(E.z);
	const genType sg = fennec::sin(E.z);

	// Calculate the matrix
	if constexpr(order == order_xyz) {
		return mat<genType, 4, 4>(
			cb*cg,           -cg*sg,             sb,    0,
			ca*sg + cg*sa*sb, ca*cg - sa*sb*sg, -ca*sa, 0,
			sa*sg - ca*cg*sb, ca*sb*sg + cg*sa,  ca*cb, 0,
			0,                0,                 0,     1
		);
	} else if constexpr(order == order_xzy) {
		return mat<genType, 4, 4>(
			cb*cg,           -sg,    cg*sb,            0,
			ca*cb*sg + sa*sb, ca*cg, cb*sa*sg - ca*sb, 0,
			cb*sa*sg - ca*sb, cg*sa, ca*cb + sa*sb*sg, 0,
			0,                0,     0,                1
		);
	} else if constexpr(order == order_yxz) {
		return mat<genType, 4, 4>(
			cb*cg + sa*sb*sg, cg*sa*sb - cb*sg,  ca*sb, 0,
			ca*sg,            ca*cg,            -sa,    0,
			cb*sa*sg - cg*sb, cb*cg*sa + sb*sg,  ca*cb, 0,
			0,                0,                 0,     1
		);
	} else if constexpr(order == order_yzx) {
		return mat<genType, 4, 4>(
			 cb*cg, sa*sb - ca*cb*sg, ca*sb + cb*sa*sg, 0,
			 sg,    ca*cg,           -cg*sa,            0,
			-cg*sb, ca*sb*sg + cb*sa, ca*cb - sa*sb*sg, 0,
			 0,     0,                0,                1
		);
	} else if constexpr(order == order_zxy) {
		return mat<genType, 4, 4>(
			 cb*cg - sa*sb*sg, -ca*sg, cb*sa*sg + cg*sb, 0,
			 cb*sg + cg*sa*sb,  ca*cg, sb*sg - cb*cg*sa, 0,
			-ca*sb,             sa,    ca*cb,            0,
			 0,                 0,     0,                1
		);
	} else if constexpr(order == order_zyx) {
		return mat<genType, 4, 4>(
			 cb*cg, cg*sa*sb - ca*sg, ca*cg*sb + sa*sg, 0,
			 cb*sg, ca*cg + sa*sb*sg, ca*sb*sg - cg*sa, 0,
			-sb,    cb*sa,            ca*cb,            0,
			 0,     0,                0,                1
		);
	}

	return mat<genType, 4, 4>();
}

}

#endif // FENNEC_MATH_EXT_TRANSFORM_H
