using Microsoft.Extensions.Logging;
using RobotNet10.Common;
using RobotNet10.NavigationTune.Navigation.Core;
using RobotNet10.NavigationTune.Services;
using RobotNet10.NavigationTune.Shared.Interfaces;
using RobotNet10.NavigationTune.Shared.Models;

namespace RobotNet10.NavigationTune.Execution;

/// <summary>
/// Navigation phases for state machine
/// </summary>
public enum NavigationPhase
{
    InitialRotation,    // Rotating to face initial lookahead point
    PathFollowing,      // Following path using Pure Pursuit
    FinalRotation,      // Rotating to final goal heading
    Completed           // Navigation complete
}

/// <summary>
/// Test executor implementation
/// Executes test scenarios using simplified navigation controllers; control loop runs on WatchThread (50Hz).
/// </summary>
public class TestExecutor : ITestExecutor
{
    private const int ControlLoopFrequency = 50; // 50Hz
    private const double Dt = 1.0 / ControlLoopFrequency;
    private static readonly int ControlLoopIntervalMs = 1000 / ControlLoopFrequency;

    private readonly ILocalizationProvider _localization;
    private readonly IVelocityProvider _velocityProvider;
    private readonly SafetyMonitor _safetyMonitor;
    private readonly SafetyConfig _safetyConfig;
    private readonly ILogger<TestExecutor>? _logger;

    private TestStatus _status = TestStatus.Preparing;
    private TestScenario? _currentScenario;
    private NavigationParameterSet? _currentParameters;
    private ReferencePath? _referencePath;
    private List<TelemetryData> _telemetryData = new();
    private CancellationTokenSource? _cancellationTokenSource;

    // Controllers
    private PID? _movePid;
    private PID? _rotatePid;
    private PurePursuitSimplified? _purePursuit;
    private StanleySimplified? _stanley;
    private VelocityEstimatorSimplified? _velocityEstimator;

    // State
    private Pose2D _currentPose;
    private Twist2D _currentTwist;
    private double _linearVelocityCommand = 0;
    private double _angularVelocityCommand = 0;
    private DateTime _startTime;
    private double _totalDistanceTraveled = 0;
    private Pose2D _lastPose;
    private int _telemetryCreateFrequency = 15;
    private int _telemetryCreateCount = 0;

    // Navigation phase state machine
    private NavigationPhase _navigationPhase = NavigationPhase.InitialRotation;
    private double _targetHeading = 0; // Target heading for rotation phases

    // WatchThread control loop
    private WatchThread<TestExecutor>? _controlLoopThread;
    private TaskCompletionSource<TestExecutionResult>? _tcs;
    private volatile bool _controlLoopDone;
    private double _oldDistanceToGoal;
    private int _stallCounter;

    public TestExecutor(
        ILocalizationProvider localization,
        IVelocityProvider velocityProvider,
        SafetyConfig? safetyConfig = null,
        ILogger<TestExecutor>? logger = null)
    {
        _localization = localization;
        _velocityProvider = velocityProvider;
        _safetyConfig = safetyConfig ?? new SafetyConfig();
        _safetyMonitor = new SafetyMonitor(_safetyConfig);
        _logger = logger;
    }

