extends Node

signal connected(s:StreamPeerTCP)
signal data(data:Array)
signal disconnected
signal error

@onready var _stream: StreamPeerTCP = StreamPeerTCP.new()

func _ready() -> void:
	self.connect_to_host("127.0.0.1",3883)

func _process(delta: float) -> void:
	var old_status = _stream.get_status()
	_stream.poll()
	var new_status = _stream.get_status()
	if old_status != new_status:
		match new_status:
			_stream.STATUS_NONE:
				emit_signal("disconnected")
			_stream.STATUS_CONNECTING:
				print("Connecting.")
			_stream.STATUS_CONNECTED:
				print("Connected.")
				emit_signal("connected",_stream.poll()
)
			_stream.STATUS_ERROR:
				print("Error with socket stream.")
				emit_signal("error")

	if new_status == _stream.STATUS_CONNECTED:
		var available_bytes: int = _stream.get_available_bytes()
		if available_bytes > 0:
			var res = _stream.get_partial_data(available_bytes)
			if res[0] != OK:
				emit_signal("error")
			else:
				emit_signal("data", res[1])

func connect_to_host(host: String, port: int) -> void:
	print("Connecting to %s:%d" % [host, port])
	if _stream.connect_to_host(host, port) != OK:
		print("Error connecting to host.")
		emit_signal("error")

func send(data: PackedByteArray) -> bool:
	if _stream.get_status() != _stream.STATUS_CONNECTED:
		print("Error: Stream is not currently connected.")
		return false
	var error: int = _stream.put_data(data)
	if error != OK:
		print("Error writing to stream: ", error)
		return false
	return true
	

func _on_data(data : Array):
	var bytes = PackedByteArray(data)
	var as_cookie = bytes.get_string_from_ascii()
	
	# Cookie Hack!
	if as_cookie.begins_with("vrpn: ver."):
		# kaboom we just send back the same cookie :) 
		self.send(bytes)
	else:
		# non-cookie 
		var header := StreamPeerBuffer.new()
		header.data_array = bytes.slice(0,aligned_size(20))
		header.big_endian = true
		
		# Header
		var length = header.get_32() # length of message
		var time_sec = header.get_32() # time sec
		var time_msec = header.get_32() # 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(bytes)
		
		# sender description
		if message_type == -1:
			var body := StreamPeerBuffer.new()
			body.data_array = bytes.slice(24,length-24)
			body.big_endian = true
			var body_length = body.get_32()			
			print(body_length)
			
			#var sender_name = peer.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 = bytes.slice(24,length-24)
			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)
			

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

			
func _on_connected(s : StreamPeerTCP):
	print("Connected to",s.get_connected_host()) # Replace with function body.
	
func _on_disconnected():
	print("Disconnected") # Replace with function body.

func _on_error():
	print("Error") # Replace with function body.