BQP/srcs/RobotNet10/Shared/RobotNet10.Shared/Numbers/Vector2.cs

using System.Runtime.CompilerServices;

namespace RobotNet10.Shared.Numbers;

/// <summary>
/// Custom 2D vector struct that supports JSON serialization.
/// Replacement for System.Numerics.Vector2 which doesn't serialize properly.
/// </summary>
public struct Vector2 : IEquatable<Vector2>
{
    /// <summary>
    /// X component
    /// </summary>
    public double X { get; set; }

    /// <summary>
    /// Y component
    /// </summary>
    public double Y { get; set; }

    /// <summary>
    /// Creates a new Vector2
    /// </summary>
    public Vector2(double x, double y)
    {
        X = x;
        Y = y;
    }

    /// <summary>
    /// Creates a Vector2 with both components set to the same value
    /// </summary>
    public Vector2(double value)
    {
        X = Y = value;
    }

    #region Static Properties

    /// <summary>
    /// Returns a Vector2 with both components set to zero
    /// </summary>
    public static Vector2 Zero => new(0, 0);

    /// <summary>
    /// Returns a Vector2 with both components set to one
    /// </summary>
    public static Vector2 One => new(1, 1);

    /// <summary>
    /// Returns the unit vector for the X axis (1, 0)
    /// </summary>
    public static Vector2 UnitX => new(1, 0);

    /// <summary>
    /// Returns the unit vector for the Y axis (0, 1)
    /// </summary>
    public static Vector2 UnitY => new(0, 1);

    #endregion

    #region Properties

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

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

    #endregion

    #region Methods

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

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

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

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

    #endregion

    #region Static Methods

    /// <summary>
    /// Calculates the dot product of two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static double Dot(Vector2 left, Vector2 right)
    {
        return left.X * right.X + left.Y * right.Y;
    }

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

    /// <summary>
    /// Returns the distance between two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static double Distance(Vector2 value1, Vector2 value2)
    {
        double dx = value1.X - value2.X;
        double dy = value1.Y - value2.Y;
        return Math.Sqrt(dx * dx + dy * dy);
    }

    /// <summary>
    /// Returns the squared distance between two vectors (faster than Distance)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static double DistanceSquared(Vector2 value1, Vector2 value2)
    {
        double dx = value1.X - value2.X;
        double dy = value1.Y - value2.Y;
        return dx * dx + dy * dy;
    }

    /// <summary>
    /// Performs linear interpolation between two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 Lerp(Vector2 value1, Vector2 value2, double amount)
    {
        return new Vector2(
            value1.X + (value2.X - value1.X) * amount,
            value1.Y + (value2.Y - value1.Y) * amount
        );
    }

    /// <summary>
    /// Returns a vector with the minimum components of two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 Min(Vector2 value1, Vector2 value2)
    {
        return new Vector2(
            Math.Min(value1.X, value2.X),
            Math.Min(value1.Y, value2.Y)
        );
    }

    /// <summary>
    /// Returns a vector with the maximum components of two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 Max(Vector2 value1, Vector2 value2)
    {
        return new Vector2(
            Math.Max(value1.X, value2.X),
            Math.Max(value1.Y, value2.Y)
        );
    }

    /// <summary>
    /// Returns a vector whose components are the absolute values of the input vector
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 Abs(Vector2 value)
    {
        return new Vector2(
            Math.Abs(value.X),
            Math.Abs(value.Y)
        );
    }

    /// <summary>
    /// Clamps a vector to the specified minimum and maximum values
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 Clamp(Vector2 value, Vector2 min, Vector2 max)
    {
        return new Vector2(
            Math.Clamp(value.X, min.X, max.X),
            Math.Clamp(value.Y, min.Y, max.Y)
        );
    }

    /// <summary>
    /// Reflects a vector off a surface with the specified normal
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 Reflect(Vector2 vector, Vector2 normal)
    {
        double dot = Dot(vector, normal);
        return vector - 2.0 * dot * normal;
    }

    /// <summary>
    /// Transforms a Vector2 by a Matrix3x2
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 Transform(Vector2 position, Matrix3x2 matrix)
    {
        return new Vector2(
            position.X * matrix.M11 + position.Y * matrix.M21 + matrix.M31,
            position.X * matrix.M12 + position.Y * matrix.M22 + matrix.M32
        );
    }

    #endregion

    #region Operators

    /// <summary>
    /// Adds two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 operator +(Vector2 left, Vector2 right)
    {
        return new Vector2(left.X + right.X, left.Y + right.Y);
    }

    /// <summary>
    /// Subtracts two vectors
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 operator -(Vector2 left, Vector2 right)
    {
        return new Vector2(left.X - right.X, left.Y - right.Y);
    }

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

    /// <summary>
    /// Multiplies two vectors component-wise
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 operator *(Vector2 left, Vector2 right)
    {
        return new Vector2(left.X * right.X, left.Y * right.Y);
    }

    /// <summary>
    /// Multiplies a vector by a scalar
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 operator *(Vector2 left, double right)
    {
        return new Vector2(left.X * right, left.Y * right);
    }

    /// <summary>
    /// Multiplies a scalar by a vector
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 operator *(double left, Vector2 right)
    {
        return new Vector2(left * right.X, left * right.Y);
    }

    /// <summary>
    /// Divides two vectors component-wise
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 operator /(Vector2 left, Vector2 right)
    {
        return new Vector2(left.X / right.X, left.Y / right.Y);
    }

    /// <summary>
    /// Divides a vector by a scalar
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static Vector2 operator /(Vector2 left, double right)
    {
        double invRight = 1.0 / right;
        return new Vector2(left.X * invRight, left.Y * invRight);
    }

    /// <summary>
    /// Checks if two vectors are equal
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator ==(Vector2 left, Vector2 right)
    {
        return left.X == right.X && left.Y == right.Y;
    }

    /// <summary>
    /// Checks if two vectors are not equal
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    public static bool operator !=(Vector2 left, Vector2 right)
    {
        return left.X != right.X || left.Y != right.Y;
    }

    #endregion

    #region Equality

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

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

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

    #endregion

    #region String

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

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

    #endregion
}