    public async Task<TestExecutionResult> ExecuteAsync(
        TestScenario scenario,
        NavigationParameterSet parameters,
        Action<TelemetryData>? onTelemetryUpdate = null,
        CancellationToken cancellationToken = default,
        Action<TestExecutionResult>? onComplete = null)
    {
        if (_status == TestStatus.Running)
            throw new InvalidOperationException("Test is already running");

        _currentScenario = scenario;
        _currentParameters = parameters;
        _status = TestStatus.Preparing;
        _telemetryData.Clear();
        _safetyMonitor.Reset();
        _controlLoopDone = false;
        _oldDistanceToGoal = 0;
        _stallCounter = 0;
        _navigationPhase = NavigationPhase.InitialRotation;

        // Generate reference path
        var pathPoints = scenario.GenerateReferencePath();
        _referencePath = new ReferencePath
        {
            Points = pathPoints,
            TotalLength = pathPoints.Count > 0 ? pathPoints[^1].DistanceFromStart : 0
        };

        // Initialize controllers
        _movePid = new PID(parameters.MovePidConfig);
        _rotatePid = new PID(parameters.RotatePidConfig);

        // Initialize path following controller based on selected type
        if (parameters.ControllerType == PathFollowingController.Stanley)
        {
            _stanley = new StanleySimplified(parameters.StanleyConfig);
            _stanley.SetPath(pathPoints);
            Console.WriteLine("Using Stanley Controller for path following");
        }
        else
        {
            _purePursuit = new PurePursuitSimplified(parameters.PurePursuitConfig, parameters.StanleyConfig);
            _purePursuit.SetPath(pathPoints);
            Console.WriteLine("Using Pure Pursuit Controller for path following");
        }

        _velocityEstimator = new VelocityEstimatorSimplified(
            parameters.EstimatorConfig,
            parameters.SignalConfig,
            parameters.MotorDynamicsConfig
        );

        // Initialize pose from localization
        _currentPose = new Pose2D(_localization.X, _localization.Y, _localization.Theta);
        _lastPose = _currentPose;
        _startTime = DateTime.UtcNow;

        // Calculate initial target heading for initial rotation phase
        _targetHeading = CalculateInitialTargetHeading();

        _cancellationTokenSource = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
        _status = TestStatus.Running;

        if (_currentScenario != null)
        {
            var goalPose = _currentScenario.GetGoalPose();
            Console.WriteLine($"Starting test execution towards goal at ({goalPose.X}, {goalPose.Y})");
        }

        _velocityProvider.SetAcceleration(parameters.NavigationConfig.Acceleration);
        _velocityProvider.SetDeceleration(parameters.NavigationConfig.Deceleration);

        if (onComplete != null)
        {
            // Fire-and-forget: run control loop on WatchThread, return immediately with Running
            var thread = new WatchThread<TestExecutor>(
                ControlLoopIntervalMs,
                () =>
                {
                    if (_controlLoopDone) return;
                    try
                    {
                        if (!RunOneControlTick(onTelemetryUpdate))
                        {
                            _controlLoopDone = true;
                            var result = BuildFinalResult();
                            var t = _controlLoopThread;
                            _controlLoopThread = null;
                            onComplete(result);
                            _ = Task.Run(() =>
                            {
                                Thread.Sleep(50);
                                t?.Stop();
                            });
                        }
                    }
                    catch (OperationCanceledException)
                    {
                        _status = TestStatus.Aborted;
                        _velocityProvider.SetVelocity(0, 0);
                        _controlLoopDone = true;
                        var result = BuildFinalResult();
                        var t = _controlLoopThread;
                        _controlLoopThread = null;
                        onComplete(result);
                        _ = Task.Run(() => { Thread.Sleep(50); t?.Stop(); });
                    }
                    catch (Exception ex)
                    {
                        _logger?.LogError(ex, "Control loop error");
                        _status = TestStatus.Error;
                        _velocityProvider.SetVelocity(0, 0);
                        _controlLoopDone = true;
                        var result = BuildFinalResult();
                        result.ErrorMessage = ex.Message;
                        var t = _controlLoopThread;
                        _controlLoopThread = null;
                        onComplete(result);
                        _ = Task.Run(() => { Thread.Sleep(50); t?.Stop(); });
                    }
                },
                _logger);
            _controlLoopThread = thread;
            thread.Start();
            return new TestExecutionResult
            {
                TestRunId = Guid.Empty,
                Status = TestStatus.Running,
                StartTime = _startTime
            };
        }

        // Await mode: run on WatchThread and wait for TCS
        _tcs = new TaskCompletionSource<TestExecutionResult>(TaskCreationOptions.RunContinuationsAsynchronously);
        var watchThread = new WatchThread<TestExecutor>(
            ControlLoopIntervalMs,
            () =>
            {
                if (_controlLoopDone) return;
                try
                {
                    if (!RunOneControlTick(onTelemetryUpdate))
                    {
                        _controlLoopDone = true;
                        var result = BuildFinalResult();
                        _tcs?.TrySetResult(result);
                        var t = _controlLoopThread;
                        _controlLoopThread = null;
                        _ = Task.Run(() => { Thread.Sleep(50); t?.Stop(); });
                    }
                }
                catch (OperationCanceledException)
                {
                    _status = TestStatus.Aborted;
                    _velocityProvider.SetVelocity(0, 0);
                    _controlLoopDone = true;
                    _tcs?.TrySetResult(BuildFinalResult());
                    var t = _controlLoopThread;
                    _controlLoopThread = null;
                    _ = Task.Run(() => { Thread.Sleep(50); t?.Stop(); });
                }
                catch (Exception ex)
                {
                    _logger?.LogError(ex, "Control loop error");
                    _status = TestStatus.Error;
                    _velocityProvider.SetVelocity(0, 0);
                    _controlLoopDone = true;
                    var errResult = BuildFinalResult();
                    errResult.ErrorMessage = ex.Message;
                    _tcs?.TrySetResult(errResult);
                    var t = _controlLoopThread;
                    _controlLoopThread = null;
                    _ = Task.Run(() => { Thread.Sleep(50); t?.Stop(); });
                }
            },
            _logger);
        _controlLoopThread = watchThread;
        watchThread.Start();

        try
        {
            return await _tcs.Task;
        }
        finally
        {
            _controlLoopThread?.Stop();
            _controlLoopThread = null;
        }
    }

