using RobotNet.VDA5050.Order;
using RobotNet10.Common;
using RobotNet10.Common.Models;
using RobotNet10.NavigationTune.Shared.Models;
using RobotNet10.RobotApp.Services.Exceptions;
using RobotNet10.RobotApp.Services.Robot;
using RobotNet10.RobotApp.Services.Robot.Helper;
using RobotNet10.RobotApp.Services.Robot.Models;
using RobotNet10.RobotApp.Services.Simulation;
using RobotDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection;

namespace RobotNet10.RobotApp.Services.Navigation.CSharp;

/// <summary>
/// Configuration cho Pure Pursuit controller
/// </summary>
public class PurePursuitConfig
{
    /// <summary>
    /// Lookahead distance minimum (m)
    /// </summary>
    public double LookaheadMin { get; set; } = 0.3;

    /// <summary>
    /// Hệ số tỷ lệ lookahead với vận tốc (s)
    /// </summary>
    public double Kdd { get; set; } = 1.0;

    /// <summary>
    /// Lookahead distance maximum (m)
    /// </summary>
    public double LookaheadMax { get; set; } = 2.0;

    /// <summary>
    /// [LEGACY] Gain cho curvature (nếu cần scale steering)
    /// Note: Not used in current implementation.
    ///       Replaced by adaptive KCurvature for lookahead adjustment.
    ///       Kept for backward compatibility.
    /// </summary>
    public double CurvatureGain { get; set; } = 1.0;

    /// <summary>
    /// Ngưỡng để coi như đạt waypoint (m)
    /// </summary>
    public double WaypointTolerance { get; set; } = 0.1;

    /// <summary>
    /// Maximum angular velocity during tracking (rad/s)
    /// </summary>
    public double MaxAngularVelocity { get; set; } = 1.5;

    /// <summary>
    /// Path waypoint spacing resolution (meters)
    /// </summary>
    public double ResolutionSplit { get; set; } = 0.05;

    #region Adaptive Lookahead Parameters

    /// <summary>
    /// Goal region distance - start reducing lookahead when closer than this (m)
    /// Default: 1.5m
    /// </summary>
    public double GoalRegionDistance { get; set; } = 1.5;

    /// <summary>
    /// Curvature adaptation factor (higher = more lookahead reduction on curves)
    /// Default: 2.0
    /// </summary>
    public double KCurvature { get; set; } = 2.0;

    /// <summary>
    /// Minimum lookahead time ratio (seconds) - for dynamic min limit
    /// Default: 0.3s
    /// </summary>
    public double MinLookaheadTimeRatio { get; set; } = 0.3;

    /// <summary>
    /// Maximum lookahead time ratio (seconds) - for dynamic max limit
    /// Default: 2.0s
    /// </summary>
    public double MaxLookaheadTimeRatio { get; set; } = 2.0;

    /// <summary>
    /// Switch to Stanley controller when within this distance to goal (m)
    /// Default: 0.5m
    /// </summary>
    public double FinalApproachThreshold { get; set; } = 1;

    #endregion
}

public class PurePursuit(PurePursuitConfig PurePursuitConfig, StanleyConfig StanleyConfig)
{
    public OrderNode[] OrderNodes = [];
    public OrderEdge[] OrderEdges = [];
    public OrderNode? LastOrderNode = null;
    public List<NavigationNode> Waypoints_Value = [];

    private Dictionary<string, (int start, int end)>? _segmentCache;

    private NavigationNode? Goal;

    private int closesEdgeIndex = 0;
    private int _currentWaypointAheadIndex = 0; // For Stanley controller
    private bool _isApproachGoal = false;        // Flag for final approach mode

    // Local Planner: approach curve stored as a virtual edge
    private int _approachWaypointCount = 0;
    private int _connectionEdgeIndex = 0;
    private OrderEdge? _approachOrderEdge;   // Virtual edge for segment cache lookup
    private SpaceEdge? _approachSpaceEdge;   // Geometry for projection in GetClosesEdges

    public PurePursuit WithPath(Node[] nodes, Edge[] edges, double currentTheta)
    {
        if (nodes.Length < 2) throw new SimulationException(RobotErrors.Error1002(nodes.Length));
        if (edges.Length < 1) throw new SimulationException();
        if (edges.Length != nodes.Length - 1) throw new SimulationException(RobotErrors.Error1004(nodes.Length, edges.Length));
        (OrderNodes, OrderEdges) = OrderConverter.Validate(nodes, edges, currentTheta);
        Waypoints_Value = [.. PathSplit(OrderNodes, OrderEdges)];
        closesEdgeIndex = 0;
        _currentWaypointAheadIndex = 0;
        _isApproachGoal = false;
        _approachWaypointCount = 0;
        _connectionEdgeIndex = 0;
        _approachOrderEdge = null;
        _approachSpaceEdge = null;
        BuildSegmentCache();
        return this;
    }

