using System;
using System.Numerics;

namespace Engine.Core;

// TODO Comments

/// <summary>
/// Represents a 4D left handed space matrix.
/// </summary>
/// <remarks>
/// Initializes a new instance of the <see cref="Matrix4x4"/> struct with the specified values.
/// </remarks>
[System.Diagnostics.DebuggerDisplay("{ToString(),nq}")]
public readonly struct Matrix4x4(
    float m11, float m12, float m13, float m14,
    float m21, float m22, float m23, float m24,
    float m31, float m32, float m33, float m34,
    float m41, float m42, float m43, float m44
) : IEquatable<Matrix4x4>
{
    public readonly float M11 = m11, M12 = m12, M13 = m13, M14 = m14;
    public readonly float M21 = m21, M22 = m22, M23 = m23, M24 = m24;
    public readonly float M31 = m31, M32 = m32, M33 = m33, M34 = m34;
    public readonly float M41 = m41, M42 = m42, M43 = m43, M44 = m44;

    /// <summary>
    /// Extracts the position (translation) from the <see cref="Matrix4x4"/>.
    /// </summary>
    public readonly Vector3D Position => new(M41, M42, M43);

    /// <summary>
    /// Extracts the scale from the <see cref="Matrix4x4"/>.
    /// </summary>
    public readonly Vector3D Scale
    {
        get
        {
            float scaleX = new Vector3D(M11, M12, M13).Length();
            float scaleY = new Vector3D(M21, M22, M23).Length();
            float scaleZ = new Vector3D(M31, M32, M33).Length();

            if (Determinant(this) < 0)
                scaleX *= -1;

            return new(scaleX, scaleY, scaleZ);
        }
    }

    /// <summary>
    /// Extracts the rotation from the <see cref="Matrix4x4"/>.
    /// </summary>
    public readonly Quaternion Rotation => Quaternion.FromRotationMatrix4x4(this).Normalized;

    /// <summary>
    /// Represents the identity <see cref="Matrix4x4"/>.
    /// </summary>
    public static Matrix4x4 Identity => new(
        1f, 0f, 0f, 0f,
        0f, 1f, 0f, 0f,
        0f, 0f, 1f, 0f,
        0f, 0f, 0f, 1f
    );

    public static Matrix4x4 operator *(Matrix4x4 a, Matrix4x4 b) => new(
        a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31 + a.M14 * b.M41,
        a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32 + a.M14 * b.M42,
        a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33 + a.M14 * b.M43,
        a.M11 * b.M14 + a.M12 * b.M24 + a.M13 * b.M34 + a.M14 * b.M44,

        a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31 + a.M24 * b.M41,
        a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32 + a.M24 * b.M42,
        a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33 + a.M24 * b.M43,
        a.M21 * b.M14 + a.M22 * b.M24 + a.M23 * b.M34 + a.M24 * b.M44,

        a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31 + a.M34 * b.M41,
        a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32 + a.M34 * b.M42,
        a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33 + a.M34 * b.M43,
        a.M31 * b.M14 + a.M32 * b.M24 + a.M33 * b.M34 + a.M34 * b.M44,

        a.M41 * b.M11 + a.M42 * b.M21 + a.M43 * b.M31 + a.M44 * b.M41,
        a.M41 * b.M12 + a.M42 * b.M22 + a.M43 * b.M32 + a.M44 * b.M42,
        a.M41 * b.M13 + a.M42 * b.M23 + a.M43 * b.M33 + a.M44 * b.M43,
        a.M41 * b.M14 + a.M42 * b.M24 + a.M43 * b.M34 + a.M44 * b.M44
    );

    public static bool operator ==(Matrix4x4 left, Matrix4x4 right) =>
        left.M11 == right.M11 && left.M12 == right.M12 && left.M13 == right.M13 && left.M14 == right.M14 &&
        left.M21 == right.M21 && left.M22 == right.M22 && left.M23 == right.M23 && left.M24 == right.M24 &&
        left.M31 == right.M31 && left.M32 == right.M32 && left.M33 == right.M33 && left.M34 == right.M34 &&
        left.M41 == right.M41 && left.M42 == right.M42 && left.M43 == right.M43 && left.M44 == right.M44;

    public static bool operator !=(Matrix4x4 left, Matrix4x4 right) =>
        left.M11 != right.M11 || left.M12 != right.M12 || left.M13 != right.M13 || left.M14 != right.M14 ||
        left.M21 != right.M21 || left.M22 != right.M22 || left.M23 != right.M23 || left.M24 != right.M24 ||
        left.M31 != right.M31 || left.M32 != right.M32 || left.M33 != right.M33 || left.M34 != right.M34 ||
        left.M41 != right.M41 || left.M42 != right.M42 || left.M43 != right.M43 || left.M44 != right.M44;

    public static implicit operator System.Numerics.Matrix4x4(Matrix4x4 m) => new(
        m.M11, m.M12, m.M13, m.M14,
        m.M21, m.M22, m.M23, m.M24,
        m.M31, m.M32, m.M33, m.M34,
        m.M41, m.M42, m.M43, m.M44
    );

    public static implicit operator Matrix4x4(System.Numerics.Matrix4x4 m) => new(
        m.M11, m.M12, m.M13, m.M14,
        m.M21, m.M22, m.M23, m.M24,
        m.M31, m.M32, m.M33, m.M34,
        m.M41, m.M42, m.M43, m.M44
    );

    /// <summary>
    /// Calculates the determinant of the <see cref="Matrix4x4"/>.
    /// </summary>
    /// <param name="m">The <see cref="Matrix4x4"/>.</param>
    /// <returns>The determinant of the <see cref="Matrix4x4"/>.</returns>
    public static float Determinant(Matrix4x4 m) => // https://www.euclideanspace.com/maths/algebra/matrix/functions/determinant/fourD/index.htm
         m.M14 * m.M23 * m.M32 * m.M41 - m.M13 * m.M24 * m.M32 * m.M41 -
         m.M14 * m.M22 * m.M33 * m.M41 + m.M12 * m.M24 * m.M33 * m.M41 +
         m.M13 * m.M22 * m.M34 * m.M41 - m.M12 * m.M23 * m.M34 * m.M41 -
         m.M14 * m.M23 * m.M31 * m.M42 + m.M13 * m.M24 * m.M31 * m.M42 +
         m.M14 * m.M21 * m.M33 * m.M42 - m.M11 * m.M24 * m.M33 * m.M42 -
         m.M13 * m.M21 * m.M34 * m.M42 + m.M11 * m.M23 * m.M34 * m.M42 +
         m.M14 * m.M22 * m.M31 * m.M43 - m.M12 * m.M24 * m.M31 * m.M43 -
         m.M14 * m.M21 * m.M32 * m.M43 + m.M11 * m.M24 * m.M32 * m.M43 +
         m.M12 * m.M21 * m.M34 * m.M43 - m.M11 * m.M22 * m.M34 * m.M43 -
         m.M13 * m.M22 * m.M31 * m.M44 + m.M12 * m.M23 * m.M31 * m.M44 +
         m.M13 * m.M21 * m.M32 * m.M44 - m.M11 * m.M23 * m.M32 * m.M44 -
         m.M12 * m.M21 * m.M33 * m.M44 + m.M11 * m.M22 * m.M33 * m.M44;

    public static Matrix4x4 CreateTranslation(Vector3D position) => new(
        1f, 0f, 0f, 0f,
        0f, 1f, 0f, 0f,
        0f, 0f, 1f, 0f,
        position.X, position.Y, position.Z, 1
    );

    public static Matrix4x4 CreateScale(Vector3D scale) => new(
        scale.X, 0f, 0f, 0f,
        0f, scale.Y, 0f, 0f,
        0f, 0f, scale.Z, 0f,
        0f, 0f, 0f, 1f
    );

    public static Matrix4x4 CreateRotationX(float radians)
    {
        float c = Math.Cos(radians);
        float s = Math.Sin(radians);

        return new Matrix4x4(
            1f, 0f, 0f, 0f,
            0f, c, s, 0f,
            0f, -s, c, 0f,
            0f, 0f, 0f, 1f
        );
    }

    public static Matrix4x4 CreateRotationY(float radians)
    {
        float c = Math.Cos(radians);
        float s = Math.Sin(radians);

        return new Matrix4x4(
            c, 0f, -s, 0f,
            0f, 1f, 0f, 0f,
            s, 0f, c, 0f,
            0f, 0f, 0f, 1f
        );
    }

    public static Matrix4x4 CreateRotationZ(float radians)
    {
        float c = Math.Cos(radians);
        float s = Math.Sin(radians);

        return new Matrix4x4(
            c, s, 0f, 0f,
            -s, c, 0f, 0f,
            0f, 0f, 1f, 0f,
            0f, 0f, 0f, 1f
        );
    }

    // TODO Find a better calculation for this 
    public static Matrix4x4 CreateRotation(Quaternion quaternion)
    {
        Vector3D angles = quaternion.ToAngles();
        return Identity * CreateRotationX(angles.X) * CreateRotationY(angles.Y) * CreateRotationZ(angles.Z);
    }

    public static Matrix4x4 CreateLookMatrix(Vector3D forward, Vector3D up)
    {
        Vector3D z = forward.Normalized;
        Vector3D x = up.Cross(z).Normalized;
        Vector3D y = z.Cross(x);

        return new Matrix4x4(
            x.X, y.X, z.X, 0f,
            x.Y, y.Y, z.Y, 0f,
            x.Z, y.Z, z.Z, 0f,
            0f, 0f, 0f, 1f
        );
    }

    public static Matrix4x4 CreateLookMatrix(Vector3D position, Vector3D target, Vector3D up)
    {
        Vector3D z = position.FromTo(target).Normalized;
        Vector3D x = up.Cross(z).Normalized;
        Vector3D y = z.Cross(x);

        return new Matrix4x4(
            x.X, y.X, z.X, 0f,
            x.Y, y.Y, z.Y, 0f,
            x.Z, y.Z, z.Z, 0f,
            -x.Dot(position), -y.Dot(position), -z.Dot(position), 1f
        );
    }

    public static Matrix4x4 CreatePerspectiveFieldOfView(float fieldOfViewInRadians, float aspectRatio, float nearPlane, float farPlane)
    {
        float yScale = 1f / Math.Tan(fieldOfViewInRadians / 2f);
        float xScale = yScale / aspectRatio;

        return new Matrix4x4(
            xScale, 0f, 0f, 0f,
            0f, yScale, 0f, 0f,
            0f, 0f, farPlane / (farPlane - nearPlane), 1f,
            0f, 0f, -nearPlane * farPlane / (farPlane - nearPlane), 0f
        );
    }

    public static Matrix4x4 ToRightHanded(Matrix4x4 m) => new(
        m.M11, m.M12, m.M13, m.M14,
        m.M31, m.M32, m.M33, m.M34,
        m.M21, m.M22, m.M23, m.M24,
        m.M41, m.M42, m.M43, m.M44
    );

    public override bool Equals(object? obj) => obj is Matrix4x4 matrix && this == matrix;
    public bool Equals(Matrix4x4 other) => this == other;

    public override int GetHashCode() => HashCode.Combine(
        HashCode.Combine(M11, M12, M13, M14, M21, M22, M23, M24),
        HashCode.Combine(M31, M32, M33, M34, M41, M42, M43, M44)
    );

    public override string ToString() => $"Matrix4x4({M11}, {M12}, {M13}, {M14},{M21}, {M22}, {M23}, {M24},{M31}, {M32}, {M33}, {M34},{M41}, {M42}, {M43}, {M44})";
}

