using System.Buffers.Binary;
using System.Net;
using System.Net.Sockets;

namespace RobotNet10.RobotApp.Drivers.Lidar;

/// <summary>
/// UDP transport + packet decode aligned with olelidar ROS driver (driver.cpp / decoder.cpp).
/// </summary>
public sealed class Connect : IDisposable
{
    public const int kPacketSize = OleiConstants.PacketSize;

    public string device_IP { get; set; } = "192.168.254.13";
    public int device_port { get; set; } = 2368;
    public string local_ip { get; set; } = "192.168.254.10";
    public string multiaddr_ip { get; set; } = string.Empty;

    private readonly Queue<oleiPackage> _packetList = new();
    private readonly object _packetListLock = new();

    private UdpClient? _udp;
    private Thread? _receiveThread;
    private volatile bool _running;
    private IPAddress? _deviceIpAddress;

    private long _totalPacketsReceived;
    private long _droppedWrongSource;
    private long _droppedWrongSize;

    public long TotalPacketsReceived => Interlocked.Read(ref _totalPacketsReceived);
    public long DroppedWrongSource => Interlocked.Read(ref _droppedWrongSource);
    public long DroppedWrongSize => Interlocked.Read(ref _droppedWrongSize);

    public bool IsOpen => _udp != null && _running;

    public bool openPort()
    {
        try
        {
            _deviceIpAddress = IPAddress.Parse(device_IP).MapToIPv4();

            var bindAddress = string.IsNullOrWhiteSpace(local_ip)
                ? IPAddress.Any
                : IPAddress.Parse(local_ip).MapToIPv4();
            var bindEp = new IPEndPoint(bindAddress, device_port);

            _udp = new UdpClient();
            _udp.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
            _udp.Client.Bind(bindEp);
            _udp.Client.ReceiveTimeout = 500;

            if (!string.IsNullOrWhiteSpace(multiaddr_ip))
            {
                try
                {
                    _udp.JoinMulticastGroup(IPAddress.Parse(multiaddr_ip));
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"[WARN] Olei multicast join failed ({multiaddr_ip}): {ex.Message}");
                }
            }

            StartReceiveLoop();
            Console.WriteLine($"[INFO] Olei UDP listening on {bindEp.Address}:{bindEp.Port}, device {device_IP}");
            return true;
        }
        catch (Exception ex)
        {
            Console.WriteLine($"[ERROR] Cannot open Olei UDP port: {ex.Message}");
            return false;
        }
    }

    public void closePort()
    {
        _running = false;
        try
        {
            _receiveThread?.Join(TimeSpan.FromSeconds(2));
        }
        catch { /* ignore */ }

        _receiveThread = null;
        _udp?.Close();
        _udp?.Dispose();
        _udp = null;
    }

    private void StartReceiveLoop()
    {
        _running = true;
        _receiveThread = new Thread(ReceiveLoop)
        {
            IsBackground = true,
            Name = $"OleiLidar-UDP-{device_port}",
            Priority = ThreadPriority.Highest
        };
        _receiveThread.Start();
    }

    private void ReceiveLoop()
    {
        while (_running && _udp != null)
        {
            try
            {
                var remote = new IPEndPoint(IPAddress.Any, 0);
                var data = _udp.Receive(ref remote);

                var senderIp = remote.Address.MapToIPv4();
                if (_deviceIpAddress != null && !senderIp.Equals(_deviceIpAddress))
                {
                    Interlocked.Increment(ref _droppedWrongSource);
                    continue;
                }

                if (data.Length < kPacketSize)
                {
                    Interlocked.Increment(ref _droppedWrongSize);
                    continue;
                }

                Interlocked.Increment(ref _totalPacketsReceived);

                var packet = new oleiPackage
                {
                    stamp = DateTime.UtcNow,
                    data = new byte[kPacketSize]
                };
                Array.Copy(data, packet.data, kPacketSize);

                lock (_packetListLock)
                {
                    _packetList.Enqueue(packet);
                }
            }
            catch (SocketException ex) when (ex.SocketErrorCode == SocketError.TimedOut)
            {
                // Normal when no packets (ROS poll timeout).
            }
            catch (SocketException ex) when (!_running)
            {
                break;
            }
            catch (ObjectDisposedException)
            {
                break;
            }
            catch (Exception ex)
            {
                if (_running)
                    Console.WriteLine($"[ERROR] Olei UDP receive: {ex.Message}");
            }
        }
    }

    public int GetPacketCount()
    {
        lock (_packetListLock)
            return _packetList.Count;
    }

    public oleiPackage? TryDequeuePacket()
    {
        lock (_packetListLock)
            return _packetList.Count == 0 ? null : _packetList.Dequeue();
    }

    public void Dispose() => closePort();
}