    /// <summary>
    /// One tick of the control loop (50Hz). Returns false when done (goal/cancel/error/stall/safety).
    /// Implements state machine: InitialRotation -> PathFollowing -> FinalRotation -> Completed
    /// </summary>
    private bool RunOneControlTick(Action<TelemetryData>? onTelemetryUpdate)
    {
        if (_currentScenario is null) return false;
        if (_cancellationTokenSource?.Token.IsCancellationRequested == true)
        {
            _status = TestStatus.Aborted;
            _velocityProvider.SetVelocity(0, 0);
            return false;
        }

        var goalPose = _currentScenario.GetGoalPose();

        // Read current state
        _currentPose = new Pose2D(_localization.X, _localization.Y, _localization.Theta);
        var (linearVel, angularVel) = _velocityProvider.GetActualVelocity();

        var distanceToGoal = Pose2D.Distance(_currentPose, goalPose);

        // Estimate hybrid velocity
        var vHybrid = _velocityEstimator!.EstimateVelocity(
            _linearVelocityCommand,
            linearVel,
            Dt);
        var confidence = _velocityEstimator.GetConfidence();
        _currentTwist = new Twist2D(vHybrid, angularVel);

        // Create telemetry and check safety
        var commandTwist = new Twist2D(_linearVelocityCommand, _angularVelocityCommand);
        var telemetry = CreateTelemetryData(_currentPose, _currentTwist, commandTwist, distanceToGoal);
        if (!_safetyMonitor.CheckSafety(telemetry, _referencePath!))
        {
            _status = TestStatus.EmergencyStopped;
            _velocityProvider.SetVelocity(0, 0);
            Console.WriteLine("Emergency stop triggered due to safety violation.");
            return false;
        }

        if (_telemetryCreateCount++ >= (ControlLoopFrequency / _telemetryCreateFrequency))
        {
            _telemetryData.Add(telemetry);
            _telemetryCreateCount = 0;
        }

        var reachedRadius = _currentParameters?.NavigationConfig.ReachedRadius ?? 0.015f;

        // State machine logic
        switch (_navigationPhase)
        {
            case NavigationPhase.InitialRotation:
                {
                    // Check if initial rotation is needed
                    double headingError = NavigationMath.NormalizeAngle(_targetHeading - _currentPose.Theta);
                    double initialRotationThresholdRad = (_currentParameters?.NavigationConfig.InitialRotationThreshold ?? 5.0f) * Math.PI / 180.0;

                    if (Math.Abs(headingError) < initialRotationThresholdRad)
                    {
                        // Heading is good enough, skip to path following
                        Console.WriteLine($"Initial heading acceptable ({Math.Abs(headingError) * 180 / Math.PI:F2}°), skipping initial rotation");
                        _navigationPhase = NavigationPhase.PathFollowing;
                        _rotatePid!.Reset();
                    }
                    else
                    {
                        // Perform initial rotation
                        if (PerformInitialRotation())
                        {
                            // Rotation complete, move to path following
                            _navigationPhase = NavigationPhase.PathFollowing;
                            _rotatePid!.Reset();
                        }
                    }

                    onTelemetryUpdate?.Invoke(telemetry);
                    _totalDistanceTraveled += (float)Pose2D.Distance(_lastPose, _currentPose);
                    _lastPose = _currentPose;
                    return true;
                }

            case NavigationPhase.PathFollowing:
                {
                    if (distanceToGoal <= reachedRadius)
                    {
                        // Position reached, move to final rotation
                        _targetHeading = CalculateFinalTargetHeading();
                        _navigationPhase = NavigationPhase.FinalRotation;
                        _rotatePid!.Reset();
                        Console.WriteLine($"Position reached, starting final rotation to {_targetHeading * 180 / Math.PI:F2}°. Current pose: [{_currentPose.X} - {_currentPose.Y}], Goal: [{goalPose.X} - {goalPose.Y}], Distance: {distanceToGoal}");

                        onTelemetryUpdate?.Invoke(telemetry);
                        _totalDistanceTraveled += (float)Pose2D.Distance(_lastPose, _currentPose);
                        _lastPose = _currentPose;
                        return true;
                    }

                    // Stall detection (only during path following)
                    if (_oldDistanceToGoal >= distanceToGoal) _stallCounter = 0;
                    else if (distanceToGoal < 0.3)
                    {
                        _stallCounter++;
                        if (_stallCounter >= ControlLoopFrequency * 0.1)
                        {
                            _status = TestStatus.Error;
                            _velocityProvider.SetVelocity(0, 0);
                            Console.WriteLine("Test execution stalled: no progress towards goal.");

                            _targetHeading = CalculateFinalTargetHeading();
                            _navigationPhase = NavigationPhase.FinalRotation;
                            _rotatePid!.Reset();
                            return true;
                        }
                    }
                    _oldDistanceToGoal = distanceToGoal;

                    // Calculate max linear velocity with PID deceleration
                    double maxLinearVel;
                    if (distanceToGoal > 5.0)
                        maxLinearVel = _currentParameters!.NavigationConfig.MaxLinearVelocity;
                    else
                    {
                        var pidOutput = _movePid!.PID_step(distanceToGoal,
                            _currentParameters!.NavigationConfig.MaxLinearVelocity,
                            _currentParameters.NavigationConfig.MinLinearVelocity,
                            Dt);
                        maxLinearVel = pidOutput;
                    }

                    // Path following using selected controller
                    double linearVelCmd, angularVelCmd;

                    if (_currentParameters!.ControllerType == PathFollowingController.Stanley)
                    {
                        // Stanley Controller
                        (linearVelCmd, angularVelCmd) = _stanley!.CalculateVelocity(
                            _currentPose.X,
                            _currentPose.Y,
                            _currentPose.Theta,
                            vHybrid,
                            maxLinearVel);
                    }
                    else
                    {
                        // Pure Pursuit Controller
                        (linearVelCmd, angularVelCmd) = _purePursuit!.CalculateAngularVelocity(
                            _currentPose.X,
                            _currentPose.Y,
                            _currentPose.Theta,
                            vHybrid,
                            maxLinearVel);
                    }

                    var linearVelSign = Math.Sign(linearVelCmd);
                    _linearVelocityCommand = (float)Math.Clamp(
                        Math.Abs(linearVelCmd),
                        _currentParameters!.NavigationConfig.MinLinearVelocity,
                        _currentParameters.NavigationConfig.MaxLinearVelocity);
                    _angularVelocityCommand = (float)Math.Clamp(
                        angularVelCmd,
                        -_currentParameters.NavigationConfig.MaxAngularVelocity,
                        _currentParameters.NavigationConfig.MaxAngularVelocity);

                    _velocityProvider.SetVelocity(linearVelSign * _linearVelocityCommand, _angularVelocityCommand);

                    onTelemetryUpdate?.Invoke(telemetry);
                    _totalDistanceTraveled += (float)Pose2D.Distance(_lastPose, _currentPose);
                    _lastPose = _currentPose;
                    return true;
                }

            case NavigationPhase.FinalRotation:
                {
                    // Perform final rotation to goal heading
                    if (PerformFinalRotation())
                    {
                        // Final rotation complete, navigation done
                        _navigationPhase = NavigationPhase.Completed;
                        _status = TestStatus.Completed;
                        _velocityProvider.SetVelocity(0, 0);
                        Console.WriteLine("Navigation complete!");
                        return false;
                    }

                    onTelemetryUpdate?.Invoke(telemetry);
                    _totalDistanceTraveled += (float)Pose2D.Distance(_lastPose, _currentPose);
                    _lastPose = _currentPose;
                    return true;
                }

            case NavigationPhase.Completed:
                {
                    _status = TestStatus.Completed;
                    _velocityProvider.SetVelocity(0, 0);
                    return false;
                }

            default:
                return false;
        }
    }

