using GeoVLog.Core.Models;
using System;
using System.Globalization;
using System.Text;

namespace GeoVLog.Core.Parsers;

/// <summary>
/// Parser for the SBG Ellipse2-N proprietary $PRDID sentence carrying pitch, roll and heading.
/// Only messages with a valid checksum are parsed.
/// </summary>
internal static class ImuParser
{
    public static bool TryParse(ReadOnlySpan<byte> msg, DateTime arrivalUtc, out ImuReading parsed)
    {
        parsed = default!;
        var line = Encoding.ASCII.GetString(msg).Trim();
        int star = line.IndexOf('*');
        if (star <= 0 || star + 3 > line.Length)
            return false;
        byte cs = 0;
        for (int i = 1; i < star; i++)
            cs ^= (byte)line[i];
        if (!byte.TryParse(line.AsSpan(star + 1, 2), NumberStyles.HexNumber, CultureInfo.InvariantCulture, out var given) || given != cs)
            return false;

        string[] parts = line[..star].Split(',');
        if (parts.Length < 4 || parts[0] != "$PRDID")
            return false;
        if (!double.TryParse(parts[1], NumberStyles.Float, CultureInfo.InvariantCulture, out double pitch))
            return false;
        if (!double.TryParse(parts[2], NumberStyles.Float, CultureInfo.InvariantCulture, out double roll))
            return false;
        if (!double.TryParse(parts[3], NumberStyles.Float, CultureInfo.InvariantCulture, out double heading))
            return false;

        parsed = new ImuReading
        {
            TimestampUtc = arrivalUtc,
            RawLine = line,
            PitchDeg = pitch,
            RollDeg = roll,
            HeadingDeg = heading
        };
        return true;
    }
}