read whole blocks of data and marshalling to messages

SocketClient.gd

@@ -59,11 +59,11 @@ func _on_data(data : Array):
 	var as_cookie = bytes.get_string_from_ascii()
 	
 	# Cookie Hack!
-	if as_cookie.begins_with("vrpn: ver."):
+	if as_cookie.begins_with(VRPN.magic_cookie_start): # 
 		# kaboom we just send back the same cookie :) 
 		self.send(bytes)
 	else:
-		VRPN.marshal(bytes)
+		VRPN.marshall_block(bytes)
 
 
 			

VRPN.gd

@@ -2,7 +2,81 @@ extends Node
 
 class_name VRPN
 
-static func marshal(data : PackedByteArray):
+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 true:
+			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]))
+		
+		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:
+		-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]))
+		_:
+			print("unhandled message type {0}".format([message_type]))
+	pass
+
+static func marshal(data : PackedByteArray) -> void:
+	
+	# actual size
+	print("Data size %d " % data.size())
 	
 	# need to fix that 
 	var block_offset : int = 0
@@ -31,8 +105,12 @@ static func marshal(data : PackedByteArray):
 		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]))
+	print("block_offset:{0} header_size:{1} length:{2}".format([block_offset,header_size,length]))
+
+
 
 	# sender description
 	match message_type:
@@ -44,7 +122,8 @@ static func marshal(data : PackedByteArray):
 			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]))
+			print("sender id {0} is '{1}'".format([sender_id,sender_name]))
+			#print(body.data_array)
 		4: # quat pos
 			# set body
 			var body := StreamPeerBuffer.new()
@@ -66,5 +145,5 @@ static func marshal(data : PackedByteArray):
 			print("message id {0}".format([message_type]))
 
 
-static func aligned_size(actual_size : int,alignment : int = 8) -> int:		
+static func aligned_size(actual_size : int, alignment : int = 8) -> int:
 	return (actual_size + alignment - 1) & ~(alignment - 1)