public sealed class oleiPackage
{
    public DateTime stamp { get; set; }
    public byte[] data { get; set; } = new byte[OleiConstants.PacketSize];
}

public sealed class DecoderConfig
{
    public double AngleMin { get; set; } = 0.0;
    public double AngleMax { get; set; } = 360.0;
    public double RangeMin { get; set; } = 0.2;
    public double RangeMax { get; set; } = 30.0;
    public int Poly { get; set; } = 1;
    public bool Inverted { get; set; }
    public double StepDeg { get; set; } = 0.225;
    public string FrameId { get; set; } = "olelidar";
}

public static class OleiConstants
{
    public const int DataHeadSize = 40;
    public const int PointBytes = 8;
    public const int BlocksPerPacket = 150;
    public const int PacketSize = DataHeadSize + BlocksPerPacket * PointBytes;
    public const float DistanceResolution = 0.001f;
    public const float AzimuthResolutionDeg = 0.01f;
}

public readonly struct DataPoint
{
    public readonly ushort Azimuth;
    public readonly ushort Distance;
    public readonly ushort Reflectivity;

    public DataPoint(ReadOnlySpan<byte> span)
    {
        Azimuth = BinaryPrimitives.ReadUInt16LittleEndian(span);
        Distance = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(2));
        Reflectivity = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(4));
    }
}

public readonly struct DataBlock
{
    public readonly DataPoint Point;
    public DataBlock(ReadOnlySpan<byte> span) => Point = new DataPoint(span);
}

public readonly struct DataHeader
{
    public readonly byte[] Code;
    public readonly uint Timestamp;
    public readonly ushort Rpm;
    public readonly uint Rsv;

    public DataHeader(ReadOnlySpan<byte> span)
    {
        Code = span.Slice(22, 2).ToArray();
        Timestamp = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(28));
        Rpm = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(32));
        Rsv = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(36));
    }
}

public readonly struct Packet
{
    public readonly DataHeader Header;
    public readonly DataBlock[] Blocks;

    public Packet(ReadOnlySpan<byte> span)
    {
        if (span.Length < OleiConstants.PacketSize)
            throw new ArgumentException($"packet must be {OleiConstants.PacketSize} bytes");

        Header = new DataHeader(span.Slice(0, OleiConstants.DataHeadSize));
        Blocks = new DataBlock[OleiConstants.BlocksPerPacket];
        var offset = OleiConstants.DataHeadSize;
        for (var i = 0; i < OleiConstants.BlocksPerPacket; i++)
        {
            Blocks[i] = new DataBlock(span.Slice(offset, OleiConstants.PointBytes));
            offset += OleiConstants.PointBytes;
        }
    }
}

/// <summary>
/// Packet decoder — port of olelidar/src/decoder.cpp PacketCb / DecodeAndFill / PublishMsg.
/// </summary>
public sealed class DecodeLidar
{
    public readonly List<ushort> scanAngleInVec = new();
    public readonly List<ushort> scanRangeInVec = new();
    public readonly List<ushort> scanIntensityInVec = new();

    private readonly List<ushort> _scanAngleVec = new();
    private readonly List<ushort> _scanRangeVec = new();
    private readonly List<ushort> _scanIntensityVec = new();

    public ushort AzimuthLast { get; private set; }
    public ushort AzimuthNow { get; private set; }
    public ushort AzimuthFirst { get; private set; } = 0xFFFF;

    private DateTime _machineTimeBase;
    private uint _innerTimestampBaseMs;
    private bool _isTimeBase;
    private uint _lastStampMs;

    /// <summary>End-of-scan time mapped from lidar internal clock (UTC).</summary>
    public DateTime? LastCompletedScanLidarEndUtc { get; private set; }

    public float Frequency { get; private set; }
    public byte LidarType { get; private set; } = 0x01;
    public int Direction { get; private set; }
    public double StepDeg { get; private set; } = 0.225;

    private DecoderConfig _config = new();
    private readonly object _locker = new();