    private TestExecutionResult BuildFinalResult()
    {
        var endTime = DateTime.UtcNow;
        var duration = (endTime - _startTime).TotalSeconds;
        return new TestExecutionResult
        {
            TestRunId = Guid.NewGuid(),
            Status = _status,
            TelemetryData = _telemetryData,
            SafetyViolations = _safetyMonitor.GetViolations(),
            StartTime = _startTime,
            EndTime = endTime,
            Duration = duration
        };
    }

    private TelemetryData CreateTelemetryData(Pose2D pose, Twist2D twist, Twist2D commandTwist, double distanceToGoal)
    {
        var closestPoint = _referencePath!.GetClosestPoint(pose);
        var cte = closestPoint != null
            ? Math.Sqrt(Math.Pow(pose.X - closestPoint.X, 2) + Math.Pow(pose.Y - closestPoint.Y, 2))
            : 0;

        // Calculate heading error from Direction
        // Reference heading is calculated from direction of movement (tangent to path)
        double headingError = 0;
        double refTheta = 0;
        if (closestPoint != null)
        {
            // Find next point to determine direction
            int closestIndex = _referencePath.Points.IndexOf(closestPoint);
            if (closestIndex >= 0 && closestIndex < _referencePath.Points.Count - 1)
            {
                var nextPoint = _referencePath.Points[closestIndex + 1];
                double dx = nextPoint.X - closestPoint.X;
                double dy = nextPoint.Y - closestPoint.Y;
                refTheta = Math.Atan2(dy, dx);
            }
            else if (closestIndex > 0)
            {
                // Use previous point
                var prevPoint = _referencePath.Points[closestIndex - 1];
                double dx = closestPoint.X - prevPoint.X;
                double dy = closestPoint.Y - prevPoint.Y;
                refTheta = Math.Atan2(dy, dx);
            }
            // Adjust for backward direction
            if (closestPoint.Direction == RobotDirection.BACKWARD)
            {
                refTheta = NavigationMath.NormalizeAngle(refTheta + Math.PI);
            }
            refTheta = NavigationMath.NormalizeAngle(refTheta);
            headingError = NavigationMath.NormalizeAngle(pose.Theta - refTheta);
        }

        // Lookahead distance only applicable for Pure Pursuit
        var lookaheadDistance = 0.0;
        if (_currentParameters?.ControllerType == PathFollowingController.PurePursuit)
        {
            lookaheadDistance = _purePursuit?.GetCurrentLookahead(
                twist.Linear,
                _velocityEstimator?.GetConfidence() ?? 1.0
            ) ?? 0;
        }

        var refPose = closestPoint != null
            ? new Pose2D(closestPoint.X, closestPoint.Y, refTheta)
            : pose;

        return new TelemetryData
        {
            TimestampMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds(),
            RobotPose = pose,
            RobotTwist = twist,
            CommandTwist = commandTwist,
            ReferencePose = refPose,
            CrossTrackError = cte,
            HeadingError = headingError,
            LookaheadDistance = lookaheadDistance,
            ModelConfidence = _velocityEstimator?.GetConfidence() ?? 1.0,
            DistanceToGoal = distanceToGoal,
            Phase = DetermineCurrentPhase(distanceToGoal)
        };
    }

