AeThex-Engine-Core/engine/modules/aethex_kinect/kinect_bridge.cpp

/**************************************************************************/
/*  kinect_bridge.cpp                                                     */
/**************************************************************************/
/*                         This file is part of:                          */
/*                          AETHEX ENGINE                                 */
/*                     https://aethex.foundation                          */
/**************************************************************************/
/* Copyright (c) 2026-present AeThex Labs.                                */
/**************************************************************************/

#include "kinect_bridge.h"
#include "core/config/engine.h"
#include <cstring>

KinectBridge *KinectBridge::singleton = nullptr;

KinectBridge *KinectBridge::get_singleton() {
	return singleton;
}

void KinectBridge::_bind_methods() {
	// Server control
	ClassDB::bind_method(D_METHOD("start", "port"), &KinectBridge::start, DEFVAL(9000));
	ClassDB::bind_method(D_METHOD("stop"), &KinectBridge::stop);
	ClassDB::bind_method(D_METHOD("is_active"), &KinectBridge::is_active);
	ClassDB::bind_method(D_METHOD("get_port"), &KinectBridge::get_port);

	// Mode
	ClassDB::bind_method(D_METHOD("set_mode", "mode"), &KinectBridge::set_mode);
	ClassDB::bind_method(D_METHOD("get_mode"), &KinectBridge::get_mode);
	ADD_PROPERTY(PropertyInfo(Variant::INT, "mode", PROPERTY_HINT_ENUM, "Foundry,GameForge"), "set_mode", "get_mode");

	// Skeleton
	ClassDB::bind_method(D_METHOD("get_skeleton"), &KinectBridge::get_skeleton);
	ClassDB::bind_method(D_METHOD("is_skeleton_detected"), &KinectBridge::is_skeleton_detected);
	ClassDB::bind_method(D_METHOD("get_joint_position", "joint_id"), &KinectBridge::get_joint_position);

	// Gesture detector
	ClassDB::bind_method(D_METHOD("get_gesture_detector"), &KinectBridge::get_gesture_detector);

	// Debug
	ClassDB::bind_method(D_METHOD("set_debug_mode", "enabled"), &KinectBridge::set_debug_mode);
	ClassDB::bind_method(D_METHOD("is_debug_mode"), &KinectBridge::is_debug_mode);

	// Signals
	ADD_SIGNAL(MethodInfo("skeleton_detected", PropertyInfo(Variant::BOOL, "detected")));
	ADD_SIGNAL(MethodInfo("joint_updated",
			PropertyInfo(Variant::INT, "joint_id"),
			PropertyInfo(Variant::VECTOR3, "position"),
			PropertyInfo(Variant::INT, "tracking_state")));
	ADD_SIGNAL(MethodInfo("gesture_received",
			PropertyInfo(Variant::STRING, "gesture_type"),
			PropertyInfo(Variant::DICTIONARY, "params")));
	ADD_SIGNAL(MethodInfo("identity_forged",
			PropertyInfo(Variant::DICTIONARY, "signature_data")));
	ADD_SIGNAL(MethodInfo("forge_progress",
			PropertyInfo(Variant::FLOAT, "progress")));

	// Enums
	BIND_ENUM_CONSTANT(MODE_FOUNDRY);
	BIND_ENUM_CONSTANT(MODE_GAMEFORGE);
}

void KinectBridge::_notification(int p_what) {
	switch (p_what) {
		case NOTIFICATION_PROCESS: {
			if (!is_running || udp_peer.is_null()) {
				return;
			}

			// Process incoming OSC packets
			while (udp_peer->get_available_packet_count() > 0) {
				Vector<uint8_t> packet;
				Error err = udp_peer->get_packet_buffer(packet);
				if (err == OK && packet.size() > 0) {
					_process_osc_packet(packet);
				}
			}
		} break;

		case NOTIFICATION_READY: {
			set_process(true);
		} break;
	}
}

