using System.Runtime.InteropServices;

namespace Olei.LidarSensor;

/// <summary>
/// Data block structure for each measurement point (8 bytes)
/// All data is in little-endian format
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct LidarDataBlock
{
    /// <summary>
    /// Invalid angle marker
    /// </summary>
    public const ushort INVALID_ANGLE = 0xFF00;

    /// <summary>
    /// Raw angle value (0~35999)
    /// Unit: 0.01°/LSB, range 0° ~ 359.99°
    /// Data block is invalid if this value >= 0xFF00
    /// </summary>
    public ushort AngleRaw;

    /// <summary>
    /// Distance readout data (unsigned integer)
    /// Actual distance = readout data × distance scale
    /// </summary>
    public ushort DistanceRaw;

    /// <summary>
    /// Signal strength (0~65535)
    /// Indicates the strength of the received signal
    /// </summary>
    public ushort SignalStrength;

    /// <summary>
    /// Reserved for future use
    /// </summary>
    public ushort Reserved;

    /// <summary>
    /// Check if this data block is valid
    /// </summary>
    public readonly bool IsValid => AngleRaw < INVALID_ANGLE;

    /// <summary>
    /// Get angle in degrees (0.00° ~ 359.99°)
    /// </summary>
    public readonly double GetAngleDegrees()
    {
        return AngleRaw * 0.01;
    }

    /// <summary>
    /// Get angle in radians
    /// </summary>
    public readonly double GetAngleRadians()
    {
        return AngleRaw * 0.01 * Math.PI / 180.0;
    }

    /// <summary>
    /// Get actual distance based on distance scale
    /// </summary>
    /// <param name="distanceScale">Distance scale from header</param>
    /// <returns>Actual distance in the same unit as distance scale</returns>
    public readonly double GetDistance(byte distanceScale)
    {
        return DistanceRaw * distanceScale;
    }

    /// <summary>
    /// Get X coordinate (distance * cos(angle))
    /// </summary>
    /// <param name="distanceScale">Distance scale from header</param>
    public readonly double GetX(byte distanceScale)
    {
        double distance = GetDistance(distanceScale);
        double angleRad = GetAngleRadians();
        return distance * Math.Cos(angleRad);
    }

    /// <summary>
    /// Get Y coordinate (distance * sin(angle))
    /// </summary>
    /// <param name="distanceScale">Distance scale from header</param>
    public readonly double GetY(byte distanceScale)
    {
        double distance = GetDistance(distanceScale);
        double angleRad = GetAngleRadians();
        return distance * Math.Sin(angleRad);
    }
}