using FluentAssertions;
using Xunit;
using Xunit.Abstractions;
using RobotNet10.NavigationTune.Shared.Models;
using RobotNet10.NavigationTune.Services;
using RobotNet10.NavigationTune.Test.Helpers;

namespace RobotNet10.NavigationTune.Test.Services;

public class TuningAdvisorTests
{
    private readonly TuningAdvisor _advisor;
    private readonly ITestOutputHelper _output;

    public TuningAdvisorTests(ITestOutputHelper output)
    {
        _advisor = new TuningAdvisor();
        _output = output;
    }

    #region Test Data Helpers

    /// <summary>
    /// Create telemetry with explicit phase, timestamp, and configurable errors.
    /// </summary>
    private static TelemetryData CreatePhasedTelemetry(
        TelemetryPhase phase,
        long timestampMs,
        double x = 0, double y = 0, double theta = 0,
        double linearVel = 1.0, double angularVel = 0.0,
        double cte = 0.0, double headingError = 0.0,
        double distanceToGoal = 5.0,
        double modelConfidence = 1.0,
        double commandLinearVel = 1.0)
    {
        return new TelemetryData
        {
            TimestampMs = timestampMs,
            Phase = phase,
            RobotPose = new Pose2D(x, y, theta),
            RobotTwist = new Twist2D(linearVel, angularVel),
            CommandTwist = new Twist2D(commandLinearVel, angularVel),
            ReferencePose = new Pose2D(x, 0, theta),
            CrossTrackError = cte,
            HeadingError = headingError,
            LookaheadDistance = 1.0,
            ModelConfidence = modelConfidence,
            DistanceToGoal = distanceToGoal
        };
    }

    /// <summary>
    /// Create telemetry for PathFollowing phase with good tracking (small CTE, small heading error).
    /// </summary>
    private static List<TelemetryData> CreateGoodPathFollowingTelemetry(int count = 50, long startMs = 0)
    {
        var data = new List<TelemetryData>();
        for (int i = 0; i < count; i++)
        {
            data.Add(CreatePhasedTelemetry(
                TelemetryPhase.PathFollowing,
                startMs + i * 300, // ~3.3 Hz
                x: i * 0.1, cte: 0.02, headingError: 0.02,
                distanceToGoal: 10.0 - i * 0.1));
        }
        return data;
    }

    /// <summary>
    /// Create telemetry for FinalApproach phase with configurable CTE and heading.
    /// </summary>
    private static List<TelemetryData> CreateFinalApproachTelemetry(
        double cte, double headingErrorRad, int count = 20, long startMs = 15000)
    {
        var data = new List<TelemetryData>();
        for (int i = 0; i < count; i++)
        {
            data.Add(CreatePhasedTelemetry(
                TelemetryPhase.FinalApproach,
                startMs + i * 300,
                x: 8.0 + i * 0.05, cte: cte, headingError: headingErrorRad,
                linearVel: 0.3, distanceToGoal: 0.5 - i * 0.02));
        }
        return data;
    }

    /// <summary>
    /// Create telemetry for FinalRotation phase.
    /// </summary>
    private static List<TelemetryData> CreateFinalRotationTelemetry(
        double headingErrorRad, int count = 10, long startMs = 21000)
    {
        var data = new List<TelemetryData>();
        for (int i = 0; i < count; i++)
        {
            data.Add(CreatePhasedTelemetry(
                TelemetryPhase.FinalRotation,
                startMs + i * 300,
                x: 9.0, cte: 0.01, headingError: headingErrorRad * (1.0 - i * 0.08),
                linearVel: 0.0, angularVel: 0.5,
                distanceToGoal: 0.01));
        }
        return data;
    }

    /// <summary>
    /// Create telemetry for InitialRotation phase.
    /// </summary>
    private static List<TelemetryData> CreateInitialRotationTelemetry(
        int count = 10, long startMs = 0)
    {
        var data = new List<TelemetryData>();
        for (int i = 0; i < count; i++)
        {
            data.Add(CreatePhasedTelemetry(
                TelemetryPhase.InitialRotation,
                startMs + i * 300,
                x: 0, y: 0, theta: i * 0.1,
                linearVel: 0.0, angularVel: 1.5,
                cte: 0.0, headingError: 0.5 - i * 0.05,
                distanceToGoal: 10.0));
        }
        return data;
    }

