using System.Reflection;
using System.Text.Json;
using RobotNet10.NavigationTune.Shared.Interfaces;
using RobotNet10.NavigationTune.Shared.Models;

namespace RobotNet10.NavigationTune.Services;

/// <summary>
/// Rule-based tuning advisor that analyzes telemetry and metrics
/// to produce parameter adjustment suggestions after each test run.
/// Phase-aware: analyzes each navigation phase independently with phase-specific thresholds.
/// </summary>
public class TuningAdvisor : ITuningAdvisor
{
    private const double Deg2Rad = Math.PI / 180.0;

    #region Phase Thresholds

    /// <summary>
    /// Per-phase thresholds for pattern detection.
    /// null means the detector is skipped for that phase.
    /// </summary>
    private static class PhaseThresholds
    {
        // CTE RMS threshold (meters) — triggers LargeCTE detector
        public static double? GetCteRmsThreshold(TelemetryPhase phase) => phase switch
        {
            TelemetryPhase.PathFollowing => 0.10,
            TelemetryPhase.FinalApproach => 0.02,
            _ => null // InitialRotation, FinalRotation: CTE not relevant
        };

        // CTE Peak threshold (meters)
        public static double? GetCtePeakThreshold(TelemetryPhase phase) => phase switch
        {
            TelemetryPhase.PathFollowing => 0.20,
            TelemetryPhase.FinalApproach => 0.05,
            _ => null
        };

        // Heading error RMS threshold (degrees)
        public static double? GetHeadingRmsThresholdDeg(TelemetryPhase phase) => phase switch
        {
            TelemetryPhase.InitialRotation => 10.0,
            TelemetryPhase.PathFollowing => 8.0,
            TelemetryPhase.FinalApproach => 2.0,
            TelemetryPhase.FinalRotation => 3.0,
            _ => null
        };

        // Angular velocity StdDev threshold (rad/s) — triggers Oscillation detector
        public static double? GetAngVelStdDevThreshold(TelemetryPhase phase) => phase switch
        {
            TelemetryPhase.InitialRotation => 1.5,
            TelemetryPhase.PathFollowing => 0.3,
            TelemetryPhase.FinalApproach => 0.2,
            TelemetryPhase.FinalRotation => 1.0,
            _ => null
        };

        // Zero-crossing rate threshold (crossings/sec) — triggers Oscillation detector
        public static double GetZeroCrossingThreshold(TelemetryPhase phase) => phase switch
        {
            TelemetryPhase.PathFollowing => 3.0,
            TelemetryPhase.FinalApproach => 2.5,
            _ => 3.0
        };

        // Acceleration StdDev threshold (m/s²) — triggers JerkyMotion detector
        public static double? GetAccelStdDevThreshold(TelemetryPhase phase) => phase switch
        {
            TelemetryPhase.PathFollowing => 0.5,
            TelemetryPhase.FinalApproach => 0.3,
            _ => null
        };

        // Which detectors to run per phase
        public static bool ShouldRunOscillation(TelemetryPhase phase) =>
            phase is TelemetryPhase.PathFollowing or TelemetryPhase.FinalApproach;

        public static bool ShouldRunLargeCTE(TelemetryPhase phase) =>
            phase is TelemetryPhase.PathFollowing or TelemetryPhase.FinalApproach;

        public static bool ShouldRunLargeHeadingError(TelemetryPhase phase) =>
            phase is TelemetryPhase.PathFollowing or TelemetryPhase.FinalApproach
                or TelemetryPhase.FinalRotation;

        public static bool ShouldRunCornerCutting(TelemetryPhase phase) =>
            phase is TelemetryPhase.PathFollowing;

        public static bool ShouldRunGoalOvershoot(TelemetryPhase phase) =>
            phase is TelemetryPhase.FinalApproach;

        public static bool ShouldRunSluggishResponse(TelemetryPhase phase) =>
            phase is TelemetryPhase.PathFollowing or TelemetryPhase.FinalApproach;

        public static bool ShouldRunJerkyMotion(TelemetryPhase phase) =>
            phase is TelemetryPhase.PathFollowing or TelemetryPhase.FinalApproach;

        public static bool ShouldRunVelocityEstimation(TelemetryPhase phase) =>
            phase is TelemetryPhase.PathFollowing;
    }

    #endregion

    #region Public API

    public TuningReport Analyze(
        List<TelemetryData> telemetryData,
        TestMetrics metrics,
        NavigationParameterSet currentParameters,
        ReferencePath? referencePath = null)
    {
        var report = new TuningReport
        {
            ControllerType = currentParameters.ControllerType,
            TelemetrySamplesAnalyzed = telemetryData.Count
        };

        // Step 1: Segment telemetry by phase
        var phaseSegments = SegmentByPhase(telemetryData);

        // Step 2: Compute per-phase metrics
        var phaseMetricsMap = new Dictionary<TelemetryPhase, PhaseMetrics>();
        foreach (var (phase, data) in phaseSegments)
        {
            phaseMetricsMap[phase] = ComputePhaseMetrics(phase, data);
        }

        // Step 3: Populate report phase info
        report.PhaseSampleCounts = phaseSegments.ToDictionary(kv => kv.Key, kv => kv.Value.Count);
        report.PhaseMetricsMap = phaseMetricsMap;

        // Step 4: Detect patterns per phase with phase-specific thresholds
        var patterns = DetectPatternsPhaseAware(phaseSegments, phaseMetricsMap, metrics);
        report.DetectedPatterns = patterns;

        // Step 5: Generate suggestions from detected patterns (phase-aware)
        var suggestions = GenerateSuggestions(patterns, currentParameters, metrics);

        // Step 6: Resolve conflicts (group by ParameterPath + TargetPhase)
        suggestions = ResolveConflicts(suggestions);

        // Step 7: Sort by priority then confidence
        suggestions = suggestions
            .OrderByDescending(s => s.Priority)
            .ThenByDescending(s => s.Confidence)
            .ToList();

        report.Suggestions = suggestions;

        // Step 8: Generate phase-aware overall assessment
        report.OverallAssessment = GenerateOverallAssessment(metrics, patterns, suggestions, phaseMetricsMap);

        return report;
    }

    public NavigationParameterSet ApplySuggestion(
        NavigationParameterSet parameters,
        TuningSuggestion suggestion)
    {
        var clone = DeepClone(parameters);
        SetParameterValue(clone, suggestion.ParameterPath, suggestion.SuggestedValue);
        clone.UpdatedAt = DateTime.UtcNow;
        return clone;
    }

    public NavigationParameterSet ApplyAllSuggestions(
        NavigationParameterSet parameters,
        List<TuningSuggestion> suggestions)
    {
        var clone = DeepClone(parameters);
        foreach (var suggestion in suggestions)
        {
            SetParameterValue(clone, suggestion.ParameterPath, suggestion.SuggestedValue);
        }
        clone.UpdatedAt = DateTime.UtcNow;
        return clone;
    }

    #endregion

    #region Pattern Detection (Phase-Aware)

