using System.Runtime.CompilerServices;

namespace RobotNet10.Shared.Numbers;

/// <summary>
/// Custom quaternion struct that supports JSON serialization.
/// Replacement for System.Numerics.Quaternion which doesn't serialize properly.
/// Represents rotation in 3D space using the formula: q = w + xi + yj + zk
/// </summary>
public struct Quaternion : IEquatable<Quaternion>
{
    /// <summary>
    /// X component of the vector part
    /// </summary>
    public double X { get; set; }

    /// <summary>
    /// Y component of the vector part
    /// </summary>
    public double Y { get; set; }

    /// <summary>
    /// Z component of the vector part
    /// </summary>
    public double Z { get; set; }

    /// <summary>
    /// W component (scalar/real part)
    /// </summary>
    public double W { get; set; }

    /// <summary>
    /// Creates a new Quaternion
    /// </summary>
    public Quaternion(double x, double y, double z, double w)
    {
        X = x;
        Y = y;
        Z = z;
        W = w;
    }

    /// <summary>
    /// Creates a quaternion from a vector and scalar parts
    /// </summary>
    public Quaternion(Vector3 vectorPart, double scalarPart)
    {
        X = vectorPart.X;
        Y = vectorPart.Y;
        Z = vectorPart.Z;
        W = scalarPart;
    }

    #region Static Properties

    /// <summary>
    /// Returns the identity quaternion (no rotation)
    /// </summary>
    public static Quaternion Identity => new(0, 0, 0, 1);

    public static Quaternion FromYawRadian(double yaw)
    {
        var halfYaw = yaw / 2.0;
        return new Quaternion(0, 0, Math.Sin(halfYaw), Math.Cos(halfYaw));
    }

    #endregion

    #region Properties

    /// <summary>
    /// Returns the length (magnitude) of the quaternion
    /// </summary>
    public readonly double Length()
    {
        return Math.Sqrt(X * X + Y * Y + Z * Z + W * W);
    }

    /// <summary>
    /// Returns the squared length of the quaternion (faster than Length)
    /// </summary>
    public readonly double LengthSquared()
    {
        return X * X + Y * Y + Z * Z + W * W;
    }

    /// <summary>
    /// Returns true if this is a unit quaternion
    /// </summary>
    public readonly bool IsIdentity
    {
        get
        {
            return X == 0 && Y == 0 && Z == 0 && W == 1;
        }
    }

    #endregion

    #region Methods

    /// <summary>
    /// Returns a normalized copy of this quaternion (unit length)
    /// </summary>
    public readonly Quaternion Normalize()
    {
        double length = Length();
        if (length < double.Epsilon)
            return Identity;

        double invLength = 1.0 / length;
        return new Quaternion(X * invLength, Y * invLength, Z * invLength, W * invLength);
    }

    /// <summary>
    /// Normalizes this quaternion in place
    /// </summary>
    public void NormalizeInPlace()
    {
        double length = Length();
        if (length < double.Epsilon)
        {
            X = Y = Z = 0;
            W = 1;
            return;
        }

        double invLength = 1.0 / length;
        X *= invLength;
        Y *= invLength;
        Z *= invLength;
        W *= invLength;
    }

    /// <summary>
    /// Returns the conjugate of this quaternion (negated vector part)
    /// For unit quaternions, conjugate equals inverse
    /// </summary>
    public readonly Quaternion Conjugate()
    {
        return new Quaternion(-X, -Y, -Z, W);
    }

    /// <summary>
    /// Returns the inverse of this quaternion
    /// </summary>
    public readonly Quaternion Inverse()
    {
        double lengthSq = LengthSquared();
        if (lengthSq < double.Epsilon)
            return Identity;

        double invLengthSq = 1.0 / lengthSq;
        return new Quaternion(-X * invLengthSq, -Y * invLengthSq, -Z * invLengthSq, W * invLengthSq);
    }

    #endregion

    #region Static Methods