    /// <summary>
    /// Create default TestMetrics for test scenarios.
    /// </summary>
    private static TestMetrics CreateDefaultMetrics(
        double cteRms = 0.05, double ctePeak = 0.10, double cteMean = 0.04,
        double headingRms = 0.05, double goalPosError = 0.02, double goalHeadingError = 0.03,
        double velStdDev = 0.05, double accelStdDev = 0.2,
        double pathLengthRatio = 1.05, double completionTime = 10.0,
        double overallScore = 80.0, bool passedCriteria = true)
    {
        return new TestMetrics
        {
            CrossTrackErrorRMS = cteRms,
            CrossTrackErrorPeak = ctePeak,
            CrossTrackErrorMean = cteMean,
            HeadingErrorRMS = headingRms,
            HeadingErrorPeak = headingRms * 1.5,
            GoalPositionError = goalPosError,
            GoalHeadingError = goalHeadingError,
            VelocityStdDev = velStdDev,
            AccelerationStdDev = accelStdDev,
            PathLengthRatio = pathLengthRatio,
            CompletionTime = completionTime,
            AverageSpeed = 1.0,
            MaxSpeed = 1.5,
            OverallScore = overallScore,
            TrackingScore = 80.0,
            SmoothnessScore = 85.0,
            EfficiencyScore = 90.0,
            PassedCriteria = passedCriteria
        };
    }

    #endregion

    #region Phase Segmentation Tests