    /// <summary>
    /// Detect patterns across all phases using phase-specific thresholds.
    /// </summary>
    private List<DiagnosticPattern> DetectPatternsPhaseAware(
        Dictionary<TelemetryPhase, List<TelemetryData>> phaseSegments,
        Dictionary<TelemetryPhase, PhaseMetrics> phaseMetricsMap,
        TestMetrics globalMetrics)
    {
        var patterns = new List<DiagnosticPattern>();

        foreach (var (phase, data) in phaseSegments)
        {
            if (phase == TelemetryPhase.Completed || data.Count < 5)
                continue;

            if (!phaseMetricsMap.TryGetValue(phase, out var pm))
                continue;

            // Oscillation
            if (PhaseThresholds.ShouldRunOscillation(phase))
            {
                var p = DetectOscillation(data, pm,
                    PhaseThresholds.GetZeroCrossingThreshold(phase),
                    PhaseThresholds.GetAngVelStdDevThreshold(phase) ?? 0.3,
                    phase);
                if (p != null) patterns.Add(p);
            }

            // Large CTE
            if (PhaseThresholds.ShouldRunLargeCTE(phase))
            {
                var threshold = PhaseThresholds.GetCteRmsThreshold(phase);
                if (threshold.HasValue)
                {
                    var p = DetectLargeCTE(data, pm, threshold.Value, phase);
                    if (p != null) patterns.Add(p);
                }
            }

            // Large Heading Error (phase-specific)
            if (PhaseThresholds.ShouldRunLargeHeadingError(phase))
            {
                var thresholdDeg = PhaseThresholds.GetHeadingRmsThresholdDeg(phase);
                if (thresholdDeg.HasValue)
                {
                    var p = DetectLargeHeadingError(pm, thresholdDeg.Value, phase);
                    if (p != null) patterns.Add(p);
                }
            }

            // Corner Cutting (PathFollowing only)
            if (PhaseThresholds.ShouldRunCornerCutting(phase))
            {
                var p = DetectCornerCutting(data, pm, phase);
                if (p != null) patterns.Add(p);
            }

            // Goal Overshoot (FinalApproach only — uses actual phase data instead of 80% approximation)
            if (PhaseThresholds.ShouldRunGoalOvershoot(phase))
            {
                var p = DetectGoalOvershoot(data, pm, phase);
                if (p != null) patterns.Add(p);
            }

            // Sluggish Response
            if (PhaseThresholds.ShouldRunSluggishResponse(phase))
            {
                var spikeThreshold = phase == TelemetryPhase.FinalApproach ? 0.03 : 0.08;
                var recoveryThreshold = phase == TelemetryPhase.FinalApproach ? 0.02 : 0.05;
                var p = DetectSluggishResponse(data, spikeThreshold, recoveryThreshold, phase);
                if (p != null) patterns.Add(p);
            }

            // Jerky Motion
            if (PhaseThresholds.ShouldRunJerkyMotion(phase))
            {
                var accelThreshold = PhaseThresholds.GetAccelStdDevThreshold(phase) ?? 0.5;
                var p = DetectJerkyMotion(data, pm, accelThreshold, phase);
                if (p != null) patterns.Add(p);
            }

            // Velocity Estimation (PathFollowing only)
            if (PhaseThresholds.ShouldRunVelocityEstimation(phase))
            {
                var p = DetectVelocityEstimationIssues(data, phase);
                if (p != null) patterns.Add(p);
            }
        }

        // Global detectors (use TestMetrics, not phase-specific)
        var goalHeading = DetectGoalHeadingError(globalMetrics);
        if (goalHeading != null)
        {
            goalHeading.DetectedInPhase = TelemetryPhase.FinalRotation;
            patterns.Add(goalHeading);
        }

        var pathEfficiency = DetectPathEfficiencyIssues(globalMetrics);
        if (pathEfficiency != null)
        {
            pathEfficiency.DetectedInPhase = TelemetryPhase.PathFollowing;
            patterns.Add(pathEfficiency);
        }

        return patterns;
    }

    /// <summary>
    /// Detect angular velocity oscillation via zero-crossing rate (phase-aware).
    /// </summary>
    private DiagnosticPattern? DetectOscillation(
        List<TelemetryData> data,
        PhaseMetrics pm,
        double zeroCrossingThreshold,
        double angVelStdDevThreshold,
        TelemetryPhase phase)
    {
        if (data.Count < 10) return null;

        var angularVelocities = data.Select(d => d.RobotTwist.Angular).ToList();

        int crossings = 0;
        for (int i = 1; i < angularVelocities.Count; i++)
        {
            if (angularVelocities[i - 1] * angularVelocities[i] < 0)
                crossings++;
        }

        double durationS = pm.DurationMs / 1000.0;
        if (durationS <= 0) return null;

        double crossingRate = crossings / durationS;

        if (crossingRate < zeroCrossingThreshold || pm.AngularVelocityStdDev < angVelStdDevThreshold)
            return null;

        double severity = Math.Clamp((crossingRate - zeroCrossingThreshold) / 5.0, 0.3, 1.0);

        return new DiagnosticPattern
        {
            Category = DiagnosticCategory.Oscillation,
            Description = $"[{phase}] Angular velocity oscillation: {crossingRate:F1} zero-crossings/sec, " +
                          $"stddev = {pm.AngularVelocityStdDev:F3} rad/s",
            Severity = severity,
            DetectedInPhase = phase,
            Evidence = new()
            {
                ["ZeroCrossingRate"] = crossingRate,
                ["AngularVelocityStdDev"] = pm.AngularVelocityStdDev
            }
        };
    }

    /// <summary>
    /// Detect consistently large cross-track error (phase-aware with configurable threshold).
    /// </summary>
    private DiagnosticPattern? DetectLargeCTE(
        List<TelemetryData> data,
        PhaseMetrics pm,
        double threshold,
        TelemetryPhase phase)
    {
        if (pm.CrossTrackErrorRMS < threshold * 0.7) return null;

        int exceedCount = data.Count(d => d.CrossTrackError > threshold);
        double exceedFraction = data.Count > 0 ? (double)exceedCount / data.Count : 0;

        if (pm.CrossTrackErrorRMS < threshold && exceedFraction < 0.30) return null;

        double severity = Math.Clamp(pm.CrossTrackErrorRMS / threshold - 0.5, 0.3, 1.0);

        return new DiagnosticPattern
        {
            Category = DiagnosticCategory.LargeCTE,
            Description = $"[{phase}] Large cross-track error: RMS={pm.CrossTrackErrorRMS:F4}m, " +
                          $"Peak={pm.CrossTrackErrorPeak:F4}m, {exceedFraction:P0} exceed {threshold}m (target: {threshold}m)",
            Severity = severity,
            DetectedInPhase = phase,
            Evidence = new()
            {
                ["CTE_RMS"] = pm.CrossTrackErrorRMS,
                ["CTE_Peak"] = pm.CrossTrackErrorPeak,
                ["CTE_Mean"] = pm.CrossTrackErrorMean,
                ["ExceedFraction"] = exceedFraction,
                ["Threshold"] = threshold
            }
        };
    }

    /// <summary>
    /// Detect large heading error for a specific phase.
    /// </summary>
    private DiagnosticPattern? DetectLargeHeadingError(
        PhaseMetrics pm,
        double thresholdDeg,
        TelemetryPhase phase)
    {
        double headingRmsDeg = pm.HeadingErrorRMS / Deg2Rad;
        if (headingRmsDeg < thresholdDeg * 0.7) return null;

        double severity = Math.Clamp((headingRmsDeg - thresholdDeg * 0.7) / (thresholdDeg * 0.6), 0.3, 1.0);

        return new DiagnosticPattern
        {
            Category = DiagnosticCategory.GoalHeadingError,
            Description = $"[{phase}] Large heading error: RMS={headingRmsDeg:F1}°, " +
                          $"Peak={pm.HeadingErrorPeak / Deg2Rad:F1}° (target: {thresholdDeg:F0}°)",
            Severity = severity,
            DetectedInPhase = phase,
            Evidence = new()
            {
                ["HeadingErrorRMSDeg"] = headingRmsDeg,
                ["HeadingErrorPeakDeg"] = pm.HeadingErrorPeak / Deg2Rad,
                ["ThresholdDeg"] = thresholdDeg
            }
        };
    }

    /// <summary>
    /// Detect corner cutting: CTE spikes at high-curvature path segments (phase-aware).
    /// </summary>
    private DiagnosticPattern? DetectCornerCutting(
        List<TelemetryData> data,
        PhaseMetrics pm,
        TelemetryPhase phase)
    {
        if (data.Count < 20) return null;

        var curvatures = new List<(int Index, double Curvature)>();
        for (int i = 1; i < data.Count; i++)
        {
            double dTheta = Math.Abs(NormalizeAngle(
                data[i].ReferencePose.Theta - data[i - 1].ReferencePose.Theta));
            double dx = data[i].ReferencePose.X - data[i - 1].ReferencePose.X;
            double dy = data[i].ReferencePose.Y - data[i - 1].ReferencePose.Y;
            double ds = Math.Sqrt(dx * dx + dy * dy);
            if (ds > 0.001)
                curvatures.Add((i, dTheta / ds));
        }

        if (curvatures.Count == 0) return null;

        double medianCurvature = GetMedian(curvatures.Select(c => c.Curvature).ToList());
        double curvatureThreshold = Math.Max(1.0, medianCurvature * 3.0);

        var highCurvatureIndices = curvatures
            .Where(c => c.Curvature > curvatureThreshold)
            .Select(c => c.Index)
            .ToHashSet();

        if (highCurvatureIndices.Count < 3) return null;

        if (pm.CrossTrackErrorMean < 0.001) return null;

        double cteMeanAtCurves = highCurvatureIndices
            .Select(i => data[i].CrossTrackError)
            .Average();

        double ratio = cteMeanAtCurves / pm.CrossTrackErrorMean;

        if (ratio < 1.5 || cteMeanAtCurves < 0.08) return null;

        double severity = Math.Clamp((ratio - 1.5) / 2.0, 0.3, 1.0);

        return new DiagnosticPattern
        {
            Category = DiagnosticCategory.CornerCutting,
            Description = $"[{phase}] Corner cutting: CTE at curves = {cteMeanAtCurves:F4}m " +
                          $"({ratio:F1}x mean {pm.CrossTrackErrorMean:F4}m)",
            Severity = severity,
            DetectedInPhase = phase,
            Evidence = new()
            {
                ["CTEAtCurves"] = cteMeanAtCurves,
                ["CTEMeanOverall"] = pm.CrossTrackErrorMean,
                ["CurveToOverallRatio"] = ratio,
                ["HighCurvaturePoints"] = highCurvatureIndices.Count
            }
        };
    }

