#!/usr/bin/env python3
# -*- coding: utf-8 -*-

from smbus2 import SMBus, i2c_msg
import time

I2C_BUS_NR = 2          # /dev/i2c-2
UBLOX_ADDR = 0x42       # NEO-M8N I2C (DDC) address

# UBX-MON-VER poll frame: B5 62 0A 04 00 00 0E 34
UBX_MON_VER_POLL = bytes([
    0xB5, 0x62,  # sync chars
    0x0A, 0x04,  # class = MON (0x0A), id = VER (0x04)
    0x00, 0x00,  # length LSB/MSB = 0
    0x0E, 0x34   # CK_A, CK_B
])


def ubx_checksum(data: bytes):
    """
    Calculate UBX checksum.
    Input 'data' must be [CLASS, ID, LEN_L, LEN_H, payload...]
    Returns (CK_A, CK_B).
    """
    ck_a = 0
    ck_b = 0
    for b in data:
        ck_a = (ck_a + b) & 0xFF
        ck_b = (ck_b + ck_a) & 0xFF
    return ck_a, ck_b


def read_available_length(bus: SMBus, timeout=2.0) -> int:
    """
    Read the current number of bytes available from DDC registers 0xFD/0xFE.
    Returns 0 on timeout.
    """
    start = time.time()
    while time.time() - start < timeout:
        # Write register address 0xFD, then read 2 bytes (low, high)
        write = i2c_msg.write(UBLOX_ADDR, [0xFD])
        read = i2c_msg.read(UBLOX_ADDR, 2)
        bus.i2c_rdwr(write, read)
        lo, hi = list(read)
        available = lo | (hi << 8)
        if available > 0:
            return available
        time.sleep(0.05)
    return 0


def read_ddc_stream(bus: SMBus, length: int) -> bytes:
    """
    Read 'length' bytes from DDC data stream register 0xFF.
    """
    write = i2c_msg.write(UBLOX_ADDR, [0xFF])
    read = i2c_msg.read(UBLOX_ADDR, length)
    bus.i2c_rdwr(write, read)
    return bytes(read)


def find_mon_ver_payload(buf: bytes):
    """
    Search for a UBX-MON-VER (class=0x0A, id=0x04) frame in 'buf'.
    Returns the payload bytes if found, otherwise None.
    """
    i = 0
    while i + 7 <= len(buf):  # we need at least header + len + checksum
        if buf[i] == 0xB5 and buf[i+1] == 0x62:
            if i + 6 > len(buf):
                break
            msg_class = buf[i+2]
            msg_id = buf[i+3]
            length = buf[i+4] | (buf[i+5] << 8)
            frame_end = i + 6 + length + 2  # header+len + payload + checksum
            if frame_end > len(buf):
                # Frame not fully received
                break
            if msg_class == 0x0A and msg_id == 0x04:
                payload = buf[i+6: i+6+length]
                ck_a, ck_b = ubx_checksum(buf[i+2:i+6+length])
                if ck_a == buf[frame_end-2] and ck_b == buf[frame_end-1]:
                    return payload
            # Move forward one byte and continue searching
            i += 1
        else:
            i += 1
    return None


def decode_version_strings(payload: bytes):
    """
    Decode swVersion and hwVersion from MON-VER payload.

    Correct structure for u-blox 8 / M8:
    - [0:30]  : swVersion (C string, 30 bytes)
    - [30:40] : hwVersion (C string, 10 bytes)
    - [40:..] : extension strings, 30 bytes each
    """
    if len(payload) < 40:
        return None, None, []

    def c_str(slice_bytes: bytes) -> str:
        return slice_bytes.split(b'\0', 1)[0].decode('ascii', errors='ignore').strip()

    sw = c_str(payload[0:30])
    hw = c_str(payload[30:40])   # <-- 10 bytes only

    extras = []
    offset = 40                  # <-- extensions start here
    while offset + 30 <= len(payload):
        s = c_str(payload[offset:offset+30])
        if s:
            extras.append(s)
        offset += 30

    return sw, hw, extras


def main():
    with SMBus(I2C_BUS_NR) as bus:
        # 1) Send UBX-MON-VER poll frame to 0xFF data stream register
        write = i2c_msg.write(UBLOX_ADDR, [0xFF] + list(UBX_MON_VER_POLL))
        bus.i2c_rdwr(write)

        # 2) Wait for receiver to respond, check how many bytes are available
        available = read_available_length(bus, timeout=2.0)
        if available == 0:
            print("No data received from NEO-M8N within timeout.")
            return

        # Safety limit to avoid reading unexpectedly large amounts of data
        if available > 512:
            available = 512

        # 3) Read those bytes from the DDC data stream
        data = read_ddc_stream(bus, available)

        # 4) Search for UBX-MON-VER in the incoming data
        payload = find_mon_ver_payload(data)
        if payload is None:
            print("UBX-MON-VER message not found in received data.")
            return

        sw, hw, extras = decode_version_strings(payload)

        print("=== NEO-M8N Version info ===")
        print("Firmware (swVersion):", sw or "<unknown>")
        print("Hardware (hwVersion):", hw or "<unknown>")
        if extras:
            print("Extra info:")
            for line in extras:
                print("  -", line)


if __name__ == "__main__":
    main()