using RobotNet10.NavigationTune.Shared.Interfaces;
using RobotNet10.NavigationTune.Shared.Models;

namespace RobotNet10.NavigationTune.Services;

/// <summary>
/// Metrics calculator implementation
/// </summary>
public class MetricsCalculator : IMetricsCalculator
{
    public TestMetrics CalculateMetrics(
        List<TelemetryData> telemetryData,
        ReferencePath referencePath)
    {
        if (telemetryData.Count == 0)
            throw new ArgumentException("Telemetry data cannot be empty", nameof(telemetryData));

        var tracking = CalculateTrackingAccuracy(telemetryData, referencePath);
        var smoothness = CalculateSmoothness(telemetryData);
        var efficiency = CalculateEfficiency(telemetryData, referencePath);

        var metrics = new TestMetrics
        {
            // Tracking Accuracy
            CrossTrackErrorRMS = tracking.CrossTrackErrorRMS,
            CrossTrackErrorPeak = tracking.CrossTrackErrorPeak,
            CrossTrackErrorMean = tracking.CrossTrackErrorMean,
            CrossTrackErrorStdDev = tracking.CrossTrackErrorStdDev,
            HeadingErrorRMS = tracking.HeadingErrorRMS,
            HeadingErrorPeak = tracking.HeadingErrorPeak,
            GoalPositionError = tracking.GoalPositionError,
            GoalHeadingError = tracking.GoalHeadingError,

            // Smoothness
            VelocityStdDev = smoothness.VelocityStdDev,
            AccelerationStdDev = smoothness.AccelerationStdDev,

            // Efficiency
            PathLengthRatio = efficiency.PathLengthRatio,
            CompletionTime = efficiency.CompletionTime,
            AverageSpeed = efficiency.AverageSpeed,
            MaxSpeed = efficiency.MaxSpeed
        };

        // Calculate scores
        var weights = new ScoringWeights();
        metrics.TrackingScore = CalculateTrackingScore(tracking);
        metrics.SmoothnessScore = CalculateSmoothnessScore(smoothness);
        metrics.EfficiencyScore = CalculateEfficiencyScore(efficiency);
        metrics.OverallScore = CalculateOverallScore(metrics, weights);

        // Check if passed criteria
        metrics.PassedCriteria = CheckAcceptanceCriteria(metrics);

        // Ensure no NaN/Infinity so SignalR and DB serialization do not fail
        SanitizeMetrics(metrics);
        return metrics;
    }

    private static double ToFinite(double value, double fallback = 0)
    {
        return double.IsFinite(value) ? value : fallback;
    }

    private static void SanitizeMetrics(TestMetrics m)
    {
        m.CrossTrackErrorRMS = ToFinite(m.CrossTrackErrorRMS);
        m.CrossTrackErrorPeak = ToFinite(m.CrossTrackErrorPeak);
        m.CrossTrackErrorMean = ToFinite(m.CrossTrackErrorMean);
        m.CrossTrackErrorStdDev = ToFinite(m.CrossTrackErrorStdDev);
        m.HeadingErrorRMS = ToFinite(m.HeadingErrorRMS);
        m.HeadingErrorPeak = ToFinite(m.HeadingErrorPeak);
        m.GoalPositionError = ToFinite(m.GoalPositionError);
        m.GoalHeadingError = ToFinite(m.GoalHeadingError);
        m.VelocityStdDev = ToFinite(m.VelocityStdDev);
        m.AccelerationStdDev = ToFinite(m.AccelerationStdDev);
        m.PathLengthRatio = ToFinite(m.PathLengthRatio, 1);
        m.CompletionTime = ToFinite(m.CompletionTime);
        m.AverageSpeed = ToFinite(m.AverageSpeed);
        m.MaxSpeed = ToFinite(m.MaxSpeed);
        m.OverallScore = ToFinite(m.OverallScore);
        m.TrackingScore = ToFinite(m.TrackingScore);
        m.SmoothnessScore = ToFinite(m.SmoothnessScore);
        m.EfficiencyScore = ToFinite(m.EfficiencyScore);
    }

