// =====================================================================================================================
//  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_DETAIL_MATRIX_H
#define FENNEC_MATH_DETAIL_MATRIX_H

#include <fennec/math/detail/_forward.h>

namespace fennec
{

// specialization for mat2
template<typename scalar, size_t rows, size_t...cols> requires(rows == 2 && sizeof...(cols) == 2)
constexpr scalar determinant(const matrix<scalar, rows, cols...>& m) {
	return m[0][0] * m[1][1] - m[1][0] * m[0][1];
}

// specialization for mat3
template<typename scalar, size_t rows, size_t...cols> requires(rows == 3 && sizeof...(cols) == 3)
constexpr scalar determinant(const matrix<scalar, rows, cols...>& m) {
	// Cofactor expansion over first column
	return m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
	-      m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
	+      m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
}

// specialization for mat4
template<typename scalar, size_t rows, size_t...cols> requires(rows == 4 && sizeof...(cols) == 4)
constexpr scalar determinant(const matrix<scalar, rows, cols...>& m) {
	// Cofactor expansion over the first column
	scalar d00 = m[2][2] * m[3][3] - m[3][2] * m[2][3];
	scalar d01 = m[2][1] * m[3][3] - m[3][1] * m[2][3];
	scalar d02 = m[2][1] * m[3][2] - m[3][1] * m[2][2];
	scalar d03 = m[2][0] * m[3][3] - m[3][0] * m[2][3];
	scalar d04 = m[2][0] * m[3][2] - m[3][0] * m[2][2];
	scalar d05 = m[2][0] * m[3][1] - m[3][0] * m[2][1];

	scalar f0 = +(m[1][1]*d00 - m[1][2]*d01 + m[1][3]*d02);
	scalar f1 = -(m[1][0]*d00 - m[1][2]*d03 + m[1][3]*d04);
	scalar f2 = +(m[1][0]*d01 - m[1][1]*d03 + m[1][3]*d05);
	scalar f3 = -(m[1][0]*d02 - m[1][1]*d04 + m[1][2]*d05);

	return m[0][0]*f0 + m[0][1]*f1 + m[0][2]*f2 + m[0][3]*f3;
}


// specialization for mat2
template<typename scalar, size_t rows, size_t...cols> requires(rows == 2 && sizeof...(cols) == 2)
constexpr matrix<scalar, rows, cols...> inverse(const matrix<scalar, rows, cols...>& m) {
	scalar di = scalar(1) / fennec::determinant(m);
	return matrix<scalar, rows, cols...>(
		m[1][1] * di, -m[0][1] * di, -m[1][0] * di, m[0][0] * di
	);
}

// specialization for mat3
template<typename scalar, size_t rows, size_t...cols> requires(rows == 3 && sizeof...(cols) == 3)
constexpr matrix<scalar, rows, cols...> inverse(const matrix<scalar, rows, cols...>& m) {
	scalar di = scalar(1) / fennec::determinant(m);

	matrix<scalar, rows, cols...> i(
		 (m[1][1] * m[2][2] - m[2][1] * m[1][2]) * di
	,	-(m[0][1] * m[2][2] - m[2][1] * m[0][2]) * di
	,	 (m[0][1] * m[1][2] - m[1][1] * m[0][2]) * di

	,	-(m[1][0] * m[2][2] - m[2][0] * m[1][2]) * di
	,	 (m[0][0] * m[2][2] - m[2][0] * m[0][2]) * di
	,	-(m[0][0] * m[1][2] - m[1][0] * m[0][2]) * di

	,	 (m[1][0] * m[2][1] - m[2][0] * m[1][1]) * di
	,	-(m[0][0] * m[2][1] - m[2][0] * m[0][1]) * di
	,	 (m[0][0] * m[1][1] - m[1][0] * m[0][1]) * di
	);

	return i;
}

template<typename scalar, size_t rows, size_t...cols> requires(rows == 4 && sizeof...(cols) == 4)
constexpr matrix<scalar, rows, cols...> inverse(const matrix<scalar, rows, cols...>& m) {
	scalar s0 = m[0][0] * m[1][1] - m[1][0] * m[0][1];
	scalar s1 = m[0][0] * m[1][2] - m[1][0] * m[0][2];
	scalar s2 = m[0][0] * m[1][3] - m[1][0] * m[0][3];
	scalar s3 = m[0][1] * m[1][2] - m[1][1] * m[0][2];
	scalar s4 = m[0][1] * m[1][3] - m[1][1] * m[0][3];
	scalar s5 = m[0][2] * m[1][3] - m[1][2] * m[0][3];

	scalar c5 = m[2][2] * m[3][3] - m[3][2] * m[2][3];
	scalar c4 = m[2][1] * m[3][3] - m[3][1] * m[2][3];
	scalar c3 = m[2][1] * m[3][2] - m[3][1] * m[2][2];
	scalar c2 = m[2][0] * m[3][3] - m[3][0] * m[2][3];
	scalar c1 = m[2][0] * m[3][2] - m[3][0] * m[2][2];
	scalar c0 = m[2][0] * m[3][1] - m[3][0] * m[2][1];

	// Should check for 0 determinant
	scalar invdet = (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0);
	invdet = invdet ? scalar(1.0) / invdet : scalar(0); // this will get reduced to cmov, div still happens

	matrix<scalar, rows, cols...> i(
		( m[1][1] * c5 - m[1][2] * c4 + m[1][3] * c3) * invdet
	,	(-m[0][1] * c5 + m[0][2] * c4 - m[0][3] * c3) * invdet
	,	( m[3][1] * s5 - m[3][2] * s4 + m[3][3] * s3) * invdet
	,	(-m[2][1] * s5 + m[2][2] * s4 - m[2][3] * s3) * invdet

	,	(-m[1][0] * c5 + m[1][2] * c2 - m[1][3] * c1) * invdet
	,	( m[0][0] * c5 - m[0][2] * c2 + m[0][3] * c1) * invdet
	,	(-m[3][0] * s5 + m[3][2] * s2 - m[3][3] * s1) * invdet
	,	( m[2][0] * s5 - m[2][2] * s2 + m[2][3] * s1) * invdet

	,	( m[1][0] * c4 - m[1][1] * c2 + m[1][3] * c0) * invdet
	,	(-m[0][0] * c4 + m[0][1] * c2 - m[0][3] * c0) * invdet
	,	( m[3][0] * s4 - m[3][1] * s2 + m[3][3] * s0) * invdet
	,	(-m[2][0] * s4 + m[2][1] * s2 - m[2][3] * s0) * invdet

	,	(-m[1][0] * c3 + m[1][1] * c1 - m[1][2] * c0) * invdet
	,	( m[0][0] * c3 - m[0][1] * c1 + m[0][2] * c0) * invdet
	,	(-m[3][0] * s3 + m[3][1] * s1 - m[3][2] * s0) * invdet
	,	( m[2][0] * s3 - m[2][1] * s1 + m[2][2] * s0) * invdet
	);

	return i;
}


}


#endif // FENNEC_MATH_DETAIL_MATRIX_H
