using Microsoft.Extensions.Options;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;
using RobotNet10.Shared.Numbers;
using System.Diagnostics;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Evaluates scan matching quality at a given pose against occupancy grid.
/// Provides reliability [0,1] and MAE metrics without running particle filter.
/// Directly evaluates the quality of pose from CartographerSharp.
/// </summary>
public class ScanMatchingQualityEvaluator : IDisposable
{
    #region Fields and Constructor

    private readonly CartographerConfiguration _config;
    private readonly ILogger<ScanMatchingQualityEvaluator> _logger;
    private readonly Lock _lock = new();

    // Occupancy grid and distance map
    private OccupancyGrid? _occupancyGrid;
    private double[,]? _distanceMap; // Distance to nearest occupied cell (meters)
    private double _mapResolution;
    private double _mapOriginX, _mapOriginY, _mapOriginYaw;
    private int _mapWidth, _mapHeight;

    // Likelihood field model constants
    private double _normConstHit, _denomHit, _measurementModelRandom;

    // Latest scan and pose for periodic evaluation
    private RangeDataPayload? _latestScan;
    private Pose? _latestPose;
    private string? _latestDeviceId;
    private DateTime _lastScanTime = DateTime.MinValue;

    // Periodic evaluation state
    private bool _running;
    private Timer? _periodicTimer;
    private readonly TimeSpan _evaluationInterval;

    // Latest metrics
    private double _reliability = 0.5;
    private double? _mae;
    private DateTime _lastUpdateTime = DateTime.MinValue;

    // Primary lidar ID for filtering
    private string? _primaryLidarId;

