/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using System.Collections.Concurrent;

namespace CartographerSharp.Mapping.Internal;

/// <summary>
/// Thread-safe work queue for serializing pose graph operations.
/// Similar to Cartographer C++ WorkQueue.
/// 
/// Operations are added to the queue non-blocking, and processed
/// sequentially by a background thread to ensure thread-safety.
/// </summary>
public class WorkQueue : IDisposable
{
    private readonly ConcurrentQueue<WorkItem> _queue = new();
    private readonly Lock _lock = new();
    private readonly CancellationTokenSource _cancellationTokenSource = new();
    private readonly AutoResetEvent _newItemEvent = new(false);
    private readonly ManualResetEvent _optimizationDoneEvent = new(false);
    private Thread? _processingThread;
    private bool _running = true;
    private bool _disposed = false;

    /// <summary>
    /// Event raised when work queue needs optimization.
    /// IMPORTANT: Handler MUST call NotifyOptimizationDone() when optimization completes,
    /// otherwise work queue will be blocked forever.
    /// </summary>
    public event EventHandler? OptimizationNeeded;
    
    /// <summary>
    /// Gets whether the work queue is empty.
    /// </summary>
    public bool IsEmpty => _queue.IsEmpty;
    
    /// <summary>
    /// Gets the number of items in the work queue.
    /// </summary>
    public int Count => _queue.Count;
    
    public WorkQueue()
    {
        StartProcessing();
    }
    
    /// <summary>
    /// Starts background thread to process work queue with high priority.
    /// </summary>
    private void StartProcessing()
    {
        _processingThread = new Thread(() => ProcessWorkQueue(_cancellationTokenSource.Token))
        {
            // IMPORTANT (Linux RT): do NOT run Highest priority here.
            // This thread should never starve the sensor/scan-matching pipeline.
            Priority = ThreadPriority.Highest,
            IsBackground = true,
            Name = "CartographerWorkQueue"
        };
        _processingThread.Start();
    }
    
    /// <summary>
    /// Adds a work item to the queue. Non-blocking and thread-safe.
    /// </summary>
    public void AddWorkItem(WorkItem workItem)
    {
        ObjectDisposedException.ThrowIf(_disposed, this);

        if (!_running)
        {
            throw new InvalidOperationException("WorkQueue is not running");
        }

        _queue.Enqueue(workItem);
        // Wake processing thread if it is waiting.
        _newItemEvent.Set();
    }

    /// <summary>
    /// Notifies work queue that optimization has completed.
    /// This allows the work queue processing thread to resume.
    /// Match C++ behavior: After HandleWorkQueue completes (which runs optimization),
    /// DrainWorkQueue is called again to continue processing.
    /// </summary>
    public void NotifyOptimizationDone()
    {
        _optimizationDoneEvent.Set();
    }
    
    /// <summary>
    /// Processes work items until queue is empty or optimization is needed.
    /// Called by background thread.
    /// Match C++ behavior: When optimization is needed, STOP processing and WAIT
    /// until optimization completes (signaled via NotifyOptimizationDone).
    /// </summary>
    private void ProcessWorkQueue(CancellationToken cancellationToken)
    {
        Thread.BeginThreadAffinity();
        try
        {
            while (!cancellationToken.IsCancellationRequested && _running)
            {
                bool processedAny = false;
                bool optimizationNeeded = false;

                while (_queue.TryDequeue(out var workItem))
                {
                    if (cancellationToken.IsCancellationRequested)
                    {
                        break;
                    }

                    var result = workItem.Action();

                    if (result == WorkItemResult.RunOptimization)
                    {
                        optimizationNeeded = true;
                        // Stop processing to allow optimization to run
                        break;
                    }

                    processedAny = true;
                }

                // Match C++: When optimization needed, STOP work queue and WAIT for optimization to complete
                if (optimizationNeeded)
                {
                    // Reset event before invoking (in case it was set previously)
                    _optimizationDoneEvent.Reset();

                    // Invoke optimization handler synchronously
                    // CRITICAL: Handler MUST call NotifyOptimizationDone() when done
                    OptimizationNeeded?.Invoke(this, EventArgs.Empty);

                    // WAIT for optimization to complete before continuing work queue
                    // This matches C++ behavior where DrainWorkQueue() stops until HandleWorkQueue completes
                    _optimizationDoneEvent.WaitOne();

                    // After optimization completes, continue processing work queue
                    continue;
                }

                // Avoid busy-spinning (especially harmful on RT kernels). If we didn't
                // process anything and no optimization was requested, wait for new work.
                if (!processedAny)
                {
                    // Wakeups happen via AddWorkItem(). Also periodically wake to observe cancellation.
                    // Check cancellation before waiting
                    if (cancellationToken.IsCancellationRequested || !_running)
                    {
                        break;
                    }
                    _newItemEvent.WaitOne(TimeSpan.FromMilliseconds(50));
                }
            }
        }
        catch
        {
            throw;
        }
        finally
        {
            Thread.EndThreadAffinity();
        }
    }
    
