using CartographerSharp.Sensor;
using Microsoft.Extensions.Options;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Motion;
using RobotNet10.Shared.Geometry;
using System.Collections.Concurrent;
using System.Diagnostics;
using RobotNet10.Shared.Numbers;
using RobotNet10.RobotApp.SLAM.Cartographer;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Sensor pipeline: subscribes to Lidar/IMU/Odometry, queues data, and forwards to caller via actions.
/// Does not reference ITrajectoryBuilder or sample point cloud; CartographerService provides AddRangeData, AddImuData, AddOdometryData.
/// </summary>
internal sealed class SensorPipeline(
    IDeviceProvider _deviceProvider,
    IOdometryEstimator _odometryEstimator,
    IOptions<CartographerConfiguration> configuration,
    ILogger _logger,
    Action<string, RangeDataPayload> _addRangeData,
    Action<string, ImuData> _addImuData,
    Action<string, OdometryData> _addOdometryData) : IDisposable
{
    #region Fields

    private readonly CartographerConfiguration _config = configuration.Value;
    private readonly Lock _lock = new();

    private readonly List<ILidar> _subscribedLidars = [];
    private IInertialMeasurementUnit? _subscribedImu;
    private bool _odometrySubscribed;

    private volatile bool _active;

    private Dictionary<string, LidarSensorConfiguration> _lidarConfigs = [];
    private Dictionary<string, Transform> _lidarTransforms = [];
    private Transform? _imuTransform;

    private const int NotProcessing = 0;
    private const int Processing = 1;

    // Per-device processing flags: each lidar has its own flag
    private readonly ConcurrentDictionary<string, int> _isProcessingLidar = new();
    private long _lastValidEnqueueTimeTicks; // Use Volatile.Read/Write for thread-safe access
    private const int LidarRoundTimeoutMs = 300;

    private volatile List<string> _lidarDeviceOrder = [];

    private struct LidarScanQueueItem
    {
        public string DeviceId { get; set; }
        public LidarScanDataEventArgs ScanData { get; set; }
    }

    private readonly ConcurrentQueue<LidarScanQueueItem> _scanDataQueue = new();

    // Transformed lidar data queue item (output of transform thread, input to processing thread)
    private struct TransformedLidarQueueItem
    {
        public string DeviceId { get; set; }
        public RangeDataPayload Payload { get; set; }
    }

    private readonly ConcurrentQueue<TransformedLidarQueueItem> _transformedQueue = new();

    private struct ImuDataQueueItem
    {
        public string DeviceId { get; set; }
        public Vector3 LinearAcceleration { get; set; }
        public Vector3 AngularVelocity { get; set; }
        public DateTime Timestamp { get; set; }
    }

    private readonly ConcurrentQueue<ImuDataQueueItem> _imuDataQueue = new();

    private Thread? _lidarTransformThread;
    private volatile bool _lidarTransformThreadRunning;

    private Thread? _lidarProcessingThread;
    private volatile bool _lidarProcessingThreadRunning;

    private Thread? _imuProcessingThread;
    private volatile bool _imuThreadRunning;

    private Thread? _odometryProcessingThread;
    private volatile bool _odometryThreadRunning;

    private volatile bool _disposed;

    // ManualResetEventSlim for efficient blocking when queues are empty
    private readonly ManualResetEventSlim _lidarDataAvailable = new(false);
    private readonly ManualResetEventSlim _transformedDataAvailable = new(false);
    private readonly ManualResetEventSlim _imuDataAvailable = new(false);

    // Queue capacity limits (Fix 9)
    private const int MaxScanQueueSize = 5;
    private const int MaxImuQueueSize = 50;

    #endregion

    #region Initialization

    public async Task InitializeAsync()
    {
        await DiscoverAndSubscribeLidarsAsync();
        await SubscribeToImuAsync();
        SubscribeToOdometry();
    }

    public void ResumeSubscriptions()
    {
        lock (_lock)
        {
            if (_active)
            {
                _logger.LogDebug("SensorPipeline.ResumeSubscriptions: Already active, skipping");
                return;
            }

            _logger.LogInformation("SensorPipeline.ResumeSubscriptions: Resuming {LidarCount} lidars", _subscribedLidars.Count);

            foreach (var lidar in _subscribedLidars)
            {
                lidar.ScanDataReceived += OnLidarScanDataReceived;
            }
            _subscribedImu?.ImuDataChanged += OnImuDataChanged;

            if (_config.UseOdometry && _odometrySubscribed)
                StartOdometryProcessingThread();
            StartLidarProcessingThread();
            if (_subscribedImu != null)
                StartImuProcessingThread();

            _active = true;
            _logger.LogInformation("SensorPipeline.ResumeSubscriptions: Sensors resumed, active=true");
        }
    }

    public void PauseSubscriptions()
    {
        lock (_lock)
        {
            if (!_active)
            {
                _logger.LogDebug("SensorPipeline.PauseSubscriptions: Already inactive, skipping");
                return;
            }

            _logger.LogInformation("SensorPipeline.PauseSubscriptions: Pausing sensors");

            foreach (var lidar in _subscribedLidars)
            {
                lidar.ScanDataReceived -= OnLidarScanDataReceived;
            }

            _subscribedImu?.ImuDataChanged -= OnImuDataChanged;

            if (_odometrySubscribed)
                StopOdometryProcessingThread();
            StopLidarProcessingThread();
            if (_subscribedImu != null)
                StopImuProcessingThread();

            _active = false;
            _logger.LogInformation("SensorPipeline.PauseSubscriptions: Sensors paused, active=false");
        }
    }

    #endregion

    #region Subscriptions

    private async Task DiscoverAndSubscribeLidarsAsync()
    {
        var allLidars = await _deviceProvider.GetDevicesByTypeAsync(DeviceType.Lidar);
        var lidarList = allLidars.OfType<ILidar>().ToList();
        if (lidarList.Count == 0)
            throw new InvalidOperationException("No Lidars found in device provider");

        var lidarConfigsDict = new Dictionary<string, LidarSensorConfiguration>();
        var lidarTransformsDict = new Dictionary<string, Transform>();
        foreach (var lidar in lidarList)
        {
            var deviceId = (lidar as DeviceBase)?.DeviceId;
            if (string.IsNullOrEmpty(deviceId))
                continue;
            var sensorConfig = _config.Sensors.Lidars.FirstOrDefault(cfg => cfg.DeviceId == deviceId && cfg.Enabled);
            if (sensorConfig != null)
            {
                lidarConfigsDict[deviceId] = sensorConfig;
                lidarTransformsDict[deviceId] = sensorConfig.Transform;
                _subscribedLidars.Add(lidar);
            }
            else
            {
                // FIXED: Skip lidars not configured in Cartographer.Sensors.Lidars
                // This prevents unintended lidars (e.g., those used only for Detection)
                // from being subscribed to CartographerSharp SLAM pipeline
                _logger.LogDebug("SensorPipeline: Lidar {DeviceId} found but not enabled in Cartographer.Sensors.Lidars config, skipping", deviceId);
            }
        }
        if (_subscribedLidars.Count == 0)
            throw new InvalidOperationException("No Lidars to subscribe to");
        _lidarConfigs = lidarConfigsDict;
        _lidarTransforms = lidarTransformsDict;

        // Initialize lidar device order and per-device processing flags
        _lidarDeviceOrder = [.. _subscribedLidars
            .Select(lidar => (lidar as DeviceBase)?.DeviceId)
            .Where(id => !string.IsNullOrEmpty(id))
            .Cast<string>()];

        foreach (var lidarId in _lidarDeviceOrder)
        {
            _isProcessingLidar[lidarId] = NotProcessing;
        }
    }

    private async Task SubscribeToImuAsync()
    {
        if (!_config.Sensors.Imu.Enabled || string.IsNullOrEmpty(_config.Sensors.Imu.DeviceId))
            return;
        _imuTransform = _config.Sensors.Imu.Transform;
        var imu = await _deviceProvider.GetDeviceAsync(_config.Sensors.Imu.DeviceId);
        if (imu is IInertialMeasurementUnit imuDevice)
            _subscribedImu = imuDevice;
        else
            _logger.LogWarning("SensorPipeline: IMU device {DeviceId} not found or not an IMU", _config.Sensors.Imu.DeviceId);
    }

    private void SubscribeToOdometry()
    {
        if (!_config.UseOdometry || _odometrySubscribed)
            return;
        _odometrySubscribed = true;
    }

    #endregion

    #region Lidar Processing

    private void StartLidarProcessingThread()
    {
        if (_lidarTransformThreadRunning || _lidarProcessingThreadRunning)
            return;

        // Start transform thread
        _lidarTransformThreadRunning = true;
        _lidarTransformThread = new Thread(LidarTransformThreadProc) { Name = "LidarTransform", Priority = ThreadPriority.Highest, IsBackground = true };
        _lidarTransformThread.Start();

        // Start processing thread
        _lidarProcessingThreadRunning = true;
        _lidarProcessingThread = new Thread(LidarProcessingThreadProc) { Name = "LidarProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
        _lidarProcessingThread.Start();
    }

    private void StopLidarProcessingThread()
    {
        // Stop transform thread
        if (_lidarTransformThreadRunning)
        {
            _lidarTransformThreadRunning = false;
            _lidarDataAvailable.Set(); // Wake thread to check flag
            if (_lidarTransformThread != null)
            {
                _lidarTransformThread.Join(2000);
                if (_lidarTransformThread.IsAlive)
                    _logger.LogWarning("SensorPipeline: Lidar transform thread did not stop in time");
                _lidarTransformThread = null;
            }
        }

        // Stop processing thread
        if (_lidarProcessingThreadRunning)
        {
            _lidarProcessingThreadRunning = false;
            _transformedDataAvailable.Set(); // Wake thread to check flag
            if (_lidarProcessingThread != null)
            {
                _lidarProcessingThread.Join(2000);
                if (_lidarProcessingThread.IsAlive)
                    _logger.LogWarning("SensorPipeline: Lidar processing thread did not stop in time");
                _lidarProcessingThread = null;
            }
        }
    }

    /// <summary>
    /// Transform thread: dequeues raw scan data, transforms to RangeDataPayload, enqueues to transformed queue.
    /// Does NOT check _isProcessingLidar flags - just processes everything in _scanDataQueue.
    /// </summary>
    private void LidarTransformThreadProc()
    {
        Thread.BeginThreadAffinity();
        while (_lidarTransformThreadRunning)
        {
            _lidarDataAvailable.Wait(50); // timeout to check _lidarTransformThreadRunning
            _lidarDataAvailable.Reset();

            if (_scanDataQueue.IsEmpty)
                continue;

            while (_scanDataQueue.TryDequeue(out var queueItem))
            {
                if (_disposed || !_active ||
                    !_lidarConfigs.TryGetValue(queueItem.DeviceId, out var lidarConfig) ||
                    !_lidarTransforms.TryGetValue(queueItem.DeviceId, out var lidarTransform))
                    continue;

                try
                {
                    var (baseLink, sensorFrame, actualAngleMin, actualAngleMax, angleIncrement) = SensorDataTransformHelper.ToTimedPointCloudDataBaseAndSensorFrame(
                        queueItem.ScanData.Timestamp, queueItem.ScanData.MeasurementData,
                        lidarTransform, lidarConfig.AngleMin, lidarConfig.AngleMax);

                    _transformedQueue.Enqueue(new TransformedLidarQueueItem
                    {
                        DeviceId = queueItem.DeviceId,
                        Payload = new RangeDataPayload
                        {
                            BaseLink = baseLink,
                            SensorFrame = sensorFrame,
                            ActualAngleMin = actualAngleMin,
                            ActualAngleMax = actualAngleMax,
                            ActualAngleIncrement = angleIncrement
                        }
                    });
                    _transformedDataAvailable.Set();
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "SensorPipeline: Error transforming Lidar from {DeviceId}", queueItem.DeviceId);
                }
            }
        }
        Thread.EndThreadAffinity();
    }

    /// <summary>
    /// Processing thread: dequeues transformed data, calls _addRangeData, handles round tracking and timeout.
    /// </summary>
    private void LidarProcessingThreadProc()
    {
        Thread.BeginThreadAffinity();
        var processedLidarsInRound = new HashSet<string>();
        var expectedLidarCount = _lidarDeviceOrder.Count;

        while (_lidarProcessingThreadRunning)
        {
            _transformedDataAvailable.Wait(50); // timeout to check _lidarProcessingThreadRunning
            _transformedDataAvailable.Reset();

            if (_transformedQueue.IsEmpty)
            {
                // Check timeout if we're in the middle of a round (at least one lidar processed)
                if (processedLidarsInRound.Count > 0)
                {
                    var lastEnqueueTicks = Volatile.Read(ref _lastValidEnqueueTimeTicks);
                    var elapsedMs = (DateTime.UtcNow.Ticks - lastEnqueueTicks) / TimeSpan.TicksPerMillisecond;
                    if (elapsedMs >= LidarRoundTimeoutMs)
                    {
                        var missingLidars = _lidarDeviceOrder.Except(processedLidarsInRound).ToList();
                        _logger.LogError(
                            "SensorPipeline: Lidar round timeout after {ElapsedMs}ms. Processed {ProcessedCount}/{ExpectedCount} lidars. Processed: [{ProcessedList}]. Missing: [{MissingList}]",
                            elapsedMs, processedLidarsInRound.Count, expectedLidarCount,
                            string.Join(", ", processedLidarsInRound),
                            string.Join(", ", missingLidars));
                        throw new TimeoutException($"SensorPipeline: Lidar round timeout - processed {processedLidarsInRound.Count}/{expectedLidarCount} lidars, missing: [{string.Join(", ", missingLidars)}]");
                    }
                }
                continue;
            }

            while (_transformedQueue.TryDequeue(out var queueItem))
            {
                if (_disposed || !_active)
                    continue;

                try
                {
                    _addRangeData(queueItem.DeviceId, queueItem.Payload);

                    // Track processed lidar in this round
                    processedLidarsInRound.Add(queueItem.DeviceId);

                    // Check if all lidars have been processed in this round
                    if (processedLidarsInRound.Count >= expectedLidarCount)
                    {
                        // Reset all per-device flags to allow new round
                        foreach (var lidarId in _lidarDeviceOrder)
                        {
                            _isProcessingLidar[lidarId] = NotProcessing;
                        }
                        processedLidarsInRound.Clear();
                    }
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "SensorPipeline: Error processing Lidar from {DeviceId}", queueItem.DeviceId);
                }
            }
        }
        Thread.EndThreadAffinity();
    }

    #endregion

    #region IMU Processing

    private void StartImuProcessingThread()
    {
        if (_imuThreadRunning)
            return;
        _imuThreadRunning = true;
        _imuProcessingThread = new Thread(ImuProcessingThreadProc) { Name = "ImuProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
        _imuProcessingThread.Start();
    }

    private void StopImuProcessingThread()
    {
        if (!_imuThreadRunning)
            return;
        _imuThreadRunning = false;
        _imuDataAvailable.Set(); // Wake thread to check flag
        if (_imuProcessingThread != null)
        {
            _imuProcessingThread.Join(2000);
            if (_imuProcessingThread.IsAlive)
                _logger.LogWarning("SensorPipeline: IMU processing thread did not stop in time");
            _imuProcessingThread = null;
        }
    }

    private void ImuProcessingThreadProc()
    {
        Thread.BeginThreadAffinity();
        var imuUpdateIntervalMs = _config.Sensors.ImuUpdateIntervalMs;
        var imuMinIntervalTicks = imuUpdateIntervalMs * TimeSpan.TicksPerMillisecond;
        var lastProcessTimeTicks = 0L;
        while (_imuThreadRunning)
        {
            _imuDataAvailable.Wait(50); // timeout to check _imuThreadRunning
            _imuDataAvailable.Reset();

            while (_imuDataQueue.TryDequeue(out var queueItem))
            {
                if (_disposed || !_active || _imuTransform == null)
                    continue;

                // Rate limiting: skip if too soon since last process (when interval > 0)
                var currentTicks = queueItem.Timestamp.Ticks;
                if (imuMinIntervalTicks > 0 && lastProcessTimeTicks > 0 &&
                    currentTicks - lastProcessTimeTicks < imuMinIntervalTicks)
                    continue;

                try
                {
                    var imuData = SensorDataTransformHelper.ToImuData(
                        queueItem.LinearAcceleration, queueItem.AngularVelocity, queueItem.Timestamp, _imuTransform.Value);
                    _addImuData(queueItem.DeviceId, imuData);
                    lastProcessTimeTicks = currentTicks;
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "SensorPipeline: Error processing IMU from {DeviceId}", queueItem.DeviceId);
                }
            }
        }
        Thread.EndThreadAffinity();
    }

    #endregion

    #region Odometry Processing

    private void StartOdometryProcessingThread()
    {
        if (_odometryThreadRunning)
            return;
        _odometryThreadRunning = true;
        _odometryProcessingThread = new Thread(OdometryProcessingThreadProc) { Name = "OdometryProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
        _odometryProcessingThread.Start();
    }

    private void StopOdometryProcessingThread()
    {
        if (!_odometryThreadRunning)
            return;
        _odometryThreadRunning = false;
        if (_odometryProcessingThread != null)
        {
            _odometryProcessingThread.Join(1000);
            if (_odometryProcessingThread.IsAlive)
                _logger.LogWarning("SensorPipeline: Odometry processing thread did not stop in time");
            _odometryProcessingThread = null;
        }
    }

    private void OdometryProcessingThreadProc()
    {
        Thread.BeginThreadAffinity();
        try
        {
            var spinWait = new SpinWait();
            var stopwatch = Stopwatch.StartNew();
            var odometryUpdateIntervalMs = _config.Sensors.OdometryUpdateIntervalMs;
            var targetTicksPerInterval = odometryUpdateIntervalMs * Stopwatch.Frequency / 1000;
            var lastProcessTimeTicks = stopwatch.ElapsedTicks;
            while (_odometryThreadRunning)
            {
                var currentTicks = stopwatch.ElapsedTicks;
                var remaining = targetTicksPerInterval - (currentTicks - lastProcessTimeTicks);
                if (remaining > Stopwatch.Frequency / 100) // > 10ms remaining
                    Thread.Sleep(1);
                else
                    spinWait.SpinOnce();

                currentTicks = stopwatch.ElapsedTicks;
                if (currentTicks - lastProcessTimeTicks >= targetTicksPerInterval)
                {
                    if (!_disposed && _active)
                    {
                        try
                        {
                            var currentPose = _odometryEstimator.CurrentPose;
                            var currentTimeTicks = DateTime.UtcNow.Ticks;

                            // Extract theta from quaternion
                            var currentTheta = currentPose.Orientation.ToYawRadian();
                            var currentX = currentPose.Position.X;
                            var currentY = currentPose.Position.Y;

                            var odometryData = SensorDataTransformHelper.ToOdometryData(currentPose, currentTimeTicks);
                            _addOdometryData("odometry", odometryData);
                        }
                        catch (Exception ex)
                        {
                            _logger.LogError(ex, "SensorPipeline: Error processing odometry at {Ticks}", DateTime.UtcNow.Ticks);
                        }
                    }
                    lastProcessTimeTicks = currentTicks;
                }
            }
        }
        finally
        {
            Thread.EndThreadAffinity();
        }
    }

    #endregion

    #region Event Handlers

    private void OnLidarScanDataReceived(object? sender, LidarScanDataEventArgs e)
    {
        if (_disposed || !_active || sender is not DeviceBase device)
            return;
        if (_scanDataQueue.Count >= MaxScanQueueSize)
            return; // Drop: processing can't keep up

        var deviceId = device.DeviceId;

        // Check if this lidar already has data in the current round
        if (_isProcessingLidar.TryGetValue(deviceId, out var status) && status == Processing)
            return; // Drop: this lidar already has data in current round

        // Set flag to indicate this lidar has data in current round
        _isProcessingLidar[deviceId] = Processing;

        try
        {
            _scanDataQueue.Enqueue(new LidarScanQueueItem { DeviceId = deviceId, ScanData = e });
            Volatile.Write(ref _lastValidEnqueueTimeTicks, DateTime.UtcNow.Ticks);
            _lidarDataAvailable.Set();
        }
        catch (Exception ex)
        {
            // Reset flag on error so this lidar can try again
            _isProcessingLidar[deviceId] = NotProcessing;
            _logger.LogError(ex, "SensorPipeline: Error enqueueing Lidar from {DeviceId}", deviceId);
        }
    }

    private void OnImuDataChanged(object? sender, ImuDataChangedEventArgs e)
    {
        if (sender is not DeviceBase device || _disposed || string.IsNullOrEmpty(device.DeviceId) || _imuTransform == null || !_active)
            return;
        if (_imuDataQueue.Count >= MaxImuQueueSize)
            return; // Drop: processing can't keep up
        try
        {
            _imuDataQueue.Enqueue(new ImuDataQueueItem
            {
                DeviceId = device.DeviceId,
                LinearAcceleration = e.Acceleration.Accel.Linear,
                AngularVelocity = e.AngularVelocity.Vector,
                Timestamp = e.Timestamp
            });
            _imuDataAvailable.Set();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "SensorPipeline: Error enqueueing IMU from {DeviceId}", device.DeviceId);
        }
    }

    #endregion

    #region Processing Status

    /// <summary>
    /// Wait for all queued data to be processed, with timeout.
    /// Returns true if all queues are drained within the timeout.
    /// </summary>
    public bool WaitForProcessingComplete(int timeoutMs = 200)
    {
        var sw = Stopwatch.StartNew();
        var spin = new SpinWait();
        while (sw.ElapsedMilliseconds < timeoutMs)
        {
            // Check if all queues are empty and no lidar is being processed
            var allLidarsNotProcessing = _isProcessingLidar.Values.All(status => status == NotProcessing);
            if (_scanDataQueue.IsEmpty && _transformedQueue.IsEmpty && _imuDataQueue.IsEmpty && allLidarsNotProcessing)
                return true;
            spin.SpinOnce();
        }
        return false;
    }

    #endregion

    #region IDisposable

    public void Dispose()
    {
        if (_disposed)
            return;
        _disposed = true;
        try
        {
            PauseSubscriptions();
            foreach (var lidar in _subscribedLidars)
                lidar.ScanDataReceived -= OnLidarScanDataReceived;
            _subscribedLidars.Clear();
            _subscribedImu?.ImuDataChanged -= OnImuDataChanged;
            _subscribedImu = null;
            if (_odometrySubscribed)
            {
                StopOdometryProcessingThread();
                _odometrySubscribed = false;
            }
            StopLidarProcessingThread();
            StopImuProcessingThread();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "SensorPipeline: Error during Dispose");
        }
        finally
        {
            _lidarDataAvailable.Dispose();
            _transformedDataAvailable.Dispose();
            _imuDataAvailable.Dispose();
            GC.SuppressFinalize(this);
        }
    }

    #endregion
}