/// <summary>
/// Provides extension methods for <see cref="Matrix4x4"/> type.
/// </summary>
public static class Matrix4x4Extensions
{
    /// <inheritdoc cref="Matrix4x4.Determinant(Matrix4x4)" />
    public static float Determinant(this Matrix4x4 matrix) => Matrix4x4.Determinant(matrix);

    /// <inheritdoc cref="Matrix4x4.CreateTranslation(Vector3D)" />
    public static Matrix4x4 ApplyTranslation(this Matrix4x4 matrix, Vector3D translation) => matrix * Matrix4x4.CreateTranslation(translation);

    /// <inheritdoc cref="Matrix4x4.CreateScale(Vector3D)" />
    public static Matrix4x4 ApplyScale(this Matrix4x4 matrix, Vector3 scale) => matrix * Matrix4x4.CreateScale(scale);

    /// <inheritdoc cref="Matrix4x4.CreateRotationZ(float)" />
    public static Matrix4x4 ApplyRotationX(this Matrix4x4 matrix, float radians) => matrix * Matrix4x4.CreateRotationX(radians);

    /// <inheritdoc cref="Matrix4x4.CreateRotationY(float)" />
    public static Matrix4x4 ApplyRotationY(this Matrix4x4 matrix, float radians) => matrix * Matrix4x4.CreateRotationY(radians);

