namespace RobotNet10.NavigationTune.Shared.Models;

/// <summary>
/// Base class for test scenarios
/// </summary>
public abstract class TestScenario
{
    public Guid Id { get; set; } = Guid.NewGuid();
    public string Name { get; set; } = string.Empty;
    public string Description { get; set; } = string.Empty;
    public TrajectoryType Type { get; set; }
    public DateTime CreatedAt { get; set; } = DateTime.UtcNow;
    public bool IsDefault { get; set; }

    /// <summary>
    /// Generate reference path for this scenario
    /// </summary>
    public abstract List<PathPoint> GenerateReferencePath();

    /// <summary>
    /// Check if goal is reached
    /// </summary>
    public abstract bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f);

    /// <summary>
    /// Get goal pose
    /// </summary>
    public abstract Pose2D GetGoalPose();
}

/// <summary>
/// Trajectory type
/// </summary>
public enum TrajectoryType
{
    StraightLine = 1,
    Circle = 2,
    Square = 3,
    SCurve = 4,
    Custom = 99
}

/// <summary>
/// Direction for robot movement
/// </summary>
public enum RobotDirection
{
    FORWARD,
    BACKWARD
}

/// <summary>
/// Point on reference path
/// </summary>
public class PathPoint
{
    public double X { get; set; }
    public double Y { get; set; }
    public RobotDirection Direction { get; set; } = RobotDirection.FORWARD;
    public double DistanceFromStart { get; set; } // cumulative distance
}

/// <summary>
/// Reference path
/// </summary>
public class ReferencePath
{
    public List<PathPoint> Points { get; set; } = new();
    public double TotalLength { get; set; }

    public PathPoint? GetClosestPoint(Pose2D pose)
    {
        if (Points.Count == 0) return null;

        double minDistance = double.MaxValue;
        PathPoint? closest = null;

        foreach (var point in Points)
        {
            double dx = pose.X - point.X;
            double dy = pose.Y - point.Y;
            double distance = Math.Sqrt(dx * dx + dy * dy);

            if (distance < minDistance)
            {
                minDistance = distance;
                closest = point;
            }
        }

        return closest;
    }

    public PathPoint? GetPointAtDistance(double distance)
    {
        if (Points.Count == 0) return null;
        if (distance <= 0) return Points[0];
        if (distance >= TotalLength) return Points[^1];

        // Find segment containing this distance
        for (int i = 0; i < Points.Count - 1; i++)
        {
            if (distance >= Points[i].DistanceFromStart && distance <= Points[i + 1].DistanceFromStart)
            {
                // Interpolate
                double segmentLength = Points[i + 1].DistanceFromStart - Points[i].DistanceFromStart;
                double t = (distance - Points[i].DistanceFromStart) / segmentLength;

                return new PathPoint
                {
                    X = Points[i].X + t * (Points[i + 1].X - Points[i].X),
                    Y = Points[i].Y + t * (Points[i + 1].Y - Points[i].Y),
                    Direction = Points[i].Direction, // Use direction from start point
                    DistanceFromStart = distance
                };
            }
        }

        return Points[^1];
    }
}