using System.Collections.Concurrent;
using System.Diagnostics;
using System.Net;
using System.Net.Sockets;

namespace Olei.LidarSensor;

/// <summary>
/// High-performance UDP server for receiving and parsing Olei LiDAR data
/// Uses Socket API with pre-allocated buffers for zero-allocation receive
/// Optimized for low latency and minimal memory allocation
/// </summary>
public sealed class OleiLidarServer : IDisposable
{
    private readonly int _port;
    private Socket? _socket;
    private Thread? _receiveThread;
    private volatile bool _isRunning;
    private readonly Lock _lock = new();

    // Pre-allocated receive buffer (reused across all receives)
    private byte[]? _receiveBuffer;
    private const int RECEIVE_BUFFER_SIZE = LidarDataPacket.PACKET_SIZE * 16;  // Buffer for multiple packets

    // Packet pool for reuse
    private readonly ConcurrentBag<LidarDataPacket> _packetPool;
    private const int INITIAL_PACKET_POOL_SIZE = 10;
    private const int MAX_PACKET_POOL_SIZE = 50;

    // Pool size tracking (atomic counter for thread-safe accurate tracking)
    private long _packetPoolCount;

    // Statistics
    private long _totalPacketsReceived;
    private long _totalPacketsParsed;
    private long _totalParseErrors;
    private long _totalPacketsCreated;

    // Scan frequency calculation (optimized - update only once per second)
    private readonly Stopwatch _frequencyStopwatch = new();
    private long _packetsReceivedInCurrentSecond;
    private long _lastFrequencyUpdateMs;

    /// <summary>
    /// Event raised when a valid LiDAR packet is received and parsed
    /// </summary>
    public event EventHandler<LidarDataPacket>? DataReceived;

    /// <summary>
    /// Event raised when an error occurs during packet reception or parsing
    /// </summary>
    public event EventHandler<LidarErrorEventArgs>? ErrorOccurred;

    /// <summary>
    /// Total number of packets received (including invalid ones)
    /// </summary>
    public long TotalPacketsReceived => Interlocked.Read(ref _totalPacketsReceived);

    /// <summary>
    /// Total number of packets successfully parsed
    /// </summary>
    public long TotalPacketsParsed => Interlocked.Read(ref _totalPacketsParsed);

    /// <summary>
    /// Total number of parse errors
    /// </summary>
    public long TotalParseErrors => Interlocked.Read(ref _totalParseErrors);

    /// <summary>
    /// Total number of packets created (for monitoring allocation rate)
    /// </summary>
    public long TotalPacketsCreated => Interlocked.Read(ref _totalPacketsCreated);

    /// <summary>
    /// Current number of packets in the pool
    /// </summary>
    public long CurrentPoolSize => Interlocked.Read(ref _packetPoolCount);

    /// <summary>
    /// Scan frequency (Hz)
    /// Typically 10-20 Hz for Olei LiDAR
    /// </summary>
    public double? ScanFrequencyHz { get; private set; }

    /// <summary>
    /// Check if server is currently running
    /// </summary>
    public bool IsRunning => _isRunning;

    /// <summary>
    /// Create a new Olei LiDAR server
    /// </summary>
    /// <param name="port">UDP port to listen on</param>
    public OleiLidarServer(int port)
    {
        _port = port;
        _packetPool = [];
        _packetPoolCount = 0;
        _totalPacketsCreated = 0;

        // Pre-allocate packet pool
        for (int i = 0; i < INITIAL_PACKET_POOL_SIZE; i++)
        {
            _packetPool.Add(new LidarDataPacket());
            Interlocked.Increment(ref _packetPoolCount);
            Interlocked.Increment(ref _totalPacketsCreated);
        }
    }

    /// <summary>
    /// Start receiving LiDAR data
    /// </summary>
    public void Start()
    {
        lock (_lock)
        {
            if (_isRunning)
                return;

            try
            {
                // Create socket with optimized settings
                _socket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
                _socket.Bind(new IPEndPoint(IPAddress.Any, _port));

                // Set socket options for high-performance UDP reception
                _socket.ReceiveBufferSize = 2 * 1024 * 1024;  // 2MB OS buffer
                _socket.ReceiveTimeout = 0;  // Blocking receive (no timeout)

                // Allocate receive buffer once
                _receiveBuffer = new byte[RECEIVE_BUFFER_SIZE];

                _isRunning = true;

                // Reset frequency calculation
                _packetsReceivedInCurrentSecond = 0;
                _lastFrequencyUpdateMs = 0;
                _frequencyStopwatch.Restart();

                // Start dedicated receive thread with high priority
                _receiveThread = new Thread(ReceiveLoop)
                {
                    Name = $"OleiLidar-Receive-{_port}",
                    IsBackground = false,
                    Priority = ThreadPriority.Highest
                };
                _receiveThread.Start();

                Console.WriteLine($"[OleiLidarServer] Started on port {_port}");
            }
            catch (Exception ex)
            {
                _isRunning = false;
                OnError(new LidarErrorEventArgs($"Failed to start server: {ex.Message}", ex));
                throw;
            }
        }
    }

