extends Node

class_name VRPN

const magic_cookie_start : String = "vrpn: ver."

static func marshall_block(data : PackedByteArray) -> void:
	
	# 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

	while data.size() > block_offset:
		# 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  # datetime sec
		var time_msec := header.get_32() as int # datetime 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("sender_id:{0} message_type:{1} block_offset:{2} header_size:{3} length:{4}".format([sender_id, message_type, block_offset,header_size,length]))
		
		# directly hand over marshalling for body
		marshall_body(data.slice(block_offset+header_size,block_offset+length),message_type)
		
		# next datablock
		block_offset += aligned_size(length)
		
		
static func marshall_body(data : PackedByteArray,message_type : int):
	var body := StreamPeerBuffer.new()
	body.data_array = data
	body.big_endian = true
		# sender description
	match message_type: # message_type_ids are supposedly dynamic
		-1,-2:
			# get length of string
			var body_length = body.get_32()
			# get string
			var sender_name = body.get_string(body_length)
			print("sender name is '%s'" % sender_name)
			#print(body.data_array)
		4: # quat pos
			# 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]))
		18:
			var num_buttons = body.get_32()
			var buttons = {}
			for i in num_buttons:
				var button_id = body.get_32()
				var button_state = body.get_32()
				buttons[button_id] = button_state
			
			#print("sensor with {0} buttons".format([buttons]))
		_:
			print("unhandled message type {0}".format([message_type]))


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