    private TelemetryPhase DetermineCurrentPhase(double distanceToGoal)
    {
        return _navigationPhase switch
        {
            NavigationPhase.InitialRotation => TelemetryPhase.InitialRotation,
            NavigationPhase.PathFollowing => DeterminePathFollowingSubPhase(distanceToGoal),
            NavigationPhase.FinalRotation => TelemetryPhase.FinalRotation,
            NavigationPhase.Completed => TelemetryPhase.Completed,
            _ => TelemetryPhase.PathFollowing
        };
    }

    private TelemetryPhase DeterminePathFollowingSubPhase(double distanceToGoal)
    {
        if (_purePursuit != null && _purePursuit.IsInFinalApproach)
            return TelemetryPhase.FinalApproach;
        if (_stanley != null && _currentParameters != null &&
            distanceToGoal < _currentParameters.StanleyConfig.GoalApproachDistance)
            return TelemetryPhase.FinalApproach;
        return TelemetryPhase.PathFollowing;
    }


    public void Pause()
    {
        if (_status == TestStatus.Running)
        {
            _status = TestStatus.Paused;
            _velocityProvider.SetVelocity(0, 0);
        }
    }

    public void Resume()
    {
        if (_status == TestStatus.Paused)
        {
            _status = TestStatus.Running;
        }
    }