    public ScanMatchingQualityEvaluator(
        IOptions<CartographerConfiguration> configuration,
        ILogger<ScanMatchingQualityEvaluator> logger)
    {
        _config = configuration?.Value ?? throw new ArgumentNullException(nameof(configuration));
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));

        // Get evaluation interval from configuration
        _evaluationInterval = TimeSpan.FromSeconds(_config.Mcl.ReliabilityMonitoring.MonitoringIntervalSeconds);
    }

    #endregion

    #region Public API

    /// <summary>
    /// Start periodic scan quality evaluation
    /// </summary>
    public void Start(OccupancyGrid occupancyGrid, Pose? initialPose, string? primaryLidarId)
    {
        lock (_lock)
        {
            if (_running)
            {
                _logger.LogWarning("ScanMatchingQualityEvaluator: Already running, ignoring Start call");
                return;
            }

            try
            {
                _occupancyGrid = occupancyGrid ?? throw new ArgumentNullException(nameof(occupancyGrid));
                _primaryLidarId = primaryLidarId;

                // Build distance map from occupancy grid
                BuildDistanceMap(occupancyGrid);

                // Initialize likelihood field model constants
                InitializeMeasurementModel();

                // Create periodic timer
                _periodicTimer = new Timer(
                    EvaluateScanQuality,
                    null,
                    _evaluationInterval,
                    _evaluationInterval);

                _running = true;
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "ScanMatchingQualityEvaluator: Failed to start");
                _running = false;
                throw;
            }
        }
    }

    /// <summary>
    /// Stop periodic evaluation
    /// </summary>
    public void Stop()
    {
        lock (_lock)
        {
            if (!_running)
            {
                return;
            }

            _running = false;

            _periodicTimer?.Dispose();
            _periodicTimer = null;

            _latestScan = null;
            _latestPose = null;
            _latestDeviceId = null;
            _occupancyGrid = null;
            _distanceMap = null;
        }
    }

    /// <summary>
    /// Cache latest scan and pose for periodic evaluation
    /// </summary>
    public void OnScanReceived(string deviceId, RangeDataPayload payload, Pose? currentPose)
    {
        lock (_lock)
        {
            if (!_running)
            {
                return;
            }

            // Filter by primary lidar if specified
            if (!string.IsNullOrEmpty(_primaryLidarId) && deviceId != _primaryLidarId)
            {
                return;
            }

            _latestScan = payload;
            _latestPose = currentPose;
            _latestDeviceId = deviceId;
            _lastScanTime = DateTime.UtcNow;
        }
    }

    /// <summary>
    /// Get latest evaluation metrics
    /// </summary>
    public EvaluationMetrics GetLatestMetrics()
    {
        lock (_lock)
        {
            return new EvaluationMetrics(_reliability, _mae, _lastUpdateTime);
        }
    }

    #endregion

    #region Distance Map Setup

    /// <summary>
    /// Build distance map from occupancy grid using Felzenszwalb-Huttenlocher distance transform
    /// Distance map stores distance (in meters) to nearest occupied cell for each grid cell
    /// </summary>
    private void BuildDistanceMap(OccupancyGrid grid)
    {
        _mapWidth = grid.Width;
        _mapHeight = grid.Height;
        _mapResolution = grid.Resolution;
        _mapOriginX = grid.Origin.Position.X;
        _mapOriginY = grid.Origin.Position.Y;
        _mapOriginYaw = grid.Origin.Orientation.ToYawRadian();

        // Create binary map: 0 = occupied (100), 1 = free
        var binaryMap = new byte[_mapHeight, _mapWidth];
        for (int v = 0; v < _mapHeight; v++)
        {
            for (int u = 0; u < _mapWidth; u++)
            {
                int idx = v * _mapWidth + u;
                binaryMap[v, u] = grid.Data[idx] == 100 ? (byte)0 : (byte)1;
            }
        }

        // Compute Euclidean distance transform
        _distanceMap = DistanceTransformHelper.ComputeEuclidean(binaryMap, _mapWidth, _mapHeight, _mapResolution);

        _logger.LogDebug("ScanMatchingQualityEvaluator: Built distance map {Width}x{Height} at {Res}m resolution",
            _mapWidth, _mapHeight, _mapResolution);
    }

    #endregion

    #region Measurement Model

    /// <summary>
    /// Initialize measurement model constants (likelihood field model)
    /// </summary>
    private void InitializeMeasurementModel()
    {
        double varHit = _config.Mcl.VarHit;
        double zHit = _config.Mcl.ZHit;
        double zRand = _config.Mcl.ZRand;

        _normConstHit = 1.0 / Math.Sqrt(2.0 * Math.PI * varHit);
        _denomHit = 2.0 * varHit;
        _measurementModelRandom = zRand;
    }

    #endregion

    #region Scan Evaluation

    /// <summary>
    /// Timer callback: Evaluate scan quality at current pose
    /// </summary>
    private void EvaluateScanQuality(object? state)
    {
        try
        {
            // Read cached scan and pose
            RangeDataPayload? scan;
            Pose? pose;
            bool running;

            lock (_lock)
            {
                running = _running;
                scan = _latestScan;
                pose = _latestPose;
            }

            if (!running || scan == null || pose == null)
            {
                return;
            }

            var stopwatch = Stopwatch.StartNew();

            // Convert scan to ranges
            var pointsForScan = scan.SensorFrame.Ranges
                .Select(r => new Vector2(r.Position.X, r.Position.Y))
                .ToList();

            double angleMinDeg = scan.ActualAngleMin * (180.0 / Math.PI);
            double angleMaxDeg = scan.ActualAngleMax * (180.0 / Math.PI);

            // Evaluate scan at current pose
            var result = EvaluateScanAtPose(pose.Value, pointsForScan, angleMinDeg, angleMaxDeg);

            stopwatch.Stop();

            // Update metrics
            lock (_lock)
            {
                if (_running)
                {
                    _reliability = result.Reliability;
                    _mae = result.Mae;
                    _lastUpdateTime = DateTime.UtcNow;

                    _logger.LogDebug(
                        "ScanMatchingQualityEvaluator: reliability={Reliability:F3}, mae={Mae:F4}m, validBeams={Beams}, elapsed={Elapsed}ms",
                        result.Reliability,
                        result.Mae,
                        result.ValidBeams,
                        stopwatch.ElapsedMilliseconds);
                }
            }
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex, "ScanMatchingQualityEvaluator: Evaluation failed, keeping previous metrics");
        }
    }

    /// <summary>
    /// Evaluate scan quality at given pose using likelihood field model
    /// </summary>
    private EvaluationResult EvaluateScanAtPose(Pose pose, List<Vector2> points, double angleMinDeg, double angleMaxDeg)
    {
        if (_distanceMap == null)
        {
            return new EvaluationResult(0.5, 1.0, 0.0, 0);
        }

        double totalError = 0;
        double totalLikelihood = 0;
        int validBeams = 0;

        // Pose in map frame
        double poseX = pose.Position.X;
        double poseY = pose.Position.Y;
        double poseYaw = pose.Orientation.ToYawRadian();

        int numPoints = points.Count;
        double angleRangeDeg = angleMaxDeg - angleMinDeg;
        int scanStep = _config.Mcl.ScanStep;

        for (int i = 0; i < numPoints; i += scanStep)
        {
            var point = points[i];
            double range = Math.Sqrt(point.X * point.X + point.Y * point.Y);

            // Skip invalid ranges
            if (range < _config.TrajectoryBuilder.MinRange || range > _config.TrajectoryBuilder.MaxRange)
            {
                continue;
            }

            // Beam angle in sensor frame
            double beamAngleDeg = angleMinDeg + (angleRangeDeg * i / numPoints);
            double beamAngleRad = beamAngleDeg * (Math.PI / 180.0);

            // Transform beam endpoint to map frame
            double cosYaw = Math.Cos(poseYaw);
            double sinYaw = Math.Sin(poseYaw);
            double beamEndX = poseX + (point.X * cosYaw - point.Y * sinYaw);
            double beamEndY = poseY + (point.X * sinYaw + point.Y * cosYaw);

            // Convert to grid coordinates
            double dx = beamEndX - _mapOriginX;
            double dy = beamEndY - _mapOriginY;
            double cosOrigin = Math.Cos(_mapOriginYaw);
            double sinOrigin = Math.Sin(_mapOriginYaw);
            double gridX = (dx * cosOrigin + dy * sinOrigin) / _mapResolution;
            double gridY = (-dx * sinOrigin + dy * cosOrigin) / _mapResolution;

            int u = (int)Math.Round(gridX);
            int v = (int)Math.Round(gridY);

            // Check bounds
            if (u < 0 || u >= _mapWidth || v < 0 || v >= _mapHeight)
            {
                continue;
            }

            // Get distance to nearest obstacle
            double dist = _distanceMap[v, u];

            // Calculate error (MAE)
            totalError += dist;

            // Calculate likelihood using likelihood field model
            double pHit = _normConstHit * Math.Exp(-(dist * dist) / _denomHit);
            double likelihood = _config.Mcl.ZHit * pHit + _measurementModelRandom;
            totalLikelihood += likelihood;

            validBeams++;
        }

        // Calculate metrics
        double mae = validBeams > 0 ? totalError / validBeams : 1.0;
        double avgLikelihood = validBeams > 0 ? totalLikelihood / validBeams : 0.0;

        // Calculate reliability from MAE and likelihood
        double reliability = CalculateReliability(mae, avgLikelihood);

        return new EvaluationResult(reliability, mae, avgLikelihood, validBeams);
    }

    /// <summary>
    /// Calculate reliability [0,1] from MAE and average likelihood
    /// </summary>
    private double CalculateReliability(double mae, double avgLikelihood)
    {
        // MAE-based component (exponential decay)
        // Good: < 0.05m → 1.0
        // Poor: > 0.5m → ~0.0
        double maeScore = Math.Exp(-10.0 * mae);

        // Likelihood-based component (already normalized 0-1)
        double likelihoodScore = Math.Clamp(avgLikelihood, 0.0, 1.0);

        // Combine: MAE more important (70%), likelihood 30%
        double reliability = maeScore * 0.7 + likelihoodScore * 0.3;

        return Math.Clamp(reliability, 0.0, 1.0);
    }

    #endregion

    #region IDisposable

    public void Dispose()
    {
        Stop();
        GC.SuppressFinalize(this);
    }

    #endregion

    /// <summary>
    /// Evaluation result
    /// </summary>
    private record EvaluationResult(double Reliability, double Mae, double AvgLikelihood, int ValidBeams);

    /// <summary>
    /// Evaluation metrics (public)
    /// </summary>
    public record EvaluationMetrics(double Reliability, double? Mae, DateTime UpdateTime);
}