using System.Buffers.Binary;
using System.Linq;
using System.Net;
using System.Net.NetworkInformation;
using System.Net.Sockets;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared;
using RobotNet10.Shared.Sensor;

namespace RobotNet10.RobotApp.Drivers.Lidar;

/// <summary>
/// UDP connection for Olei GS1-5 (V3 protocol). Packet size 1136 bytes, not 1240.
/// </summary>
internal sealed class OleiGS15UdpConnection
{
    public const int ExpectedPacketSize = 1136;

    private readonly Queue<Gs15Packet> _packetQueue = new();
    private readonly object _packetLock = new();

    public string device_IP { get; set; } = "192.168.110.22";
    public int device_port { get; set; } = 2468;
    public string local_ip { get; set; } = "192.168.110.114";
    public UdpClient? udp;
    public IPEndPoint check_ip_device = new(IPAddress.Any, 0);

    public bool openPort()
    {
        try
        {
            if (!IpExists(local_ip))
            {
                Console.WriteLine($"[ERROR] OleiGS15: local_ip {local_ip} does NOT exist on any NIC.");
                return false;
            }
            if (IsPortBusy(device_port))
            {
                Console.WriteLine($"[ERROR] OleiGS15: Port {device_port} is already in use!");
                return false;
            }
            udp = new UdpClient(new IPEndPoint(IPAddress.Parse(local_ip), device_port));
            check_ip_device = new IPEndPoint(IPAddress.Parse(local_ip), device_port);
            udp.Client.ReceiveTimeout = 1000;
            Console.WriteLine("[INFO] OleiGS15 UDP port opened.");
            return true;
        }
        catch (Exception ex)
        {
            Console.WriteLine($"[ERROR] OleiGS15: Cannot open UDP {local_ip}:{device_port} - {ex.Message}");
            return false;
        }
    }

    public void readRawdata()
    {
        if (udp == null) return;
        try
        {
            byte[] data = udp.Receive(ref check_ip_device);
            if (check_ip_device.Address == null || check_ip_device.Address.Equals(IPAddress.Any) || check_ip_device.Address.Equals(IPAddress.None) || check_ip_device.Port == 0)
                return;
            if (check_ip_device.Address.ToString() != device_IP)
                return;
            if (data.Length != ExpectedPacketSize)
                return;

            var packet = new Gs15Packet { stamp = DateTime.UtcNow, data = data };
            lock (_packetLock)
            {
                _packetQueue.Enqueue(packet);
            }
        }
        catch (SocketException ex)
        {
            if (ex.SocketErrorCode != SocketError.TimedOut)
                Console.WriteLine($"[ERROR] OleiGS15 socket: {ex.Message}");
        }
        catch (Exception) { }
    }

    public Gs15Packet? TryDequeuePacket()
    {
        lock (_packetLock)
        {
            if (_packetQueue.Count == 0) return null;
            return _packetQueue.Dequeue();
        }
    }

    private static bool IpExists(string ip)
    {
        foreach (var ni in NetworkInterface.GetAllNetworkInterfaces())
        {
            foreach (var ua in ni.GetIPProperties().UnicastAddresses)
            {
                if (ua.Address.ToString() == ip) return true;
            }
        }
        return false;
    }

    private static bool IsPortBusy(int port)
    {
        var listeners = IPGlobalProperties.GetIPGlobalProperties().GetActiveUdpListeners();
        return listeners.Any(ep => ep.Port == port);
    }
}

internal sealed class Gs15Packet
{
    public DateTime stamp;
    public byte[] data = Array.Empty<byte>();
}

public sealed class OleiGS15DriverConfig
{
    public int Version { get; set; } = 3;
    public string ScannerIp { get; set; } = "192.168.110.22";
    public string LocalIp { get; set; } = "192.168.110.114";
    public int Port { get; set; } = 2468;
    public string Transport { get; set; } = "udp";
    public string FrameId { get; set; } = "olelidar";
    public string ScanTopic { get; set; } = "scan";
    public bool Inverted { get; set; } = false;
    public bool TimeFromLidar { get; set; } = true;
    public bool Ntp { get; set; } = false;
    public string PacketType { get; set; } = "B";
    public double RangeMin { get; set; } = 0.08;
    public double RangeMax { get; set; } = 50.0;
    public bool AutoReconnectEnabled { get; set; } = true;
    public int ReconnectDelayMs { get; set; } = 3000;
    public int MaxReconnectAttempts { get; set; } = 0;