    /// <summary>
    /// Detect goal overshoot using actual FinalApproach phase data (phase-aware).
    /// </summary>
    private DiagnosticPattern? DetectGoalOvershoot(
        List<TelemetryData> data,
        PhaseMetrics pm,
        TelemetryPhase phase)
    {
        if (data.Count < 5) return null;

        double minDist = double.MaxValue;
        int minIdx = 0;
        for (int i = 0; i < data.Count; i++)
        {
            if (data[i].DistanceToGoal < minDist)
            {
                minDist = data[i].DistanceToGoal;
                minIdx = i;
            }
        }

        double maxAfterMin = 0;
        for (int i = minIdx + 1; i < data.Count; i++)
        {
            maxAfterMin = Math.Max(maxAfterMin, data[i].DistanceToGoal);
        }

        double overshootMagnitude = maxAfterMin - minDist;
        if (overshootMagnitude < 0.03) return null;

        double severity = Math.Clamp(overshootMagnitude / 0.10, 0.3, 1.0);

        return new DiagnosticPattern
        {
            Category = DiagnosticCategory.GoalOvershoot,
            Description = $"[{phase}] Goal overshoot: overshot by {overshootMagnitude:F4}m " +
                          $"(min {minDist:F4}m → increased to {maxAfterMin:F4}m)",
            Severity = severity,
            DetectedInPhase = phase,
            Evidence = new()
            {
                ["OvershootMagnitude"] = overshootMagnitude,
                ["MinDistanceToGoal"] = minDist,
                ["MaxDistanceAfterMin"] = maxAfterMin,
                ["GoalPositionError"] = pm.GoalPositionError
            }
        };
    }

    /// <summary>
    /// Detect sluggish error correction with configurable thresholds (phase-aware).
    /// </summary>
    private DiagnosticPattern? DetectSluggishResponse(
        List<TelemetryData> data,
        double spikeThreshold,
        double recoveryThreshold,
        TelemetryPhase phase)
    {
        if (data.Count < 20) return null;

        var correctionTimes = new List<double>();
        bool inSpike = false;
        long spikeStartMs = 0;

        for (int i = 0; i < data.Count; i++)
        {
            if (!inSpike && data[i].CrossTrackError > spikeThreshold)
            {
                inSpike = true;
                spikeStartMs = data[i].TimestampMs;
            }
            else if (inSpike && data[i].CrossTrackError < recoveryThreshold)
            {
                double correctionTimeS = (data[i].TimestampMs - spikeStartMs) / 1000.0;
                if (correctionTimeS > 0)
                    correctionTimes.Add(correctionTimeS);
                inSpike = false;
            }
        }

        if (correctionTimes.Count < 2) return null;

        double avgCorrectionTime = correctionTimes.Average();
        double correctionTimeThreshold = phase == TelemetryPhase.FinalApproach ? 1.0 : 2.0;
        if (avgCorrectionTime < correctionTimeThreshold) return null;

        double severity = Math.Clamp((avgCorrectionTime - correctionTimeThreshold) / 3.0, 0.3, 1.0);

        return new DiagnosticPattern
        {
            Category = DiagnosticCategory.SluggishResponse,
            Description = $"[{phase}] Sluggish error correction: avg {avgCorrectionTime:F1}s to correct CTE " +
                          $"({correctionTimes.Count} events)",
            Severity = severity,
            DetectedInPhase = phase,
            Evidence = new()
            {
                ["AvgCorrectionTimeS"] = avgCorrectionTime,
                ["CorrectionEventCount"] = correctionTimes.Count,
                ["MaxCorrectionTimeS"] = correctionTimes.Max()
            }
        };
    }

    /// <summary>
    /// Detect jerky motion from high acceleration variance (phase-aware).
    /// </summary>
    private DiagnosticPattern? DetectJerkyMotion(
        List<TelemetryData> data,
        PhaseMetrics pm,
        double accelThreshold,
        TelemetryPhase phase)
    {
        if (pm.AccelerationStdDev < accelThreshold) return null;

        var velocities = data.Select(d => d.RobotTwist.Linear).ToList();
        int jerkEvents = 0;
        for (int i = 2; i < velocities.Count; i++)
        {
            double accel1 = velocities[i - 1] - velocities[i - 2];
            double accel2 = velocities[i] - velocities[i - 1];
            if (accel1 * accel2 < 0)
                jerkEvents++;
        }

        double durationS = pm.DurationMs / 1000.0;
        double jerkRate = durationS > 0 ? jerkEvents / durationS : 0;

        double severity = Math.Clamp((pm.AccelerationStdDev - accelThreshold) / 1.0, 0.3, 1.0);

        return new DiagnosticPattern
        {
            Category = DiagnosticCategory.JerkyMotion,
            Description = $"[{phase}] Jerky motion: accel stddev = {pm.AccelerationStdDev:F3} m/s², " +
                          $"vel stddev = {pm.VelocityStdDev:F3} m/s, jerk rate = {jerkRate:F1}/s",
            Severity = severity,
            DetectedInPhase = phase,
            Evidence = new()
            {
                ["AccelerationStdDev"] = pm.AccelerationStdDev,
                ["VelocityStdDev"] = pm.VelocityStdDev,
                ["JerkRate"] = jerkRate
            }
        };
    }

    /// <summary>
    /// Detect velocity estimation issues (phase-aware).
    /// </summary>
    private DiagnosticPattern? DetectVelocityEstimationIssues(
        List<TelemetryData> data,
        TelemetryPhase phase)
    {
        if (data.Count < 10) return null;

        double sumSquaredGap = 0;
        int lowConfidenceCount = 0;

        foreach (var d in data)
        {
            double gap = d.CommandTwist.Linear - d.RobotTwist.Linear;
            sumSquaredGap += gap * gap;
            if (d.ModelConfidence < 0.5)
                lowConfidenceCount++;
        }

        double rmsGap = Math.Sqrt(sumSquaredGap / data.Count);
        double avgCommand = data.Where(d => Math.Abs(d.CommandTwist.Linear) > 0.01)
            .Select(d => Math.Abs(d.CommandTwist.Linear))
            .DefaultIfEmpty(1.0)
            .Average();
        double gapPercent = avgCommand > 0 ? rmsGap / avgCommand : 0;
        double lowConfidenceFraction = (double)lowConfidenceCount / data.Count;

        if (gapPercent < 0.20 && lowConfidenceFraction < 0.40) return null;

        double severity = Math.Clamp(Math.Max(gapPercent - 0.15, lowConfidenceFraction - 0.3), 0.3, 1.0);

        return new DiagnosticPattern
        {
            Category = DiagnosticCategory.VelocityEstimation,
            Description = $"[{phase}] Velocity estimation issues: RMS gap = {rmsGap:F3} m/s ({gapPercent:P0}), " +
                          $"low confidence in {lowConfidenceFraction:P0} of samples",
            Severity = severity,
            DetectedInPhase = phase,
            Evidence = new()
            {
                ["RMSGap"] = rmsGap,
                ["GapPercent"] = gapPercent,
                ["LowConfidenceFraction"] = lowConfidenceFraction,
                ["AvgConfidence"] = data.Average(d => d.ModelConfidence)
            }
        };
    }