    public void Stop()
    {
        _cancellationTokenSource?.Cancel();
        _velocityProvider.SetVelocity(0, 0);
        _status = TestStatus.Aborted;
        Console.WriteLine("Test execution stopped by user.");
    }

    public void EmergencyStop()
    {
        _cancellationTokenSource?.Cancel();
        _velocityProvider.SetVelocity(0, 0);
        _status = TestStatus.EmergencyStopped;
    }

    public TestStatus GetStatus() => _status;

    public TestProgress GetProgress()
    {
        if (_currentScenario == null || _referencePath == null)
            return new TestProgress();

        var goalPose = _currentScenario.GetGoalPose();
        var distanceToGoal = Pose2D.Distance(_currentPose, goalPose);
        var totalDistance = _referencePath.TotalLength;
        var progressPercent = totalDistance > 0
            ? 1.0 - (distanceToGoal / totalDistance)
            : 0;

        var elapsedTime = (DateTime.UtcNow - _startTime).TotalSeconds;
        var estimatedTimeRemaining = progressPercent > 0.01f
            ? elapsedTime / progressPercent - elapsedTime
            : 0;

        return new TestProgress
        {
            ProgressPercent = Math.Clamp(progressPercent, 0, 1),
            DistanceTraveled = _totalDistanceTraveled,
            DistanceToGoal = distanceToGoal,
            ElapsedTime = elapsedTime,
            EstimatedTimeRemaining = estimatedTimeRemaining
        };
    }

    /// <summary>
    /// Calculate initial target heading to first lookahead point
    /// Returns the angle from current position to the lookahead point on the path
    /// </summary>
    private double CalculateInitialTargetHeading()
    {
        if (_referencePath == null || _referencePath.Points.Count < 2)
            return _currentPose.Theta;

        // Get closest point on path
        var closestPoint = _referencePath.GetClosestPoint(_currentPose);
        if (closestPoint == null)
            return _currentPose.Theta;

        int closestIndex = _referencePath.Points.IndexOf(closestPoint);
        if (closestIndex < 0)
            return _currentPose.Theta;

        // Calculate a simple lookahead distance (use minimum lookahead)
        double lookaheadDistance = (_currentParameters?.PurePursuitConfig.LookaheadMin + _currentParameters?.PurePursuitConfig.LookaheadMax ) / 2 ?? 1;

        // Find target point at lookahead distance
        PathPoint? targetPoint = FindTargetPointAtDistance(closestIndex, lookaheadDistance);
        if (targetPoint == null)
            targetPoint = _referencePath.Points[^1]; // Use goal if no target found

        // Calculate angle to target point
        double dx = targetPoint.X - _currentPose.X;
        double dy = targetPoint.Y - _currentPose.Y;
        double heading = Math.Atan2(dy, dx);

        // Adjust for backward direction
        if (targetPoint.Direction == RobotDirection.BACKWARD)
        {
            heading = NavigationMath.NormalizeAngle(heading + Math.PI);
        }

        return NavigationMath.NormalizeAngle(heading);
    }

    /// <summary>
    /// Calculate final target heading at goal (from second-to-last waypoint to goal)
    /// Similar to PurePursuitSimplified.CalculateGoalHeading()
    /// </summary>
    private double CalculateFinalTargetHeading()
    {
        if (_referencePath == null || _referencePath.Points.Count < 2)
            return _currentPose.Theta;

        var goalPoint = _referencePath.Points[^1];
        var secondToLast = _referencePath.Points[^2];

        double dx = goalPoint.X - secondToLast.X;
        double dy = goalPoint.Y - secondToLast.Y;
        double heading = Math.Atan2(dy, dx);

        // Adjust for backward direction
        if (goalPoint.Direction == RobotDirection.BACKWARD)
        {
            heading = NavigationMath.NormalizeAngle(heading + Math.PI);
        }

        return NavigationMath.NormalizeAngle(heading);
    }