    [Fact]
    public void Analyze_WithPhasedTelemetry_ShouldPopulatePhaseSampleCounts()
    {
        // Arrange
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateInitialRotationTelemetry(10));
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50, startMs: 3000));
        telemetry.AddRange(CreateFinalApproachTelemetry(0.02, 0.02, 20, startMs: 18000));
        telemetry.AddRange(CreateFinalRotationTelemetry(0.03, 10, startMs: 24000));

        var metrics = CreateDefaultMetrics();
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert
        report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.InitialRotation);
        report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.PathFollowing);
        report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.FinalApproach);
        report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.FinalRotation);
        report.PhaseSampleCounts[TelemetryPhase.InitialRotation].Should().Be(10);
        report.PhaseSampleCounts[TelemetryPhase.PathFollowing].Should().Be(50);
        report.PhaseSampleCounts[TelemetryPhase.FinalApproach].Should().Be(20);
        report.PhaseSampleCounts[TelemetryPhase.FinalRotation].Should().Be(10);
    }

    [Fact]
    public void Analyze_WithPhasedTelemetry_ShouldPopulatePhaseMetricsMap()
    {
        // Arrange
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
        telemetry.AddRange(CreateFinalApproachTelemetry(0.05, 0.05, 20, startMs: 15000));

        var metrics = CreateDefaultMetrics();
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert
        report.PhaseMetricsMap.Should().ContainKey(TelemetryPhase.PathFollowing);
        report.PhaseMetricsMap.Should().ContainKey(TelemetryPhase.FinalApproach);

        var pfMetrics = report.PhaseMetricsMap[TelemetryPhase.PathFollowing];
        pfMetrics.SampleCount.Should().Be(50);
        pfMetrics.CrossTrackErrorRMS.Should().BeGreaterThan(0);

        var faMetrics = report.PhaseMetricsMap[TelemetryPhase.FinalApproach];
        faMetrics.SampleCount.Should().Be(20);
        faMetrics.CrossTrackErrorRMS.Should().BeApproximately(0.05, 0.001);
    }

    [Fact]
    public void Analyze_WithLegacyTelemetry_ShouldTreatAsPathFollowing()
    {
        // Arrange — telemetry without Phase set (null)
        var telemetry = TestHelpers.CreateTelemetryHistory(50);
        var metrics = CreateDefaultMetrics();
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — all samples should be grouped as PathFollowing
        report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.PathFollowing);
        report.PhaseSampleCounts[TelemetryPhase.PathFollowing].Should().Be(50);
    }

    #endregion

    #region Pattern Detection Phase-Aware Tests

    [Fact]
    public void Analyze_WithGoodTracking_ShouldNotDetectLargeCTE()
    {
        // Arrange — CTE well below thresholds for both phases
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50)); // CTE = 0.02
        telemetry.AddRange(CreateFinalApproachTelemetry(0.01, 0.01, 20, startMs: 15000)); // CTE = 0.01

        var metrics = CreateDefaultMetrics(cteRms: 0.02, ctePeak: 0.03, cteMean: 0.02);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — no LargeCTE patterns
        report.DetectedPatterns
            .Where(p => p.Category == DiagnosticCategory.LargeCTE)
            .Should().BeEmpty();
    }

    [Fact]
    public void Analyze_FinalApproach_WithCTEAbove002_ShouldDetectLargeCTE()
    {
        // Arrange — PathFollowing CTE good (0.05 < 0.10), FinalApproach CTE bad (0.04 > 0.02)
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50)); // CTE = 0.02 (good for PF)
        telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.02, 20, startMs: 15000)); // CTE = 0.04 (bad for FA, > 0.02)

        var metrics = CreateDefaultMetrics(cteRms: 0.03);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — should detect LargeCTE only in FinalApproach
        var faCtePattern = report.DetectedPatterns
            .Where(p => p.Category == DiagnosticCategory.LargeCTE && p.DetectedInPhase == TelemetryPhase.FinalApproach)
            .ToList();
        faCtePattern.Should().HaveCount(1);
        faCtePattern[0].Evidence.Should().ContainKey("Threshold");
        faCtePattern[0].Evidence["Threshold"].Should().BeApproximately(0.02, 0.001);

        // PathFollowing should NOT have LargeCTE (0.02 < 0.10 threshold)
        report.DetectedPatterns
            .Where(p => p.Category == DiagnosticCategory.LargeCTE && p.DetectedInPhase == TelemetryPhase.PathFollowing)
            .Should().BeEmpty();
    }

    [Fact]
    public void Analyze_FinalApproach_WithHeadingAbove2Deg_ShouldDetectLargeHeadingError()
    {
        // Arrange — FinalApproach heading = 5° > 2° threshold
        double headingRad = 5.0 * Math.PI / 180.0;
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
        telemetry.AddRange(CreateFinalApproachTelemetry(0.01, headingRad, 20, startMs: 15000));

        var metrics = CreateDefaultMetrics();
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — heading error detected in FinalApproach (target: 2°)
        var faHeadingPattern = report.DetectedPatterns
            .Where(p => p.Category == DiagnosticCategory.GoalHeadingError && p.DetectedInPhase == TelemetryPhase.FinalApproach)
            .ToList();
        faHeadingPattern.Should().HaveCount(1);
        faHeadingPattern[0].Evidence.Should().ContainKey("ThresholdDeg");
        faHeadingPattern[0].Evidence["ThresholdDeg"].Should().BeApproximately(2.0, 0.1);
    }

    [Fact]
    public void Analyze_PathFollowing_WithOscillation_ShouldDetectInCorrectPhase()
    {
        // Arrange — oscillating angular velocity in PathFollowing
        var telemetry = new List<TelemetryData>();
        for (int i = 0; i < 60; i++)
        {
            double angVel = Math.Sin(i * 2.0) * 1.5; // High frequency, high amplitude
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.PathFollowing,
                i * 100, // 10 Hz → 6 seconds
                x: i * 0.05, angularVel: angVel,
                cte: 0.03, distanceToGoal: 5.0));
        }

        var metrics = CreateDefaultMetrics();
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert
        var oscillation = report.DetectedPatterns
            .Where(p => p.Category == DiagnosticCategory.Oscillation && p.DetectedInPhase == TelemetryPhase.PathFollowing)
            .ToList();
        oscillation.Should().HaveCount(1);
    }

    [Fact]
    public void Analyze_InitialRotation_ShouldNotRunCTEDetector()
    {
        // Arrange — InitialRotation with high CTE (expected, since robot is rotating in place)
        var telemetry = new List<TelemetryData>();
        for (int i = 0; i < 20; i++)
        {
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.InitialRotation,
                i * 300,
                cte: 0.5, // Huge CTE — but should be ignored in InitialRotation
                headingError: 0.3,
                linearVel: 0.0, angularVel: 2.0,
                distanceToGoal: 10.0));
        }

        var metrics = CreateDefaultMetrics();
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — no LargeCTE for InitialRotation
        report.DetectedPatterns
            .Where(p => p.Category == DiagnosticCategory.LargeCTE && p.DetectedInPhase == TelemetryPhase.InitialRotation)
            .Should().BeEmpty();
    }

    [Fact]
    public void Analyze_GoalOvershoot_ShouldDetectInFinalApproachOnly()
    {
        // Arrange — FinalApproach data where distance-to-goal decreases then increases (overshoot)
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));

        // FinalApproach: distance decreases to 0.01 then increases to 0.08 (overshoot = 0.07)
        for (int i = 0; i < 20; i++)
        {
            double dist;
            if (i < 12)
                dist = 0.3 - i * 0.024; // Decreasing to ~0.01
            else
                dist = 0.01 + (i - 12) * 0.01; // Increasing after min

            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.FinalApproach,
                15000 + i * 300,
                cte: 0.02, distanceToGoal: dist, linearVel: 0.3));
        }

        var metrics = CreateDefaultMetrics();
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert
        var overshoot = report.DetectedPatterns
            .Where(p => p.Category == DiagnosticCategory.GoalOvershoot)
            .ToList();
        overshoot.Should().HaveCount(1);
        overshoot[0].DetectedInPhase.Should().Be(TelemetryPhase.FinalApproach);
    }

    #endregion

    #region Suggestion Generation Phase-Aware Tests

    [Fact]
    public void Analyze_FinalApproach_LargeCTE_PurePursuit_ShouldSuggestPrecisionParams()
    {
        // Arrange — FinalApproach CTE = 0.04 > 0.02 target, PurePursuit controller
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
        telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.02, 20, startMs: 15000));

        var metrics = CreateDefaultMetrics(cteRms: 0.03);
        var parameters = TestHelpers.CreateDefaultParameterSet();
        parameters.ControllerType = PathFollowingController.PurePursuit;

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — should suggest FinalApproach-specific PP parameters
        var faSuggestions = report.Suggestions
            .Where(s => s.TargetPhase == TelemetryPhase.FinalApproach)
            .ToList();

        faSuggestions.Should().NotBeEmpty("FinalApproach CTE exceeds 0.02m target");

        // Should suggest increasing FinalApproachThreshold or GoalRegionDistance
        var ppFinalParams = faSuggestions
            .Where(s => s.ParameterPath.Contains("FinalApproachThreshold") ||
                        s.ParameterPath.Contains("GoalRegionDistance"))
            .ToList();
        ppFinalParams.Should().NotBeEmpty("should suggest PP FinalApproach precision params");
    }

    [Fact]
    public void Analyze_FinalApproach_LargeCTE_Stanley_ShouldSuggestGoalParams()
    {
        // Arrange — FinalApproach CTE = 0.04 > 0.02 target, Stanley controller
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
        telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.02, 20, startMs: 15000));

        var metrics = CreateDefaultMetrics(cteRms: 0.03);
        var parameters = TestHelpers.CreateDefaultParameterSet();
        parameters.ControllerType = PathFollowingController.Stanley;
        parameters.StanleyConfig = new StanleyConfig();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — should suggest Stanley FinalApproach parameters
        var faSuggestions = report.Suggestions
            .Where(s => s.TargetPhase == TelemetryPhase.FinalApproach)
            .ToList();

        faSuggestions.Should().NotBeEmpty("FinalApproach CTE exceeds 0.02m target");

        // Should suggest GoalGainMultiplier or GoalApproachDistance
        var stanleyGoalParams = faSuggestions
            .Where(s => s.ParameterPath.Contains("GoalGainMultiplier") ||
                        s.ParameterPath.Contains("GoalApproachDistance") ||
                        s.ParameterPath.Contains("LowSpeedAngularGain"))
            .ToList();
        stanleyGoalParams.Should().NotBeEmpty("should suggest Stanley goal precision params");
    }

    [Fact]
    public void Analyze_AllSuggestions_ShouldHaveTargetPhaseSet()
    {
        // Arrange — mixed-phase telemetry with some issues
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
        telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.05, 20, startMs: 15000)); // Bad CTE + heading

        var metrics = CreateDefaultMetrics(goalHeadingError: 0.2); // ~11° heading error
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — every suggestion should have TargetPhase populated
        foreach (var suggestion in report.Suggestions)
        {
            suggestion.TargetPhase.Should().NotBeNull(
                $"suggestion for {suggestion.ParameterPath} should have TargetPhase set");
        }
    }

    [Fact]
    public void Analyze_AllPatterns_ShouldHaveDetectedInPhaseSet()
    {
        // Arrange
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
        telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.05, 20, startMs: 15000));

        var metrics = CreateDefaultMetrics(goalHeadingError: 0.2, pathLengthRatio: 1.25);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — every pattern should have DetectedInPhase populated
        foreach (var pattern in report.DetectedPatterns)
        {
            pattern.DetectedInPhase.Should().NotBeNull(
                $"pattern {pattern.Category} should have DetectedInPhase set");
        }
    }

    #endregion

    #region Conflict Resolution Phase-Aware Tests

    [Fact]
    public void Analyze_SameParamDifferentPhases_ShouldKeepBothSuggestions()
    {
        // Arrange — Create telemetry that triggers different suggestions for the same param
        // PathFollowing oscillation (→ decrease K) + FinalApproach large CTE (→ increase K for Stanley)
        var telemetry = new List<TelemetryData>();

        // PathFollowing with oscillation
        for (int i = 0; i < 60; i++)
        {
            double angVel = Math.Sin(i * 2.0) * 1.5;
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.PathFollowing,
                i * 100,
                x: i * 0.05, angularVel: angVel,
                cte: 0.03, distanceToGoal: 5.0));
        }

        // FinalApproach with high CTE
        telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.02, 20, startMs: 6000));

        var metrics = CreateDefaultMetrics();
        var parameters = TestHelpers.CreateDefaultParameterSet();
        parameters.ControllerType = PathFollowingController.Stanley;
        parameters.StanleyConfig = new StanleyConfig();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — suggestions for different phases should coexist even if same param
        // (conflict resolution groups by ParameterPath + TargetPhase)
        var pfSuggestions = report.Suggestions.Where(s => s.TargetPhase == TelemetryPhase.PathFollowing).ToList();
        var faSuggestions = report.Suggestions.Where(s => s.TargetPhase == TelemetryPhase.FinalApproach).ToList();

        // Both phases should have suggestions
        pfSuggestions.Should().NotBeEmpty("PathFollowing oscillation should generate suggestions");
        faSuggestions.Should().NotBeEmpty("FinalApproach CTE should generate suggestions");
    }

    #endregion

    #region Assessment Generation Tests

    [Fact]
    public void Analyze_Assessment_ShouldIncludePerPhaseInfo()
    {
        // Arrange
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
        telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.05, 20, startMs: 15000));

        var metrics = CreateDefaultMetrics();
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — assessment should mention phase names
        report.OverallAssessment.Should().NotBeNullOrEmpty();
        // If there are patterns, the assessment should mention phases
        if (report.DetectedPatterns.Count > 0)
        {
            // Should contain phase-specific info (PathFollowing or FinalApproach)
            bool hasPhaseInfo = report.OverallAssessment.Contains("PathFollowing") ||
                                report.OverallAssessment.Contains("FinalApproach");
            hasPhaseInfo.Should().BeTrue("assessment should include per-phase metrics info");
        }
    }

    [Fact]
    public void Analyze_WithNoIssues_ShouldReturnExcellentAssessment()
    {
        // Arrange — perfect tracking in all phases
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
        telemetry.AddRange(CreateFinalApproachTelemetry(0.01, 0.01, 20, startMs: 15000));

        var metrics = CreateDefaultMetrics(overallScore: 95, passedCriteria: true);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert
        if (report.DetectedPatterns.Count == 0)
        {
            report.OverallAssessment.Should().Contain("Excellent");
        }
    }

    #endregion

    #region ApplySuggestion Tests

    [Fact]
    public void ApplySuggestion_ShouldModifyParameterValue()
    {
        // Arrange
        var parameters = TestHelpers.CreateDefaultParameterSet();
        var suggestion = new TuningSuggestion
        {
            ParameterPath = "PurePursuitConfig.LookaheadMin",
            CurrentValue = parameters.PurePursuitConfig.LookaheadMin,
            SuggestedValue = 0.5,
            TargetPhase = TelemetryPhase.PathFollowing
        };

        // Act
        var result = _advisor.ApplySuggestion(parameters, suggestion);

        // Assert
        result.PurePursuitConfig.LookaheadMin.Should().BeApproximately(0.5, 0.001);
        // Original should be unchanged
        parameters.PurePursuitConfig.LookaheadMin.Should().NotBe(0.5);
    }

    [Fact]
    public void ApplyAllSuggestions_ShouldApplyMultiple()
    {
        // Arrange
        var parameters = TestHelpers.CreateDefaultParameterSet();
        var suggestions = new List<TuningSuggestion>
        {
            new()
            {
                ParameterPath = "PurePursuitConfig.LookaheadMin",
                CurrentValue = parameters.PurePursuitConfig.LookaheadMin,
                SuggestedValue = 0.5,
                TargetPhase = TelemetryPhase.PathFollowing
            },
            new()
            {
                ParameterPath = "PurePursuitConfig.FinalApproachThreshold",
                CurrentValue = parameters.PurePursuitConfig.FinalApproachThreshold,
                SuggestedValue = 0.35,
                TargetPhase = TelemetryPhase.FinalApproach
            }
        };

        // Act
        var result = _advisor.ApplyAllSuggestions(parameters, suggestions);

        // Assert
        result.PurePursuitConfig.LookaheadMin.Should().BeApproximately(0.5, 0.001);
        result.PurePursuitConfig.FinalApproachThreshold.Should().BeApproximately(0.35, 0.001);
    }

    #endregion

    #region Edge Case Tests

    [Fact]
    public void Analyze_WithMinimalTelemetry_ShouldNotThrow()
    {
        // Arrange — only a few samples per phase (below min threshold of 5)
        var telemetry = new List<TelemetryData>();
        for (int i = 0; i < 3; i++)
        {
            telemetry.Add(CreatePhasedTelemetry(TelemetryPhase.PathFollowing, i * 300,
                cte: 0.5, distanceToGoal: 5.0));
        }

        var metrics = CreateDefaultMetrics();
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var action = () => _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — should not throw, just return empty patterns
        action.Should().NotThrow();
        var report = _advisor.Analyze(telemetry, metrics, parameters);
        report.Should().NotBeNull();
        report.TelemetrySamplesAnalyzed.Should().Be(3);
    }

    [Fact]
    public void Analyze_WithOnlyCompletedPhase_ShouldSkipDetection()
    {
        // Arrange — only Completed phase telemetry
        var telemetry = new List<TelemetryData>();
        for (int i = 0; i < 10; i++)
        {
            telemetry.Add(CreatePhasedTelemetry(TelemetryPhase.Completed, i * 300,
                cte: 0.0, distanceToGoal: 0.0));
        }

        var metrics = CreateDefaultMetrics();
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — Completed phase is skipped, only global detectors may fire
        report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.Completed);
        // Phase-specific patterns should be absent (Completed is skipped)
        report.DetectedPatterns
            .Where(p => p.DetectedInPhase == TelemetryPhase.Completed)
            .Should().BeEmpty();
    }

    [Fact]
    public void Analyze_SuggestionsPriority_ShouldBeSortedHighestFirst()
    {
        // Arrange — create telemetry with multiple issues
        var telemetry = new List<TelemetryData>();
        telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
        telemetry.AddRange(CreateFinalApproachTelemetry(0.06, 0.1, 20, startMs: 15000)); // Bad CTE and heading

        var metrics = CreateDefaultMetrics(goalHeadingError: 0.2);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var report = _advisor.Analyze(telemetry, metrics, parameters);

        // Assert — sorted by priority descending, then confidence descending
        if (report.Suggestions.Count > 1)
        {
            for (int i = 1; i < report.Suggestions.Count; i++)
            {
                var prev = report.Suggestions[i - 1];
                var curr = report.Suggestions[i];

                if (prev.Priority == curr.Priority)
                    prev.Confidence.Should().BeGreaterThanOrEqualTo(curr.Confidence);
                else
                    ((int)prev.Priority).Should().BeGreaterThanOrEqualTo((int)curr.Priority);
            }
        }
    }

    #endregion

    #region Demo Output Tests

    private void PrintReport(TuningReport report, string scenarioName)
    {
        _output.WriteLine($"{"",0}");
        _output.WriteLine($"{"═════════════════════════════════════════════════════════════",0}");
        _output.WriteLine($"  TUNING REPORT: {scenarioName}");
        _output.WriteLine($"{"═════════════════════════════════════════════════════════════",0}");
        _output.WriteLine($"  Controller: {report.ControllerType}");
        _output.WriteLine($"  Samples Analyzed: {report.TelemetrySamplesAnalyzed}");
        _output.WriteLine($"  Has Critical Issues: {report.HasCriticalIssues}");
        _output.WriteLine($"  High Priority Count: {report.HighPriorityCount}");

        // Phase Sample Counts
        _output.WriteLine($"\n  ── Phase Sample Counts ──");
        foreach (var kv in report.PhaseSampleCounts)
            _output.WriteLine($"    {kv.Key,-20} : {kv.Value} samples");

        // Phase Metrics
        _output.WriteLine($"\n  ── Phase Metrics ──");
        foreach (var kv in report.PhaseMetricsMap)
        {
            var m = kv.Value;
            _output.WriteLine($"    [{kv.Key}] ({m.SampleCount} samples, {m.DurationMs:F0}ms)");
            _output.WriteLine($"      CTE  RMS={m.CrossTrackErrorRMS:F4}m  Peak={m.CrossTrackErrorPeak:F4}m  Mean={m.CrossTrackErrorMean:F4}m");
            _output.WriteLine($"      Heading RMS={m.HeadingErrorRMS * 180 / Math.PI:F2}°  Peak={m.HeadingErrorPeak * 180 / Math.PI:F2}°");
            _output.WriteLine($"      AngVel StdDev={m.AngularVelocityStdDev:F4}  VelStdDev={m.VelocityStdDev:F4}  AccelStdDev={m.AccelerationStdDev:F4}");
            if (kv.Key == TelemetryPhase.FinalApproach || kv.Key == TelemetryPhase.FinalRotation)
                _output.WriteLine($"      GoalPos={m.GoalPositionError:F4}m  GoalHeading={m.GoalHeadingErrorDeg:F2}°");
        }

        // Detected Patterns
        _output.WriteLine($"\n  ── Detected Patterns ({report.DetectedPatterns.Count}) ──");
        foreach (var p in report.DetectedPatterns)
        {
            _output.WriteLine($"    [{p.DetectedInPhase}] {p.Category} (severity={p.Severity:F2})");
            _output.WriteLine($"      {p.Description}");
            if (p.Evidence.Count > 0)
            {
                var evidence = string.Join(", ", p.Evidence.Select(e => $"{e.Key}={e.Value:F4}"));
                _output.WriteLine($"      Evidence: {evidence}");
            }
        }

        // Suggestions
        _output.WriteLine($"\n  ── Suggestions ({report.Suggestions.Count}) ──");
        foreach (var s in report.Suggestions)
        {
            _output.WriteLine($"    [{s.Priority}] {s.ParameterDisplayName} (phase={s.TargetPhase})");
            _output.WriteLine($"      {s.ParameterPath}: {s.CurrentValue:F4} → {s.SuggestedValue:F4} ({s.ChangePercent:+0.0;-0.0}%)");
            _output.WriteLine($"      Confidence={s.Confidence:F2}  Reason: {s.Reason}");
            _output.WriteLine($"      Impact: {s.ExpectedImpact}");
        }

        // Overall Assessment
        _output.WriteLine($"\n  ── Overall Assessment ──");
        _output.WriteLine($"  {report.OverallAssessment}");
        _output.WriteLine($"{"═════════════════════════════════════════════════════════════",0}\n");
    }

    [Fact]
    public void Demo_Scenario1_GoodTracking_FinalApproachNeedsWork()
    {
        // Scenario: PathFollowing tốt, FinalApproach CTE > 0.02m, Heading > 2°
        var telemetry = new List<TelemetryData>();

        // InitialRotation: quay tại chỗ 1s
        telemetry.AddRange(CreateInitialRotationTelemetry(8, startMs: 0));

        // PathFollowing: bám đường tốt 5s (CTE = 0.03m)
        for (int i = 0; i < 50; i++)
        {
            double noise = Math.Sin(i * 0.5) * 0.01;
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.PathFollowing,
                2400 + i * 100,
                x: i * 0.1, y: noise,
                cte: 0.03 + noise, headingError: 0.03 + noise * 0.5,
                linearVel: 1.0 + noise, angularVel: noise * 2,
                distanceToGoal: 8.0 - i * 0.15));
        }

        // FinalApproach: CTE = 0.04 (vượt ngưỡng 0.02), Heading = 4° (vượt ngưỡng 2°)
        double headingRad4deg = 4.0 * Math.PI / 180.0;
        for (int i = 0; i < 25; i++)
        {
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.FinalApproach,
                7400 + i * 200,
                x: 5.0 + i * 0.04, y: 0.04 - i * 0.001,
                cte: 0.04 - i * 0.0005, headingError: headingRad4deg * (1.0 - i * 0.02),
                linearVel: 0.3 - i * 0.008, angularVel: 0.05,
                distanceToGoal: 0.5 - i * 0.02));
        }

        // FinalRotation: heading OK
        telemetry.AddRange(CreateFinalRotationTelemetry(0.04, 10, startMs: 12400));

        var metrics = CreateDefaultMetrics(
            cteRms: 0.035, ctePeak: 0.06, cteMean: 0.03,
            headingRms: 0.05, goalPosError: 0.03, goalHeadingError: 0.04,
            overallScore: 75);
        var parameters = TestHelpers.CreateDefaultParameterSet();
        parameters.ControllerType = PathFollowingController.PurePursuit;

        var report = _advisor.Analyze(telemetry, metrics, parameters);
        PrintReport(report, "Good PathFollowing, FinalApproach needs precision (PurePursuit)");

        // Verify key behaviors
        report.DetectedPatterns
            .Where(p => p.Category == DiagnosticCategory.LargeCTE && p.DetectedInPhase == TelemetryPhase.FinalApproach)
            .Should().NotBeEmpty("FinalApproach CTE=0.04 > threshold 0.02");

        report.Suggestions
            .Where(s => s.TargetPhase == TelemetryPhase.FinalApproach)
            .Should().NotBeEmpty("should suggest FinalApproach improvements");
    }

    [Fact]
    public void Demo_Scenario2_Oscillation_InPathFollowing_Stanley()
    {
        // Scenario: Dao động mạnh ở PathFollowing (Stanley), FinalApproach CTE cao
        var telemetry = new List<TelemetryData>();

        // PathFollowing: oscillation rõ rệt
        for (int i = 0; i < 80; i++)
        {
            double angVel = Math.Sin(i * 1.5) * 1.2; // dao động lớn
            double cte = 0.06 + Math.Sin(i * 1.5) * 0.04;
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.PathFollowing,
                i * 100,
                x: i * 0.08, y: Math.Sin(i * 0.3) * 0.05,
                cte: cte, headingError: 0.06,
                linearVel: 0.8, angularVel: angVel,
                distanceToGoal: 10.0 - i * 0.12));
        }

        // FinalApproach: CTE vẫn cao
        for (int i = 0; i < 20; i++)
        {
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.FinalApproach,
                8000 + i * 200,
                x: 6.4 + i * 0.03, y: 0.03,
                cte: 0.05 - i * 0.001, headingError: 0.05,
                linearVel: 0.25, angularVel: 0.1,
                distanceToGoal: 0.4 - i * 0.018));
        }

        var metrics = CreateDefaultMetrics(
            cteRms: 0.07, ctePeak: 0.12, cteMean: 0.06,
            headingRms: 0.06, velStdDev: 0.15, accelStdDev: 0.6,
            pathLengthRatio: 1.18, overallScore: 60, passedCriteria: false);
        var parameters = TestHelpers.CreateDefaultParameterSet();
        parameters.ControllerType = PathFollowingController.Stanley;
        parameters.StanleyConfig = new StanleyConfig();

        var report = _advisor.Analyze(telemetry, metrics, parameters);
        PrintReport(report, "Oscillation + High CTE (Stanley)");

        report.DetectedPatterns
            .Where(p => p.Category == DiagnosticCategory.Oscillation)
            .Should().NotBeEmpty("strong oscillation in PathFollowing");

        report.Suggestions.Count.Should().BeGreaterThan(2, "multiple issues → multiple suggestions");
    }

    [Fact]
    public void Demo_Scenario3_ExcellentPerformance()
    {
        // Scenario: Robot chạy hoàn hảo — không có vấn đề gì
        var telemetry = new List<TelemetryData>();

        // InitialRotation: nhanh gọn, heading error nhỏ (< 10° threshold)
        for (int i = 0; i < 6; i++)
        {
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.InitialRotation, i * 200,
                theta: i * 0.05, linearVel: 0, angularVel: 0.8,
                cte: 0, headingError: 0.1 - i * 0.015, distanceToGoal: 8,
                commandLinearVel: 0));
        }

        // PathFollowing: CTE rất nhỏ, command = actual velocity
        for (int i = 0; i < 60; i++)
        {
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.PathFollowing,
                1200 + i * 100,
                x: i * 0.12, cte: 0.015, headingError: 0.01,
                linearVel: 1.2, angularVel: 0.02,
                distanceToGoal: 8.0 - i * 0.12,
                commandLinearVel: 1.2));
        }

        // FinalApproach: CTE = 0.01 < 0.02 ✓, Heading < 1° ✓
        for (int i = 0; i < 20; i++)
        {
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.FinalApproach,
                7200 + i * 200,
                x: 7.2 + i * 0.04, cte: 0.01, headingError: 0.01,
                linearVel: 0.3, angularVel: 0.01,
                distanceToGoal: 0.4 - i * 0.02,
                commandLinearVel: 0.3));
        }

        // FinalRotation
        for (int i = 0; i < 8; i++)
        {
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.FinalRotation,
                11200 + i * 200,
                x: 8.0, cte: 0.005, headingError: 0.02 * (1 - i * 0.1),
                linearVel: 0, angularVel: 0.3, distanceToGoal: 0.01,
                commandLinearVel: 0));
        }

        var metrics = CreateDefaultMetrics(
            cteRms: 0.015, ctePeak: 0.025, cteMean: 0.013,
            headingRms: 0.015, goalPosError: 0.01, goalHeadingError: 0.01,
            velStdDev: 0.03, accelStdDev: 0.1,
            pathLengthRatio: 1.02, overallScore: 95, passedCriteria: true);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        var report = _advisor.Analyze(telemetry, metrics, parameters);
        PrintReport(report, "Excellent Performance — No Issues");

        report.OverallAssessment.Should().Contain("Excellent");
        report.DetectedPatterns.Count.Should().BeLessThanOrEqualTo(1, "nearly perfect run");
    }

    [Fact]
    public void Demo_Scenario4_GoalOvershoot_WithJerkyMotion()
    {
        // Scenario: Overshoot tại goal + chuyển động giật
        var telemetry = new List<TelemetryData>();

        // PathFollowing: có acceleration jitter
        for (int i = 0; i < 50; i++)
        {
            double accelJitter = (i % 3 == 0) ? 0.8 : -0.3;
            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.PathFollowing,
                i * 100,
                x: i * 0.1,
                cte: 0.04, headingError: 0.03,
                linearVel: 1.0 + accelJitter * 0.3,
                angularVel: accelJitter * 0.2,
                commandLinearVel: 1.0,
                distanceToGoal: 8.0 - i * 0.15));
        }

        // FinalApproach: overshoot — distance giảm rồi tăng lại
        for (int i = 0; i < 25; i++)
        {
            double dist;
            if (i < 15)
                dist = 0.4 - i * 0.027;  // giảm tới ~0.005
            else
                dist = 0.005 + (i - 15) * 0.008; // tăng lại (overshoot)

            telemetry.Add(CreatePhasedTelemetry(
                TelemetryPhase.FinalApproach,
                5000 + i * 200,
                x: 5.0 + i * 0.03,
                cte: 0.03, headingError: 0.04,
                linearVel: 0.25, distanceToGoal: dist));
        }

        var metrics = CreateDefaultMetrics(
            cteRms: 0.04, ctePeak: 0.08, cteMean: 0.035,
            headingRms: 0.04, goalPosError: 0.05,
            velStdDev: 0.2, accelStdDev: 0.7,
            overallScore: 65, passedCriteria: false);
        var parameters = TestHelpers.CreateDefaultParameterSet();
        parameters.ControllerType = PathFollowingController.PurePursuit;

        var report = _advisor.Analyze(telemetry, metrics, parameters);
        PrintReport(report, "Goal Overshoot + Jerky Motion (PurePursuit)");

        report.DetectedPatterns
            .Where(p => p.Category == DiagnosticCategory.GoalOvershoot)
            .Should().NotBeEmpty("distance-to-goal increases after minimum");
    }

    #endregion
}