using System.Text;

namespace Sick.ColaB.Parsers;

/// <summary>
/// Parser for SICK scanner LMDscandata telegram
/// Supports both binary (ColaB) and ASCII (ColaA) formats
/// </summary>
public static class LmdScandataParser
{
    // Constants for validation
    private const int MinReasonableDistanceCount = 50;
    private const int MaxReasonableDistanceCount = 5000;

    /// <summary>
    /// Parse LMDscandata from binary ColaB format
    /// This is the primary parser for binary protocol data
    /// </summary>
    public static ScanDataResult? ParseBinary(ReadOnlySpan<byte> commandData)
    {
        try
        {
            // Binary LMDscandata format:
            // Starts with ASCII header: "sRA LMDscandata " or "sSN LMDscandata "
            // Then binary data follows:
            // - Various fields (version, device number, etc.)
            // - "DIST1" (5 bytes ASCII) followed by:
            //   - Scale factor (4 bytes double)
            //   - Scale offset (4 bytes double)
            //   - Starting angle (4 bytes signed int, units: 1/10000 degree)
            //   - Angular step (2 bytes unsigned short, units: 1/10000 degree)
            //   - Number of items (2 bytes, big endian uint16)
            //   - Distance values (each 2 bytes, big endian uint16, units: mm)
            // - "RSSI1" (5 bytes ASCII) followed by:
            //   - Number of items (2 bytes, big endian uint16)
            //   - RSSI values (each 1 or 2 bytes depending on resolution)

            if (commandData.Length < 50)
            {
                return null;
            }

            // Check if it contains "LMDscandata"
            var commandStart = Encoding.ASCII.GetString(commandData[..Math.Min(50, commandData.Length)]);
            if (!commandStart.Contains("LMDscandata", StringComparison.Ordinal))
            {
                return null;
            }

            double startAngleDeg = 0.0;
            double angularStepDeg = 0.0;
            double scaleFactor = 1.0;
            double scaleOffset = 0.0;

            // Find "DIST1" in the data
            var dist1Pattern = Encoding.ASCII.GetBytes("DIST1");
            var dist1Index = FindPattern(commandData, dist1Pattern);

            if (dist1Index == -1)
            {
                return null;
            }

            // Parse header fields at fixed offsets after DIST1
            int headerStart = dist1Index + 5; // Right after "DIST1"

            // Scale factor (4 bytes double, big endian) at offset headerStart
            if (headerStart + 4 <= commandData.Length)
            {
                uint scaleFactorInt = ReadUInt32BigEndian(commandData, headerStart);
                scaleFactor = BitConverter.ToSingle(BitConverter.GetBytes(scaleFactorInt), 0);
            }

            // Scale offset (4 bytes double, big endian) at offset headerStart + 4
            if (headerStart + 8 <= commandData.Length)
            {
                uint scaleOffsetInt = ReadUInt32BigEndian(commandData, headerStart + 4);
                scaleOffset = BitConverter.ToSingle(BitConverter.GetBytes(scaleOffsetInt), 0);
            }

            // Starting angle (4 bytes signed int, big endian) at offset headerStart + 8
            if (headerStart + 12 <= commandData.Length)
            {
                int startAngleInt = (int)ReadUInt32BigEndian(commandData, headerStart + 8);
                startAngleDeg = startAngleInt / 10000.0;
            }

            // Angular step width (2 bytes unsigned short, big endian) at offset headerStart + 12
            if (headerStart + 14 <= commandData.Length)
            {
                ushort angularStepInt = ReadUInt16BigEndian(commandData, headerStart + 12);
                angularStepDeg = angularStepInt / 10000.0;
            }

            // Find distance count - try multiple offsets
            if (!TryFindDistanceCount(commandData, headerStart, out ushort distCount, out int distCountOffset))
            {
                return null;
            }

            // Read distance values (each 2 bytes, big endian, units: mm)
            var distValuesOffset = distCountOffset + 2;
            if (distValuesOffset + (distCount * 2) > commandData.Length)
            {
                return null;
            }

            var ranges = new List<double>();

            for (int i = 0; i < distCount; i++)
            {
                var offset = distValuesOffset + (i * 2);
                if (offset + 2 > commandData.Length)
                {
                    break;
                }

                // Read uint16 big endian
                ushort distValue = ReadUInt16BigEndian(commandData, offset);
                // Apply scale factor and convert from mm to meters
                double distanceM = (distValue * scaleFactor + scaleOffset) / 1000.0;
                ranges.Add(distanceM);
            }

            // Find "RSSI1" for intensities (optional)
            var rssi1Pattern = Encoding.ASCII.GetBytes("RSSI1");
            var rssi1Index = FindPattern(commandData, rssi1Pattern, rssiSearchStart: distValuesOffset + (distCount * 2));
            var intensities = new List<double>();

            if (rssi1Index != -1)
            {
                // Read RSSI count
                var rssiCountOffset = rssi1Index + 5;
                if (rssiCountOffset + 2 <= commandData.Length)
                {
                    ushort rssiCount = ReadUInt16BigEndian(commandData, rssiCountOffset);

                    // Read RSSI values (usually 1 byte each, but can be 2 bytes for 16-bit resolution)
                    var rssiValuesOffset = rssiCountOffset + 2;

                    // Try to determine if RSSI is 8-bit or 16-bit
                    bool use16BitRssi = (rssiValuesOffset + (rssiCount * 2) <= commandData.Length) &&
                                       (rssiCount == ranges.Count);

                    if (use16BitRssi)
                    {
                        // 16-bit RSSI values (big endian)
                        for (int i = 0; i < rssiCount && i < ranges.Count; i++)
                        {
                            ushort rssiValue = ReadUInt16BigEndian(commandData, rssiValuesOffset + (i * 2));
                            // Normalize to 0-1 range (assuming max value is 65535)
                            intensities.Add(rssiValue);
                        }
                    }
                    else if (rssiValuesOffset + rssiCount <= commandData.Length)
                    {
                        // 8-bit RSSI values
                        for (int i = 0; i < rssiCount && i < ranges.Count; i++)
                        {
                            // Normalize to 0-1 range (assuming max value is 255)
                            intensities.Add(commandData[rssiValuesOffset + i]);
                        }
                    }
                    else
                    {
                        for (int i = 0; i < ranges.Count; i++)
                        {
                            intensities.Add(1.0);
                        }
                    }
                }
            }

            if (ranges.Count == 0)
            {
                return null;
            }

            return new ScanDataResult
            {
                Ranges = [.. ranges],
                Intensities = intensities.Count > 0 ? [.. intensities] : null,
                StartAngleDeg = startAngleDeg,
                AngularStepDeg = angularStepDeg,
                ScaleFactor = scaleFactor,
                ScaleOffset = scaleOffset,
                Timestamp = DateTime.UtcNow
            };
        }
        catch
        {
            return null;
        }
    }