    public TrackingAccuracyMetrics CalculateTrackingAccuracy(
        List<TelemetryData> telemetryData,
        ReferencePath referencePath)
    {
        var cteValues = new List<double>();
        var headingErrors = new List<double>();

        foreach (var data in telemetryData)
        {
            cteValues.Add(data.CrossTrackError);
            headingErrors.Add(Math.Abs(data.HeadingError));
        }

        // Calculate RMS
        double cteRMS = CalculateRMS(cteValues);
        double headingRMS = CalculateRMS(headingErrors);

        // Goal accuracy (last 10% of data)
        int goalSampleCount = Math.Max(1, telemetryData.Count / 10);
        var finalData = telemetryData.TakeLast(goalSampleCount).ToList();
        double goalPositionError = finalData.Average(d => d.DistanceToGoal);
        double goalHeadingError = finalData.Average(d => Math.Abs(d.HeadingError));

        return new TrackingAccuracyMetrics
        {
            CrossTrackErrorRMS = cteRMS,
            CrossTrackErrorPeak = cteValues.Max(),
            CrossTrackErrorMean = cteValues.Average(),
            CrossTrackErrorStdDev = CalculateStdDev(cteValues),
            HeadingErrorRMS = headingRMS,
            HeadingErrorPeak = headingErrors.Max(),
            GoalPositionError = goalPositionError,
            GoalHeadingError = goalHeadingError
        };
    }

    /// <summary>
    /// Nominal control loop period (50Hz) in seconds.
    /// </summary>
    private const double NominalDtSeconds = 1.0 / 50.0;

    /// <summary>
    /// Max dt (s) for smoothness calculation.
    /// </summary>
    private const double MaxDtSeconds = 0.5;

    /// <summary>
    /// Percentage of samples to trim from start/end to remove transient periods (startup/shutdown).
    /// 5% means skip first 5% and last 5% of trajectory.
    /// </summary>
    private const double TransientTrimPercent = 0.05f;

    /// <summary>
    /// Maximum physically plausible acceleration for the robot (m/s²).
    /// Velocity changes exceeding this per sample are considered outliers.
    /// </summary>
    private const double MaxPlausibleAcceleration = 3.0;

    /// <summary>
    /// Threshold multiplier for spike detection.
    /// A point is considered a spike if it deviates from neighbors by more than
    /// SpikeThresholdMultiplier * median_change_of_neighbors.
    /// </summary>
    private const double SpikeThresholdMultiplier = 3.0;

    /// <summary>
    /// Minimum absolute deviation (m/s) to consider as potential spike.
    /// Prevents small natural variations from being filtered.
    /// </summary>
    private const double MinSpikeDeviation = 0.02f;

    public SmoothnessMetrics CalculateSmoothness(List<TelemetryData> telemetryData)
    {
        if (telemetryData.Count < 3)
            return new SmoothnessMetrics();

        // Step 1: Trim transient periods (startup/shutdown)
        var stableData = TrimTransientPeriod(telemetryData, TransientTrimPercent);
        if (stableData.Count < 3)
            return new SmoothnessMetrics();

        // Step 2: Calculate dt from stable data
        long totalSpanMs = stableData[^1].TimestampMs - stableData[0].TimestampMs;
        int intervalCount = stableData.Count - 1;
        double avgDtSeconds = intervalCount > 0 && totalSpanMs > 0
            ? (totalSpanMs / 1000.0) / intervalCount
            : NominalDtSeconds;
        double dt = Math.Clamp(avgDtSeconds, NominalDtSeconds, MaxDtSeconds);

        // Step 3: Extract and clean velocity data (multi-stage filtering)
        var rawVelocities = stableData.Select(d => d.RobotTwist.Linear).ToList();

        // Stage 3a: Remove single-cycle spikes first (noise from sensor glitches)
        var despikedVelocities = RemoveSingleCycleSpikes(rawVelocities);

        // Stage 3b: Remove remaining outliers using acceleration-based detection
        var velocities = RemoveVelocityOutliers(despikedVelocities, dt, MaxPlausibleAcceleration);

        // Step 4: Calculate accelerations (linear: m/s²)
        var accelerations = new List<double>();
        for (int i = 1; i < velocities.Count; i++)
        {
            double accel = (velocities[i] - velocities[i - 1]) / dt;
            accelerations.Add(accel);
        }

        // Step 5: Compute standard deviations
        return new SmoothnessMetrics
        {
            VelocityStdDev = CalculateStdDev(velocities),
            AccelerationStdDev = accelerations.Count > 0 ? CalculateStdDev(accelerations) : 0
        };
    }

    /// <summary>
    /// Trim transient periods from start and end of trajectory.
    /// Transient periods (startup/shutdown) naturally have high velocity/acceleration variance.
    /// </summary>
    private static List<TelemetryData> TrimTransientPeriod(List<TelemetryData> data, double trimPercent)
    {
        if (data.Count < 10) return data; // Too short to trim

        int trimCount = Math.Max(1, (int)(data.Count * trimPercent));
        int startIndex = trimCount;
        int endIndex = data.Count - trimCount;

        if (endIndex <= startIndex) return data; // Would result in empty list

        return data.Skip(startIndex).Take(endIndex - startIndex).ToList();
    }