    /// <inheritdoc cref="Matrix4x4.CreateRotationZ(float)" />
    public static Matrix4x4 ApplyRotationZ(this Matrix4x4 matrix, float radians) => matrix * Matrix4x4.CreateRotationZ(radians);

    /// <inheritdoc cref="Matrix4x4.CreateRotation(Quater)" />
    public static Matrix4x4 ApplyRotation(this Matrix4x4 matrix, Quaternion quaternion) => matrix * Matrix4x4.CreateRotation(quaternion);

    /// <inheritdoc cref="Matrix4x4.CreateLookMatrix( Vector3D, Vector3D)" />
    public static Matrix4x4 ApplyLookRotationTo(this Matrix4x4 matrix, Vector3D forward, Vector3 up) => matrix * Matrix4x4.CreateLookMatrix(forward, up);

    /// <inheritdoc cref="Matrix4x4.CreateLookMatrix(Vector3D, Vector3D, Vector3D)" />
    public static Matrix4x4 CreateLookMatrixTo(this Vector3D from, Vector3D to, Vector3 up) => Matrix4x4.CreateLookMatrix(from, to, up);

    /// <inheritdoc cref="Matrix4x4.CreatePerspectiveFieldOfView(float, float, float, float)" />
    public static Matrix4x4 ApplyPerspectiveFieldOfView(this Matrix4x4 matrix, float fieldOfViewInRadians, float aspectRatio, float nearPlane, float farPlane)
        => matrix * Matrix4x4.CreatePerspectiveFieldOfView(fieldOfViewInRadians, aspectRatio, nearPlane, farPlane);

    /// <inheritdoc cref="Matrix4x4.ToRightHanded(Matrix4x4) />
    public static Matrix4x4 ToRightHanded(this Matrix4x4 matrix) => Matrix4x4.ToRightHanded(matrix);
}