namespace RobotNet10.NavigationTune.Shared.Models;

/// <summary>
/// Straight line test scenario (client-side version)
/// </summary>
public class StraightLineScenario : TestScenario
{
    public double Length { get; set; } = 10.0; // meters
    public double StartX { get; set; } = 0.0;
    public double StartY { get; set; } = 0.0;
    public double StartTheta { get; set; } = 0.0; // radians
    public double Resolution { get; set; } = 0.05; // meters between points

    public StraightLineScenario()
    {
        Name = "Straight Line 10m";
        Description = "Robot moves in a straight line for 10 meters";
        Type = TrajectoryType.StraightLine;
    }

    public override List<PathPoint> GenerateReferencePath()
    {
        var points = new List<PathPoint>();
        double absLength = Math.Abs(Length);
        bool isBackward = Length < 0;
        
        // For backward movement, reverse the direction
        double directionTheta = isBackward ? StartTheta + Math.PI : StartTheta;
        
        // Normalize directionTheta to [-π, π]
        while (directionTheta > Math.PI) directionTheta -= 2 * Math.PI;
        while (directionTheta < -Math.PI) directionTheta += 2 * Math.PI;

        var direction = isBackward ? RobotDirection.BACKWARD : RobotDirection.FORWARD;

        // Start point
        points.Add(new PathPoint
        {
            X = StartX,
            Y = StartY,
            Direction = direction,
            DistanceFromStart = 0.0
        });

        // Generate intermediate points
        int numPoints = (int)(absLength / Resolution);
        for (int i = 1; i <= numPoints; i++)
        {
            double distance = i * Resolution;
            if (distance > absLength) distance = absLength;

            points.Add(new PathPoint
            {
                X = StartX + distance * Math.Cos(directionTheta),
                Y = StartY + distance * Math.Sin(directionTheta),
                Direction = direction,
                DistanceFromStart = distance
            });
        }

        // Ensure end point is exactly at absLength
        if (points[^1].DistanceFromStart < absLength)
        {
            points.Add(new PathPoint
            {
                X = StartX + absLength * Math.Cos(directionTheta),
                Y = StartY + absLength * Math.Sin(directionTheta),
                Direction = direction,
                DistanceFromStart = absLength
            });
        }

        return points;
    }

    public override bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f)
    {
        var goal = GetGoalPose();
        double distance = Pose2D.Distance(currentPose, goal);
        return distance <= tolerance;
    }

    public override Pose2D GetGoalPose()
    {
        double absLength = Math.Abs(Length);
        bool isBackward = Length < 0;
        double directionTheta = isBackward ? StartTheta + Math.PI : StartTheta;
        
        // Normalize directionTheta to [-π, π]
        while (directionTheta > Math.PI) directionTheta -= 2 * Math.PI;
        while (directionTheta < -Math.PI) directionTheta += 2 * Math.PI;
        
        return new Pose2D(
            StartX + absLength * Math.Cos(directionTheta),
            StartY + absLength * Math.Sin(directionTheta),
            directionTheta
        );
    }
}