import struct

from ubx.msg_types import *

UBX_SYNC_WORD     = struct.pack('BB', UBX_SYNC1, UBX_SYNC2)

def checksum(data: bytes) -> bytes:
	ck_a = 0
	ck_b = 0
	for i in range(len(data)):
		ck_a = (ck_a + data[i]) & 0xFF
		ck_b = (ck_b + ck_a) & 0xFF

	return struct.pack("BB", ck_a, ck_b)

def frame_create(_class: int, _id: int, _data: bytes = b'') -> bytes:
	_hdr = struct.pack('B', _class)
	_hdr += struct.pack('B', _id)
	_hdr += struct.pack('<H', len(_data))
	_check = checksum(_hdr + _data)

	return UBX_SYNC_WORD + _hdr + _data + _check

class Hdr:
	def __init__(self, _class: int, _id: int, _length: int):
		self.mclass = _class
		self.mid = _id
		self.length = _length


def frame_parse_hdr(frame: bytes) -> Hdr|None:
	hdr = Hdr(0,0,0)

	if len(frame) < 4:
		return None

	hdr.mclass = struct.unpack('B', frame[0:1])[0]
	hdr.mid = struct.unpack('B', frame[1:2])[0]
	hdr.length = struct.unpack('<H', frame[2:4])[0]

	return hdr

def frame_parse(frame: bytes):
	_data = None
	_hdr = Hdr(0,0,0)

	while True:
		_hdr = frame_parse_hdr(frame[0:4])
		if _hdr is None:
			break

		if (4+_hdr.length+2) != len(frame):
			break

		_check_soll = frame[-2:]
		_check_ist = checksum(frame[0:-2])
		if _check_soll != _check_ist:
			break

		_data = frame[4:-2]
		break

	return _data, _hdr

if __name__ == "__main__":
	result = frame_create(1, 2, b'Hallo')
	print(result)
	payload, hdr = frame_parse(result)
	print(payload)