    /// <summary>
    /// Drains the work queue synchronously (for testing or final processing).
    /// Match C++: Process work items until queue is empty or optimization is needed.
    ///
    /// If processing thread is alive, waits for queue to empty.
    /// If processing thread is dead, processes remaining items.
    /// </summary>
    public void DrainWorkQueue()
    {
        // If processing thread is still alive, just wait for it to drain the queue
        if (_processingThread?.IsAlive == true)
        {
            // Wait for queue to be empty (processing thread will handle it)
            while (!_queue.IsEmpty)
            {
                Thread.Sleep(10);
            }
            return;
        }

        // Processing thread is dead, we need to drain the queue ourselves
        bool processWorkQueue = true;
        while (processWorkQueue)
        {
            if (!_queue.TryDequeue(out var workItem))
            {
                // Queue is empty
                return;
            }

            var result = workItem.Action();
            // Match C++: Continue processing if kDoNotRunOptimization, stop if kRunOptimization
            processWorkQueue = result == WorkItemResult.DoNotRunOptimization;

            if (result == WorkItemResult.RunOptimization)
            {
                // Signal optimization needed (caller should handle this)
                OptimizationNeeded?.Invoke(this, EventArgs.Empty);
                // Stop processing to allow optimization to run
                // Caller should call DrainWorkQueue() again after optimization
                return;
            }
        }
    }
    
    /// <summary>
    /// Waits for queue to be empty (with timeout).
    /// </summary>
    public void WaitForQueueToEmptyAsync(TimeSpan timeout)
    {
        var startTime = DateTime.UtcNow;
        
        while (DateTime.UtcNow - startTime < timeout)
        {
            if (_queue.IsEmpty)
            {
                // Give a small delay to ensure no new items are being added
                Thread.Sleep(10);
                
                if (_queue.IsEmpty)
                {
                    return;
                }
            }
            
            Thread.Sleep(10);
        }
        
        var remainingCount = _queue.Count;
        if (remainingCount > 0)
        {
            throw new TimeoutException($"Work queue not empty after timeout. Remaining items: {remainingCount}");
        }
    }
    
    public void Stop()
    {
        lock (_lock)
        {
            _running = false;
        }
    }
    
    public void Dispose()
    {
        if (_disposed)
        {
            return;
        }
        
        _running = false;
        _cancellationTokenSource.Cancel();
        
        _newItemEvent.Set(); // wake thread so it can exit promptly
        
        // Wait for thread to finish (with timeout)
        if (_processingThread != null)
        {
            // Check if thread is already finished before joining
            if (_processingThread.IsAlive)
            {
                if (!_processingThread.Join(TimeSpan.FromSeconds(5)))
                {
                    // Thread didn't finish in time, but continue cleanup
                }
            }
        }
        
        // Clear event handlers to prevent memory leaks
        OptimizationNeeded = null;
        
        // Drain remaining work items to prevent memory leaks
        DrainWorkQueue();

        _newItemEvent.Dispose();
        _optimizationDoneEvent.Dispose();
        _cancellationTokenSource.Dispose();
        _disposed = true;

        GC.SuppressFinalize(this);
    }
}