    public void ResetTracking()
    {
        closesEdgeIndex = 0;
        _currentWaypointAheadIndex = 0;
        _isApproachGoal = false;
    }

    /// <summary>
    /// Sinh approach waypoints (cubic Bezier) từ vị trí robot đến path rồi prepend vào Waypoints_Value.
    /// Approach curve CHỈ dựa vào vị trí (X,Y) robot và tangent tại connection point — KHÔNG dùng robotTheta.
    /// Robot sẽ Rotate tại chỗ đến heading đầu curve trước khi Moving.
    /// </summary>
    public ApproachResult GenerateAndPrependApproachPath(
        double robotX, double robotY, int closestIndex, LocalPlannerConfig config)
    {
        if (!config.Enabled || Waypoints_Value.Count < 2)
            return ApproachResult.Disabled;

        // Bước 1: Tính khoảng cách đến path
        double distToPath = CalculateDistance(robotX, robotY,
            Waypoints_Value[closestIndex].X, Waypoints_Value[closestIndex].Y);

        if (distToPath < config.OnPathThreshold)
            return ApproachResult.AlreadyOnPath;

        if (distToPath > config.MaxApproachDistance)
            return ApproachResult.TooFarFromPath;

        // Bước 2: Tính mergeDistance (dynamic theo distToPath)
        double mergeDistance = config.MergeDistanceGain * distToPath;
        mergeDistance = Math.Clamp(mergeDistance, config.MergeDistanceMin, config.MergeDistanceMax);

        // Bước 3: Tìm connection point trên path
        int connectionIndex = closestIndex;
        double accDist = 0;
        while (accDist < mergeDistance && connectionIndex < Waypoints_Value.Count - 2)
        {
            double segLen = CalculateDistance(
                Waypoints_Value[connectionIndex].X, Waypoints_Value[connectionIndex].Y,
                Waypoints_Value[connectionIndex + 1].X, Waypoints_Value[connectionIndex + 1].Y);
            accDist += segLen;
            connectionIndex++;
        }
        var connectionPoint = Waypoints_Value[connectionIndex];

        // Bước 4: Tính path tangent tại connection point
        double pathTangent;
        if (connectionIndex < Waypoints_Value.Count - 1)
        {
            double tx = Waypoints_Value[connectionIndex + 1].X - connectionPoint.X;
            double ty = Waypoints_Value[connectionIndex + 1].Y - connectionPoint.Y;
            pathTangent = Math.Atan2(ty, tx);
        }
        else if (connectionIndex > 0)
        {
            double tx = connectionPoint.X - Waypoints_Value[connectionIndex - 1].X;
            double ty = connectionPoint.Y - Waypoints_Value[connectionIndex - 1].Y;
            pathTangent = Math.Atan2(ty, tx);
        }
        else
        {
            return ApproachResult.Disabled;
        }

        // Bước 5: Tính 4 control points cho Cubic Bezier
        // P0 = robot position, P3 = connection point
        // P1 = dọc hướng P0→P3 (KHÔNG dùng robotTheta)
        // P2 = tiếp cận theo pathTangent
        double p0x = robotX, p0y = robotY;
        double p3x = connectionPoint.X, p3y = connectionPoint.Y;

        double dist = CalculateDistance(p0x, p0y, p3x, p3y);
        if (dist < config.ResolutionSplit)
            return ApproachResult.AlreadyOnPath;

        // Direction P0→P3
        double dirX = (p3x - p0x) / dist;
        double dirY = (p3y - p0y) / dist;

        double d1 = config.ControlArmRatioStart * dist;
        double d2 = config.ControlArmRatioEnd * dist;

        double p1x = p0x + d1 * dirX;
        double p1y = p0y + d1 * dirY;
        double p2x = p3x - d2 * Math.Cos(pathTangent);
        double p2y = p3y - d2 * Math.Sin(pathTangent);

        // Bước 6: Sample waypoints trên curve
        var approachEdge = new SpaceEdge()
        {
            StartX = p0x, StartY = p0y,
            EndX = p3x, EndY = p3y,
            ControlPoint1X = p1x, ControlPoint1Y = p1y,
            ControlPoint2X = p2x, ControlPoint2Y = p2y,
            Degree = 3
        };

        double length = SpaceCompute.GetEdgeLength(approachEdge, config.ResolutionSplit);
        if (length < config.ResolutionSplit)
            return ApproachResult.AlreadyOnPath;

        double step = config.ResolutionSplit / length;
        var approachWaypoints = new List<NavigationNode>();

        for (double t = 0; t < 1 - step; t += step)
        {
            (double x, double y) = SpaceCompute.BezierPoint(t, approachEdge);
            approachWaypoints.Add(new NavigationNode
            {
                Id = Guid.NewGuid(),
                NodeId = string.Empty,
                X = x,
                Y = y,
                Direction = connectionPoint.Direction,
                Speed = connectionPoint.Speed
            });
        }

        if (approachWaypoints.Count == 0)
            return ApproachResult.AlreadyOnPath;

        // Bước 7a: Xác định edge chứa connectionIndex (trước khi modify Waypoints_Value)
        int connectionEdgeIdx = 0;
        if (_segmentCache is not null)
        {
            for (int i = 0; i < OrderEdges.Length; i++)
            {
                if (_segmentCache.TryGetValue(OrderEdges[i].EdgeId, out var range)
                    && connectionIndex >= range.start && connectionIndex <= range.end)
                {
                    connectionEdgeIdx = i;
                    break;
                }
            }
        }

        // Bước 7b: Lưu approach curve như 1 virtual edge (cho projection + segment cache)
        _approachSpaceEdge = approachEdge;
        _approachOrderEdge = new OrderEdge
        {
            EdgeId = "__approach__",
            Degree = 3,
            ControlPoint1X = p1x, ControlPoint1Y = p1y,
            ControlPoint2X = p2x, ControlPoint2Y = p2y,
            Direction = connectionPoint.Direction,
            Speed = connectionPoint.Speed,
        };

        // Bước 7c: Insert approach waypoints ngay trước connectionIndex + xóa phần trước
        _approachWaypointCount = approachWaypoints.Count;
        Waypoints_Value.InsertRange(connectionIndex, approachWaypoints);
        if (connectionIndex > 0)
        {
            Waypoints_Value.RemoveRange(0, connectionIndex);
        }
        // Kết quả: [approach_0, ..., approach_n, connectionPoint, ..., goal]
        //           0                            _approachWaypointCount

        _connectionEdgeIndex = connectionEdgeIdx;
        closesEdgeIndex = connectionEdgeIdx;

        // Bước 7d: Rebuild segment cache (approach edge + real edges từ connectionEdge trở đi)
        BuildSegmentCache(_connectionEdgeIndex);

        return ApproachResult.ApproachGenerated;
    }