    /// <summary>
    /// Remove single-cycle spikes from velocity data using multi-pass filtering.
    /// A spike is detected when a single point deviates significantly from both neighbors,
    /// while the neighbors themselves are consistent with each other.
    ///
    /// Detection criteria for point i:
    /// 1. |v[i] - v[i-1]| > threshold (large jump from previous)
    /// 2. |v[i] - v[i+1]| > threshold (large jump to next)
    /// 3. |v[i+1] - v[i-1]| <= threshold (neighbors are consistent)
    ///
    /// When spike is detected, replace with average of neighbors.
    /// Multi-pass ensures consecutive spikes are also handled.
    /// </summary>
    private static List<double> RemoveSingleCycleSpikes(List<double> velocities)
    {
        if (velocities.Count < 3)
            return [.. velocities];

        var current = velocities;
        const int maxPasses = 3; // Multiple passes for consecutive spikes

        for (int pass = 0; pass < maxPasses; pass++)
        {
            var cleaned = RemoveSingleCycleSpikesOnePass(current);

            // Check if any changes were made
            bool changed = false;
            for (int i = 0; i < current.Count && !changed; i++)
            {
                if (Math.Abs(current[i] - cleaned[i]) > 1e-9)
                    changed = true;
            }

            current = cleaned;
            if (!changed) break; // No more spikes found
        }

        return current;
    }

    /// <summary>
    /// Single pass of spike removal.
    /// </summary>
    private static List<double> RemoveSingleCycleSpikesOnePass(List<double> velocities)
    {
        var cleaned = new List<double>(velocities.Count) { velocities[0] };

        // Calculate median absolute change for adaptive threshold
        var changes = new List<double>();
        for (int i = 1; i < velocities.Count; i++)
        {
            double change = Math.Abs(velocities[i] - velocities[i - 1]);
            if (change > 1e-9) // Ignore zero changes
                changes.Add(change);
        }

        double medianChange = changes.Count > 0 ? GetMedian(changes) : 0.01f;
        double spikeThreshold = Math.Max(SpikeThresholdMultiplier * medianChange, MinSpikeDeviation);

        // Process middle points using 3-point window
        for (int i = 1; i < velocities.Count - 1; i++)
        {
            double prev = cleaned[^1]; // Use already-cleaned previous value
            double curr = velocities[i];
            double next = velocities[i + 1];

            double changeToPrev = Math.Abs(curr - prev);
            double changeToNext = Math.Abs(curr - next);
            double neighborConsistency = Math.Abs(next - prev);

            // Spike detection: current deviates from both neighbors, but neighbors are consistent
            bool isSpike = changeToPrev > spikeThreshold &&
                          changeToNext > spikeThreshold &&
                          neighborConsistency <= spikeThreshold;

            if (isSpike)
            {
                // Replace spike with average of neighbors
                cleaned.Add((prev + next) / 2.0);
            }
            else
            {
                cleaned.Add(curr);
            }
        }

        cleaned.Add(velocities[^1]); // Keep last point

        return cleaned;
    }

    /// <summary>
    /// Calculate median of a list.
    /// </summary>
    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;