Error KinectBridge::start(int p_port) {
	if (is_running) {
		stop();
	}

	listen_port = p_port;

	udp_peer.instantiate();
	Error err = udp_peer->bind(listen_port);

	if (err != OK) {
		ERR_PRINT(vformat("[KinectBridge] Failed to bind UDP port %d: %s", listen_port, itos(err)));
		udp_peer.unref();
		return err;
	}

	is_running = true;
	print_line(vformat("[KinectBridge] Listening on UDP port %d", listen_port));
	print_line("[KinectBridge] Mode: " + String(current_mode == MODE_FOUNDRY ? "FOUNDRY" : "GAMEFORGE"));

	return OK;
}

void KinectBridge::stop() {
	if (udp_peer.is_valid()) {
		udp_peer->close();
		udp_peer.unref();
	}
	is_running = false;
	print_line("[KinectBridge] Stopped");
}

void KinectBridge::set_mode(Mode p_mode) {
	current_mode = p_mode;
	if (gesture_detector.is_valid()) {
		// Update gesture detector mode
	}
}

Vector3 KinectBridge::get_joint_position(int p_joint_id) const {
	if (skeleton.is_valid()) {
		return skeleton->get_joint_position(p_joint_id);
	}
	return Vector3();
}

void KinectBridge::set_debug_mode(bool p_enabled) {
	// Store in skeleton or local flag
}

bool KinectBridge::is_debug_mode() const {
	return false;
}

// OSC Parsing

void KinectBridge::_process_osc_packet(const Vector<uint8_t> &p_data) {
	if (p_data.size() < 8) {
		return;
	}

	// Check for OSC bundle
	String header;
	for (int i = 0; i < 8 && i < p_data.size(); i++) {
		header += char(p_data[i]);
	}

	if (header.begins_with("#bundle")) {
		// Parse bundle - skip header (8) + timestamp (8)
		int offset = 16;
		while (offset < p_data.size() - 4) {
			// Read message size
			int32_t size = _read_osc_int32(p_data, offset);
			if (size > 0 && offset + size <= p_data.size()) {
				Vector<uint8_t> msg_data;
				msg_data.resize(size);
				for (int i = 0; i < size; i++) {
					msg_data.write[i] = p_data[offset + i];
				}
				int msg_offset = 0;

				String address = _read_osc_string(msg_data, msg_offset);
				String type_tags = _read_osc_string(msg_data, msg_offset);

				Array args;
				for (int i = 0; i < type_tags.length(); i++) {
					char tag = type_tags[i];
					if (tag == ',') continue;
					if (tag == 'i') {
						args.push_back(_read_osc_int32(msg_data, msg_offset));
					} else if (tag == 'f') {
						args.push_back(_read_osc_float32(msg_data, msg_offset));
					} else if (tag == 's') {
						args.push_back(_read_osc_string(msg_data, msg_offset));
					}
				}

				_dispatch_osc_message(address, args);
				offset += size;
			} else {
				break;
			}
		}
	} else {
		// Single message
		int offset = 0;
		String address = _read_osc_string(p_data, offset);
		String type_tags = _read_osc_string(p_data, offset);

		Array args;
		for (int i = 0; i < type_tags.length(); i++) {
			char tag = type_tags[i];
			if (tag == ',') continue;
			if (tag == 'i') {
				args.push_back(_read_osc_int32(p_data, offset));
			} else if (tag == 'f') {
				args.push_back(_read_osc_float32(p_data, offset));
			} else if (tag == 's') {
				args.push_back(_read_osc_string(p_data, offset));
			}
		}

		_dispatch_osc_message(address, args);
	}
}

String KinectBridge::_read_osc_string(const Vector<uint8_t> &p_data, int &r_offset) {
	String result;
	while (r_offset < p_data.size() && p_data[r_offset] != 0) {
		result += char(p_data[r_offset]);
		r_offset++;
	}
	r_offset++; // Skip null terminator
	// Align to 4 bytes
	r_offset = ((r_offset + 3) / 4) * 4;
	return result;
}

