namespace RobotNet10.NavigationTune.Shared.Models;

/// <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; } = new();
    
    /// <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;

        double cumulativeDistance = 0.0;

        // Process each edge
        foreach (var edge in Edges)
        {
            var edgePoints = SplitEdge(edge, Resolution);
            
            if (edgePoints.Count == 0)
                continue;

            // 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;
                }

                // Use direction from edge
                RobotDirection direction = edge.Direction;

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

        return points;
    }

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

        // Calculate edge length
        double edgeLength = CalculateEdgeLength(edge);
        
        if (edgeLength <= 0)
        {
            // Single point at start
            double theta = CalculateThetaAt(edge, 0.0);
            points.Add((edge.StartX, edge.StartY, theta));
            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 / numPoints : 0.0;
            
            var (x, y) = CalculatePointAt(edge, t);
            double theta = CalculateThetaAt(edge, t);
            
            points.Add((x, y, theta));
        }

        return points;
    }

    /// <summary>
    /// Calculate point on edge at parameter t (0.0 to 1.0)
    /// </summary>
    private (double X, double Y) CalculatePointAt(PathEdge edge, double t)
    {
        t = Math.Clamp(t, 0.0, 1.0);

        return edge.Degree switch
        {
            1 => CalculateLinearPoint(edge, t),
            2 => CalculateQuadraticBezierPoint(edge, t),
            3 => CalculateCubicBezierPoint(edge, t),
            _ => CalculateLinearPoint(edge, t) // Default to linear
        };
    }

    /// <summary>
    /// Linear interpolation (Degree 1)
    /// </summary>
    private (double X, double Y) CalculateLinearPoint(PathEdge edge, double t)
    {
        double x = edge.StartX + t * (edge.EndX - edge.StartX);
        double y = edge.StartY + t * (edge.EndY - edge.StartY);
        return (x, y);
    }

    /// <summary>
    /// Quadratic Bezier curve (Degree 2)
    /// P(t) = (1-t)²P₀ + 2(1-t)tP₁ + t²P₂
    /// </summary>
    private (double X, double Y) CalculateQuadraticBezierPoint(PathEdge edge, double t)
    {
        if (!edge.ControlPoint1X.HasValue || !edge.ControlPoint1Y.HasValue)
        {
            // Fallback to linear if control point not provided
            return CalculateLinearPoint(edge, t);
        }

        double oneMinusT = 1.0 - t;
        double x = oneMinusT * oneMinusT * edge.StartX +
                   2 * oneMinusT * t * edge.ControlPoint1X.Value +
                   t * t * edge.EndX;
        double y = oneMinusT * oneMinusT * edge.StartY +
                   2 * oneMinusT * t * edge.ControlPoint1Y.Value +
                   t * t * edge.EndY;
        return (x, y);
    }

    /// <summary>
    /// Cubic Bezier curve (Degree 3)
    /// P(t) = (1-t)³P₀ + 3(1-t)²tP₁ + 3(1-t)t²P₂ + t³P₃
    /// </summary>
    private (double X, double Y) CalculateCubicBezierPoint(PathEdge edge, double t)
    {
        if (!edge.ControlPoint1X.HasValue || !edge.ControlPoint1Y.HasValue ||
            !edge.ControlPoint2X.HasValue || !edge.ControlPoint2Y.HasValue)
        {
            // Fallback to quadratic or linear if control points not provided
            if (edge.ControlPoint1X.HasValue && edge.ControlPoint1Y.HasValue)
                return CalculateQuadraticBezierPoint(edge, t);
            return CalculateLinearPoint(edge, t);
        }

        double oneMinusT = 1.0 - t;
        double oneMinusT2 = oneMinusT * oneMinusT;
        double oneMinusT3 = oneMinusT2 * oneMinusT;
        double t2 = t * t;
        double t3 = t2 * t;

        double x = oneMinusT3 * edge.StartX +
                   3 * oneMinusT2 * t * edge.ControlPoint1X.Value +
                   3 * oneMinusT * t2 * edge.ControlPoint2X.Value +
                   t3 * edge.EndX;
        double y = oneMinusT3 * edge.StartY +
                   3 * oneMinusT2 * t * edge.ControlPoint1Y.Value +
                   3 * oneMinusT * t2 * edge.ControlPoint2Y.Value +
                   t3 * edge.EndY;
        return (x, y);
    }

    /// <summary>
    /// Calculate tangent angle (theta) at parameter t
    /// </summary>
    private double CalculateThetaAt(PathEdge edge, double t)
    {
        const double epsilon = 0.001;
        double t1 = Math.Clamp(t, 0.0, 1.0);
        double t2 = Math.Clamp(t + epsilon, 0.0, 1.0);

        var (x1, y1) = CalculatePointAt(edge, t1);
        var (x2, y2) = CalculatePointAt(edge, t2);

        double dx = x2 - x1;
        double dy = y2 - y1;
        double theta = Math.Atan2(dy, dx);

        return NormalizeAngle(theta);
    }

    /// <summary>
    /// Calculate approximate length of edge
    /// </summary>
    private double CalculateEdgeLength(PathEdge edge)
    {
        // For linear: direct distance
        if (edge.Degree == 1)
        {
            double dx = edge.EndX - edge.StartX;
            double dy = edge.EndY - edge.StartY;
            return Math.Sqrt(dx * dx + dy * dy);
        }

        // For curves: approximate by sampling
        const int samples = 20;
        double length = 0.0;
        var (prevX, prevY) = CalculatePointAt(edge, 0.0);

        for (int i = 1; i <= samples; i++)
        {
            double t = i / samples;
            var (x, y) = CalculatePointAt(edge, t);
            double dx = x - prevX;
            double dy = y - prevY;
            length += Math.Sqrt(dx * dx + dy * dy);
            prevX = x;
            prevY = y;
        }

        return length;
    }

    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);
        return distance <= tolerance;
    }

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

        var lastEdge = Edges[^1];
        double theta = CalculateThetaAt(lastEdge, 1.0);
        return new Pose2D(lastEdge.EndX, lastEdge.EndY, theta);
    }

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