    /// <summary>
    /// Parse LMDscandata from ASCII format (via ColaB decoding)
    /// This is a fallback parser when binary parsing fails
    /// </summary>
    public static ScanDataResult? ParseAscii(ReadOnlySpan<byte> commandData)
    {
        try
        {
            // Decode command string to check if it's scan data
            var commandString = ColaBHelper.DecodeCommand(commandData);

            // Check if this is scan data telegram
            if (!commandString.Contains("LMDscandata", StringComparison.Ordinal))
            {
                return null;
            }

            return ParseAsciiString(commandString);
        }
        catch
        {
            return null;
        }
    }

    /// <summary>
    /// Parse LMDscandata from ASCII string format
    /// Format: "sSN LMDscandata <version> <device> ... DIST1 <count> <distances> ... RSSI1 <count> <intensities> ..."
    /// </summary>
    public static ScanDataResult? ParseAsciiString(string telegram)
    {
        try
        {
            // Split telegram into fields (space-separated)
            var fields = telegram.Split([' '], StringSplitOptions.RemoveEmptyEntries);

            if (fields.Length < 20)
            {
                return null;
            }

            var ranges = new List<double>();
            var intensities = new List<double>();

            int distIndex = -1;
            int rssiIndex = -1;

            // Find DIST1 field
            for (int i = 0; i < fields.Length; i++)
            {
                if (fields[i].StartsWith("DIST", StringComparison.Ordinal))
                {
                    distIndex = i;
                    break;
                }
            }

            // Find RSSI1 field
            for (int i = 0; i < fields.Length; i++)
            {
                if (fields[i].StartsWith("RSSI", StringComparison.Ordinal))
                {
                    rssiIndex = i;
                    break;
                }
            }

            // Parse distances
            if (distIndex >= 0 && distIndex + 1 < fields.Length)
            {
                // Next field after DIST1 is the count (in hex)
                if (int.TryParse(fields[distIndex + 1], System.Globalization.NumberStyles.HexNumber, null, out int distCount))
                {
                    // Parse distance values (in hex, units are mm, convert to meters)
                    for (int i = 0; i < distCount && distIndex + 2 + i < fields.Length; i++)
                    {
                        if (int.TryParse(fields[distIndex + 2 + i], System.Globalization.NumberStyles.HexNumber, null, out int distValue))
                        {
                            // Convert from mm to meters
                            ranges.Add(distValue / 1000.0);
                        }
                    }
                }
            }

            // Parse intensities (RSSI)
            if (rssiIndex >= 0 && rssiIndex + 1 < fields.Length)
            {
                // Next field after RSSI1 is the count (in hex)
                if (int.TryParse(fields[rssiIndex + 1], System.Globalization.NumberStyles.HexNumber, null, out int rssiCount))
                {
                    // Parse RSSI values (in hex)
                    for (int i = 0; i < rssiCount && rssiIndex + 2 + i < fields.Length; i++)
                    {
                        if (int.TryParse(fields[rssiIndex + 2 + i], System.Globalization.NumberStyles.HexNumber, null, out int rssiValue))
                        {
                            intensities.Add(rssiValue);
                        }
                    }
                }
            }

            if (ranges.Count == 0)
            {
                return null;
            }

            return new ScanDataResult
            {
                Ranges = [.. ranges],
                Intensities = intensities.Count > 0 ? [.. intensities] : null,
                StartAngleDeg = 0.0, // ASCII format doesn't include angle info
                AngularStepDeg = 0.0,
                ScaleFactor = 1.0,
                ScaleOffset = 0.0,
                Timestamp = DateTime.UtcNow
            };
        }
        catch
        {
            return null;
        }
    }

