using GeoVLog.Core.Models;
using System;

namespace GeoVLog.Core.Parsers;

/// <summary>
/// Parser for FreeFlight RA-4500 radar-altimeter frames.
/// Validates the frame structure, collapses DLE stuffing and checks the checksum.
/// </summary>
internal static class RadarAltParser
{
    private const byte Dle = 0x10;
    private const byte Etx = 0x03;
    private const byte Id = 0xDF;
    private const byte Len = 0x03;

    public static bool TryParse(ReadOnlySpan<byte> msg, DateTime arrivalUtc, out RadarAltReading parsed)
    {
        parsed = default!;
        if (msg.Length < 5)
            return false;
        if (msg[0] != Dle || msg[^2] != Dle || msg[^1] != Etx)
            return false;

        Span<byte> buf = stackalloc byte[6];
        int bi = 0;

        for (int i = 1; i < msg.Length - 2 && bi < buf.Length; i++)
        {
            byte b = msg[i];
            if (b == Dle)
            {
                if (i + 1 >= msg.Length - 2 || msg[i + 1] != Dle)
                    return false; // unexpected DLE not stuffed
                buf[bi++] = Dle;
                i++; // skip stuffed byte
            }
            else
            {
                buf[bi++] = b;
            }
        }

        if (bi != buf.Length)
            return false;
        if (buf[0] != Id || buf[1] != Len)
            return false;

        byte computed = 0;
        for (int i = 0; i < 5; i++)
            computed += buf[i];
        computed = (byte)((~computed + 1) & 0xFF);
        if (computed != buf[5])
            return false;

        ushort altitude = (ushort)((buf[2] << 8) | buf[3]);
        byte status = buf[4];

        parsed = new RadarAltReading
        {
            TimestampUtc = arrivalUtc,
            RawPayload = msg.ToArray(),
            AltitudeFt = altitude,
            StatusByte = status
        };
        return true;
    }
}