using System.Runtime.InteropServices;

namespace Olei.LidarSensor;

/// <summary>
/// High-performance parser for LiDAR data packets
/// Uses Span&lt;byte&gt; and MemoryMarshal for zero-allocation parsing
/// </summary>
public static class LidarPacketParser
{
    /// <summary>
    /// Parse raw UDP packet data into LidarDataPacket
    /// </summary>
    /// <param name="buffer">Raw byte buffer from UDP socket</param>
    /// <param name="length">Actual length of received data</param>
    /// <param name="packet">Output packet to populate (reusable)</param>
    /// <returns>True if parsing successful, false if invalid data</returns>
    public static bool TryParse(byte[] buffer, int length, LidarDataPacket packet)
    {
        if (length < LidarDataPacket.PACKET_SIZE)
            return false;

        return TryParse(buffer.AsSpan(0, length), packet);
    }

    /// <summary>
    /// Parse raw UDP packet data into LidarDataPacket using Span
    /// High performance, zero-allocation parsing
    /// </summary>
    /// <param name="data">Raw byte span</param>
    /// <param name="packet">Output packet to populate (reusable)</param>
    /// <returns>True if parsing successful, false if invalid data</returns>
    public static bool TryParse(ReadOnlySpan<byte> data, LidarDataPacket packet)
    {
        if (data.Length < LidarDataPacket.PACKET_SIZE)
            return false;

        // Parse header (40 bytes)
        if (!TryParseHeader(data.Slice(0, LidarDataPacket.HEADER_SIZE), out var header))
            return false;

        packet.Header = header;
        packet.ReceivedTime = DateTime.UtcNow;

        // Parse data blocks (150 blocks × 8 bytes)
        ReadOnlySpan<byte> dataBlocksSpan = data.Slice(
            LidarDataPacket.HEADER_SIZE,
            LidarDataPacket.DATA_BLOCKS_TOTAL_SIZE
        );

        for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
        {
            int offset = i * LidarDataPacket.DATA_BLOCK_SIZE;
            ReadOnlySpan<byte> blockSpan = dataBlocksSpan.Slice(offset, LidarDataPacket.DATA_BLOCK_SIZE);

            packet.DataBlocks[i] = ParseDataBlock(blockSpan);
        }

        return true;
    }

    /// <summary>
    /// Parse header from byte span
    /// </summary>
    private static bool TryParseHeader(ReadOnlySpan<byte> data, out LidarHeader header)
    {
        if (data.Length < LidarDataPacket.HEADER_SIZE)
        {
            header = default;
            return false;
        }

        // Use MemoryMarshal for fast struct parsing
        header = MemoryMarshal.Read<LidarHeader>(data);

        // Validate frame ID
        return header.IsValidFrame;
    }

    /// <summary>
    /// Parse single data block from byte span
    /// </summary>
    private static LidarDataBlock ParseDataBlock(ReadOnlySpan<byte> data)
    {
        // Use MemoryMarshal for fast struct parsing
        return MemoryMarshal.Read<LidarDataBlock>(data);
    }

    /// <summary>
    /// Validate packet without full parsing
    /// Useful for quick validation before processing
    /// </summary>
    public static bool IsValidPacket(ReadOnlySpan<byte> data)
    {
        if (data.Length < LidarDataPacket.PACKET_SIZE)
            return false;

        // Check frame ID (first 4 bytes)
        uint frameId = MemoryMarshal.Read<uint>(data);
        return frameId == LidarHeader.FRAME_ID;
    }

    /// <summary>
    /// Extract distance scale from packet without full parsing
    /// </summary>
    public static byte GetDistanceScale(ReadOnlySpan<byte> data)
    {
        if (data.Length < 7)
            return 0;

        return data[6]; // Distance scale is at offset 6
    }

    /// <summary>
    /// Extract timestamp from packet without full parsing
    /// </summary>
    public static uint GetTimestamp(ReadOnlySpan<byte> data)
    {
        if (data.Length < 32)
            return 0;

        return MemoryMarshal.Read<uint>(data.Slice(28, 4));
    }

    /// <summary>
    /// Extract error status from packet without full parsing
    /// </summary>
    public static byte GetErrorStatus(ReadOnlySpan<byte> data)
    {
        if (data.Length < 36)
            return 0;

        return data[35]; // Error status is at offset 35
    }

    /// <summary>
    /// Quick check if packet has any errors
    /// </summary>
    public static bool HasErrors(ReadOnlySpan<byte> data)
    {
        return GetErrorStatus(data) != 0;
    }
}