namespace RobotNet10.RobotApp.Detection;

/// <summary>
/// Represents a 2D point with additional laser scan metadata
/// </summary>
public class Point(double x, double y, double range, double alpha)
{
    /// <summary>
    /// X coordinate in meters
    /// </summary>
    public double X { get; set; } = x;

    /// <summary>
    /// Y coordinate in meters
    /// </summary>
    public double Y { get; set; } = y;

    /// <summary>
    /// Range of point from Lidar sensor in meters
    /// </summary>
    public double Range { get; set; } = range;

    /// <summary>
    /// Angle of laser beam to the point in radians
    /// </summary>
    public double Alpha { get; set; } = alpha;

    /// <summary>
    /// Cluster ID assigned by clustering algorithm (-1 if unassigned)
    /// </summary>
    public int Cluster { get; set; } = -1;

    /// <summary>
    /// Reachability distance for OPTICS algorithm
    /// </summary>
    public double ReachabilityDistance { get; set; } = double.MaxValue;

    /// <summary>
    /// Whether this point has been processed by the algorithm
    /// </summary>
    public bool Processed { get; set; } = false;
}

/// <summary>
/// Simple 2D point structure for cluster results
/// </summary>
public struct Point2D(double x, double y) : IEquatable<Point2D>, IComparable<Point2D>
{
    /// <summary>
    /// X coordinate in meters
    /// </summary>
    public double X { get; set; } = x;

    /// <summary>
    /// Y coordinate in meters
    /// </summary>
    public double Y { get; set; } = y;

    public readonly bool Equals(Point2D other)
    {
        return Math.Abs(X - other.X) < 3e-3 && Math.Abs(Y - other.Y) < 3e-3;
    }

    public override readonly bool Equals(object? obj)
    {
        return obj is Point2D other && Equals(other);
    }

    public override readonly int GetHashCode()
    {
        return HashCode.Combine(X, Y);
    }

    public readonly int CompareTo(Point2D other)
    {
        if (!X.Equals(other.X))
            return X.CompareTo(other.X);
        return Y.CompareTo(other.Y);
    }

    public static bool operator ==(Point2D left, Point2D right)
    {
        return left.Equals(right);
    }

    public static bool operator !=(Point2D left, Point2D right)
    {
        return !left.Equals(right);
    }
}