int32_t KinectBridge::_read_osc_int32(const Vector<uint8_t> &p_data, int &r_offset) {
	if (r_offset + 4 > p_data.size()) return 0;

	// Big-endian
	int32_t val = (p_data[r_offset] << 24) |
				  (p_data[r_offset + 1] << 16) |
				  (p_data[r_offset + 2] << 8) |
				  p_data[r_offset + 3];
	r_offset += 4;
	return val;
}

float KinectBridge::_read_osc_float32(const Vector<uint8_t> &p_data, int &r_offset) {
	if (r_offset + 4 > p_data.size()) return 0.0f;

	// Big-endian to little-endian
	uint8_t bytes[4] = {
		p_data[r_offset + 3],
		p_data[r_offset + 2],
		p_data[r_offset + 1],
		p_data[r_offset]
	};
	r_offset += 4;

	float result;
	memcpy(&result, bytes, 4);
	return result;
}

void KinectBridge::_dispatch_osc_message(const String &p_address, const Array &p_args) {
	if (p_address == "/skeleton/detected") {
		bool detected = p_args.size() > 0 ? (int)p_args[0] != 0 : false;
		skeleton_detected = detected;
		emit_signal(SNAME("skeleton_detected"), detected);

		if (detected) {
			print_line("[KinectBridge] Skeleton DETECTED");
		} else {
			print_line("[KinectBridge] Skeleton LOST");
		}
	}
	else if (p_address == "/skeleton/joint") {
		if (p_args.size() >= 5 && skeleton.is_valid()) {
			int joint_id = p_args[0];
			Vector3 pos((float)p_args[1], (float)p_args[2], (float)p_args[3]);
			int tracking_state = p_args[4];

			skeleton->set_joint(joint_id, pos, tracking_state);
			emit_signal(SNAME("joint_updated"), joint_id, pos, tracking_state);

			// Update gesture detector
			if (gesture_detector.is_valid()) {
				gesture_detector->update_joint(joint_id, pos, tracking_state);
			}
		}
	}
	else if (p_address == "/gesture/arms_raised") {
		Dictionary params;
		if (p_args.size() > 0) params["duration"] = p_args[0];
		params["completed"] = true;
		emit_signal(SNAME("gesture_received"), "arms_raised", params);
	}
	else if (p_address == "/gesture/arms_raised_progress") {
		if (p_args.size() >= 2) {
			emit_signal(SNAME("forge_progress"), (float)p_args[1]);
		}
	}
	else if (p_address == "/gesture/push") {
		Dictionary params;
		params["hand"] = "right";
		if (p_args.size() > 1) params["strength"] = p_args[1];
		emit_signal(SNAME("gesture_received"), "push", params);
	}
	else if (p_address == "/gesture/swipe") {
		Dictionary params;
		if (p_args.size() > 1) params["direction"] = p_args[1];
		emit_signal(SNAME("gesture_received"), "swipe", params);
	}
	else if (p_address == "/foundry/identity_forged") {
		Dictionary signature;
		if (p_args.size() > 0) signature["seed"] = p_args[0];
		if (p_args.size() > 1) signature["color_hue"] = p_args[1];
		if (p_args.size() > 2) signature["particle_density"] = p_args[2];
		if (p_args.size() > 3) signature["movement_signature"] = p_args[3];

		emit_signal(SNAME("identity_forged"), signature);
		print_line(vformat("[KinectBridge] 🔥 IDENTITY FORGED! Seed: %d", (int)signature["seed"]));
	}
}

KinectBridge::KinectBridge() {
	if (singleton == nullptr) {
		singleton = this;
	}

	skeleton.instantiate();
	gesture_detector.instantiate();
}

KinectBridge::~KinectBridge() {
	stop();
	if (singleton == this) {
		singleton = nullptr;
	}
}