    /// <summary>
    /// Stop receiving LiDAR data
    /// </summary>
    public void Stop()
    {
        lock (_lock)
        {
            if (!_isRunning)
                return;

            _isRunning = false;

            try
            {
                _socket?.Close();
                _socket?.Dispose();
                _socket = null;

                // Wait for receive thread to exit (with timeout)
                _receiveThread?.Join(TimeSpan.FromSeconds(2));
                _receiveThread = null;

                // Clean up receive buffer
                _receiveBuffer = null;

                _frequencyStopwatch.Stop();

                Console.WriteLine($"[OleiLidarServer] Stopped on port {_port}");
            }
            catch (Exception ex)
            {
                OnError(new LidarErrorEventArgs($"Error stopping server: {ex.Message}", ex));
            }
        }
    }

    /// <summary>
    /// Main receive loop running in dedicated thread
    /// Uses blocking Socket.ReceiveFrom with pre-allocated buffer for zero-allocation receive
    /// </summary>
    private void ReceiveLoop()
    {
        Thread.BeginThreadAffinity();

        try
        {
            EndPoint remoteEndPoint = new IPEndPoint(IPAddress.Any, 0);

            while (_isRunning)
            {
                try
                {
                    if (_socket == null || _receiveBuffer == null)
                        break;

                    // Blocking receive into pre-allocated buffer (zero allocation!)
                    int receivedBytes = _socket.ReceiveFrom(_receiveBuffer, ref remoteEndPoint);

                    if (receivedBytes <= 0)
                        continue;

                    // Process ALL packets in received buffer
                    int offset = 0;
                    while (offset + LidarDataPacket.PACKET_SIZE <= receivedBytes)
                    {
                        // Create span directly from pre-allocated buffer (zero-copy)
                        ReadOnlySpan<byte> packetData = _receiveBuffer.AsSpan(offset, LidarDataPacket.PACKET_SIZE);

                        // Quick validation before processing
                        if (LidarPacketParser.IsValidPacket(packetData))
                        {
                            Interlocked.Increment(ref _totalPacketsReceived);
                            ProcessPacketSpan(packetData);
                        }

                        offset += LidarDataPacket.PACKET_SIZE;
                    }

                    // Note: Leftover bytes warning removed from hot path for performance
                }
                catch (SocketException ex) when (ex.SocketErrorCode == SocketError.Interrupted)
                {
                    // Socket was closed, exit loop
                    break;
                }
                catch (ObjectDisposedException)
                {
                    // Socket was disposed, exit loop
                    break;
                }
                catch (Exception ex)
                {
                    OnError(new LidarErrorEventArgs($"Error receiving packet: {ex.Message}", ex));
                }
            }
        }
        finally
        {
            Thread.EndThreadAffinity();
        }
    }

    /// <summary>
    /// Process received packet from ReadOnlySpan (zero-copy)
    /// </summary>
    private void ProcessPacketSpan(ReadOnlySpan<byte> data)
    {
        LidarDataPacket? packet = null;

        try
        {
            // Get packet from pool or create new one
            if (_packetPool.TryTake(out packet))
            {
                Interlocked.Decrement(ref _packetPoolCount);
            }
            else
            {
                packet = new LidarDataPacket();
                Interlocked.Increment(ref _totalPacketsCreated);
            }

            // Parse packet (already validated)
            if (LidarPacketParser.TryParse(data, packet))
            {
                Interlocked.Increment(ref _totalPacketsParsed);

                // Update scan frequency (optimized - only check timestamp, no lock)
                UpdateScanFrequencyOptimized();

                // Raise event and automatically return packet to pool after processing
                OnDataReceived(packet);
            }
            else
            {
                Interlocked.Increment(ref _totalParseErrors);
            }
        }
        catch (Exception ex)
        {
            Interlocked.Increment(ref _totalParseErrors);
            OnError(new LidarErrorEventArgs($"Error processing packet: {ex.Message}", ex));
        }
        finally
        {
            // Always return packet to pool, even if event handler throws exception
            if (packet != null)
            {
                ReturnPacketToPool(packet);
            }
        }
    }