    /// <summary>
    /// Detect large heading error at goal (global — uses TestMetrics).
    /// </summary>
    private DiagnosticPattern? DetectGoalHeadingError(TestMetrics metrics)
    {
        double thresholdRad = 5.0 * Deg2Rad;

        if (metrics.GoalHeadingError < thresholdRad) return null;

        double severity = Math.Clamp((metrics.GoalHeadingError - thresholdRad) / (10.0 * Deg2Rad), 0.3, 1.0);

        return new DiagnosticPattern
        {
            Category = DiagnosticCategory.GoalHeadingError,
            Description = $"[FinalRotation] Large heading error at goal: {metrics.GoalHeadingError / Deg2Rad:F1}° " +
                          $"(threshold: {thresholdRad / Deg2Rad:F0}°)",
            Severity = severity,
            Evidence = new()
            {
                ["GoalHeadingErrorDeg"] = metrics.GoalHeadingError / Deg2Rad,
                ["GoalHeadingErrorRad"] = metrics.GoalHeadingError
            }
        };
    }

    /// <summary>
    /// Detect path efficiency issues (global — uses TestMetrics).
    /// </summary>
    private DiagnosticPattern? DetectPathEfficiencyIssues(TestMetrics metrics)
    {
        if (metrics.PathLengthRatio < 1.15) return null;

        double severity = Math.Clamp((metrics.PathLengthRatio - 1.15) / 0.30, 0.3, 1.0);

        return new DiagnosticPattern
        {
            Category = DiagnosticCategory.PathEfficiency,
            Description = $"[PathFollowing] Path efficiency issue: actual path is {(metrics.PathLengthRatio - 1.0) * 100:F1}% " +
                          $"longer than reference (ratio = {metrics.PathLengthRatio:F3})",
            Severity = severity,
            Evidence = new()
            {
                ["PathLengthRatio"] = metrics.PathLengthRatio,
                ["CompletionTime"] = metrics.CompletionTime,
                ["AverageSpeed"] = metrics.AverageSpeed
            }
        };
    }

    #endregion

    #region Suggestion Generation (Phase-Aware)

    private List<TuningSuggestion> GenerateSuggestions(
        List<DiagnosticPattern> patterns,
        NavigationParameterSet parameters,
        TestMetrics metrics)
    {
        var suggestions = new List<TuningSuggestion>();
        double dataQuality = Math.Min(1.0, metrics.CompletionTime > 0 ? 1.0 : 0.5);

        bool isPurePursuit = parameters.ControllerType == PathFollowingController.PurePursuit;

        // Group patterns by phase for targeted suggestions
        // DetectedInPhase is always set by DetectPatternsPhaseAware, but model allows null for backward compat
        foreach (var group in patterns.GroupBy(p => p.DetectedInPhase))
        {
            var phasePatterns = group.ToDictionary(p => p.Category, p => p);
            var phase = group.Key;

            if (isPurePursuit)
                suggestions.AddRange(SuggestForPurePursuit(phasePatterns, parameters, dataQuality, phase));
            else
                suggestions.AddRange(SuggestForStanley(phasePatterns, parameters, dataQuality, phase));

            suggestions.AddRange(SuggestForCommon(phasePatterns, parameters, dataQuality, metrics, phase));
        }

        return suggestions;
    }

