extends Node

class_name VRPN

static func marshal(data : PackedByteArray):
	
	# need to fix that 
	var block_offset : int = 0
	var header_size = aligned_size(20) # kinda redundant as we take the seq number as well
	
	# reader for stream
	var header := StreamPeerBuffer.new()
	# get block addresses
	header.data_array = data.slice(block_offset,block_offset+header_size)
	# make sure we read as big endian
	header.big_endian = true 
	
	# 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() as int  # sender id 
	var message_type := header.get_32() as int # type of message (payload)
	var sequence_num := header.get_32() as int # 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
	#print("block_offset:{0} header_size:{1} length:{2}".format([block_offset,header_size,length]))

	# sender description
	match message_type:
		-1:
			var body := StreamPeerBuffer.new()
			body.data_array = data.slice(block_offset+header_size)
			body.big_endian = true
			# get length of string
			var body_length = body.get_32()
			# get string
			var sender_name = body.get_string(body_length)
			#print("sender id {0} is '{1}'".format([sender_id,sender_name]))
		4: # quat pos
			# set body
			var body := StreamPeerBuffer.new()
			# set header size
			body.data_array = data.slice(block_offset+header_size,block_offset+header_size+length)
			body.big_endian = true
			# get id
			var sensor_id = body.get_32()
			var padding = body.get_32()
			var pos = Vector3(body.get_double(),body.get_double(),body.get_double())
			# VRPN quaternions are w,xyz
			var quat_w = body.get_double()
			var quat_x = body.get_double()
			var quat_y = body.get_double()
			var quat_z = body.get_double()
			var quat = Quaternion(quat_x,quat_y,quat_z,quat_w)
			#print("sensor id {0} {1} {2}".format([sensor_id,pos,quat]))
		_:
			print("message id {0}".format([message_type]))


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