import struct

from msg_container import MsgContainer
from msg_sink import MsgSink


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) -> bytes:
	_sync = b'\xb5\x62'
	_hdr = struct.pack('B', _class)
	_hdr += struct.pack('B', _id)
	_hdr += struct.pack('<H', len(_data))
	_check = checksum(_hdr + _data)

	return _sync + _hdr + _data + _check

def frame_parse(frame: bytes):
	_class = 0
	_id = 0
	_data = None

	while True:
		hdr = frame[0:2]
		if hdr != b'\xb5\x62':
			break

		_class = struct.unpack('B', frame[2:3])[0]
		_id = struct.unpack('B', frame[3:4])[0]
		_length = struct.unpack('<H', frame[4:6])[0]
		if (6+_length+2) != len(frame):
			break

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

		_data = frame[6:-2]
		break

	return _data, _class, _id

class UbxReceiver(MsgSink):
	def __init__(self, name: str, mid: int, mclass: int):
		MsgSink.__init__(self)
		self.name = name
		self.mid = mid
		self.mclass = mclass

	def on_recv(self, msg: MsgContainer):
		data, mclass, mid = frame_parse(msg.data)
		print(f"{mclass}:{mid}:{data}")


	def is_msg(self, msg: MsgContainer):
		data, mclass, mid = frame_parse(msg.data)
		if data is None:
			return False

		if mclass != self.mclass:
			return False

		if mid != self.mid:
			return False

		return True

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