    private List<TuningSuggestion> SuggestForPurePursuit(
        Dictionary<DiagnosticCategory, DiagnosticPattern> patterns,
        NavigationParameterSet p,
        double dataQuality,
        TelemetryPhase? phase)
    {
        var suggestions = new List<TuningSuggestion>();
        var pp = p.PurePursuitConfig;

        // --- Oscillation (PathFollowing) ---
        if (patterns.TryGetValue(DiagnosticCategory.Oscillation, out var oscillation))
        {
            // PathFollowing oscillation → PP lookahead/angular velocity
            if (phase is null or TelemetryPhase.PathFollowing)
            {
                suggestions.Add(CreateSuggestion(
                    "PurePursuitConfig.LookaheadMin", "PP Lookahead Min",
                    pp.LookaheadMin,
                    Math.Min(pp.LookaheadMin * 1.4, pp.LookaheadMax * 0.5),
                    $"[{phase}] Increase minimum lookahead to reduce oscillation",
                    SuggestionPriority.High, oscillation.Severity * 0.9 * dataQuality,
                    DiagnosticCategory.Oscillation,
                    "Smoother angular velocity, less jitter",
                    phase));

                suggestions.Add(CreateSuggestion(
                    "PurePursuitConfig.Kdd", "PP Velocity Lookahead Gain",
                    pp.Kdd,
                    pp.Kdd * 1.2,
                    $"[{phase}] Increase velocity-based lookahead gain for smoother tracking at speed",
                    SuggestionPriority.Medium, oscillation.Severity * 0.7 * dataQuality,
                    DiagnosticCategory.Oscillation,
                    "More predictive at higher speeds",
                    phase));

                suggestions.Add(CreateSuggestion(
                    "PurePursuitConfig.MaxAngularVelocity", "PP Max Angular Velocity",
                    pp.MaxAngularVelocity,
                    Math.Max(pp.MaxAngularVelocity * 0.85, 0.8),
                    $"[{phase}] Reduce max angular velocity to dampen oscillation",
                    SuggestionPriority.Medium, oscillation.Severity * 0.8 * dataQuality,
                    DiagnosticCategory.Oscillation,
                    "Less aggressive turning, reduced overshoot",
                    phase));
            }

            // FinalApproach oscillation → FinalApproach params
            if (phase == TelemetryPhase.FinalApproach)
            {
                suggestions.Add(CreateSuggestion(
                    "PurePursuitConfig.FinalApproachThreshold", "PP Final Approach Threshold",
                    pp.FinalApproachThreshold,
                    Math.Min(pp.FinalApproachThreshold * 1.3, 0.5),
                    "[FinalApproach] Switch to precision mode earlier to reduce approach oscillation",
                    SuggestionPriority.High, oscillation.Severity * 0.85 * dataQuality,
                    DiagnosticCategory.Oscillation,
                    "Earlier precision mode, less oscillation near goal",
                    TelemetryPhase.FinalApproach));
            }
        }

        // --- Large CTE ---
        if (patterns.TryGetValue(DiagnosticCategory.LargeCTE, out var largeCTE))
        {
            if (phase is null or TelemetryPhase.PathFollowing)
            {
                suggestions.Add(CreateSuggestion(
                    "PurePursuitConfig.LookaheadMin", "PP Lookahead Min",
                    pp.LookaheadMin,
                    Math.Max(pp.LookaheadMin * 0.8, 0.15),
                    $"[{phase}] Decrease minimum lookahead for tighter path following",
                    SuggestionPriority.High, largeCTE.Severity * 0.9 * dataQuality,
                    DiagnosticCategory.LargeCTE,
                    "Reduced cross-track error, tighter path following",
                    phase));

                suggestions.Add(CreateSuggestion(
                    "PurePursuitConfig.KCurvature", "PP Curvature Sensitivity",
                    pp.KCurvature,
                    Math.Min(pp.KCurvature * 1.3, 5.0),
                    $"[{phase}] Increase curvature sensitivity to reduce lookahead on curves",
                    SuggestionPriority.Medium, largeCTE.Severity * 0.7 * dataQuality,
                    DiagnosticCategory.LargeCTE,
                    "Better tracking on curved sections",
                    phase));
            }

            // FinalApproach Large CTE → precision parameters
            if (phase == TelemetryPhase.FinalApproach)
            {
                suggestions.Add(CreateSuggestion(
                    "PurePursuitConfig.FinalApproachThreshold", "PP Final Approach Threshold",
                    pp.FinalApproachThreshold,
                    Math.Min(pp.FinalApproachThreshold * 1.5, 0.5),
                    "[FinalApproach] Switch to precision mode earlier — CTE exceeds 0.02m target",
                    SuggestionPriority.High, largeCTE.Severity * 0.95 * dataQuality,
                    DiagnosticCategory.LargeCTE,
                    "Earlier precision mode for tighter final approach tracking",
                    TelemetryPhase.FinalApproach));

                suggestions.Add(CreateSuggestion(
                    "PurePursuitConfig.GoalRegionDistance", "PP Goal Region Distance",
                    pp.GoalRegionDistance,
                    Math.Min(pp.GoalRegionDistance * 1.3, 3.0),
                    "[FinalApproach] Increase goal region to start reducing lookahead earlier for better precision",
                    SuggestionPriority.High, largeCTE.Severity * 0.85 * dataQuality,
                    DiagnosticCategory.LargeCTE,
                    "Smoother transition to precision tracking near goal",
                    TelemetryPhase.FinalApproach));

                suggestions.Add(CreateSuggestion(
                    "MovePidConfig.Kp", "Move PID Kp",
                    p.MovePidConfig.Kp,
                    Math.Min(p.MovePidConfig.Kp * 1.2, 3.0),
                    "[FinalApproach] Increase deceleration PID gain for more responsive speed control near goal",
                    SuggestionPriority.Medium, largeCTE.Severity * 0.7 * dataQuality,
                    DiagnosticCategory.LargeCTE,
                    "More responsive deceleration for precision approach",
                    TelemetryPhase.FinalApproach));
            }
        }

        // --- Corner Cutting (PathFollowing only) ---
        if (patterns.TryGetValue(DiagnosticCategory.CornerCutting, out var cornerCutting)
            && phase is null or TelemetryPhase.PathFollowing)
        {
            suggestions.Add(CreateSuggestion(
                "PurePursuitConfig.LookaheadMax", "PP Lookahead Max",
                pp.LookaheadMax,
                Math.Max(pp.LookaheadMax * 0.8, pp.LookaheadMin * 1.5),
                $"[{phase}] Reduce max lookahead to prevent cutting corners",
                SuggestionPriority.High, cornerCutting.Severity * 0.9 * dataQuality,
                DiagnosticCategory.CornerCutting,
                "Less corner cutting, tighter curve following",
                phase));

            suggestions.Add(CreateSuggestion(
                "PurePursuitConfig.KCurvature", "PP Curvature Sensitivity",
                pp.KCurvature,
                Math.Min(pp.KCurvature * 1.4, 6.0),
                $"[{phase}] Increase curvature sensitivity to shorten lookahead on curves",
                SuggestionPriority.High, cornerCutting.Severity * 0.85 * dataQuality,
                DiagnosticCategory.CornerCutting,
                "Significantly tighter tracking through curves",
                phase));

            suggestions.Add(CreateSuggestion(
                "PurePursuitConfig.Kdd", "PP Velocity Lookahead Gain",
                pp.Kdd,
                Math.Max(pp.Kdd * 0.8, 0.5),
                $"[{phase}] Decrease velocity-based lookahead to reduce lookahead at speed on curves",
                SuggestionPriority.Medium, cornerCutting.Severity * 0.6 * dataQuality,
                DiagnosticCategory.CornerCutting,
                "Less speed-dependent lookahead stretch",
                phase));
        }

        // --- Goal Overshoot (FinalApproach) ---
        if (patterns.TryGetValue(DiagnosticCategory.GoalOvershoot, out var goalOvershoot))
        {
            suggestions.Add(CreateSuggestion(
                "PurePursuitConfig.FinalApproachThreshold", "PP Final Approach Threshold",
                pp.FinalApproachThreshold,
                Math.Min(pp.FinalApproachThreshold * 1.5, 0.5),
                "[FinalApproach] Increase final approach distance to switch to precision mode earlier",
                SuggestionPriority.High, goalOvershoot.Severity * 0.9 * dataQuality,
                DiagnosticCategory.GoalOvershoot,
                "Earlier deceleration near goal, less overshoot",
                TelemetryPhase.FinalApproach));

            suggestions.Add(CreateSuggestion(
                "PurePursuitConfig.GoalRegionDistance", "PP Goal Region Distance",
                pp.GoalRegionDistance,
                Math.Min(pp.GoalRegionDistance * 1.3, 3.0),
                "[FinalApproach] Increase goal region to start reducing lookahead earlier",
                SuggestionPriority.Medium, goalOvershoot.Severity * 0.7 * dataQuality,
                DiagnosticCategory.GoalOvershoot,
                "Smoother deceleration profile near goal",
                TelemetryPhase.FinalApproach));
        }

        // --- Sluggish Response ---
        if (patterns.TryGetValue(DiagnosticCategory.SluggishResponse, out var sluggish))
        {
            if (phase is null or TelemetryPhase.PathFollowing)
            {
                suggestions.Add(CreateSuggestion(
                    "PurePursuitConfig.LookaheadMin", "PP Lookahead Min",
                    pp.LookaheadMin,
                    Math.Max(pp.LookaheadMin * 0.85, 0.15),
                    $"[{phase}] Decrease minimum lookahead for more reactive error correction",
                    SuggestionPriority.Medium, sluggish.Severity * 0.7 * dataQuality,
                    DiagnosticCategory.SluggishResponse,
                    "Faster error correction, more responsive tracking",
                    phase));

                suggestions.Add(CreateSuggestion(
                    "PurePursuitConfig.MaxAngularVelocity", "PP Max Angular Velocity",
                    pp.MaxAngularVelocity,
                    Math.Min(pp.MaxAngularVelocity * 1.2, 3.0),
                    $"[{phase}] Increase max angular velocity for faster corrections",
                    SuggestionPriority.Medium, sluggish.Severity * 0.8 * dataQuality,
                    DiagnosticCategory.SluggishResponse,
                    "Faster angular corrections during tracking",
                    phase));
            }
        }

        // --- FinalApproach Heading Error ---
        if (patterns.TryGetValue(DiagnosticCategory.GoalHeadingError, out var headingErr)
            && phase == TelemetryPhase.FinalApproach)
        {
            suggestions.Add(CreateSuggestion(
                "PurePursuitConfig.HeadingTolerance", "PP Heading Tolerance",
                pp.HeadingTolerance,
                Math.Max(pp.HeadingTolerance * 0.7, 1.0),
                "[FinalApproach] Tighten heading tolerance for more precise final alignment",
                SuggestionPriority.High, headingErr.Severity * 0.85 * dataQuality,
                DiagnosticCategory.GoalHeadingError,
                "More precise heading alignment near goal",
                TelemetryPhase.FinalApproach));
        }

        return suggestions;
    }

