class_name MotionService extends RefCounted

@export var RATE_HZ := 64

var running := false

var delta:float:
	get: return 1.0 / RATE_HZ
	set(value): pass

var _threads:Dictionary[int, Thread]
var _clock:Thread
var _semaphore:Semaphore
var _mutex:Mutex

var tick := 0:
	set(value):
		tick = value % RATE_HZ

func _init() -> void:
	_clock = Thread.new()
	_mutex = Mutex.new()
	_semaphore = Semaphore.new()
	#multiplayer.peer_connected.connect(_on_peer_connected)
	#multiplayer.peer_disconnected.connect(_on_peer_disconnected)

func register(id:int) -> void:
	if !running:
		running = true
		_clock.start(_loop)
		prints("motion_service: starting clock")
	prints("motion_service: register peer", id)
	_threads[id] = Thread.new()
	_threads[id].start(_worker.bind(id))

func unregister(id:int) -> void:
	if running:
		prints("motion_service: unregistering peer", id)
		if _threads[id].is_alive():
			_threads[id].wait_to_finish()
		_threads.erase(id)
		if len(_threads) == 0:
			running = false
			if _clock.is_alive():
				_clock.wait_to_finish()
				_clock = null

func _exit_tree() -> void:
	if running:
		for id:int in _threads.keys():
			unregister(id)

func _loop() -> void:
	var next_time:float = Time.get_ticks_msec() / 1000.
	while running:
		_semaphore.post(len(_threads))
		_mutex.lock()
		next_time += delta
		_mutex.unlock()
		var sleep_time:float = next_time - (Time.get_ticks_msec() / 1000.)
		if sleep_time > 0:
			# delay current thread
			OS.delay_msec(int(sleep_time * 1000))
		else:
			_mutex.lock()
			# we are behind; reset next_time to avoid drift
			next_time = Time.get_ticks_msec() / 1000.
			_mutex.unlock()

		_mutex.lock()
		tick += 1
		_mutex.unlock()

func _worker(peer_id:int) -> void:
	while running:
		_semaphore.wait() # wait until posted
		#prints("tick", tick, "for peer", peer_id)

		#var player:Player = owner.players.get_node(str(peer_id))
		#if player:
			#player.send_position.rpc(player.global_position)