    /// <summary>
    /// Rebuild toàn bộ Waypoints_Value từ OrderNodes/OrderEdges đã lưu.
    /// Dùng khi cần sinh lại approach từ vị trí mới (thay vì ClearApproachWaypoints).
    /// Flow: RebuildPath() → check local planner → GenerateAndPrependApproachPath() nếu cần.
    /// </summary>
    public void RebuildPath()
    {
        string? goalId = Goal?.NodeId;

        Waypoints_Value = [.. PathSplit(OrderNodes, OrderEdges)];
        _approachWaypointCount = 0;
        _connectionEdgeIndex = 0;
        _approachOrderEdge = null;
        _approachSpaceEdge = null;
        BuildSegmentCache();

        if (!string.IsNullOrEmpty(goalId))
            UpdateGoal(goalId);
    }

    public void UpdateGoal(string goalId)
    {
        var goal = Waypoints_Value.FirstOrDefault(n => n.NodeId == goalId);
        if (goal is not null) Goal = goal;
    }

    private NavigationNode[] PathSplit(OrderNode[] nodes, OrderEdge[] edges)
    {
        List<NavigationNode> navigationNode = [new()
            {
                Id = Guid.NewGuid(),
                NodeId = nodes[0].NodeId,
                X = nodes[0].X,
                Y = nodes[0].Y,
                Theta = nodes[0].Theta,
                Direction = edges[0].Direction,
                Speed = edges[0].Speed,
            }];
        foreach (var edge in edges)
        {
            var startNode = nodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
            var endNode = nodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
            if (startNode is null) throw new PathPlannerException(RobotErrors.Error1008(edge.EdgeId, edge.StartNodeId));
            if (endNode is null) throw new PathPlannerException(RobotErrors.Error1009(edge.EdgeId, edge.EndNodeId));

            var spaceEdge = new SpaceEdge()
            {
                StartX = startNode.X,
                StartY = startNode.Y,
                EndX = endNode.X,
                EndY = endNode.Y,
                ControlPoint1X = edge.ControlPoint1X ?? 0,
                ControlPoint1Y = edge.ControlPoint1Y ?? 0,
                ControlPoint2X = edge.ControlPoint2X ?? 0,
                ControlPoint2Y = edge.ControlPoint2Y ?? 0,
                Degree = edge.Degree,
            };

            double length = SpaceCompute.GetEdgeLength(spaceEdge, PurePursuitConfig.ResolutionSplit);
            if (length <= 0) continue;
            double step = PurePursuitConfig.ResolutionSplit / length;

            for (double t = step; t <= 1 - step; t += step)
            {
                (double x, double y) = SpaceCompute.BezierPoint(t, spaceEdge);
                navigationNode.Add(new()
                {
                    Id = Guid.NewGuid(),
                    NodeId = string.Empty,
                    X = x,
                    Y = y,
                    Theta = null,
                    Direction = edge.Direction,
                    Speed = edge.Speed,
                });
            }
            navigationNode.Add(new()
            {
                Id = Guid.NewGuid(),
                NodeId = endNode.NodeId,
                X = endNode.X,
                Y = endNode.Y,
                Theta = endNode.Theta,
                Direction = edge.Direction,
                Speed = edge.Speed,
            });
        }
        return [.. navigationNode];
    }