    private List<TuningSuggestion> SuggestForStanley(
        Dictionary<DiagnosticCategory, DiagnosticPattern> patterns,
        NavigationParameterSet p,
        double dataQuality,
        TelemetryPhase? phase)
    {
        var suggestions = new List<TuningSuggestion>();
        var sc = p.StanleyConfig;

        // --- Oscillation ---
        if (patterns.TryGetValue(DiagnosticCategory.Oscillation, out var oscillation))
        {
            if (phase is null or TelemetryPhase.PathFollowing)
            {
                suggestions.Add(CreateSuggestion(
                    "StanleyConfig.K", "Stanley CTE Gain (K)",
                    sc.K,
                    Math.Max(sc.K * 0.8, 1.0),
                    $"[{phase}] Decrease CTE gain to reduce oscillation",
                    SuggestionPriority.High, oscillation.Severity * 0.9 * dataQuality,
                    DiagnosticCategory.Oscillation,
                    "Less aggressive CTE correction, reduced oscillation",
                    phase));

                suggestions.Add(CreateSuggestion(
                    "StanleyConfig.Ks", "Stanley Softening (Ks)",
                    sc.Ks,
                    Math.Min(sc.Ks * 1.3, 0.3),
                    $"[{phase}] Increase softening constant to reduce aggressiveness at low speed",
                    SuggestionPriority.Medium, oscillation.Severity * 0.7 * dataQuality,
                    DiagnosticCategory.Oscillation,
                    "Gentler correction especially at low speeds",
                    phase));
            }

            if (phase == TelemetryPhase.FinalApproach)
            {
                suggestions.Add(CreateSuggestion(
                    "StanleyConfig.LowSpeedAngularGain", "Stanley Low Speed Angular Gain",
                    sc.LowSpeedAngularGain,
                    Math.Max(sc.LowSpeedAngularGain * 0.8, 0.5),
                    "[FinalApproach] Reduce low-speed angular gain to prevent oscillation near goal",
                    SuggestionPriority.High, oscillation.Severity * 0.85 * dataQuality,
                    DiagnosticCategory.Oscillation,
                    "Less aggressive rotation at low speed near goal",
                    TelemetryPhase.FinalApproach));

                suggestions.Add(CreateSuggestion(
                    "StanleyConfig.GoalGainMultiplier", "Stanley Goal Gain Multiplier",
                    sc.GoalGainMultiplier,
                    Math.Max(sc.GoalGainMultiplier * 0.85, 1.2),
                    "[FinalApproach] Decrease goal gain multiplier to reduce oscillation near goal",
                    SuggestionPriority.Medium, oscillation.Severity * 0.7 * dataQuality,
                    DiagnosticCategory.Oscillation,
                    "Less aggressive K increase near goal",
                    TelemetryPhase.FinalApproach));
            }
        }

        // --- Large CTE ---
        if (patterns.TryGetValue(DiagnosticCategory.LargeCTE, out var largeCTE))
        {
            if (phase is null or TelemetryPhase.PathFollowing)
            {
                suggestions.Add(CreateSuggestion(
                    "StanleyConfig.K", "Stanley CTE Gain (K)",
                    sc.K,
                    Math.Min(sc.K * 1.25, 6.0),
                    $"[{phase}] Increase CTE gain for tighter path following",
                    SuggestionPriority.High, largeCTE.Severity * 0.9 * dataQuality,
                    DiagnosticCategory.LargeCTE,
                    "Faster CTE correction, tighter tracking",
                    phase));
            }

            // FinalApproach Large CTE → precision parameters
            if (phase == TelemetryPhase.FinalApproach)
            {
                suggestions.Add(CreateSuggestion(
                    "StanleyConfig.GoalGainMultiplier", "Stanley Goal Gain Multiplier",
                    sc.GoalGainMultiplier,
                    Math.Min(sc.GoalGainMultiplier * 1.3, 3.5),
                    "[FinalApproach] Increase goal gain multiplier — CTE exceeds 0.02m target",
                    SuggestionPriority.High, largeCTE.Severity * 0.95 * dataQuality,
                    DiagnosticCategory.LargeCTE,
                    "Tighter K near goal for precision tracking",
                    TelemetryPhase.FinalApproach));

                suggestions.Add(CreateSuggestion(
                    "StanleyConfig.GoalApproachDistance", "Stanley Goal Approach Distance",
                    sc.GoalApproachDistance,
                    Math.Min(sc.GoalApproachDistance * 1.4, 2.5),
                    "[FinalApproach] Increase goal approach distance to start tightening K earlier",
                    SuggestionPriority.High, largeCTE.Severity * 0.9 * dataQuality,
                    DiagnosticCategory.LargeCTE,
                    "Earlier gain increase for better final approach precision",
                    TelemetryPhase.FinalApproach));

                suggestions.Add(CreateSuggestion(
                    "StanleyConfig.LowSpeedAngularGain", "Stanley Low Speed Angular Gain",
                    sc.LowSpeedAngularGain,
                    Math.Min(sc.LowSpeedAngularGain * 1.25, 3.0),
                    "[FinalApproach] Increase low-speed angular gain for better correction at low speed near goal",
                    SuggestionPriority.Medium, largeCTE.Severity * 0.8 * dataQuality,
                    DiagnosticCategory.LargeCTE,
                    "Better low-speed correction for precision approach",
                    TelemetryPhase.FinalApproach));
            }
        }

        // --- Corner Cutting (PathFollowing only) ---
        if (patterns.TryGetValue(DiagnosticCategory.CornerCutting, out var cornerCutting)
            && phase is null or TelemetryPhase.PathFollowing)
        {
            suggestions.Add(CreateSuggestion(
                "StanleyConfig.K", "Stanley CTE Gain (K)",
                sc.K,
                Math.Min(sc.K * 1.2, 5.0),
                $"[{phase}] Increase K gain to correct CTE faster on curves",
                SuggestionPriority.High, cornerCutting.Severity * 0.8 * dataQuality,
                DiagnosticCategory.CornerCutting,
                "Tighter tracking through curves",
                phase));

            suggestions.Add(CreateSuggestion(
                "StanleyConfig.KCurvatureFF", "Stanley Curvature Feedforward",
                sc.KCurvatureFF,
                Math.Min(sc.KCurvatureFF * 1.25, 2.0),
                $"[{phase}] Increase curvature feedforward for better curve anticipation",
                SuggestionPriority.High, cornerCutting.Severity * 0.85 * dataQuality,
                DiagnosticCategory.CornerCutting,
                "Better curve anticipation, less lag through turns",
                phase));
        }

        // --- Goal Overshoot (FinalApproach) ---
        if (patterns.TryGetValue(DiagnosticCategory.GoalOvershoot, out var goalOvershoot))
        {
            suggestions.Add(CreateSuggestion(
                "StanleyConfig.GoalApproachDistance", "Stanley Goal Approach Distance",
                sc.GoalApproachDistance,
                Math.Min(sc.GoalApproachDistance * 1.4, 2.5),
                "[FinalApproach] Increase goal approach distance to start tightening K earlier",
                SuggestionPriority.High, goalOvershoot.Severity * 0.9 * dataQuality,
                DiagnosticCategory.GoalOvershoot,
                "Earlier gain increase near goal, better precision",
                TelemetryPhase.FinalApproach));

            suggestions.Add(CreateSuggestion(
                "StanleyConfig.GoalGainMultiplier", "Stanley Goal Gain Multiplier",
                sc.GoalGainMultiplier,
                Math.Max(sc.GoalGainMultiplier * 0.8, 1.2),
                "[FinalApproach] Decrease goal gain multiplier to reduce overshoot (less aggressive)",
                SuggestionPriority.Medium, goalOvershoot.Severity * 0.7 * dataQuality,
                DiagnosticCategory.GoalOvershoot,
                "Less aggressive correction near goal, reduced oscillation",
                TelemetryPhase.FinalApproach));
        }

        // --- Sluggish Response ---
        if (patterns.TryGetValue(DiagnosticCategory.SluggishResponse, out var sluggish))
        {
            if (phase is null or TelemetryPhase.PathFollowing)
            {
                suggestions.Add(CreateSuggestion(
                    "StanleyConfig.K", "Stanley CTE Gain (K)",
                    sc.K,
                    Math.Min(sc.K * 1.2, 5.0),
                    $"[{phase}] Increase K gain for faster error correction",
                    SuggestionPriority.Medium, sluggish.Severity * 0.8 * dataQuality,
                    DiagnosticCategory.SluggishResponse,
                    "Faster CTE correction",
                    phase));
            }

            if (phase == TelemetryPhase.FinalApproach)
            {
                suggestions.Add(CreateSuggestion(
                    "StanleyConfig.LowSpeedAngularGain", "Stanley Low Speed Angular Gain",
                    sc.LowSpeedAngularGain,
                    Math.Min(sc.LowSpeedAngularGain * 1.25, 3.0),
                    "[FinalApproach] Increase low-speed angular gain for faster correction near goal",
                    SuggestionPriority.Medium, sluggish.Severity * 0.8 * dataQuality,
                    DiagnosticCategory.SluggishResponse,
                    "Better low-speed correction ability near goal",
                    TelemetryPhase.FinalApproach));
            }
        }

        // --- Jerky Motion ---
        if (patterns.TryGetValue(DiagnosticCategory.JerkyMotion, out var jerky))
        {
            suggestions.Add(CreateSuggestion(
                "StanleyConfig.MaxSteeringAngle", "Stanley Max Steering Angle",
                sc.MaxSteeringAngle,
                Math.Max(sc.MaxSteeringAngle * 0.85, 0.3),
                $"[{phase}] Reduce max steering angle to limit abrupt steering changes",
                SuggestionPriority.Medium, jerky.Severity * 0.7 * dataQuality,
                DiagnosticCategory.JerkyMotion,
                "Smoother steering transitions",
                phase));
        }

        // --- FinalApproach Heading Error ---
        if (patterns.TryGetValue(DiagnosticCategory.GoalHeadingError, out var headingErr)
            && phase == TelemetryPhase.FinalApproach)
        {
            suggestions.Add(CreateSuggestion(
                "StanleyConfig.GoalGainMultiplier", "Stanley Goal Gain Multiplier",
                sc.GoalGainMultiplier,
                Math.Min(sc.GoalGainMultiplier * 1.2, 3.5),
                "[FinalApproach] Increase goal gain for tighter heading correction — heading exceeds 2° target",
                SuggestionPriority.High, headingErr.Severity * 0.85 * dataQuality,
                DiagnosticCategory.GoalHeadingError,
                "Tighter heading correction near goal",
                TelemetryPhase.FinalApproach));

            suggestions.Add(CreateSuggestion(
                "StanleyConfig.LowSpeedAngularGain", "Stanley Low Speed Angular Gain",
                sc.LowSpeedAngularGain,
                Math.Min(sc.LowSpeedAngularGain * 1.3, 3.0),
                "[FinalApproach] Increase low-speed angular gain for better heading correction near goal",
                SuggestionPriority.High, headingErr.Severity * 0.8 * dataQuality,
                DiagnosticCategory.GoalHeadingError,
                "Better heading correction at low speed",
                TelemetryPhase.FinalApproach));
        }

        return suggestions;
    }