    #region Helper Methods

    private static int FindPattern(ReadOnlySpan<byte> data, byte[] pattern, int rssiSearchStart = 0)
    {
        int searchStart = rssiSearchStart;
        for (int i = searchStart; i <= data.Length - pattern.Length; i++)
        {
            bool found = true;
            for (int j = 0; j < pattern.Length; j++)
            {
                if (data[i + j] != pattern[j])
                {
                    found = false;
                    break;
                }
            }
            if (found)
            {
                return i;
            }
        }
        return -1;
    }

    private static uint ReadUInt32BigEndian(ReadOnlySpan<byte> data, int offset)
    {
        if (offset + 4 > data.Length)
            return 0;

        return (uint)((data[offset] << 24) | (data[offset + 1] << 16) |
                     (data[offset + 2] << 8) | data[offset + 3]);
    }

    private static ushort ReadUInt16BigEndian(ReadOnlySpan<byte> data, int offset)
    {
        if (offset + 2 > data.Length)
            return 0;

        return (ushort)((data[offset] << 8) | data[offset + 1]);
    }

    private static bool TryFindDistanceCount(ReadOnlySpan<byte> commandData, int headerStart,
        out ushort distCount, out int distCountOffset)
    {
        distCount = 0;
        distCountOffset = -1;

        // Try offset +14 first (matches actual data)
        int tryOffset1 = headerStart + 14;
        if (TryReadDistanceCount(commandData, tryOffset1, out distCount, out distCountOffset))
            return true;

        // Try offset +19 (per C++ reference code)
        int tryOffset2 = headerStart + 19;
        if (TryReadDistanceCount(commandData, tryOffset2, out distCount, out distCountOffset))
            return true;

        // Search for valid count
        int searchStart = headerStart;
        int searchEnd = Math.Min(headerStart + 30, commandData.Length - 2);

        for (int offset = searchStart; offset <= searchEnd; offset++)
        {
            if (TryReadDistanceCount(commandData, offset, out distCount, out distCountOffset))
                return true;
        }

        return false;
    }

    private static bool TryReadDistanceCount(ReadOnlySpan<byte> commandData, int offset,
        out ushort count, out int countOffset)
    {
        count = 0;
        countOffset = -1;

        if (offset + 2 > commandData.Length)
            return false;

        ushort testCount = ReadUInt16BigEndian(commandData, offset);

        if (testCount >= MinReasonableDistanceCount && testCount <= MaxReasonableDistanceCount)
        {
            int requiredBytes = offset + 2 + (testCount * 2);
            if (requiredBytes <= commandData.Length)
            {
                count = testCount;
                countOffset = offset;
                return true;
            }
        }

        return false;
    }

    #endregion
}