    public void SetConfig(DecoderConfig config)
    {
        _config = config;
        StepDeg = config.StepDeg;
    }

    public bool PacketCb(oleiPackage dataMsg)
    {
        if (dataMsg.data.Length < OleiConstants.PacketSize)
            return false;

        var pkt = new Packet(dataMsg.data);
        AzimuthNow = pkt.Blocks[0].Point.Azimuth;

        if (AzimuthFirst == 0xFFFF)
        {
            LidarType = pkt.Header.Code.Length > 1 ? pkt.Header.Code[1] : (byte)0x01;
            AzimuthFirst = AzimuthNow;
            var rpm = pkt.Header.Rpm & 0x7FFF;
            Direction = pkt.Header.Rpm >> 15;
            if (_config.Inverted)
                Direction = 1 - Direction;

            if (Frequency < 0.001f && rpm > 0)
            {
                Frequency = rpm / 60.0f;
                if (LidarType == 0x01)
                    StepDeg = 0.225;
            }
        }

        if (AzimuthLast < AzimuthNow)
        {
            DecodeAndFill(pkt);
            AzimuthLast = AzimuthNow;
            return false;
        }

        AzimuthLast = AzimuthNow;

        if (AzimuthFirst >= 200)
        {
            AzimuthFirst = AzimuthNow;
            return false;
        }

        var nowStampMs = pkt.Header.Timestamp;
        if (!_isTimeBase)
        {
            _machineTimeBase = dataMsg.stamp.Kind == DateTimeKind.Utc
                ? dataMsg.stamp
                : dataMsg.stamp.ToUniversalTime();
            _innerTimestampBaseMs = nowStampMs;
            _isTimeBase = true;
        }

        _lastStampMs = nowStampMs;
        LastCompletedScanLidarEndUtc = ComputeLidarTimeUtc(nowStampMs, dataMsg.stamp);

        if (Frequency < 0.001f)
        {
            if (LidarType == 0x01)
            {
                var rpm = pkt.Header.Rpm & 0x7FFF;
                Frequency = rpm / 60.0f;
                StepDeg = 0.225;
            }
            else if (_scanAngleVec.Count > 2)
            {
                StepDeg = (_scanAngleVec[1] - _scanAngleVec[0]) / 100.0;
                Frequency = (float)(StepDeg * 10000.0 / 60.0);
            }
            else
            {
                return false;
            }
        }

        lock (_locker)
        {
            scanAngleInVec.Clear();
            scanRangeInVec.Clear();
            scanIntensityInVec.Clear();

            scanAngleInVec.AddRange(_scanAngleVec);
            scanRangeInVec.AddRange(_scanRangeVec);
            scanIntensityInVec.AddRange(_scanIntensityVec);

            if (Direction == 0)
            {
                scanRangeInVec.Reverse();
                scanIntensityInVec.Reverse();
            }

            _scanAngleVec.Clear();
            _scanRangeVec.Clear();
            _scanIntensityVec.Clear();
        }

        DecodeAndFill(pkt);
        return scanAngleInVec.Count > 0;
    }

    private DateTime ComputeLidarTimeUtc(uint lidarTimestampMs, DateTime receiveStamp)
    {
        var receiveUtc = receiveStamp.Kind == DateTimeKind.Utc
            ? receiveStamp
            : receiveStamp.ToUniversalTime();

        if (!_isTimeBase)
            return receiveUtc;

        var deltaMs = lidarTimestampMs - _innerTimestampBaseMs;
        return _machineTimeBase.AddMilliseconds(deltaMs);
    }

    private void DecodeAndFill(Packet pkt)
    {
        var rangeMaxMm = (ushort)(_config.RangeMax * 1000);
        var rangeMinMm = (ushort)(_config.RangeMin * 1000);

        for (var i = 0; i < OleiConstants.BlocksPerPacket; i++)
        {
            var dp = pkt.Blocks[i].Point;
            var azimuth = dp.Azimuth;
            var range = dp.Distance;
            var intensity = dp.Reflectivity;

            if (range > rangeMaxMm || range < rangeMinMm)
            {
                range = 0;
                intensity = 0;
            }

            if (azimuth < 0xFF00)
            {
                lock (_locker)
                {
                    _scanAngleVec.Add(azimuth);
                    _scanRangeVec.Add(range);
                    _scanIntensityVec.Add(intensity);
                }
            }
        }
    }
}