    private List<TuningSuggestion> SuggestForCommon(
        Dictionary<DiagnosticCategory, DiagnosticPattern> patterns,
        NavigationParameterSet p,
        double dataQuality,
        TestMetrics metrics,
        TelemetryPhase? phase)
    {
        var suggestions = new List<TuningSuggestion>();

        // --- Jerky Motion (common) ---
        if (patterns.TryGetValue(DiagnosticCategory.JerkyMotion, out var jerky))
        {
            var targetPhase = phase ?? TelemetryPhase.PathFollowing;

            suggestions.Add(CreateSuggestion(
                "NavigationConfig.Acceleration", "Max Acceleration",
                p.NavigationConfig.Acceleration,
                Math.Max(p.NavigationConfig.Acceleration * 0.8, 0.2),
                $"[{phase}] Reduce acceleration limit for smoother speed changes",
                SuggestionPriority.Medium, jerky.Severity * 0.8 * dataQuality,
                DiagnosticCategory.JerkyMotion,
                "Smoother acceleration profile",
                targetPhase));

            suggestions.Add(CreateSuggestion(
                "NavigationConfig.Deceleration", "Max Deceleration",
                p.NavigationConfig.Deceleration,
                Math.Max(p.NavigationConfig.Deceleration * 0.8, 0.2),
                $"[{phase}] Reduce deceleration limit for smoother braking",
                SuggestionPriority.Medium, jerky.Severity * 0.7 * dataQuality,
                DiagnosticCategory.JerkyMotion,
                "Smoother deceleration profile",
                targetPhase));

            suggestions.Add(CreateSuggestion(
                "SignalConfig.AlphaFilter", "EMA Filter Alpha",
                p.SignalConfig.AlphaFilter,
                Math.Max(p.SignalConfig.AlphaFilter * 0.75, 0.1),
                $"[{phase}] Decrease EMA filter alpha for more aggressive noise filtering",
                SuggestionPriority.Low, jerky.Severity * 0.5 * dataQuality,
                DiagnosticCategory.JerkyMotion,
                "Smoother velocity signal, less noise propagation",
                targetPhase));
        }

        // --- Goal Heading Error (FinalRotation) ---
        if (patterns.TryGetValue(DiagnosticCategory.GoalHeadingError, out var headingErr)
            && phase is null or TelemetryPhase.FinalRotation)
        {
            suggestions.Add(CreateSuggestion(
                "RotatePidConfig.Kp", "Rotate PID Kp",
                p.RotatePidConfig.Kp,
                Math.Min(p.RotatePidConfig.Kp * 1.2, 15.0),
                "[FinalRotation] Increase rotation PID proportional gain for faster heading correction",
                SuggestionPriority.High, headingErr.Severity * 0.85 * dataQuality,
                DiagnosticCategory.GoalHeadingError,
                "Faster and more precise heading alignment at goal",
                TelemetryPhase.FinalRotation));
        }

        // --- Goal Overshoot (FinalApproach - PID) ---
        if (patterns.TryGetValue(DiagnosticCategory.GoalOvershoot, out var goalOvershoot))
        {
            suggestions.Add(CreateSuggestion(
                "MovePidConfig.Kd", "Move PID Kd",
                p.MovePidConfig.Kd,
                Math.Min(p.MovePidConfig.Kd * 1.25, 2.0),
                "[FinalApproach] Increase derivative gain for better deceleration damping near goal",
                SuggestionPriority.Medium, goalOvershoot.Severity * 0.7 * dataQuality,
                DiagnosticCategory.GoalOvershoot,
                "Better damping, less velocity overshoot near goal",
                TelemetryPhase.FinalApproach));
        }

        // --- Velocity Estimation (PathFollowing) ---
        if (patterns.TryGetValue(DiagnosticCategory.VelocityEstimation, out var velEst))
        {
            suggestions.Add(CreateSuggestion(
                "EstimatorConfig.ConfidenceDecayRate", "Velocity Estimator Confidence Decay",
                p.EstimatorConfig.ConfidenceDecayRate,
                Math.Max(p.EstimatorConfig.ConfidenceDecayRate * 0.97, 0.88),
                "[PathFollowing] Decrease confidence decay rate for faster adaptation",
                SuggestionPriority.Low, velEst.Severity * 0.6 * dataQuality,
                DiagnosticCategory.VelocityEstimation,
                "Faster adaptation of blend ratio",
                TelemetryPhase.PathFollowing));
        }

        // --- Path Efficiency + Large CTE = robot moving too fast ---
        if (patterns.ContainsKey(DiagnosticCategory.PathEfficiency) &&
            patterns.ContainsKey(DiagnosticCategory.LargeCTE))
        {
            var effPattern = patterns[DiagnosticCategory.PathEfficiency];
            suggestions.Add(CreateSuggestion(
                "NavigationConfig.MaxLinearVelocity", "Max Linear Velocity",
                p.NavigationConfig.MaxLinearVelocity,
                Math.Max(p.NavigationConfig.MaxLinearVelocity * 0.85, 0.5),
                "[PathFollowing] Reduce max velocity — robot may be too fast for accurate tracking",
                SuggestionPriority.Medium, effPattern.Severity * 0.7 * dataQuality,
                DiagnosticCategory.PathEfficiency,
                "Better tracking at lower speed, reduced path length ratio",
                TelemetryPhase.PathFollowing));
        }

        return suggestions;
    }

    #endregion

    #region Conflict Resolution (Phase-Aware)

    /// <summary>
    /// Resolve conflicts grouping by (ParameterPath, TargetPhase).
    /// Different phases can have different suggestions for the same parameter.
    /// </summary>
    private List<TuningSuggestion> ResolveConflicts(List<TuningSuggestion> suggestions)
    {
        // Group by (ParameterPath, TargetPhase) — same param can have different suggestions per phase
        var grouped = suggestions.GroupBy(s => (s.ParameterPath, s.TargetPhase));
        var resolved = new List<TuningSuggestion>();

        foreach (var group in grouped)
        {
            var items = group.ToList();
            if (items.Count == 1)
            {
                resolved.Add(items[0]);
                continue;
            }

            // Check for conflicting directions within the same (param, phase)
            bool hasIncrease = items.Any(s => s.SuggestedValue > s.CurrentValue);
            bool hasDecrease = items.Any(s => s.SuggestedValue < s.CurrentValue);

            if (hasIncrease && hasDecrease)
            {
                var bestItem = items.OrderByDescending(s => s.Confidence).First();

                var secondBest = items.OrderByDescending(s => s.Confidence).Skip(1).First();
                if (Math.Abs(bestItem.Confidence - secondBest.Confidence) < 0.15)
                {
                    double compromiseValue = (bestItem.SuggestedValue + bestItem.CurrentValue) / 2.0;
                    bestItem.SuggestedValue = Math.Round(compromiseValue, 4);
                    bestItem.Reason += " [Compromise: conflicting suggestions detected - change reduced]";
                    bestItem.Confidence *= 0.7;
                }
                else
                {
                    bestItem.Reason += $" [Note: conflicting suggestion from {secondBest.RelatedPatterns.FirstOrDefault()} was overridden]";
                }

                resolved.Add(bestItem);
            }
            else
            {
                // Same direction: pick the most aggressive
                var bestItem = items.OrderByDescending(s => Math.Abs(s.SuggestedValue - s.CurrentValue)).First();
                bestItem.RelatedPatterns = items.SelectMany(s => s.RelatedPatterns).Distinct().ToList();
                resolved.Add(bestItem);
            }
        }

        return resolved;
    }

    #endregion

    #region Assessment Generation (Phase-Aware)