    /// <summary>
    /// Calculates the dot product of two quaternions
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static double Dot(Quaternion quaternion1, Quaternion quaternion2)
    {
        return quaternion1.X * quaternion2.X +
               quaternion1.Y * quaternion2.Y +
               quaternion1.Z * quaternion2.Z +
               quaternion1.W * quaternion2.W;
    }

    /// <summary>
    /// Returns the conjugate of a quaternion (static version)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion Conjugate(Quaternion value)
    {
        return value.Conjugate();
    }

    /// <summary>
    /// Returns the inverse of a quaternion (static version)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion Inverse(Quaternion value)
    {
        return value.Inverse();
    }

    /// <summary>
    /// Returns a normalized copy of a quaternion (static version)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion Normalize(Quaternion value)
    {
        return value.Normalize();
    }

    /// <summary>
    /// Multiplies two quaternions (static version)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion Multiply(Quaternion value1, Quaternion value2)
    {
        return value1 * value2;
    }

    /// <summary>
    /// Performs spherical linear interpolation between two quaternions
    /// </summary>
    public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, double amount)
    {
        double cosOmega = Dot(quaternion1, quaternion2);

        bool flip = false;
        if (cosOmega < 0.0)
        {
            flip = true;
            cosOmega = -cosOmega;
        }

        double s1, s2;

        if (cosOmega > (1.0 - 1e-6))
        {
            // Too close, do straight linear interpolation
            s1 = 1.0 - amount;
            s2 = flip ? -amount : amount;
        }
        else
        {
            double omega = Math.Acos(cosOmega);
            double invSinOmega = 1.0 / Math.Sin(omega);

            s1 = Math.Sin((1.0 - amount) * omega) * invSinOmega;
            s2 = flip
                ? -Math.Sin(amount * omega) * invSinOmega
                : Math.Sin(amount * omega) * invSinOmega;
        }

        return new Quaternion(
            s1 * quaternion1.X + s2 * quaternion2.X,
            s1 * quaternion1.Y + s2 * quaternion2.Y,
            s1 * quaternion1.Z + s2 * quaternion2.Z,
            s1 * quaternion1.W + s2 * quaternion2.W
        );
    }

    /// <summary>
    /// Performs linear interpolation between two quaternions
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, double amount)
    {
        double t = amount;
        double t1 = 1.0 - t;

        Quaternion result;

        double dot = Dot(quaternion1, quaternion2);

        if (dot >= 0.0)
        {
            result = new Quaternion(
                t1 * quaternion1.X + t * quaternion2.X,
                t1 * quaternion1.Y + t * quaternion2.Y,
                t1 * quaternion1.Z + t * quaternion2.Z,
                t1 * quaternion1.W + t * quaternion2.W
            );
        }
        else
        {
            result = new Quaternion(
                t1 * quaternion1.X - t * quaternion2.X,
                t1 * quaternion1.Y - t * quaternion2.Y,
                t1 * quaternion1.Z - t * quaternion2.Z,
                t1 * quaternion1.W - t * quaternion2.W
            );
        }

        return result.Normalize();
    }

    /// <summary>
    /// Concatenates two quaternions (applies rotation1 followed by rotation2)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion Concatenate(Quaternion value1, Quaternion value2)
    {
        // This is equivalent to value2 * value1
        double q1x = value2.X;
        double q1y = value2.Y;
        double q1z = value2.Z;
        double q1w = value2.W;

        double q2x = value1.X;
        double q2y = value1.Y;
        double q2z = value1.Z;
        double q2w = value1.W;

        // Cross product
        double cx = q1y * q2z - q1z * q2y;
        double cy = q1z * q2x - q1x * q2z;
        double cz = q1x * q2y - q1y * q2x;

        double dot = q1x * q2x + q1y * q2y + q1z * q2z;

        return new Quaternion(
            q1x * q2w + q2x * q1w + cx,
            q1y * q2w + q2y * q1w + cy,
            q1z * q2w + q2z * q1w + cz,
            q1w * q2w - dot
        );
    }

    /// <summary>
    /// Creates a quaternion from an axis and angle
    /// </summary>
    public static Quaternion CreateFromAxisAngle(Vector3 axis, double angle)
    {
        double halfAngle = angle * 0.5;
        double s = Math.Sin(halfAngle);
        double c = Math.Cos(halfAngle);

        return new Quaternion(
            axis.X * s,
            axis.Y * s,
            axis.Z * s,
            c
        );
    }

    /// <summary>
    /// Creates a quaternion from yaw, pitch, and roll angles (in radians)
    /// </summary>
    public static Quaternion CreateFromYawPitchRoll(double yaw, double pitch, double roll)
    {
        double halfRoll = roll * 0.5;
        double halfPitch = pitch * 0.5;
        double halfYaw = yaw * 0.5;

        double sinRoll = Math.Sin(halfRoll);
        double cosRoll = Math.Cos(halfRoll);
        double sinPitch = Math.Sin(halfPitch);
        double cosPitch = Math.Cos(halfPitch);
        double sinYaw = Math.Sin(halfYaw);
        double cosYaw = Math.Cos(halfYaw);

        return new Quaternion(
            cosYaw * sinPitch * cosRoll + sinYaw * cosPitch * sinRoll,
            sinYaw * cosPitch * cosRoll - cosYaw * sinPitch * sinRoll,
            cosYaw * cosPitch * sinRoll - sinYaw * sinPitch * cosRoll,
            cosYaw * cosPitch * cosRoll + sinYaw * sinPitch * sinRoll
        );
    }

    /// <summary>
    /// Creates a quaternion from a rotation matrix
    /// </summary>
    public static Quaternion CreateFromRotationMatrix(double[,] matrix)
    {
        if (matrix.GetLength(0) < 3 || matrix.GetLength(1) < 3)
            return Identity;

        double trace = matrix[0, 0] + matrix[1, 1] + matrix[2, 2];

        Quaternion q = default;

        if (trace > 0.0)
        {
            double s = Math.Sqrt(trace + 1.0);
            q.W = s * 0.5;
            s = 0.5 / s;
            q.X = (matrix[2, 1] - matrix[1, 2]) * s;
            q.Y = (matrix[0, 2] - matrix[2, 0]) * s;
            q.Z = (matrix[1, 0] - matrix[0, 1]) * s;
        }
        else
        {
            if (matrix[0, 0] >= matrix[1, 1] && matrix[0, 0] >= matrix[2, 2])
            {
                double s = Math.Sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]);
                double invS = 0.5 / s;
                q.X = 0.5 * s;
                q.Y = (matrix[1, 0] + matrix[0, 1]) * invS;
                q.Z = (matrix[2, 0] + matrix[0, 2]) * invS;
                q.W = (matrix[2, 1] - matrix[1, 2]) * invS;
            }
            else if (matrix[1, 1] > matrix[2, 2])
            {
                double s = Math.Sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]);
                double invS = 0.5 / s;
                q.X = (matrix[0, 1] + matrix[1, 0]) * invS;
                q.Y = 0.5 * s;
                q.Z = (matrix[1, 2] + matrix[2, 1]) * invS;
                q.W = (matrix[0, 2] - matrix[2, 0]) * invS;
            }
            else
            {
                double s = Math.Sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]);
                double invS = 0.5 / s;
                q.X = (matrix[0, 2] + matrix[2, 0]) * invS;
                q.Y = (matrix[1, 2] + matrix[2, 1]) * invS;
                q.Z = 0.5 * s;
                q.W = (matrix[1, 0] - matrix[0, 1]) * invS;
            }
        }

        return q;
    }

    #endregion

    #region Operators

    /// <summary>
    /// Adds two quaternions component-wise (rarely used - prefer multiplication for combining rotations)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion operator +(Quaternion value1, Quaternion value2)
    {
        return new Quaternion(
            value1.X + value2.X,
            value1.Y + value2.Y,
            value1.Z + value2.Z,
            value1.W + value2.W
        );
    }

    /// <summary>
    /// Subtracts two quaternions component-wise (rarely used)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion operator -(Quaternion value1, Quaternion value2)
    {
        return new Quaternion(
            value1.X - value2.X,
            value1.Y - value2.Y,
            value1.Z - value2.Z,
            value1.W - value2.W
        );
    }

    /// <summary>
    /// Negates a quaternion
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion operator -(Quaternion value)
    {
        return new Quaternion(-value.X, -value.Y, -value.Z, -value.W);
    }

    /// <summary>
    /// Multiplies two quaternions (combines rotations: first apply value2, then value1)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion operator *(Quaternion value1, Quaternion value2)
    {
        double q1x = value1.X;
        double q1y = value1.Y;
        double q1z = value1.Z;
        double q1w = value1.W;

        double q2x = value2.X;
        double q2y = value2.Y;
        double q2z = value2.Z;
        double q2w = value2.W;

        // Cross product
        double cx = q1y * q2z - q1z * q2y;
        double cy = q1z * q2x - q1x * q2z;
        double cz = q1x * q2y - q1y * q2x;

        double dot = q1x * q2x + q1y * q2y + q1z * q2z;

        return new Quaternion(
            q1x * q2w + q2x * q1w + cx,
            q1y * q2w + q2y * q1w + cy,
            q1z * q2w + q2z * q1w + cz,
            q1w * q2w - dot
        );
    }

    /// <summary>
    /// Multiplies a quaternion by a scalar
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion operator *(Quaternion value1, double value2)
    {
        return new Quaternion(
            value1.X * value2,
            value1.Y * value2,
            value1.Z * value2,
            value1.W * value2
        );
    }

    /// <summary>
    /// Divides a quaternion by a scalar
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Quaternion operator /(Quaternion value1, double value2)
    {
        double invValue = 1.0 / value2;
        return new Quaternion(
            value1.X * invValue,
            value1.Y * invValue,
            value1.Z * invValue,
            value1.W * invValue
        );
    }

    /// <summary>
    /// Checks if two quaternions are equal
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator ==(Quaternion value1, Quaternion value2)
    {
        return value1.X == value2.X &&
               value1.Y == value2.Y &&
               value1.Z == value2.Z &&
               value1.W == value2.W;
    }

    /// <summary>
    /// Checks if two quaternions are not equal
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator !=(Quaternion value1, Quaternion value2)
    {
        return value1.X != value2.X ||
               value1.Y != value2.Y ||
               value1.Z != value2.Z ||
               value1.W != value2.W;
    }

    /// <summary>
    /// Implicit conversion from System.Numerics.Quaternion
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static implicit operator Quaternion(System.Numerics.Quaternion value)
    {
        return new Quaternion(value.X, value.Y, value.Z, value.W);
    }

    /// <summary>
    /// Implicit conversion to System.Numerics.Quaternion
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static implicit operator System.Numerics.Quaternion(Quaternion value)
    {
        return new Quaternion((float)value.X, (float)value.Y, (float)value.Z, (float)value.W);
    }

    #endregion

    #region Equality

    /// <summary>
    /// Checks if this quaternion equals another quaternion
    /// </summary>
    public readonly bool Equals(Quaternion other)
    {
        return X == other.X && Y == other.Y && Z == other.Z && W == other.W;
    }

    /// <summary>
    /// Checks if this quaternion equals an object
    /// </summary>
    public override readonly bool Equals(object? obj)
    {
        return obj is Quaternion quaternion && Equals(quaternion);
    }

    /// <summary>
    /// Gets the hash code for this quaternion
    /// </summary>
    public override readonly int GetHashCode()
    {
        return HashCode.Combine(X, Y, Z, W);
    }

    #endregion

    #region String

    /// <summary>
    /// Returns a string representation of this quaternion
    /// </summary>
    public override readonly string ToString()
    {
        return $"{{X:{X} Y:{Y} Z:{Z} W:{W}}}";
    }

    /// <summary>
    /// Returns a formatted string representation of this quaternion
    /// </summary>
    public readonly string ToString(string format)
    {
        return $"{{X:{X.ToString(format)} Y:{Y.ToString(format)} Z:{Z.ToString(format)} W:{W.ToString(format)}}}";
    }

    #endregion
}