        if (sorted.Count % 2 == 0)
            return (sorted[mid - 1] + sorted[mid]) / 2.0;
        else
            return sorted[mid];
    }

    /// <summary>
    /// Remove velocity outliers using acceleration-based detection.
    /// If velocity change between consecutive samples exceeds physically plausible acceleration,
    /// the point is considered an outlier and interpolated.
    /// </summary>
    private static List<double> RemoveVelocityOutliers(List<double> velocities, double dt, double maxAcceleration)
    {
        if (velocities.Count < 2) return velocities;

        var cleaned = new List<double>(velocities.Count) { velocities[0] };
        double maxVelocityChange = maxAcceleration * dt;

        for (int i = 1; i < velocities.Count; i++)
        {
            double change = Math.Abs(velocities[i] - cleaned[^1]);

            if (change <= maxVelocityChange)
            {
                // Normal change, keep the value
                cleaned.Add(velocities[i]);
            }
            else
            {
                // Outlier detected - use linear interpolation
                // Look ahead to find next valid point
                double interpolatedValue = InterpolateOutlier(velocities, cleaned, i, maxVelocityChange);
                cleaned.Add(interpolatedValue);
            }
        }

        return cleaned;
    }

    /// <summary>
    /// Interpolate an outlier value by looking at surrounding valid points.
    /// </summary>
    private static double InterpolateOutlier(List<double> original, List<double> cleaned, int outlierIndex, double maxChange)
    {
        double lastValid = cleaned[^1];

        // Look ahead to find next valid point (within 5 samples)
        for (int lookAhead = 1; lookAhead <= Math.Min(5, original.Count - outlierIndex - 1); lookAhead++)
        {
            int nextIndex = outlierIndex + lookAhead;
            double nextValue = original[nextIndex];
            double totalChange = Math.Abs(nextValue - lastValid);
            double allowedChange = maxChange * (lookAhead + 1);

            if (totalChange <= allowedChange)
            {
                // Found a valid point - interpolate linearly
                double step = (nextValue - lastValid) / (lookAhead + 1);
                return lastValid + step;
            }
        }

        // No valid point found - use last valid value (hold)
        return lastValid;
    }

    public EfficiencyMetrics CalculateEfficiency(
        List<TelemetryData> telemetryData,
        ReferencePath referencePath)
    {
        if (telemetryData.Count < 2)
            return new EfficiencyMetrics();

        // Calculate actual path length
        double actualPathLength = 0;
        for (int i = 1; i < telemetryData.Count; i++)
        {
            double dx = telemetryData[i].RobotPose.X - telemetryData[i - 1].RobotPose.X;
            double dy = telemetryData[i].RobotPose.Y - telemetryData[i - 1].RobotPose.Y;
            actualPathLength += Math.Sqrt(dx * dx + dy * dy);
        }

        // Reference path length
        double referencePathLength = referencePath.TotalLength;

        // Completion time
        long duration = telemetryData[^1].TimestampMs - telemetryData[0].TimestampMs;
        double completionTime = duration / 1000.0;

        // Speeds
        var speeds = telemetryData.Select(d => Math.Abs(d.RobotTwist.Linear)).ToList();

        return new EfficiencyMetrics
        {
            PathLengthRatio = referencePathLength > 0 ? actualPathLength / referencePathLength : 1.0,
            CompletionTime = completionTime,
            AverageSpeed = speeds.Average(),
            MaxSpeed = speeds.Max()
        };
    }

    public double CalculateOverallScore(TestMetrics metrics, ScoringWeights weights)
    {
        double score = 100.0;

        // Tracking accuracy penalties (50% weight)
        score -= weights.TrackingAccuracy * (
            NormalizePenalty(metrics.CrossTrackErrorRMS, 0.10f, 20f) +
            NormalizePenalty(metrics.HeadingErrorRMS, 10f * Deg2Rad, 20f) +
            NormalizePenalty(metrics.GoalPositionError, 0.05f, 10f)
        );

        // Smoothness penalties (30% weight)
        score -= weights.Smoothness * (
            NormalizePenalty(metrics.VelocityStdDev, 0.1, 15f) +
            NormalizePenalty(metrics.AccelerationStdDev, 0.5, 15f)
        );

        // Efficiency penalties (20% weight)
        score -= weights.Efficiency * (
            NormalizePenalty(metrics.PathLengthRatio - 1.0, 0.15f, 20f)
        );

        return Math.Max(0, score);
    }

    private double CalculateTrackingScore(TrackingAccuracyMetrics tracking)
    {
        double score = 100.0;
        score -= NormalizePenalty(tracking.CrossTrackErrorRMS, 0.10f, 40f);
        score -= NormalizePenalty(tracking.HeadingErrorRMS, 10f * Deg2Rad, 40f);
        score -= NormalizePenalty(tracking.GoalPositionError, 0.05f, 20f);
        return Math.Max(0, score);
    }

    private double CalculateSmoothnessScore(SmoothnessMetrics smoothness)
    {
        double score = 100.0;
        score -= NormalizePenalty(smoothness.VelocityStdDev, 0.1, 50f);
        score -= NormalizePenalty(smoothness.AccelerationStdDev, 0.5, 50f);
        return Math.Max(0, score);
    }

    private double CalculateEfficiencyScore(EfficiencyMetrics efficiency)
    {
        double score = 100.0;
        score -= NormalizePenalty(efficiency.PathLengthRatio - 1.0, 0.15f, 100f);
        return Math.Max(0, score);
    }

    private bool CheckAcceptanceCriteria(TestMetrics metrics)
    {
        // Primary criteria (tracking)
        if (metrics.CrossTrackErrorRMS > 0.10f) return false;
        if (metrics.CrossTrackErrorPeak > 0.20f) return false;
        if (metrics.HeadingErrorRMS > 10f * Deg2Rad) return false;
        if (metrics.GoalPositionError > 0.05f) return false;

        // Secondary criteria (efficiency)
        if (metrics.PathLengthRatio > 1.15f) return false;

        return true;
    }

    private double NormalizePenalty(double actual, double threshold, double maxPenalty)
    {
        if (!double.IsFinite(actual)) return maxPenalty;
        if (actual <= threshold) return 0;

        double excess = actual - threshold;
        double penalty = (excess / threshold) * maxPenalty;
        return Math.Min(penalty, maxPenalty);
    }

    private double CalculateRMS(List<double> values)
    {
        if (values.Count == 0) return 0;
        double sumSquares = values.Sum(v => v * v);
        return Math.Sqrt(sumSquares / values.Count);
    }

    private 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 const double Deg2Rad = Math.PI / 180.0;
}