    /// <summary>
    /// Build segment cache mapping EdgeId → (startWaypointIdx, endWaypointIdx).
    /// Approach edge (nếu có) được thêm vào cache trước, sau đó build các real edges từ fromEdgeIndex.
    /// </summary>
    private void BuildSegmentCache(int fromEdgeIndex = 0)
    {
        _segmentCache = [];

        // Thêm approach edge vào cache nếu có
        if (_approachOrderEdge is not null && _approachWaypointCount > 0)
        {
            _segmentCache[_approachOrderEdge.EdgeId] = (0, _approachWaypointCount);
        }

        for (int i = fromEdgeIndex; i < OrderEdges.Length; i++)
        {
            var edge = OrderEdges[i];
            int waypointStartIdx = Waypoints_Value.FindIndex(n => n.NodeId == edge.StartNodeId);
            int waypointEndIdx = Waypoints_Value.FindIndex(n => n.NodeId == edge.EndNodeId);

            // Edge đầu tiên: start node có thể đã bị xóa
            // → lấy end của approach edge nếu có, hoặc 0
            if (waypointStartIdx == -1)
            {
                if (i == fromEdgeIndex)
                    waypointStartIdx = _approachOrderEdge is not null
                        ? _segmentCache[_approachOrderEdge.EdgeId].end
                        : 0;
                else
                    waypointStartIdx = _segmentCache[OrderEdges[i - 1].EdgeId].end;
            }
            if (waypointEndIdx == -1) waypointEndIdx = Waypoints_Value.Count - 1;

            if (waypointStartIdx > waypointEndIdx) throw new NavigationException($"Waypoint has invalid range for edge {edge.EdgeId}: start={waypointStartIdx}, end={waypointEndIdx}");

            _segmentCache[edge.EdgeId] = (waypointStartIdx, waypointEndIdx);
        }
    }

    private (OrderEdge edge, double time) GetClosesEdges(double x, double y)
    {
        double minDistance = double.MaxValue;
        OrderEdge? edgesResult = null;
        double prjTime = 0;

        // Project lên approach edge nếu robot chưa vượt qua connection edge
        if (_approachSpaceEdge is not null && _approachOrderEdge is not null
            && closesEdgeIndex <= _connectionEdgeIndex)
        {
            (_, _, var approachDist, double approachTime) = SpaceCompute.GetProjectionOnEdge(x, y, _approachSpaceEdge);
            if (approachDist < minDistance)
            {
                minDistance = approachDist;
                edgesResult = _approachOrderEdge;
                prjTime = approachTime;
            }
        }

        // Project lên các real edges (từ closesEdgeIndex)
        for (int i = closesEdgeIndex; i < OrderEdges.Length; i++)
        {
            var startNode = OrderNodes.FirstOrDefault(node => node.NodeId == OrderEdges[i].StartNodeId);
            var endNode = OrderNodes.FirstOrDefault(node => node.NodeId == OrderEdges[i].EndNodeId);
            if (startNode is null || endNode is null) continue;

            (_, _, var distance, double time) = SpaceCompute.GetProjectionOnEdge(x, y, new()
            {
                StartX = startNode.X,
                StartY = startNode.Y,
                EndX = endNode.X,
                EndY = endNode.Y,
                Degree = OrderEdges[i].Degree,
                ControlPoint1X = OrderEdges[i].ControlPoint1X ?? 0,
                ControlPoint1Y = OrderEdges[i].ControlPoint1Y ?? 0,
                ControlPoint2X = OrderEdges[i].ControlPoint2X ?? 0,
                ControlPoint2Y = OrderEdges[i].ControlPoint2Y ?? 0,
            });
            if (distance < minDistance)
            {
                minDistance = distance;
                edgesResult = OrderEdges[i];
                prjTime = time;
                closesEdgeIndex = i;
            }
        }
        return (edgesResult ?? OrderEdges[closesEdgeIndex], prjTime);
    }