    /// <summary>
    /// Find target point at lookahead distance from current position
    /// Simplified version for initial rotation calculation
    /// </summary>
    private PathPoint? FindTargetPointAtDistance(int startIndex, double lookaheadDistance)
    {
        if (_referencePath == null || startIndex >= _referencePath.Points.Count - 1)
            return null;

        double accumulatedDistance = 0;

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

            if (accumulatedDistance + segmentLength >= lookaheadDistance)
            {
                // Interpolate within this segment
                double t = (lookaheadDistance - accumulatedDistance) / segmentLength;
                return new PathPoint
                {
                    X = _referencePath.Points[i].X + t * (_referencePath.Points[i + 1].X - _referencePath.Points[i].X),
                    Y = _referencePath.Points[i].Y + t * (_referencePath.Points[i + 1].Y - _referencePath.Points[i].Y),
                    Direction = _referencePath.Points[i].Direction
                };
            }

            accumulatedDistance += segmentLength;
        }

        return null;
    }

    /// <summary>
    /// Perform initial rotation to face the initial lookahead point
    /// Returns true if rotation is complete, false if still rotating
    /// </summary>
    private bool PerformInitialRotation()
    {
        // Calculate heading error
        double headingError = NavigationMath.NormalizeAngle(_targetHeading - _currentPose.Theta);

        // Check if rotation is complete (within heading tolerance)
        double headingToleranceRad = (_currentParameters?.PurePursuitConfig.HeadingTolerance ?? 3.0f) * Math.PI / 180.0;
        if (Math.Abs(headingError) < headingToleranceRad)
        {
            _velocityProvider.SetVelocity(0, 0);
            Console.WriteLine($"Initial rotation complete. Heading error: {Math.Abs(headingError) * 180 / Math.PI:F2}�");
            return true;
        }

        // Use PID to control rotation
        double angularVelCmd = _rotatePid!.PID_step(
            headingError,
            _currentParameters!.NavigationConfig.RotateAngularVelocity,
            -_currentParameters!.NavigationConfig.RotateAngularVelocity,
            Dt);

        _angularVelocityCommand = (float)angularVelCmd;
        _velocityProvider.SetVelocity(0, _angularVelocityCommand);

        Console.WriteLine($"Initial rotation: HeadingError={headingError * 180 / Math.PI:F2}�, AngVel={_angularVelocityCommand:F3}");
        return false;
    }

    /// <summary>
    /// Perform final rotation to face the goal heading
    /// Returns true if rotation is complete, false if still rotating
    /// </summary>
    private bool PerformFinalRotation()
    {
        // Calculate heading error
        double headingError = NavigationMath.NormalizeAngle(_targetHeading - _currentPose.Theta);

        // Check if rotation is complete (within heading tolerance)
        double headingToleranceRad = (_currentParameters?.PurePursuitConfig.HeadingTolerance ?? 3.0f) * Math.PI / 180.0;
        if (Math.Abs(headingError) < headingToleranceRad)
        {
            _velocityProvider.SetVelocity(0, 0);
            Console.WriteLine($"Final rotation complete. Heading error: {Math.Abs(headingError) * 180 / Math.PI:F2}�");
            return true;
        }

        // Use PID to control rotation
        double angularVelCmd = _rotatePid!.PID_step(
            headingError,
            _currentParameters!.NavigationConfig.RotateAngularVelocity,
            -_currentParameters!.NavigationConfig.RotateAngularVelocity,
            Dt);

        _angularVelocityCommand = (float)angularVelCmd;
        _velocityProvider.SetVelocity(0, _angularVelocityCommand);

        Console.WriteLine($"Final rotation: HeadingError={headingError * 180 / Math.PI:F2}�, AngVel={_angularVelocityCommand:F3}");
        return false;
    }
}

/// <summary>
/// Interface for localization provider (abstraction for ILocalization)
/// </summary>
public interface ILocalizationProvider
{
    double X { get; }
    double Y { get; }
    double Theta { get; } // radians
}

/// <summary>
/// Interface for velocity provider (abstraction for IVelocityController)
/// </summary>
public interface IVelocityProvider
{
    (double Linear, double Angular) GetActualVelocity();
    void SetVelocity(double linearVel, double angularVel);
    double GetModelConfidence();
    void SetAcceleration(double acc);
    void SetDeceleration(double dec);
}