    public static OleiGS15DriverConfig Parse(IConfigurationSection connection)
    {
        var cfg = new OleiGS15DriverConfig
        {
            Version = connection.GetValue<int?>("Version")
                ?? connection.GetValue<int?>("version")
                ?? 3,
            ScannerIp = connection["ScannerIp"]
                ?? connection["scanner_ip"]
                ?? connection["DeviceIp"]
                ?? "192.168.110.22",
            LocalIp = connection["LocalIp"]
                ?? connection["local_ip"]
                ?? "192.168.110.114",
            Transport = connection["Transport"]
                ?? connection["transport"]
                ?? "udp",
            FrameId = connection["FrameId"]
                ?? connection["frame_id"]
                ?? "olelidar",
            ScanTopic = connection["ScanTopic"]
                ?? connection["scan_topic"]
                ?? "scan",
            PacketType = connection["PacketType"]
                ?? connection["packet_type"]
                ?? "B",
            Inverted = connection.GetValue<bool?>("Inverted")
                ?? connection.GetValue<bool?>("inverted")
                ?? false,
            TimeFromLidar = connection.GetValue<bool?>("TimeFromLidar")
                ?? connection.GetValue<bool?>("timeFromLidar")
                ?? true,
            Ntp = connection.GetValue<bool?>("Ntp")
                ?? connection.GetValue<bool?>("ntp")
                ?? false,
            RangeMin = connection.GetValue<double?>("RangeMin")
                ?? connection.GetValue<double?>("range_min")
                ?? 0.08,
            RangeMax = connection.GetValue<double?>("RangeMax")
                ?? connection.GetValue<double?>("range_max")
                ?? 50.0,
            AutoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled") ?? true,
            ReconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs") ?? 3000,
            MaxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts") ?? 0
        };

        var port = connection.GetValue<int?>("Port")
            ?? connection.GetValue<int?>("port")
            ?? connection.GetValue<int?>("DevicePort")
            ?? 2368;
        if (port is < 1 or > 65535)
        {
            throw new InvalidOperationException($"Invalid Port: {port}. Must be between 1 and 65535");
        }

        cfg.Port = port;

        if (!string.Equals(cfg.Transport, "udp", StringComparison.OrdinalIgnoreCase))
        {
            throw new InvalidOperationException("OleiGS15Driver currently supports transport=udp only");
        }

        if (cfg.RangeMin < 0)
        {
            throw new InvalidOperationException($"RangeMin must be >= 0. Current value: {cfg.RangeMin}");
        }

        if (cfg.RangeMax <= cfg.RangeMin)
        {
            throw new InvalidOperationException(
                $"RangeMax must be greater than RangeMin. RangeMin={cfg.RangeMin}, RangeMax={cfg.RangeMax}");
        }

        return cfg;
    }
}

[Device(DeviceType.Lidar, "Olei", "OleiGS15Driver", "1.0.0",
    Description = "Olei GS1-5 LiDAR driver (ROS version3-compatible UDP parser)")]
public class OleiGS15Driver : DeviceBase, ILidar
{
    private const int V3HeaderSize = 48;
    private const ushort V3Magic = 0xFEAC;

    private readonly OleiGS15DriverConfig _config;
    private readonly OleiGS15UdpConnection _connection = new();
    private readonly Lock _dataLock = new();

    private CancellationTokenSource? _updateCts;
    private Task? _updateTask;

    private LaserScan? _lastLaserScan;
    private DateTime? _lastScanDataTimestamp;
    private int _lastPointCount;
    private double? _lastScanFrequencyHz;
    private bool _portOpened;

    private V3ScanAccumulator? _scanAccumulator;
    private ushort? _lastFirstIndex;

    public OleiGS15Driver(string deviceId, string deviceName, IConfigurationSection connection)
        : base(deviceId, deviceName, DeviceType.Lidar)
    {
        _config = OleiGS15DriverConfig.Parse(connection);

        _connection.device_IP = _config.ScannerIp;
        _connection.local_ip = _config.LocalIp;
        _connection.device_port = _config.Port;

        AutoReconnectEnabled = _config.AutoReconnectEnabled;
        ReconnectDelayMs = _config.ReconnectDelayMs;
        MaxReconnectAttempts = _config.MaxReconnectAttempts;

        UpdateProperties();
    }

    protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
    {
        yield return new PropertyDescription("Version", "Version", "Phiên bản protocol")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 1,
            Category = "Cấu hình",
            DefaultValue = "3"
        };

        yield return new PropertyDescription("PacketType", "Packet Type", "Loại packet A/B/C")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 2,
            Category = "Cấu hình",
            DefaultValue = "B"
        };

        yield return new PropertyDescription("ScannerIp", "Scanner IP", "IP của Olei GS1-5")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 3,
            Category = "Kết nối",
            DefaultValue = ""
        };

        yield return new PropertyDescription("LocalIp", "Local IP", "IP local nhận UDP packet")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 4,
            Category = "Kết nối",
            DefaultValue = ""
        };

        yield return new PropertyDescription("Port", "Port", "UDP port của lidar")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 5,
            Category = "Kết nối",
            DefaultValue = "2368"
        };

        yield return new PropertyDescription("FrameId", "Frame ID", "Frame ID của LaserScan")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 6,
            Category = "Cấu hình",
            DefaultValue = "olelidar"
        };

        yield return new PropertyDescription("RangeMin", "Range Min (m)", "Khoảng cách tối thiểu")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 7,
            Category = "Cấu hình",
            DefaultValue = "0.08"
        };

        yield return new PropertyDescription("RangeMax", "Range Max (m)", "Khoảng cách tối đa")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 8,
            Category = "Cấu hình",
            DefaultValue = "50.0"
        };

        yield return new PropertyDescription("ConnectionStatus", "Connection Status", "Trạng thái kết nối lidar")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 9,
            Category = "Trạng thái",
            DefaultValue = "Disconnected"
        };

        yield return new PropertyDescription("PointCount", "Point Count", "Số điểm scan gần nhất")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 10,
            Category = "Trạng thái",
            DefaultValue = "0"
        };

        yield return new PropertyDescription("LastScanTime", "Last Scan Time", "Thời gian scan gần nhất")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 11,
            Category = "Trạng thái",
            DefaultValue = ""
        };
    }

    protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
    {
        lock (_dataLock)
        {
            _portOpened = _connection.openPort();
            if (!_portOpened)
            {
                throw new InvalidOperationException(
                    $"Failed to open Olei GS1-5 UDP port at {_config.LocalIp}:{_config.Port}");
            }
        }

        await Task.CompletedTask;
    }

    protected override async Task OnConnectAsync(CancellationToken cancellationToken)
    {
        StartUpdateLoop();
        SetProperty("ConnectionStatus", "Connected");
        await Task.CompletedTask;
    }

    protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
    {
        StopUpdateLoop();
        CloseUdpPort();
        SetProperty("ConnectionStatus", "Disconnected");
        await Task.CompletedTask;
    }

    protected override async Task OnResetAsync(CancellationToken cancellationToken)
    {
        lock (_dataLock)
        {
            _lastLaserScan = null;
            _lastScanDataTimestamp = null;
            _lastPointCount = 0;
            _lastScanFrequencyHz = null;
            _scanAccumulator = null;
            _lastFirstIndex = null;
        }

        UpdateProperties();
        await Task.CompletedTask;
    }

    protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
    {
        lock (_dataLock)
        {
            if (!_portOpened)
            {
                return false;
            }

            if (_updateTask == null || _updateTask.IsCompleted)
            {
                return false;
            }

            if (_lastScanDataTimestamp == null)
            {
                return true;
            }

            return (DateTime.UtcNow - _lastScanDataTimestamp.Value).TotalSeconds < 5.0;
        }
    }

    private void StartUpdateLoop()
    {
        lock (_dataLock)
        {
            if (_updateTask != null && !_updateTask.IsCompleted)
            {
                return;
            }

            _updateCts = new CancellationTokenSource();
            _updateTask = Task.Run(() => UpdateLoopAsync(_updateCts.Token), _updateCts.Token);
        }
    }

    private void StopUpdateLoop()
    {
        Task? updateTask;
        CancellationTokenSource? cts;

        lock (_dataLock)
        {
            updateTask = _updateTask;
            cts = _updateCts;
            _updateTask = null;
            _updateCts = null;
        }

        try
        {
            cts?.Cancel();
            updateTask?.Wait(TimeSpan.FromSeconds(2));
        }
        catch
        {
        }
        finally
        {
            cts?.Dispose();
        }
    }

    private async Task UpdateLoopAsync(CancellationToken cancellationToken)
    {
        while (!cancellationToken.IsCancellationRequested)
        {
            try
            {
                _connection.readRawdata();

                var processedAnyPacket = false;
                while (TryProcessOnePacket())
                {
                    processedAnyPacket = true;
                }

                if (!processedAnyPacket)
                {
                    await Task.Delay(5, cancellationToken);
                }
            }
            catch (OperationCanceledException)
            {
                break;
            }
            catch (Exception ex)
            {
                LastError = ex;
                OnErrorOccurred(ex, "OleiGS15 update loop error");
                await Task.Delay(100, cancellationToken);
            }
        }
    }

    private bool TryProcessOnePacket()
    {
        var packet = _connection.TryDequeuePacket();
        if (packet == null)
        {
            return false;
        }

        if (!TryParseV3Packet(packet.data, out var header, out var rangesMm, out var intensities, out var hasIntensity))
        {
            return true;
        }

        var packetTimestamp = _config.TimeFromLidar
            ? (packet.stamp.Kind == DateTimeKind.Utc ? packet.stamp : packet.stamp.ToUniversalTime())
            : DateTime.UtcNow;

        V3ScanAccumulator? completedScan = null;

        lock (_dataLock)
        {
            if (_lastFirstIndex.HasValue && _lastFirstIndex.Value > header.FirstIndex)
            {
                _scanAccumulator = null;
            }
            _lastFirstIndex = header.FirstIndex;

            if (_scanAccumulator == null)
            {
                _scanAccumulator = new V3ScanAccumulator(header, _config);
            }

            _scanAccumulator.AddPacketData(rangesMm, intensities, hasIntensity, _config);

            if (_scanAccumulator.IsCompleted)
            {
                completedScan = _scanAccumulator;
                _scanAccumulator = null;
            }
        }

        if (completedScan != null)
        {
            PublishCompletedScan(completedScan, packetTimestamp);
        }

        return true;
    }

    private void PublishCompletedScan(V3ScanAccumulator completed, DateTime packetTimestamp)
    {
        var scanTime = completed.ScanTimeSeconds > 0 ? completed.ScanTimeSeconds : 0.1f;
        var scanStamp = packetTimestamp - TimeSpan.FromSeconds(scanTime);

        var laserScan = new LaserScan
        {
            Header = new RobotNet10.Shared.Header
            {
                Seq = 0,
                Stamp = scanStamp,
                FrameId = _config.FrameId
            },
            AngleMin = completed.AngleMin,
            AngleMax = completed.AngleMax,
            AngleIncrement = completed.AngleIncrement,
            TimeIncrement = completed.TimeIncrement,
            ScanTime = scanTime,
            RangeMin = _config.RangeMin,
            RangeMax = _config.RangeMax,
            Ranges = Array.ConvertAll(completed.Ranges, x => (double)x),
            Intensities = Array.ConvertAll(completed.Intensities, x => (double)x)
        };

        lock (_dataLock)
        {
            _lastLaserScan = laserScan;
            _lastScanDataTimestamp = scanStamp;
            _lastPointCount = completed.Ranges.Length;
            _lastScanFrequencyHz = completed.ScanFrequencyHz > 0 ? completed.ScanFrequencyHz : null;
        }

        ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(scanStamp, laserScan));
        UpdateProperties();
    }

    private static bool TryParseV3Packet(
        byte[] data,
        out V3HeaderLite header,
        out ushort[] rangesMm,
        out ushort[] intensities,
        out bool hasIntensity)
    {
        header = default;
        rangesMm = Array.Empty<ushort>();
        intensities = Array.Empty<ushort>();
        hasIntensity = false;

        if (data.Length < V3HeaderSize)
        {
            return false;
        }

        var magic = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(0, 2));
        if (magic != V3Magic)
        {
            return false;
        }

        header = new V3HeaderLite
        {
            Magic = magic,
            Version = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(2, 2)),
            PacketSize = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(4, 4)),
            HeaderSize = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(8, 2)),
            DistanceRatio = data[10],
            Types = data[11],
            ScanNumber = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(12, 2)),
            PacketNumber = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(14, 2)),
            TimestampDecimal = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(16, 4)),
            TimestampInteger = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(20, 4)),
            ScanFrequencyRaw = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(24, 2)),
            NumPointsScan = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(26, 2)),
            InputStatus = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(28, 2)),
            OutputStatus = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(30, 2)),
            FieldStatus = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(32, 4)),
            StartIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(36, 2)),
            EndIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(38, 2)),
            FirstIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(40, 2)),
            NumPointsPacket = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(42, 2)),
            StatusFlags = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(44, 4))
        };

        var packetSize = header.PacketSize == 0 ? data.Length : (int)Math.Min(header.PacketSize, data.Length);
        var headerSize = header.HeaderSize == 0 ? V3HeaderSize : header.HeaderSize;
        if (headerSize < V3HeaderSize || headerSize > packetSize)
        {
            return false;
        }

        var bytesPerPoint = header.Types switch
        {
            0x00 => 2,
            0x01 => 4,
            0x10 => 4,
            _ => 0
        };

        if (bytesPerPoint == 0)
        {
            return false;
        }

        var payloadBytes = packetSize - headerSize;
        var pointCount = header.NumPointsPacket;
        if (pointCount == 0 || pointCount * bytesPerPoint > payloadBytes)
        {
            pointCount = (ushort)(payloadBytes / bytesPerPoint);
        }

        if (pointCount == 0)
        {
            return false;
        }

        rangesMm = new ushort[pointCount];

        if (header.Types == 0x01)
        {
            hasIntensity = true;
            intensities = new ushort[pointCount];
        }

        var offset = headerSize;
        for (int i = 0; i < pointCount; i++)
        {
            switch (header.Types)
            {
                case 0x00:
                    rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset, 2));
                    offset += 2;
                    break;
                case 0x01:
                    rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset, 2));
                    intensities[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset + 2, 2));
                    offset += 4;
                    break;
                case 0x10:
                    rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset + 2, 2));
                    offset += 4;
                    break;
            }
        }

        return true;
    }

    private void UpdateProperties()
    {
        DateTime? timestamp;
        int pointCount;

        lock (_dataLock)
        {
            timestamp = _lastScanDataTimestamp;
            pointCount = _lastPointCount;
        }

        SetProperty("Version", _config.Version.ToString());
        SetProperty("PacketType", _config.PacketType);
        SetProperty("ScannerIp", _config.ScannerIp);
        SetProperty("LocalIp", _config.LocalIp);
        SetProperty("Port", _config.Port.ToString());
        SetProperty("FrameId", _config.FrameId);
        SetProperty("RangeMin", _config.RangeMin.ToString("F3"));
        SetProperty("RangeMax", _config.RangeMax.ToString("F3"));
        SetProperty("PointCount", pointCount.ToString());
        SetProperty("LastScanTime", timestamp?.ToString("yyyy-MM-dd HH:mm:ss.fff") ?? "");
    }

    private void CloseUdpPort()
    {
        lock (_dataLock)
        {
            _portOpened = false;

            try
            {
                _connection.udp?.Close();
            }
            catch
            {
            }

            try
            {
                _connection.udp?.Dispose();
            }
            catch
            {
            }
        }
    }

    public LaserScan? CurrentMeasurementData
    {
        get
        {
            lock (_dataLock)
            {
                return _lastLaserScan;
            }
        }
    }

    public DateTime? LastScanDataTimestamp
    {
        get
        {
            lock (_dataLock)
            {
                return _lastScanDataTimestamp;
            }
        }
    }

    public double MinAngleRad
    {
        get
        {
            lock (_dataLock)
            {
                return _lastLaserScan?.AngleMin ?? -Math.PI;
            }
        }
    }

    public double MaxAngleRad
    {
        get
        {
            lock (_dataLock)
            {
                return _lastLaserScan?.AngleMax ?? Math.PI;
            }
        }
    }

    public double MinRangeM => _config.RangeMin;
    public double MaxRangeM => _config.RangeMax;

    public double? AngularResolutionRad
    {
        get
        {
            lock (_dataLock)
            {
                var scan = _lastLaserScan;
                if (scan?.Ranges == null || scan.Value.Ranges.Length < 2)
                {
                    return null;
                }

                return scan.Value.AngleIncrement;
            }
        }
    }

    public double? ScanFrequencyHz
    {
        get
        {
            lock (_dataLock)
            {
                return _lastScanFrequencyHz;
            }
        }
    }

    public double FieldOfViewRad => Math.Abs(MaxAngleRad - MinAngleRad);
    public bool SupportsIntensity => true;
    public double? AccuracyM => null;

    public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;

    protected override void Dispose(bool disposing)
    {
        if (disposing)
        {
            StopUpdateLoop();
            CloseUdpPort();
        }

        base.Dispose(disposing);
    }

    private readonly struct V3HeaderLite
    {
        public ushort Magic { get; init; }
        public ushort Version { get; init; }
        public uint PacketSize { get; init; }
        public ushort HeaderSize { get; init; }
        public byte DistanceRatio { get; init; }
        public byte Types { get; init; }
        public ushort ScanNumber { get; init; }
        public ushort PacketNumber { get; init; }
        public uint TimestampDecimal { get; init; }
        public uint TimestampInteger { get; init; }
        public ushort ScanFrequencyRaw { get; init; }
        public ushort NumPointsScan { get; init; }
        public ushort InputStatus { get; init; }
        public ushort OutputStatus { get; init; }
        public uint FieldStatus { get; init; }
        public ushort StartIndex { get; init; }
        public ushort EndIndex { get; init; }
        public ushort FirstIndex { get; init; }
        public ushort NumPointsPacket { get; init; }
        public uint StatusFlags { get; init; }
    }

    private sealed class V3ScanAccumulator
    {
        private int _writeIndex;

        public float[] Ranges { get; }
        public float[] Intensities { get; }
        public bool IsCompleted => _writeIndex >= Ranges.Length;
        public float AngleMin { get; }
        public float AngleMax { get; }
        public float AngleIncrement { get; }
        public float TimeIncrement { get; }
        public float ScanTimeSeconds { get; }
        public double ScanFrequencyHz { get; }

        public V3ScanAccumulator(V3HeaderLite header, OleiGS15DriverConfig config)
        {
            var actualNum = Math.Max(1, (int)(header.EndIndex - header.StartIndex));
            var pointsPerScan = Math.Max(1, (int)header.NumPointsScan);

            Ranges = new float[actualNum];
            for (int i = 0; i < Ranges.Length; i++)
            {
                Ranges[i] = 0.0f;
            }

            Intensities = new float[actualNum];

            AngleIncrement = (float)((360.0 / pointsPerScan) * Math.PI / 180.0);
            AngleMin = (float)(header.StartIndex * AngleIncrement - Math.PI);
            AngleMax = (float)(header.EndIndex * AngleIncrement - Math.PI);

            var rpm = header.ScanFrequencyRaw & 0x7FFF;
            ScanFrequencyHz = rpm > 0 ? rpm / 60.0 : 0.0;
            ScanTimeSeconds = rpm > 0 ? (float)(60.0 / rpm) : 0.1f;
            TimeIncrement = ScanTimeSeconds / pointsPerScan;
        }

        public void AddPacketData(ushort[] rangesMm, ushort[] intensities, bool hasIntensity, OleiGS15DriverConfig config)
        {
            for (int i = 0; i < rangesMm.Length && _writeIndex < Ranges.Length; i++)
            {
                var rangeM = rangesMm[i] / 1000.0f;
                if (rangeM < config.RangeMin || rangeM > config.RangeMax || rangeM <= 0)
                {
                    Ranges[_writeIndex] = 0.0f;
                }
                else
                {
                    Ranges[_writeIndex] = rangeM;
                }

                if (hasIntensity && i < intensities.Length)
                {
                    Intensities[_writeIndex] = intensities[i];
                }
                else
                {
                    Intensities[_writeIndex] = 0.0f;
                }

                _writeIndex++;
            }
        }
    }
}