namespace Olei.LidarSensor;

/// <summary>
/// Complete LiDAR data packet (1240 bytes total)
/// Contains header (40 bytes) + 150 data blocks (1200 bytes)
/// </summary>
public sealed class LidarDataPacket
{
    /// <summary>
    /// Total packet size in bytes
    /// </summary>
    public const int PACKET_SIZE = 1240;

    /// <summary>
    /// Header size in bytes
    /// </summary>
    public const int HEADER_SIZE = 40;

    /// <summary>
    /// Number of data blocks per packet
    /// </summary>
    public const int DATA_BLOCK_COUNT = 150;

    /// <summary>
    /// Size of each data block in bytes
    /// </summary>
    public const int DATA_BLOCK_SIZE = 8;

    /// <summary>
    /// Total size of all data blocks
    /// </summary>
    public const int DATA_BLOCKS_TOTAL_SIZE = DATA_BLOCK_COUNT * DATA_BLOCK_SIZE;

    /// <summary>
    /// Packet header information
    /// </summary>
    public LidarHeader Header { get; set; }

    /// <summary>
    /// Array of data blocks (150 blocks)
    /// Using array instead of list for better performance
    /// </summary>
    public LidarDataBlock[] DataBlocks { get; }

    /// <summary>
    /// Timestamp when this packet was received
    /// </summary>
    public DateTime ReceivedTime { get; set; }

    public LidarDataPacket()
    {
        DataBlocks = new LidarDataBlock[DATA_BLOCK_COUNT];
        ReceivedTime = DateTime.UtcNow;
    }

    /// <summary>
    /// Get all valid data points (where angle is valid)
    /// </summary>
    public IEnumerable<LidarDataBlock> GetValidDataBlocks()
    {
        for (int i = 0; i < DATA_BLOCK_COUNT; i++)
        {
            if (DataBlocks[i].IsValid)
            {
                yield return DataBlocks[i];
            }
        }
    }

    /// <summary>
    /// Get count of valid data blocks
    /// </summary>
    public int GetValidDataBlockCount()
    {
        int count = 0;
        for (int i = 0; i < DATA_BLOCK_COUNT; i++)
        {
            if (DataBlocks[i].IsValid)
            {
                count++;
            }
        }
        return count;
    }

    /// <summary>
    /// Get all valid points as (angle, distance) tuples
    /// </summary>
    public IEnumerable<(double angle, double distance)> GetValidPoints()
    {
        byte distanceScale = Header.DistanceScale;
        for (int i = 0; i < DATA_BLOCK_COUNT; i++)
        {
            if (DataBlocks[i].IsValid)
            {
                yield return (
                    DataBlocks[i].GetAngleDegrees(),
                    DataBlocks[i].GetDistance(distanceScale)
                );
            }
        }
    }

    /// <summary>
    /// Get all valid points in Cartesian coordinates (X, Y)
    /// </summary>
    public IEnumerable<(double x, double y)> GetValidCartesianPoints()
    {
        byte distanceScale = Header.DistanceScale;
        for (int i = 0; i < DATA_BLOCK_COUNT; i++)
        {
            if (DataBlocks[i].IsValid)
            {
                yield return (
                    DataBlocks[i].GetX(distanceScale),
                    DataBlocks[i].GetY(distanceScale)
                );
            }
        }
    }

    /// <summary>
    /// Get summary information about this packet
    /// </summary>
    public override string ToString()
    {
        return $"LidarPacket [Valid: {Header.IsValidFrame}, ValidBlocks: {GetValidDataBlockCount()}/{DATA_BLOCK_COUNT}, " +
               $"RotationRate: {Header.RotationRate}, HasError: {Header.HasError}, Time: {ReceivedTime:HH:mm:ss.fff}]";
    }
}