using RobotNet10.Common;
using RobotNet10.Common.Models;
using RobotNet10.NavigationTune.Shared.Models;

namespace RobotNet10.NavigationTune.Scenarios;

/// <summary>
/// Custom path scenario with user-defined edges
/// </summary>
public class CustomPathScenario : TestScenario
{
    /// <summary>
    /// List of edges defining the path
    /// </summary>
    public List<PathEdge> Edges { get; set; } = [];
    
    /// <summary>
    /// Resolution for splitting edges into points (meters)
    /// </summary>
    public double Resolution { get; set; } = 0.05; // meters between points

    public CustomPathScenario()
    {
        Name = "Custom Path";
        Description = "User-defined path with custom edges";
        Type = TrajectoryType.Custom;
    }

    public override List<PathPoint> GenerateReferencePath()
    {
        var points = new List<PathPoint>();
        
        if (Edges.Count == 0)
            return points;

        // Process each edge
        foreach (var edge in Edges)
        {
            var edgePoints = SplitEdge(edge, Resolution);

            if (edgePoints.Count == 0)
                continue;

            double cumulativeDistance;
            // Adjust cumulative distance for first point
            if (points.Count > 0)
            {
                // Calculate distance from last point to first point of this edge
                double dx = edgePoints[0].X - points[^1].X;
                double dy = edgePoints[0].Y - points[^1].Y;
                double connectionDistance = Math.Sqrt(dx * dx + dy * dy);
                cumulativeDistance = points[^1].DistanceFromStart + connectionDistance;
            }
            else
            {
                cumulativeDistance = 0.0;
            }

            // Add points from this edge
            for (int i = 0; i < edgePoints.Count; i++)
            {
                if (i == 0 && points.Count > 0)
                {
                    // Skip first point if it's the same as last point (edge connection)
                    double dx = edgePoints[i].X - points[^1].X;
                    double dy = edgePoints[i].Y - points[^1].Y;
                    if (Math.Sqrt(dx * dx + dy * dy) < 0.001)
                        continue;
                }

                if (i > 0)
                {
                    // Calculate distance from previous point
                    double dx = edgePoints[i].X - edgePoints[i - 1].X;
                    double dy = edgePoints[i].Y - edgePoints[i - 1].Y;
                    double segmentDistance = Math.Sqrt(dx * dx + dy * dy);
                    cumulativeDistance += segmentDistance;
                }

                points.Add(new PathPoint
                {
                    X = edgePoints[i].X,
                    Y = edgePoints[i].Y,
                    Direction = edge.Direction,
                    DistanceFromStart = cumulativeDistance
                });
            }
        }

        return points;
    }

    /// <summary>
    /// Split an edge into points based on resolution
    /// </summary>
    private static List<(double X, double Y)> SplitEdge(PathEdge edge, double resolution)
    {
        var points = new List<(double X, double Y)>();

        // Calculate edge length
        SpaceEdge spaceEdge = new()
        {
            StartX = edge.StartX,
            StartY = edge.StartY,
            EndX = edge.EndX,
            EndY = edge.EndY,
            Degree = edge.Degree,
            ControlPoint1X = edge.ControlPoint1X ?? 0,
            ControlPoint1Y = edge.ControlPoint1Y ?? 0,
            ControlPoint2X = edge.ControlPoint2X ?? 0,
            ControlPoint2Y = edge.ControlPoint2Y ?? 0,
        };
        double edgeLength = SpaceCompute.GetEdgeLength(spaceEdge, 0.1);
        
        if (edgeLength <= 0)
        {
            points.Add((edge.StartX, edge.StartY));
            return points;
        }

        // Calculate number of points based on resolution
        int numPoints = Math.Max(1, (int)(edgeLength / resolution));
        
        for (int i = 0; i <= numPoints; i++)
        {
            double t = numPoints > 0 ? i * 1.0 / numPoints : 0.0;
            var point = SpaceCompute.BezierPoint(t, spaceEdge);
            
            points.Add((point.X, point.Y));
        }

        return points;
    }



    public override bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f)
    {
        if (Edges.Count == 0)
            return false;

        var goal = GetGoalPose();
        double distance = Pose2D.Distance(currentPose, goal);
        if (distance <= tolerance) Console.WriteLine($"Robot is goal reached. Current pose: [{currentPose.X} - {currentPose.Y}], Goal: [{goal.X} - {goal.Y}], Distance: {distance}");
        return distance <= tolerance;
    }

    public override Pose2D GetGoalPose()
    {
        if (Edges.Count == 0)
            return new Pose2D(0, 0, 0);

        var lastEdge = Edges[^1];
        // Calculate theta from direction (FORWARD = 0, BACKWARD = PI)
        double goalTheta = 0;
        if (Edges.Count > 0)
        {
            // Calculate direction from last edge
            if (Edges.Count > 1)
            {
                var prevEdge = Edges[^2];
                double dx = lastEdge.EndX - prevEdge.EndX;
                double dy = lastEdge.EndY - prevEdge.EndY;
                goalTheta = Math.Atan2(dy, dx);
            }
            else
            {
                double dx = lastEdge.EndX - lastEdge.StartX;
                double dy = lastEdge.EndY - lastEdge.StartY;
                goalTheta = Math.Atan2(dy, dx);
            }
        }
        return new Pose2D(lastEdge.EndX, lastEdge.EndY, goalTheta);
    }

    private static double NormalizeAngle(double angle)
    {
        while (angle > Math.PI) angle -= 2 * Math.PI;
        while (angle < -Math.PI) angle += 2 * Math.PI;
        return angle;
    }
}