using Sick.SafetyScanners.DataStructures;
using Sick.SafetyScanners.Helpers;

namespace Sick.SafetyScanners.DataProcessing;

/// <summary>
/// Parser for derived values block from UDP packets
/// Contains configuration of data output: multiplication factor, number of beams, scan time, angles, resolution
/// Thread-safe implementation
/// </summary>
public sealed class ParseDerivedValues
{
    /// <summary>
    /// Parses the derived values block from a UDP sequence
    /// </summary>
    /// <param name="buffer">Packet buffer containing the data</param>
    /// <param name="header">Parsed data header (must contain valid block offset/size)</param>
    public DerivedValues ParseUdpSequence(PacketBuffer buffer, DataHeader header)
    {
        if (buffer == null)
            throw new ArgumentNullException(nameof(buffer));
        
        if (header == null)
            throw new ArgumentNullException(nameof(header));
        
        // Check if derived values block is enabled
        if (!CheckIfDerivedValuesIsPublished(header))
        {
            return new DerivedValues
            {
                IsEmpty = true
            };
        }
        
        // Check if header is valid
        if (header.IsEmpty)
        {
            return new DerivedValues
            {
                IsEmpty = true
            };
        }
        
        var bufferData = buffer.GetBuffer();
        var offset = header.DerivedValuesBlockOffset;
        
        // Validate buffer size
        if (offset + 20 > bufferData.Length) // Derived values block is at least 20 bytes
        {
            throw new ArgumentException(
                $"Buffer too small to contain derived values block (offset: {offset}, buffer size: {bufferData.Length})",
                nameof(buffer));
        }
        
        var span = bufferData.Span.Slice(offset);
        
        // Parse all fields from the derived values block
        // Format (all little endian):
        // Offset 0:  Multiplication factor (uint16)
        // Offset 2:  Number of beams (uint16)
        // Offset 4:  Scan time (uint16, milliseconds)
        // Offset 6:  Reserved (2 bytes)
        // Offset 8:  Start angle (int32, sensor units)
        // Offset 12: Angular beam resolution (int32, sensor units)
        // Offset 16: Interbeam period (uint32, microseconds)
        
        var multiplicationFactor = ReadWriteHelper.ReadUint16LittleEndian(span, 0);
        var numberOfBeams = ReadWriteHelper.ReadUint16LittleEndian(span, 2);
        var scanTime = ReadWriteHelper.ReadUint16LittleEndian(span, 4);
        var startAngleSensorUnits = ReadWriteHelper.ReadInt32LittleEndian(span, 8);
        var angularBeamResolutionSensorUnits = ReadWriteHelper.ReadInt32LittleEndian(span, 12);
        var interbeamPeriod = ReadWriteHelper.ReadUint32LittleEndian(span, 16);
        
        // Convert angles from sensor units to radians
        // According to COLA2 documentation: "This value, divided by 4194304, equals the actual start angle"
        // According to C++ reference: value / 4194304.0 gives degrees (see setDerivedAngularBeamResolutionDegrees)
        // The C++ reference's setDerivedAngularBeamResolution divides by 4194304.0, and setDerivedAngularBeamResolutionDegrees
        // sets degrees directly, indicating that dividing by 4194304.0 gives degrees.
        // However, looking at ParseMeasurementData which uses these values to calculate scan point angles,
        // and the fact that 4194304.0 units represent a full circle (360° = 2π radians),
        // it appears the C++ reference stores values as "full circle units" (1.0 = 360° = 2π radians).
        // So: sensorUnits / 4194304.0 = full circle units, then * 2π = radians
        // OR: sensorUnits / 4194304.0 = degrees / 360.0, so * 360 = degrees, then * π/180 = radians
        // Since user reports resolution shows 60° which is too large, let's check the actual conversion.
        // Based on user feedback that resolution shows 60° (not typical ~0.1°), the current formula
        // (multiplying by 2π) might be converting full circle units incorrectly.
        // Let's try: sensorUnits / 4194304.0 already gives degrees (as per C++ setDerivedAngularBeamResolutionDegrees pattern)
        var startAngleDeg = (startAngleSensorUnits / DerivedValues.AngleResolution);
        var angularBeamResolutionDeg = (angularBeamResolutionSensorUnits / DerivedValues.AngleResolution);
        // Convert degrees to radians
        var startAngleRad = (startAngleDeg * Math.PI / 180.0);
        var angularBeamResolutionRad = (angularBeamResolutionDeg * Math.PI / 180.0);
        
        return new DerivedValues
        {
            MultiplicationFactor = multiplicationFactor,
            NumberOfBeams = numberOfBeams,
            ScanTime = scanTime,
            StartAngle = startAngleRad,
            AngularBeamResolution = angularBeamResolutionRad,
            InterbeamPeriod = interbeamPeriod,
            IsEmpty = false
        };
    }
    
    /// <summary>
    /// Checks if derived values block is published (enabled)
    /// </summary>
    private static bool CheckIfDerivedValuesIsPublished(DataHeader header)
    {
        return !(header.DerivedValuesBlockOffset == 0 && header.DerivedValuesBlockSize == 0);
    }
}