    private string GenerateOverallAssessment(
        TestMetrics metrics,
        List<DiagnosticPattern> patterns,
        List<TuningSuggestion> suggestions,
        Dictionary<TelemetryPhase, PhaseMetrics> phaseMetricsMap)
    {
        if (patterns.Count == 0)
            return $"Excellent performance! Overall score: {metrics.OverallScore:F0}/100. " +
                   "No significant issues detected. Parameters are well-tuned for this scenario.";

        int criticalCount = suggestions.Count(s => s.Priority == SuggestionPriority.Critical);
        int highCount = suggestions.Count(s => s.Priority == SuggestionPriority.High);

        var parts = new List<string>();
        parts.Add($"Overall score: {metrics.OverallScore:F0}/100.");

        if (criticalCount > 0)
            parts.Add($"{criticalCount} critical issue(s) require immediate attention.");
        if (highCount > 0)
            parts.Add($"{highCount} high-priority suggestion(s) for improvement.");

        // Per-phase assessment
        foreach (var (phase, pm) in phaseMetricsMap.OrderBy(kv => kv.Key))
        {
            if (phase == TelemetryPhase.Completed || pm.SampleCount < 5)
                continue;

            var phaseStatus = new List<string>();

            var cteThreshold = PhaseThresholds.GetCteRmsThreshold(phase);
            if (cteThreshold.HasValue)
            {
                string cteStatus = pm.CrossTrackErrorRMS <= cteThreshold.Value ? "Good" : "Needs improvement";
                phaseStatus.Add($"CTE RMS={pm.CrossTrackErrorRMS:F4}m ({cteStatus}, target: {cteThreshold.Value}m)");
            }

            var headingThreshold = PhaseThresholds.GetHeadingRmsThresholdDeg(phase);
            if (headingThreshold.HasValue)
            {
                double headingDeg = pm.HeadingErrorRMS / Deg2Rad;
                string headingStatus = headingDeg <= headingThreshold.Value ? "Good" : "Needs improvement";
                phaseStatus.Add($"Heading RMS={headingDeg:F1}° ({headingStatus}, target: {headingThreshold.Value}°)");
            }

            if (phaseStatus.Count > 0)
                parts.Add($"{phase}: {string.Join(", ", phaseStatus)}.");
        }

        // Summarize main issues by phase
        var patternsByPhase = patterns.GroupBy(p => p.DetectedInPhase);
        foreach (var group in patternsByPhase.OrderBy(g => g.Key))
        {
            int phaseHighCount = suggestions.Count(s => s.TargetPhase == group.Key && s.Priority >= SuggestionPriority.High);
            if (phaseHighCount > 0)
                parts.Add($"{phaseHighCount} high-priority suggestion(s) for {group.Key}.");
        }

        parts.Add($"{suggestions.Count} total parameter adjustment(s) suggested.");

        if (metrics.PassedCriteria)
            parts.Add("Acceptance criteria: PASSED.");
        else
            parts.Add("Acceptance criteria: NOT PASSED.");

        return string.Join(" ", parts);
    }

    #endregion

    #region Phase Segmentation Helpers

    private static Dictionary<TelemetryPhase, List<TelemetryData>> SegmentByPhase(List<TelemetryData> data)
    {
        var segments = new Dictionary<TelemetryPhase, List<TelemetryData>>();
        foreach (var d in data)
        {
            // Legacy data (Phase == null) → PathFollowing
            var phase = d.Phase ?? TelemetryPhase.PathFollowing;
            if (!segments.TryGetValue(phase, out var list))
            {
                list = new List<TelemetryData>();
                segments[phase] = list;
            }
            list.Add(d);
        }
        return segments;
    }

    private static PhaseMetrics ComputePhaseMetrics(TelemetryPhase phase, List<TelemetryData> data)
    {
        var pm = new PhaseMetrics
        {
            Phase = phase,
            SampleCount = data.Count
        };

        if (data.Count == 0) return pm;

        pm.DurationMs = data.Count > 1 ? data[^1].TimestampMs - data[0].TimestampMs : 0;

        // CTE
        var ctes = data.Select(d => d.CrossTrackError).ToList();
        pm.CrossTrackErrorMean = ctes.Average();
        pm.CrossTrackErrorPeak = ctes.Max();
        pm.CrossTrackErrorRMS = Math.Sqrt(ctes.Average(c => c * c));

        // Heading error
        var headings = data.Select(d => Math.Abs(d.HeadingError)).ToList();
        pm.HeadingErrorPeak = headings.Max();
        pm.HeadingErrorRMS = Math.Sqrt(headings.Average(h => h * h));

        // Angular velocity StdDev
        var angVels = data.Select(d => d.RobotTwist.Angular).ToList();
        pm.AngularVelocityStdDev = CalculateStdDev(angVels);

        // Linear velocity StdDev
        var linVels = data.Select(d => d.RobotTwist.Linear).ToList();
        pm.VelocityStdDev = CalculateStdDev(linVels);

        // Acceleration StdDev (approximate from velocity differences)
        if (data.Count >= 3)
        {
            var accels = new List<double>();
            for (int i = 1; i < data.Count; i++)
            {
                double dt = (data[i].TimestampMs - data[i - 1].TimestampMs) / 1000.0;
                if (dt > 0)
                    accels.Add((data[i].RobotTwist.Linear - data[i - 1].RobotTwist.Linear) / dt);
            }
            pm.AccelerationStdDev = accels.Count > 0 ? CalculateStdDev(accels) : 0;
        }

        // Goal metrics (last sample)
        var last = data[^1];
        pm.GoalPositionError = last.DistanceToGoal;
        pm.GoalHeadingErrorDeg = Math.Abs(last.HeadingError) / Deg2Rad;

        return pm;
    }

    #endregion

    #region Helpers

    private static TuningSuggestion CreateSuggestion(
        string paramPath,
        string displayName,
        double currentValue,
        double suggestedValue,
        string reason,
        SuggestionPriority priority,
        double confidence,
        DiagnosticCategory relatedPattern,
        string expectedImpact,
        TelemetryPhase? targetPhase = null)
    {
        // Round suggested value to reasonable precision
        suggestedValue = Math.Round(suggestedValue, 4);

        // Don't suggest if the change is negligible (< 1%)
        if (currentValue != 0 && Math.Abs(suggestedValue - currentValue) / Math.Abs(currentValue) < 0.01)
            suggestedValue = currentValue; // Will be filtered out

        return new TuningSuggestion
        {
            ParameterPath = paramPath,
            ParameterDisplayName = displayName,
            CurrentValue = currentValue,
            SuggestedValue = suggestedValue,
            Reason = reason,
            Priority = priority,
            Confidence = Math.Clamp(confidence, 0, 1),
            RelatedPatterns = [relatedPattern],
            ExpectedImpact = expectedImpact,
            TargetPhase = targetPhase
        };
    }

    private static double CalculateStdDev(List<double> values)
    {
        if (values.Count == 0) return 0;
        double mean = values.Average();
        double variance = values.Average(v => (v - mean) * (v - mean));
        return Math.Sqrt(variance);
    }

    private static double GetMedian(List<double> values)
    {
        if (values.Count == 0) return 0;
        var sorted = values.OrderBy(v => v).ToList();
        int mid = sorted.Count / 2;
        return sorted.Count % 2 == 0
            ? (sorted[mid - 1] + sorted[mid]) / 2.0
            : sorted[mid];
    }

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

    private static NavigationParameterSet DeepClone(NavigationParameterSet source)
    {
        var json = JsonSerializer.Serialize(source);
        return JsonSerializer.Deserialize<NavigationParameterSet>(json)!;
    }

    private static void SetParameterValue(NavigationParameterSet target, string dotPath, double value)
    {
        var parts = dotPath.Split('.');
        object current = target;

        for (int i = 0; i < parts.Length - 1; i++)
        {
            var prop = current.GetType().GetProperty(parts[i],
                BindingFlags.Public | BindingFlags.Instance);
            if (prop == null)
                throw new ArgumentException($"Property '{parts[i]}' not found on {current.GetType().Name}");
            current = prop.GetValue(current)!;
        }

        var finalProp = current.GetType().GetProperty(parts[^1],
            BindingFlags.Public | BindingFlags.Instance);
        if (finalProp == null)
            throw new ArgumentException($"Property '{parts[^1]}' not found on {current.GetType().Name}");

        finalProp.SetValue(current, value);
    }

    #endregion
}