    public (NavigationNode node, int index) OnNode(double x, double y)
    {
        // Edge-based projection thống nhất cho cả approach edge và real edges
        (var closeEdge, double prjTime) = GetClosesEdges(x, y);
        (var startNodeIdx, var endNodeIdx) = _segmentCache is not null && _segmentCache.TryGetValue(closeEdge.EdgeId, out var cached)
            ? cached
            : (0, Waypoints_Value.Count - 1);
        int onNodeIndex = (int)(Math.Abs(endNodeIdx - startNodeIdx) * prjTime) + startNodeIdx;

        return (Waypoints_Value[onNodeIndex], onNodeIndex);
    }

    /// <summary>
    /// Calculate adaptive lookahead distance based on velocity, confidence, distance to goal, and path curvature
    /// Lookahead adapts to:
    /// 1. Velocity (faster = look further ahead)
    /// 2. Distance to goal (near goal = shorter lookahead for precision)
    /// 3. Path curvature (sharp curves = shorter lookahead for tighter tracking)
    /// 4. Confidence (low confidence = shorter lookahead for safety)
    /// 5. Velocity-based dynamic time limits
    /// </summary>
    private double GetLookaheadDistance(double vHybrid, double robotX, double robotY, int closestIndex)
    {
        // 1. Base lookahead from velocity
        double baseLookahead = PurePursuitConfig.LookaheadMin + PurePursuitConfig.Kdd * Math.Abs(vHybrid);

        // 2. Distance-to-goal adaptation
        double distanceToGoal = CalculateDistanceToGoal(robotX, robotY);
        double goalFactor = 1.0;
        if (distanceToGoal < PurePursuitConfig.GoalRegionDistance)
        {
            // Gradually reduce lookahead as we approach goal
            // At goal: factor = 0.5, At GoalRegionDistance: factor = 1.0
            goalFactor = 0.5 + 0.5 * (distanceToGoal / PurePursuitConfig.GoalRegionDistance);
        }

        // 3. Curvature adaptation
        double curvature = CalculateCurvature(closestIndex);
        // curvatureFactor ranges from 1.0 (straight) to ~0.33 (very sharp curve with KCurvature=2.0)
        double curvatureFactor = 1.0 / (1.0 + PurePursuitConfig.KCurvature * curvature);

        // 5. Combine all factors
        double adaptiveLookahead = baseLookahead * goalFactor * curvatureFactor;

        double minLookahead = PurePursuitConfig.LookaheadMin;
        double maxLookahead = PurePursuitConfig.LookaheadMax;

        if (distanceToGoal < PurePursuitConfig.GoalRegionDistance && Math.Abs(vHybrid) > 0.0)
        {
            // 6. Apply velocity-based dynamic limits
            // Minimum: look at least 0.3 seconds ahead or LookaheadMin (whichever is larger)
            minLookahead = Math.Max(PurePursuitConfig.LookaheadMin, Math.Abs(vHybrid) * PurePursuitConfig.MinLookaheadTimeRatio);

            // Maximum: look at most 2 seconds ahead or LookaheadMax (whichever is smaller)
            maxLookahead = Math.Min(PurePursuitConfig.LookaheadMax, Math.Abs(vHybrid) * PurePursuitConfig.MaxLookaheadTimeRatio);

            // Ensure min < max
            if (minLookahead > maxLookahead)
                minLookahead = maxLookahead;
        }

        adaptiveLookahead = Math.Clamp(adaptiveLookahead, minLookahead, maxLookahead);

        if (double.IsNaN(adaptiveLookahead) || adaptiveLookahead <= 0)
        {
            adaptiveLookahead = PurePursuitConfig.LookaheadMin;
        }

        return adaptiveLookahead;
    }

