using System;

namespace Syntriax.Engine.Core;

/// <summary>
/// Represents a 3D space rotation.
/// </summary>
/// <param name="x">X(i) position of the <see cref="Quaternion"/>.</param>
/// <param name="y">Y(j) position of the <see cref="Quaternion"/>.</param>
/// <param name="z">Z(k) position of the <see cref="Quaternion"/>.</param>
/// <param name="w">W(a) position of the <see cref="Quaternion"/>.</param>
/// <remarks>
/// Initializes a new instance of the <see cref="Quaternion"/> struct with the specified positions.
/// </remarks>
[System.Diagnostics.DebuggerDisplay("{ToString(),nq}, Length: {Magnitude}, LengthSquared: {MagnitudeSquared}, Normalized: {Normalized.ToString(),nq}")]
public readonly struct Quaternion(float x, float y, float z, float w)
{
    /// <summary>
    /// The X(i) imaginary of the <see cref="Quaternion"/>.
    /// </summary>
    public readonly float X = x;

    /// <summary>
    /// The Y(j) imaginary of the <see cref="Quaternion"/>.
    /// </summary>
    public readonly float Y = y;

    /// <summary>
    /// The Z(k) imaginary of the <see cref="Quaternion"/>.
    /// </summary>
    public readonly float Z = z;

    /// <summary>
    /// The W(a) scalar of the <see cref="Quaternion"/>.
    /// </summary>
    public readonly float W = w;

    /// <summary>
    /// The magnitude (length) of the <see cref="Quaternion"/>.
    /// </summary>
    public float Magnitude => Length(this);

    /// <summary>
    /// The squared magnitude (length) of the <see cref="Quaternion"/>.
    /// </summary>
    public float MagnitudeSquared => LengthSquared(this);

    /// <summary>
    /// The normalized form of the <see cref="Quaternion"/> (a <see cref="Quaternion"/> with the same direction and a magnitude of 1).
    /// </summary>
    public Quaternion Normalized => Normalize(this);

    /// <summary>
    /// Represents the <see cref="Quaternion"/> with no rotation.
    /// </summary>
    public readonly static Quaternion Zero = new(0f, 0f, 0f, 0f);

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

    public static Quaternion operator -(Quaternion quaternion) => new(-quaternion.X, -quaternion.Y, -quaternion.Z, quaternion.W);
    public static Quaternion operator +(Quaternion left, Quaternion right) => new(left.X + right.X, left.Y + right.Y, left.Z + right.Z, left.W + right.W);
    public static Quaternion operator -(Quaternion left, Quaternion right) => new(left.X - right.X, left.Y - right.Y, left.Z - right.Z, left.W - right.W);
    public static Quaternion operator *(Quaternion quaternion, float value) => new(quaternion.X * value, quaternion.Y * value, quaternion.Z * value, quaternion.W * value);
    public static Quaternion operator *(float value, Quaternion quaternion) => new(quaternion.X * value, quaternion.Y * value, quaternion.Z * value, quaternion.W * value);
    public static Quaternion operator *(Quaternion left, Quaternion right)
        => new(
            left.W * right.X + left.X * right.W + left.Y * right.Z - left.Z * right.Y,
            left.W * right.Y + left.Y * right.W + left.Z * right.X - left.X * right.Z,
            left.W * right.Z + left.Z * right.W + left.X * right.Y - left.Y * right.X,
            left.W * right.W - left.X * right.X - left.Y * right.Y - left.Z * right.Z
        );
    public static Quaternion operator /(Quaternion quaternion, float value) => new(quaternion.X / value, quaternion.Y / value, quaternion.Z / value, quaternion.W / value);
    public static bool operator ==(Quaternion left, Quaternion right) => left.X == right.X && left.Y == right.Y && left.Z == right.Z && left.W == right.W;
    public static bool operator !=(Quaternion left, Quaternion right) => left.X != right.X || left.Y != right.Y || left.Z != right.Z || left.W != right.W;

    public static implicit operator Quaternion(System.Numerics.Quaternion quaternion) => new(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);
    public static implicit operator System.Numerics.Quaternion(Quaternion quaternion) => new(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);

    /// <summary>
    /// Calculates the length of the <see cref="Quaternion"/>.
    /// </summary>
    /// <param name="quaternion">The <see cref="Quaternion"/>.</param>
    /// <returns>The length of the <see cref="Quaternion"/>.</returns>
    public static float Length(Quaternion quaternion) => Math.Sqrt(LengthSquared(quaternion));

    /// <summary>
    /// Calculates the squared length of the <see cref="Quaternion"/>.
    /// </summary>
    /// <param name="quaternion">The <see cref="Quaternion"/>.</param>
    /// <returns>The squared length of the <see cref="Quaternion"/>.</returns>
    public static float LengthSquared(Quaternion quaternion) => quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W;

    /// <summary>
    /// Adds two <see cref="Quaternion"/>s.
    /// </summary>
    /// <param name="left">The first <see cref="Quaternion"/>.</param>
    /// <param name="right">The second <see cref="Quaternion"/>.</param>
    /// <returns>The sum of the two <see cref="Quaternion"/>s.</returns>
    public static Quaternion Add(Quaternion left, Quaternion right) => left + right;

    /// <summary>
    /// Subtracts one <see cref="Quaternion"/> from another.
    /// </summary>
    /// <param name="left">The <see cref="Quaternion"/> to subtract from.</param>
    /// <param name="right">The <see cref="Quaternion"/> to subtract.</param>
    /// <returns>The result of subtracting the second <see cref="Quaternion"/> from the first.</returns>
    public static Quaternion Subtract(Quaternion left, Quaternion right) => left - right;

    /// <summary>
    /// Multiplies a <see cref="Quaternion"/> by a scalar value.
    /// </summary>
    /// <param name="quaternion">The <see cref="Quaternion"/>.</param>
    /// <param name="value">The scalar value.</param>
    /// <returns>The result of multiplying the <see cref="Quaternion"/> by the scalar value.</returns>
    public static Quaternion Multiply(Quaternion quaternion, float value) => quaternion * value;

    /// <summary>
    /// Divides a <see cref="Quaternion"/> by a scalar value.
    /// </summary>
    /// <param name="quaternion">The <see cref="Quaternion"/>.</param>
    /// <param name="value">The scalar value.</param>
    /// <returns>The result of dividing the <see cref="Quaternion"/> by the scalar value.</returns>
    public static Quaternion Divide(Quaternion quaternion, float value) => quaternion / value;

    /// <summary>
    /// Normalizes the <see cref="Quaternion"/> (creates a unit <see cref="Quaternion"/> with the same direction).
    /// </summary>
    /// <param name="quaternion">The <see cref="Quaternion"/> to normalize.</param>
    /// <returns>The normalized <see cref="Quaternion"/>.</returns>
    public static Quaternion Normalize(Quaternion quaternion) => quaternion / Length(quaternion);

    /// <summary>
    /// Inverts the direction of the <see cref="Quaternion"/>.
    /// </summary>
    /// <param name="quaternion">The <see cref="Quaternion"/>.</param>
    /// <returns>The inverted <see cref="Quaternion"/>.</returns>
    public static Quaternion Invert(Quaternion quaternion) => Conjugate(quaternion) / LengthSquared(quaternion);

    /// <summary>
    /// Conjugate of the <see cref="Quaternion"/>.
    /// </summary>
    /// <param name="quaternion">The <see cref="Quaternion"/>.</param>
    /// <returns>The inverted <see cref="Quaternion"/>.</returns>
    public static Quaternion Conjugate(Quaternion quaternion) => -quaternion;

    /// <summary>
    /// Rotates a <see cref="Vector3D"/> by applying the provided <see cref="Quaternion"/>.
    /// </summary>
    /// <param name="vector">The <see cref="Vector3D"/> to be rotated.</param>
    /// <param name="quaternion">The <see cref="Quaternion"/> to used for applying rotation.</param>
    /// <returns>The rotated <see cref="Vector3D"/>.</returns>
    public static Vector3D RotateVector(Vector3D vector, Quaternion quaternion)
    {
        Quaternion rotation = quaternion * new Quaternion(vector.X, vector.Y, vector.Z, 0) * Invert(quaternion);
        return new(rotation.X, rotation.Y, rotation.Z);
    }

    /// <summary>
    /// Performs spherical linear interpolation between two <see cref="Quaternion"/>s.
    /// </summary>
    /// <param name="from">The starting <see cref="Quaternion"/> (t = 0).</param>
    /// <param name="to">The target <see cref="Quaternion"/> (t = 1).</param>
    /// <param name="t">The interpolation parameter.</param>
    /// <returns>The interpolated <see cref="Quaternion"/>.</returns>
    public static Quaternion SLerp(Quaternion from, Quaternion to, float t)
    {
        float dot = Dot(from, to);

        if (dot < 0.0f)
        {
            from = new Quaternion(-from.X, -from.Y, -from.Z, -from.W);
            dot = -dot;
        }

        if (dot > 0.9995f)
            return Lerp(from, to, t);

        float angle = MathF.Acos(dot);
        float sinAngle = MathF.Sin(angle);

        float fromWeight = MathF.Sin((1f - t) * angle) / sinAngle;
        float toWeight = MathF.Sin(t * angle) / sinAngle;

        return from * fromWeight + to * toWeight;
    }

    /// <summary>
    /// Performs linear interpolation between two <see cref="Quaternion"/>s.
    /// </summary>
    /// <param name="from">The starting <see cref="Quaternion"/> (t = 0).</param>
    /// <param name="to">The target <see cref="Quaternion"/> (t = 1).</param>
    /// <param name="t">The interpolation parameter.</param>
    /// <returns>The interpolated <see cref="Quaternion"/>.</returns>
    public static Quaternion Lerp(Quaternion from, Quaternion to, float t) => Normalize(new(from.X.Lerp(to.X, t), from.W.Lerp(to.W, t), from.Z.Lerp(to.Z, t), from.W.Lerp(to.W, t)));

    /// <summary>
    /// Calculates the dot product of two <see cref="Quaternion"/>s.
    /// </summary>
    /// <param name="left">The first <see cref="Quaternion"/>.</param>
    /// <param name="right">The second <see cref="Quaternion"/>.</param>
    /// <returns>The dot product of the two <see cref="Quaternion"/>s.</returns>
    public static float Dot(Quaternion left, Quaternion right) => left.X * right.X + left.Y * right.Y + left.Z * right.Z + left.W * right.W;

    /// <summary>
    /// Calculates the <see cref="Quaternion"/> from given axis and angle.
    /// </summary>
    /// <param name="axis">The axis of the rotation in <see cref="Vector3D"/>.</param>
    /// <param name="angle">The angle in radians.</param>
    /// <returns>The rotation <see cref="Quaternion"/> calculated by the given parameters.</returns>
    public static Quaternion FromAxisAngle(Vector3D axis, float angle)
    {
        float halfAngle = angle * .5f;
        float sinHalf = MathF.Sin(halfAngle);
        return new Quaternion(axis.X * sinHalf, axis.Y * sinHalf, axis.Z * sinHalf, MathF.Cos(halfAngle));
    }

    /// <summary>
    /// Calculates the <see cref="Quaternion"/> from given yaw, pitch and roll values.
    /// </summary>
    /// <returns>The rotation <see cref="Quaternion"/> calculated by the given parameters.</returns>
    public static Quaternion FromAngles(float x, float y, float z)
    {
        float cosX = Math.Cos(x * .5f);
        float sinX = Math.Sin(x * .5f);
        float cosY = Math.Cos(y * .5f);
        float sinY = Math.Sin(y * .5f);
        float cozZ = Math.Cos(z * .5f);
        float sinZ = Math.Sin(z * .5f);

        return new Quaternion(
            x: sinX * cosY * cozZ - cosX * sinY * sinZ,
            y: cosX * sinY * cozZ + sinX * cosY * sinZ,
            z: cosX * cosY * sinZ - sinX * sinY * cozZ,
            w: cosX * cosY * cozZ + sinX * sinY * sinZ
        );
    }

    /// <summary>
    /// Calculates the <see cref="System.Numerics.Matrix4x4"/> from given <see cref="Quaternion"/>.
    /// </summary>
    /// <param name="axis">The axis of the rotation in <see cref="Vector3D"/>.</param>
    /// <param name="angle">The angle in radians.</param>
    /// <returns>The rotation <see cref="System.Numerics.Matrix4x4"/> calculated by the given <see cref="Quaternion"/>.</returns>
    public static System.Numerics.Matrix4x4 ToRotationMatrix4x4(Quaternion quaternion)
    {
        float m00 = 1 - 2 * (quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
        float m01 = 2 * (quaternion.X * quaternion.Y - quaternion.W * quaternion.Z);
        float m02 = 2 * (quaternion.X * quaternion.Z + quaternion.W * quaternion.Y);
        float m03 = 0;

        float m10 = 2 * (quaternion.X * quaternion.Y + quaternion.W * quaternion.Z);
        float m11 = 1 - 2 * (quaternion.X * quaternion.X + quaternion.Z * quaternion.Z);
        float m12 = 2 * (quaternion.Y * quaternion.Z - quaternion.W * quaternion.X);
        float m13 = 0;

        float m20 = 2 * (quaternion.X * quaternion.Z - quaternion.W * quaternion.Y);
        float m21 = 2 * (quaternion.Y * quaternion.Z + quaternion.W * quaternion.X);
        float m22 = 1 - 2 * (quaternion.X * quaternion.X + quaternion.Y * quaternion.Y);
        float m23 = 0;

        float m30 = 0;
        float m31 = 0;
        float m32 = 0;
        float m33 = 1;

        return new(
            m00, m01, m02, m03,
            m10, m11, m12, m13,
            m20, m21, m22, m23,
            m30, m31, m32, m33
        );
    }

    /// <summary>
    /// Checks if two <see cref="Quaternion"/>s are approximately equal within a specified epsilon range.
    /// </summary>
    /// <param name="left">The first <see cref="Quaternion"/>.</param>
    /// <param name="right">The second <see cref="Quaternion"/>.</param>
    /// <param name="epsilon">The epsilon range.</param>
    /// <returns><see cref="true"/> if the <see cref="Quaternion"/>s are approximately equal; otherwise, <see cref="false"/>.</returns>
    public static bool ApproximatelyEquals(Quaternion left, Quaternion right, float epsilon = float.Epsilon)
        => left.X.ApproximatelyEquals(right.X, epsilon) && left.Y.ApproximatelyEquals(right.Y, epsilon) && left.Z.ApproximatelyEquals(right.Z, epsilon) && left.W.ApproximatelyEquals(right.W, epsilon);

    /// <summary>
    /// Converts the <see cref="Quaternion"/> to its string representation.
    /// </summary>
    /// <returns>A string representation of the <see cref="Quaternion"/>.</returns>
    public override string ToString() => $"{nameof(Quaternion)}({W}, {X}, {Y}, {Z})";

    /// <summary>
    /// Determines whether the specified object is equal to the current <see cref="Quaternion"/>.
    /// </summary>
    /// <param name="obj">The object to compare with the current <see cref="Quaternion"/>.</param>
    /// <returns><see cref="true"/> if the specified object is equal to the current <see cref="Quaternion"/>; otherwise, <see cref="false"/>.</returns>
    public override bool Equals(object? obj) => obj is Quaternion objVec && X.Equals(objVec.X) && Y.Equals(objVec.Y) && Z.Equals(objVec.Z) && W.Equals(objVec.W);

    /// <summary>
    /// Generates a hash code for the <see cref="Quaternion"/>.
    /// </summary>
    /// <returns>A hash code for the <see cref="Quaternion"/>.</returns>
    public override int GetHashCode() => HashCode.Combine(X, Y, Z);
}

/// <summary>
/// Provides extension methods for <see cref="Quaternion"/> type.
/// </summary>
public static class QuaternionExtensions
{
    /// <inheritdoc cref="Quaternion.Length(Quaternion)" />
    public static float Length(this Quaternion quaternion) => Quaternion.Length(quaternion);

    /// <inheritdoc cref="Quaternion.LengthSquared(Quaternion)" />
    public static float LengthSquared(this Quaternion quaternion) => Quaternion.LengthSquared(quaternion);

    /// <inheritdoc cref="Quaternion.Add(Quaternion, Quaternion)" />
    public static Quaternion Add(this Quaternion left, Quaternion right) => Quaternion.Add(left, right);

    /// <inheritdoc cref="Quaternion.Subtract(Quaternion, Quaternion)" />
    public static Quaternion Subtract(this Quaternion left, Quaternion right) => Quaternion.Subtract(left, right);

    /// <inheritdoc cref="Quaternion.Multiply(Quaternion, float)" />
    public static Quaternion Multiply(this Quaternion quaternion, float value) => Quaternion.Multiply(quaternion, value);

    /// <inheritdoc cref="Quaternion.Divide(Quaternion, float)" />
    public static Quaternion Divide(this Quaternion quaternion, float value) => Quaternion.Divide(quaternion, value);

    /// <inheritdoc cref="Quaternion.Normalize(Quaternion)" />
    public static Quaternion Normalize(this Quaternion quaternion) => Quaternion.Normalize(quaternion);

    /// <inheritdoc cref="Quaternion.Invert(Quaternion)" />
    public static Quaternion Invert(this Quaternion quaternion) => Quaternion.Invert(quaternion);

    /// <inheritdoc cref="Quaternion.Conjugate(Quaternion)" />
    public static Quaternion Conjugate(this Quaternion quaternion) => Quaternion.Conjugate(quaternion);

    /// <inheritdoc cref="Quaternion.RotateVector(Vector3D, Quaternion)" />
    public static Vector3D RotateVector(this Vector3D vector, Quaternion quaternion) => Quaternion.RotateVector(vector, quaternion);

    /// <inheritdoc cref="Quaternion.SLerp(Quaternion, Quaternion, float)" />
    public static Quaternion SLerp(this Quaternion from, Quaternion to, float t) => Quaternion.SLerp(from, to, t);

    /// <inheritdoc cref="Quaternion.Lerp(Quaternion, Quaternion, float)" />
    public static Quaternion Lerp(this Quaternion from, Quaternion to, float t) => Quaternion.Lerp(from, to, t);

    /// <inheritdoc cref="Quaternion.Dot(Quaternion, Quaternion)" />
    public static float Dot(this Quaternion left, Quaternion right) => Quaternion.Dot(left, right);

    /// <inheritdoc cref="Quaternion.ToRotationMatrix4x4(Quaternion, Quaternion)" />
    public static System.Numerics.Matrix4x4 ToRotationMatrix4x4(this Quaternion quaternion) => Quaternion.ToRotationMatrix4x4(quaternion);

    /// <inheritdoc cref="Quaternion.FromAxisAngle(Vector3D, float)" />
    public static Quaternion CreateRotation(this Vector3D axis, float angle) => Quaternion.FromAxisAngle(axis, angle);

    /// <inheritdoc cref="Quaternion.ApproximatelyEquals(Quaternion, Quaternion, float)" />
    public static bool ApproximatelyEquals(this Quaternion left, Quaternion right, float epsilon = float.Epsilon) => Quaternion.ApproximatelyEquals(left, right, epsilon);
}