import struct
from msg_container import MsgContainer
from msg_sink import MsgSink
from msg_source import MsgSource
from ubx.ubx import UBX_SYNC_WORD, frame_parse, frame_create
from receiver.ubx_receiver import UbxReceiver

UBX_PKT_DEBUG = False
def ubx_pkt_debug(s: str):
	if UBX_PKT_DEBUG:
		print(s)

class UbxPacketizer(MsgSink, MsgSource):
	def __init__(self, sync_bytes: bytes = UBX_SYNC_WORD):
		MsgSink.__init__(self)
		MsgSource.__init__(self)
		self.sync_bytes = sync_bytes
		self.packet = b''
		self.wait_sync = True
		self.sync_count = 0
		self.timestamp = 0
		self.d_save = b''

	def reset(self):
		self.packet = b''
		self.wait_sync = True
		self.sync_count = 0

	def on_recv(self, msg: MsgContainer):
		for n in range (0, len(msg.data)):
			d = msg.data[n]
			if d == self.sync_bytes[self.sync_count]:
				self.d_save += struct.pack('B', d)
				self.sync_count += 1
				if self.sync_count == len(self.sync_bytes):
					ubx_pkt_debug(f"{msg.name}:UBX-sync")
					if len(self.packet) > 0:
						ubx_pkt_debug(f"{msg.name}:Remaining packet: {self.packet}")
					self.sync_count = 0
					self.d_save = b''
					self.packet = b''
					self.wait_sync = False
					self.timestamp = msg.timestamp
			else:
				if self.sync_count > 0:
					self.packet += self.d_save
					self.d_save = b''
					self.sync_count = 0
				if not self.wait_sync:
					self.packet += struct.pack('B', d)
					data, hdr = frame_parse(self.packet)
					if data is not None:
						if hdr.length == len(data):
							self.call_sinks(MsgContainer(msg.name, self.timestamp, self.packet))
							self.packet = b''
							self.wait_sync = True

	def is_msg(self, msg: MsgContainer):
		return True


if __name__ == "__main__":
	UBX_PKT_DEBUG = True
	class MySink(MsgSink):
		def __init__(self):
			MsgSink.__init__(self)

		def on_recv(self, msg: MsgContainer):
			print(msg)

		def is_msg(self, msg: MsgContainer):
			return True

	ubx_rx = UbxReceiver('test', 1, 2)

	pkt = UbxPacketizer()
	pkt.register_recv(ubx_rx)

	pkt.reset()
	frame = frame_create(_class=1, _id=2, _data=b'Hallo' + b'\xb5' + b',\x62Welt') + frame_create(_class=1, _id=2, _data=b',') + frame_create(_class=1, _id=2, _data=b'UBX') + frame_create(_class=1, _id=2, _data=b'World')
	pkt.on_recv(MsgContainer('test', 31101970.0208, frame))