extends Node

class_name VRPN
	
	
	
static func marshal(data : PackedByteArray):
	
	var block_offset : int = 0
	var header_size = aligned_size(20) # kinda redundant as we take the seq number as well
	
	#print(data)
	
	while block_offset < data.size():
		
		print("...")
		
		# reader for stream
		var header := StreamPeerBuffer.new()
		header.data_array = data.slice(block_offset,block_offset+header_size)
		header.big_endian = true # make sure we read as big endian
		
		# read header
		var length := header.get_32() as int # length of message
		var time_sec := header.get_32() as int  # time sec
		var time_msec := header.get_32() as int # time micro sec
		var sender_id = header.get_32()  # sender id 
		var message_type = header.get_32() # type of message (payload)
		var sequence_num = header.get_32() # inofficial sequence number (padding)
		
		if false:
			print("length       %d" % length)
			print("time_sec     %d" % time_sec)
			print("time_msec    %d" % time_msec)
			print("sender_id    %d" % sender_id)
			print("message_type %d" % message_type)
			print("sequence_num %d" % sequence_num)
		
		
		print("block_offset:{0} header_size:{1} length:{2}".format([block_offset,header_size,length]))
		
		# update blockoffset
		block_offset += length
		
		continue
		
		# sender description
		if message_type == -1:
			var body := StreamPeerBuffer.new()
			body.data_array = data.slice(24)
			body.big_endian = true
			var body_length = body.get_32()
			#print(body_length)
			#print(body.data_array)
			
			var sender_name = body.get_string(body_length)
			print("sender id {0} is '{1}'".format([sender_id,sender_name]))
		
		# quat pos
		if message_type == 4:
			var body := StreamPeerBuffer.new()
		
			body.data_array = data.slice(header_size,length)
			body.big_endian = true
			var sensor_id = body.get_32()
			var padding = body.get_32()
			var pos = Vector3(body.get_float(),body.get_float(),body.get_float())
			var quat_w = body.get_float()
			var quat_x = body.get_float()
			var quat_y = body.get_float()
			var quat_z = body.get_float()
			var quat = Quaternion(quat_x,quat_y,quat_z,quat_w)
			
		#print("Pos_Quat ",sensor_id,pos,quat)
	print("------------")

static func aligned_size(actual_size : int,alignment : int = 8) -> int:		
	return (actual_size + alignment - 1) & ~(alignment - 1)