    public (double linearVel, double angularVel) PurePursuit_step(double X_Ref,
                                                                                            double Y_Ref,
                                                                                            double Angle_Ref,
                                                                                            double actualLinearVelocity,
                                                                                            double maxLinearVelocity)
    {
        if (Waypoints_Value is null || Waypoints_Value.Count < 2)
            throw new NavigationException("NAV PP Waypoint not yet set");

        // 1. Get closest waypoint (KEEP ORIGINAL LOGIC)
        var (onNode, index) = OnNode(X_Ref, Y_Ref);
        if (onNode is null || Goal is null)
            throw new NavigationException("NAV PP cannot get projection node");

        // 2. Calculate adaptive lookahead distance (UPGRADED)
        double lookaheadDistance = GetLookaheadDistance(actualLinearVelocity, X_Ref, Y_Ref, index);

        // 3. Find target point with INTERPOLATION (UPGRADED)
        NavigationNode? targetPoint = FindTargetPoint(index, lookaheadDistance);
        targetPoint ??= Goal;

        // 4. Apply speed limit from target point if available
        double linearVel = maxLinearVelocity;
        double? targetSpeed = targetPoint.Speed;

        if (targetSpeed.HasValue && targetSpeed.Value > 0)
        {
            linearVel = Math.Min(maxLinearVelocity, targetSpeed.Value);
        }

        // 5. Check for final approach (PREPARATION FOR STANLEY)
        double distanceToGoal = CalculateDistanceToGoal(X_Ref, Y_Ref);
        if (targetPoint.Id == Goal.Id || distanceToGoal <= PurePursuitConfig.FinalApproachThreshold)
        {
            _isApproachGoal = true;
        }

        // 6. Normalize theta for backward (SIMPLIFIED)
        bool isBackward = onNode.Direction == RobotDirection.BACKWARD;
        if (isBackward) Angle_Ref += Math.PI;
        Angle_Ref = NormalizeAngle(Angle_Ref);

        // 7. Switch to Stanley when approaching goal (STANLEY INTEGRATION)
        if (_isApproachGoal)
        {
            var (linear, angular) = FinalApproachController(X_Ref, Y_Ref, Angle_Ref, Goal, actualLinearVelocity, linearVel, isBackward);
            return (linear, angular);
        }

        // 8. Calculate angle to target
        var dx = targetPoint.X - X_Ref;
        var dy = targetPoint.Y - Y_Ref;
        var alpha = Math.Atan2(dy, dx) - Angle_Ref;

        // Normalize alpha to [-π, π] (SIMPLIFIED)
        alpha = NormalizeAngle(alpha);

        // 9. Pure Pursuit formula: ω = 2 * v * sin(α) / L
        var angularVelocity = 2.0 * Math.Abs(actualLinearVelocity) * Math.Sin(alpha) / lookaheadDistance;

        // 10. Clamp to max angular velocity
        if (Math.Abs(angularVelocity) > PurePursuitConfig.MaxAngularVelocity)
        {
            angularVelocity = Math.Sign(angularVelocity) * PurePursuitConfig.MaxAngularVelocity;
        }

        // 11. Apply direction sign to velocities
        if (isBackward)
        {
            linearVel = -linearVel;
        }

        Console.WriteLine($"PP: Target=({targetPoint.X:F3},{targetPoint.Y:F3}), " +
                         $"Pose=({X_Ref:F3},{Y_Ref:F3},{Angle_Ref * 180 / Math.PI:F2}°), " +
                         $"Look={lookaheadDistance:F3}, Alpha={alpha * 180 / Math.PI:F2}°, " +
                         $"LVel={linearVel:F3}, AngVel={angularVelocity:F3}");

        return (linearVel, angularVelocity);
    }

