using RobotNet10.NavigationTune.Shared.Models;

namespace RobotNet10.NavigationTune.Scenarios;

/// <summary>
/// Circle test scenario
/// </summary>
public class CircleScenario : TestScenario
{
    public double Radius { get; set; } = 2.0; // meters
    public double CenterX { get; set; } = 0.0;
    public double CenterY { get; set; } = 0.0;
    public double StartAngle { get; set; } = 0.0; // radians
    public bool Clockwise { get; set; } = true;
    public double Resolution { get; set; } = 0.05; // meters between points

    public CircleScenario()
    {
        Name = $"Circle {Radius}m Radius";
        Description = $"Robot moves in a circle with radius {Radius}m";
        Type = TrajectoryType.Circle;
    }

    public override List<PathPoint> GenerateReferencePath()
    {
        var points = new List<PathPoint>();
        double circumference = 2 * Math.PI * Radius;
        int numPoints = (int)(circumference / Resolution);
        double angleStep = 2 * Math.PI / numPoints;

        if (!Clockwise)
            angleStep = -angleStep;

        var direction = Clockwise ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
        double distance = 0.0;

        for (int i = 0; i <= numPoints; i++)
        {
            double angle = StartAngle + i * angleStep;
            double x = CenterX + Radius * Math.Cos(angle);
            double y = CenterY + Radius * Math.Sin(angle);

            distance = i * Resolution;
            if (distance > circumference) distance = circumference;

            points.Add(new PathPoint
            {
                X = x,
                Y = y,
                Direction = direction,
                DistanceFromStart = distance
            });
        }

        return points;
    }

    public override bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f)
    {
        // Check if robot is close to start position
        double dx = currentPose.X - (CenterX + Radius * Math.Cos(StartAngle));
        double dy = currentPose.Y - (CenterY + Radius * Math.Sin(StartAngle));
        double distance = Math.Sqrt(dx * dx + dy * dy);
        if (distance <= tolerance) Console.WriteLine($"Robot is goal reached. Current pose: [{currentPose.X} - {currentPose.Y}], Goal: [{CenterX + Radius * Math.Cos(StartAngle)} - {CenterY + Radius * Math.Sin(StartAngle)}], Distance: {distance}");
        return distance <= tolerance;
    }

    public override Pose2D GetGoalPose()
    {
        // Goal is back at start position
        double goalX = CenterX + Radius * Math.Cos(StartAngle);
        double goalY = CenterY + Radius * Math.Sin(StartAngle);
        double goalTheta = StartAngle + (Clockwise ? Math.PI / 2 : -Math.PI / 2);

        while (goalTheta > Math.PI) goalTheta -= 2 * Math.PI;
        while (goalTheta < -Math.PI) goalTheta += 2 * Math.PI;

        return new Pose2D(goalX, goalY, goalTheta);
    }
}