"""u-blox UBX protocol: build config messages and read NAV-PVT (position, velocity, time).

The HGLRC M100-5883 has a u-blox M10 GPS. We switch it to 115200 baud, 10 fixes a
second, and binary NAV-PVT messages only (NMEA text off), which carry ground speed
straight from the Doppler measurement (accurate to a few cm/s).
"""
import struct

SYNC = b"\xb5\x62"

# Configuration keys (u-blox M10 interface description) and their value sizes.
KEYS = {
    "CFG-UART1-BAUDRATE": (0x40520001, "<I"),
    "CFG-RATE-MEAS": (0x30210001, "<H"),          # ms between measurements
    "CFG-RATE-NAV": (0x30210002, "<H"),           # measurements per solution
    "CFG-UART1OUTPROT-UBX": (0x10740001, "<B"),
    "CFG-UART1OUTPROT-NMEA": (0x10740002, "<B"),
    "CFG-MSGOUT-UBX_NAV_PVT_UART1": (0x20910007, "<B"),
    "CFG-NAVSPG-DYNMODEL": (0x20110021, "<B"),    # 4 = automotive
    "CFG-SIGNAL-BDS_ENA": (0x10310022, "<B"),
    "CFG-SIGNAL-GLO_ENA": (0x10310025, "<B"),
}
LAYER_RAM = 0x01
LAYER_BBR = 0x02  # battery-backed RAM: survives a warm restart if the module keeps power


def checksum(body):
    a = b = 0
    for byte in body:
        a = (a + byte) & 0xFF
        b = (b + a) & 0xFF
    return bytes((a, b))


def frame(msg_class, msg_id, payload=b""):
    body = struct.pack("<BBH", msg_class, msg_id, len(payload)) + payload
    return SYNC + body + checksum(body)


def valset(values, layers=LAYER_RAM | LAYER_BBR):
    """UBX-CFG-VALSET (0x06 0x8A): values is {key name: value}."""
    payload = struct.pack("<BBH", 0, layers, 0)
    for name, value in values.items():
        key_id, fmt = KEYS[name]
        payload += struct.pack("<I", key_id) + struct.pack(fmt, value)
    return frame(0x06, 0x8A, payload)


FIX_NAMES = {0: "no fix", 1: "dead reckoning", 2: "2D", 3: "3D", 4: "GPS+DR", 5: "time only"}


def parse_nav_pvt(p):
    """NAV-PVT payload (92 bytes) -> dict in plain units."""
    if len(p) < 92:
        return None
    (itow, year, month, day, hour, minute, sec, valid, t_acc, nano, fix_type, flags,
     flags2, num_sv, lon, lat, height, h_msl, h_acc, v_acc, vel_n, vel_e, vel_d,
     g_speed, head_mot, s_acc, head_acc, p_dop) = struct.unpack_from(
        "<IHBBBBBBIiBBBBiiiiIIiiiiiIIH", p, 0)
    return {
        "itow_ms": itow,
        "utc": "%04d-%02d-%02dT%02d:%02d:%02dZ" % (year, month, day, hour, minute, sec),
        "fix": fix_type,
        "fix_ok": bool(flags & 0x01) and fix_type in (3, 4),
        "sats": num_sv,
        "lat": lat * 1e-7,
        "lon": lon * 1e-7,
        "alt_m": h_msl / 1000.0,
        "speed_mps": g_speed / 1000.0,
        "vel_down_mps": vel_d / 1000.0,
        "heading_deg": head_mot * 1e-5,
        "speed_acc_mps": s_acc / 1000.0,
        "h_acc_m": h_acc / 1000.0,
    }


class Reader:
    """Feed raw bytes in; get complete UBX messages out as (class, id, payload)."""

    def __init__(self):
        self.buf = bytearray()

    def feed(self, data):
        self.buf.extend(data)
        out = []
        while True:
            start = self.buf.find(SYNC)
            if start < 0:
                # keep a trailing 0xB5 in case the 0x62 is in the next chunk
                self.buf = self.buf[-1:] if self.buf.endswith(b"\xb5") else bytearray()
                return out
            if start:
                del self.buf[:start]
            if len(self.buf) < 6:
                return out
            length = struct.unpack_from("<H", self.buf, 4)[0]
            if length > 2048:  # garbage that happened to look like a header
                del self.buf[:2]
                continue
            total = 6 + length + 2
            if len(self.buf) < total:
                return out
            body = bytes(self.buf[2:6 + length])
            if checksum(body) == bytes(self.buf[6 + length:total]):
                out.append((body[0], body[1], body[4:]))
                del self.buf[:total]
            else:
                del self.buf[:2]