    /// <summary>
    /// Stanley-based final approach controller
    /// When robot enters goal region (IsApproachGoal), uses Stanley algorithm for precise CTE-based tracking
    /// </summary>
    private (double linearVel, double angularVel) FinalApproachController(
        double robotX,
        double robotY,
        double robotTheta,
        NavigationNode goal,
        double actualLinearVelocity,
        double maxLinearVelocity,
        bool isBackward)
    {
        // Calculate front axle position
        double frontX = robotX + StanleyConfig.WheelBase * Math.Cos(robotTheta);
        double frontY = robotY + StanleyConfig.WheelBase * Math.Sin(robotTheta);

        // Find closest point on path to front axle
        var (closestPoint, closestIndex) = GetClosestAheadWaypoint(frontX, frontY);

        // Calculate heading at closest point (path direction)
        double pathHeading = CalculatePathHeading(closestIndex);

        // Calculate cross-track error (signed distance from front axle to path)
        double crossTrackError = CalculateStanleyCrossTrackError(frontX, frontY, closestPoint, pathHeading);
        if (isBackward) crossTrackError = -crossTrackError;

        // Calculate heading error (path heading - robot heading)
        double headingError = NormalizeAngle(pathHeading - robotTheta);

        // Calculate curvature at closest point (for feedforward)
        double curvature = 0;
        if (StanleyConfig.EnableCurvatureFeedforward)
        {
            curvature = CalculateCurvature(closestIndex);
        }

        // Adaptive K gain: increase when close to goal for tighter tracking
        double distanceToGoal = CalculateDistance(robotX, robotY, goal.X, goal.Y);
        double adaptiveK = StanleyConfig.K;
        if (distanceToGoal < StanleyConfig.GoalApproachDistance)
        {
            // Linearly increase K from K to K*GoalGainMultiplier as distance decreases
            double approachRatio = 1.0 - (distanceToGoal / StanleyConfig.GoalApproachDistance);
            adaptiveK = StanleyConfig.K * (1.0 + approachRatio * (StanleyConfig.GoalGainMultiplier - 1.0));
        }

        // Stanley formula: δ = ψ + arctan(K × e / (v + Ks))
        double crossTrackTerm = Math.Atan2(adaptiveK * crossTrackError, Math.Abs(actualLinearVelocity) + StanleyConfig.Ks);

        // Add curvature feedforward if enabled
        double curvatureTerm = 0;
        if (StanleyConfig.EnableCurvatureFeedforward && curvature != 0)
        {
            curvatureTerm = StanleyConfig.KCurvatureFF * Math.Atan(curvature * StanleyConfig.WheelBase);
        }

        // Total steering angle
        double steeringAngle = headingError + crossTrackTerm + curvatureTerm;

        // Clamp to maximum steering angle
        steeringAngle = Math.Clamp(steeringAngle, -StanleyConfig.MaxSteeringAngle, StanleyConfig.MaxSteeringAngle);

        // Convert steering angle to angular velocity using bicycle model
        double angularVelocity = (actualLinearVelocity * Math.Tan(steeringAngle)) / StanleyConfig.WheelBase;

        // Clamp angular velocity for final approach
        angularVelocity = Math.Clamp(angularVelocity, -StanleyConfig.MaxAngularVelocity, StanleyConfig.MaxAngularVelocity);

        // Apply direction to velocities
        double linearVel = maxLinearVelocity;
        if (isBackward)
        {
            linearVel = -linearVel;
        }

        // Debug output
        Console.WriteLine($"FA-Stanley: Front=({frontX:F3},{frontY:F3}), Closest=({closestPoint.X:F3},{closestPoint.Y:F3}), " +
                         $"Pose=({robotX:F3},{robotY:F3},{robotTheta * 180 / Math.PI:F1}°), " +
                         $"CTE={crossTrackError:F3}m, HeadErr={headingError * 180 / Math.PI:F1}°, " +
                         $"Curv={curvature:F3}, SteerAng={steeringAngle * 180 / Math.PI:F1}°, " +
                         $"K={adaptiveK:F2}, DTG={distanceToGoal:F3}m, " +
                         $"LVel={linearVel:F3}, AnVel={angularVelocity:F3}");

        return (linearVel, angularVelocity);
    }

    /// <summary>
    /// Find target point at lookahead distance from current index with interpolation
    /// This provides smooth target point selection instead of discrete waypoints
    /// Speed information is interpolated to provide accurate future speed limit
    /// </summary>
    private NavigationNode? FindTargetPoint(int startIndex, double lookaheadDistance)
    {
        if (startIndex >= Waypoints_Value.Count - 1)
            return Goal;

        double accumulatedDistance = 0;

        for (int i = startIndex; i < Waypoints_Value.Count - 1; i++)
        {
            double dx = Waypoints_Value[i + 1].X - Waypoints_Value[i].X;
            double dy = Waypoints_Value[i + 1].Y - Waypoints_Value[i].Y;
            double segmentLength = Math.Sqrt(dx * dx + dy * dy);

            if (accumulatedDistance + segmentLength >= lookaheadDistance)
            {
                // Interpolate within this segment
                double t = (lookaheadDistance - accumulatedDistance) / segmentLength;

                // Interpolate speed if both waypoints have speed info
                double? interpolatedSpeed;
                if (Waypoints_Value[i].Speed is { } speed1 && Waypoints_Value[i + 1].Speed is { } speed2)
                {
                    // Linear interpolation of speed limit
                    interpolatedSpeed = speed1 + t * (speed2 - speed1);
                }
                else
                {
                    // Use next waypoint's speed if available (upcoming constraint)
                    interpolatedSpeed = Waypoints_Value[i + 1].Speed ?? Waypoints_Value[i].Speed;
                }

                return new NavigationNode
                {
                    Id = Guid.NewGuid(),
                    NodeId = string.Empty,
                    X = Waypoints_Value[i].X + t * dx,
                    Y = Waypoints_Value[i].Y + t * dy,
                    Theta = null,
                    Direction = Waypoints_Value[i].Direction,
                    Speed = interpolatedSpeed  // Preserve speed limit for lookahead
                };
            }

            accumulatedDistance += segmentLength;
        }

        return Goal;
    }

    #region Helper Methods

    /// <summary>
    /// Calculate distance between two points
    /// </summary>
    private static double CalculateDistance(double x1, double y1, double x2, double y2)
    {
        double dx = x2 - x1;
        double dy = y2 - y1;
        return Math.Sqrt(dx * dx + dy * dy);
    }

    /// <summary>
    /// Normalize angle to [-π, π]
    /// </summary>
    private static double NormalizeAngle(double angle)
    {
        while (angle > Math.PI) angle -= 2 * Math.PI;
        while (angle < -Math.PI) angle += 2 * Math.PI;
        return angle;
    }