    /// <summary>
    /// Update scan frequency based on packets received per second
    /// Optimized version: only takes lock when actually updating (once per second)
    /// </summary>
    private void UpdateScanFrequencyOptimized()
    {
        // Increment packet count (lock-free)
        Interlocked.Increment(ref _packetsReceivedInCurrentSecond);

        // Check if 1 second has elapsed (lock-free check)
        long elapsedMs = _frequencyStopwatch.ElapsedMilliseconds;
        if (elapsedMs - Interlocked.Read(ref _lastFrequencyUpdateMs) >= 1000)
        {
            // Try to update (only one thread will succeed)
            long previousUpdateMs = Interlocked.CompareExchange(ref _lastFrequencyUpdateMs, elapsedMs, Interlocked.Read(ref _lastFrequencyUpdateMs));

            // Check if we won the race to update
            if (elapsedMs - previousUpdateMs >= 1000)
            {
                // Calculate frequency
                long packetCount = Interlocked.Exchange(ref _packetsReceivedInCurrentSecond, 0);
                double elapsedSeconds = (elapsedMs - previousUpdateMs) / 1000.0;
                ScanFrequencyHz = packetCount / elapsedSeconds;
            }
        }
    }

    /// <summary>
    /// Return packet to pool for reuse
    /// This is called automatically after event handlers complete
    /// Uses atomic operations to prevent race conditions and ensure accurate pool size
    /// </summary>
    private void ReturnPacketToPool(LidarDataPacket packet)
    {
        if (packet == null)
            return;

        // Atomically try to increment the pool count
        long newCount = Interlocked.Increment(ref _packetPoolCount);

        if (newCount <= MAX_PACKET_POOL_SIZE)
        {
            // Successfully reserved a slot in the pool
            _packetPool.Add(packet);
        }
        else
        {
            // Pool is full - decrement counter back and let packet be GC'd
            Interlocked.Decrement(ref _packetPoolCount);
        }
    }

    /// <summary>
    /// Raise DataReceived event
    /// Packet is automatically returned to pool after all handlers complete
    /// </summary>
    private void OnDataReceived(LidarDataPacket e)
    {
        DataReceived?.Invoke(this, e);
    }

    /// <summary>
    /// Raise ErrorOccurred event
    /// </summary>
    private void OnError(LidarErrorEventArgs e)
    {
        ErrorOccurred?.Invoke(this, e);
    }

    /// <summary>
    /// Reset statistics counters
    /// Note: Pool size and created count are preserved (shows cumulative allocations)
    /// </summary>
    public void ResetStatistics()
    {
        Interlocked.Exchange(ref _totalPacketsReceived, 0);
        Interlocked.Exchange(ref _totalPacketsParsed, 0);
        Interlocked.Exchange(ref _totalParseErrors, 0);

        // Reset frequency calculation
        Interlocked.Exchange(ref _packetsReceivedInCurrentSecond, 0);
        Interlocked.Exchange(ref _lastFrequencyUpdateMs, 0);
        _frequencyStopwatch.Restart();
        ScanFrequencyHz = null;
    }

    /// <summary>
    /// Get statistics summary with pool metrics
    /// </summary>
    public string GetStatistics()
    {
        long received = TotalPacketsReceived;
        long parsed = TotalPacketsParsed;
        long errors = TotalParseErrors;
        long created = TotalPacketsCreated;
        long poolSize = CurrentPoolSize;
        double successRate = received > 0 ? (parsed * 100.0 / received) : 0;
        string frequency = ScanFrequencyHz.HasValue ? $"{ScanFrequencyHz.Value:F2} Hz" : "N/A";

        return $"Received: {received}, Parsed: {parsed}, Errors: {errors}, " +
               $"Created: {created}, Pool: {poolSize}/{MAX_PACKET_POOL_SIZE}, " +
               $"Success: {successRate:F2}%, Frequency: {frequency}";
    }

    /// <summary>
    /// Dispose resources
    /// </summary>
    public void Dispose()
    {
        Stop();
        GC.SuppressFinalize(this);
    }
}

/// <summary>
/// Event args for LiDAR error events
/// </summary>
public class LidarErrorEventArgs(string message, Exception? exception = null) : EventArgs
{
    public string Message { get; } = message;
    public Exception? Exception { get; } = exception;
}