    /// <summary>
    /// Calculate distance from robot to goal point
    /// </summary>
    private double CalculateDistanceToGoal(double robotX, double robotY)
    {
        if (Goal == null)
            return double.MaxValue;

        double dx = Goal.X - robotX;
        double dy = Goal.Y - robotY;
        return Math.Sqrt(dx * dx + dy * dy);
    }

    /// <summary>
    /// Calculate path curvature at given waypoint index using 3-point circle fitting (Menger curvature)
    /// Returns curvature in 1/meters (larger value = sharper curve)
    /// </summary>
    private double CalculateCurvature(int index)
    {
        // Need at least 3 points for curvature calculation
        if (Waypoints_Value.Count < 3 || index <= 0 || index >= Waypoints_Value.Count - 1)
            return 0.0;

        var p1 = Waypoints_Value[index - 1];
        var p2 = Waypoints_Value[index];
        var p3 = Waypoints_Value[index + 1];

        // Calculate vectors
        double dx1 = p2.X - p1.X;
        double dy1 = p2.Y - p1.Y;
        double dx2 = p3.X - p2.X;
        double dy2 = p3.Y - p2.Y;

        // Cross product magnitude (2 * triangle area)
        double cross = Math.Abs(dx1 * dy2 - dy1 * dx2);

        // Side lengths of triangle
        double a = Math.Sqrt(dx1 * dx1 + dy1 * dy1);
        double b = Math.Sqrt(dx2 * dx2 + dy2 * dy2);
        double c = Math.Sqrt((p3.X - p1.X) * (p3.X - p1.X) + (p3.Y - p1.Y) * (p3.Y - p1.Y));

        // Menger curvature formula: k = 4 * Area / (a * b * c)
        // Area of triangle = cross / 2, so k = 2 * cross / (a * b * c)
        double curvature = 2.0 * cross / (a * b * c + 1e-9);  // Add small epsilon to avoid division by zero

        return curvature;
    }

    #endregion

    #region Stanley Helper Methods (for FinalApproachController)

    /// <summary>
    /// Get closest waypoint ahead to given position (for Stanley front axle tracking)
    /// </summary>
    private (NavigationNode point, int index) GetClosestAheadWaypoint(double x, double y)
    {
        if (Waypoints_Value.Count == 0)
            throw new InvalidOperationException("Path not set");

        double minDistance = double.MaxValue;
        int closestIndex = 0;

        // Start search from current index for efficiency
        for (int i = _currentWaypointAheadIndex; i < Waypoints_Value.Count; i++)
        {
            double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);

            if (distance < minDistance)
            {
                minDistance = distance;
                closestIndex = i;
            }
        }

        // Also check previous waypoints in case robot moved backwards
        for (int i = 0; i < _currentWaypointAheadIndex; i++)
        {
            double distance = CalculateDistance(x, y, Waypoints_Value[i].X, Waypoints_Value[i].Y);

            if (distance < minDistance)
            {
                minDistance = distance;
                closestIndex = i;
            }
        }

        _currentWaypointAheadIndex = closestIndex;
        return (Waypoints_Value[closestIndex], closestIndex);
    }

    /// <summary>
    /// Calculate path heading at given waypoint index (for Stanley)
    /// Uses current point and next point to determine direction
    /// </summary>
    private double CalculatePathHeading(int index)
    {
        if (index >= Waypoints_Value.Count - 1)
        {
            // Last point - use previous segment direction
            if (index > 0)
            {
                double dx = Waypoints_Value[index].X - Waypoints_Value[index - 1].X;
                double dy = Waypoints_Value[index].Y - Waypoints_Value[index - 1].Y;
                return Math.Atan2(dy, dx);
            }
            return 0;
        }

        // Use current to next point
        double dxNext = Waypoints_Value[index + 1].X - Waypoints_Value[index].X;
        double dyNext = Waypoints_Value[index + 1].Y - Waypoints_Value[index].Y;
        return Math.Atan2(dyNext, dxNext);
    }

    /// <summary>
    /// Calculate signed cross-track error (for Stanley)
    /// Positive: front axle is to the left of path
    /// Negative: front axle is to the right of path
    /// </summary>
    private static double CalculateStanleyCrossTrackError(double frontX, double frontY, NavigationNode closestPoint, double pathHeading)
    {
        // Vector from closest point to front axle
        double dx = frontX - closestPoint.X;
        double dy = frontY - closestPoint.Y;

        // Path direction vector
        double pathDx = Math.Cos(pathHeading);
        double pathDy = Math.Sin(pathHeading);

        // Cross product to get signed perpendicular distance
        // positive = left, negative = right
        double crossTrackError = dx * pathDy - dy * pathDx;

        return crossTrackError;
    }

    #endregion

}