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

#include "kinect_skeleton.h"

void KinectSkeleton::_bind_methods() {
	// Joint access
	ClassDB::bind_method(D_METHOD("set_joint", "joint_id", "position", "tracking_state"), &KinectSkeleton::set_joint);
	ClassDB::bind_method(D_METHOD("get_joint_position", "joint_id"), &KinectSkeleton::get_joint_position);
	ClassDB::bind_method(D_METHOD("get_joint_tracking_state", "joint_id"), &KinectSkeleton::get_joint_tracking_state);
	ClassDB::bind_method(D_METHOD("is_joint_tracked", "joint_id"), &KinectSkeleton::is_joint_tracked);

	// Bulk access
	ClassDB::bind_method(D_METHOD("get_all_joints"), &KinectSkeleton::get_all_joints);

	// Static helpers
	ClassDB::bind_static_method("KinectSkeleton", D_METHOD("get_joint_name", "joint_id"), &KinectSkeleton::get_joint_name);
	ClassDB::bind_static_method("KinectSkeleton", D_METHOD("get_bone_connections"), &KinectSkeleton::get_bone_connections);

	// Joint ID constants
	BIND_ENUM_CONSTANT(JOINT_HIP_CENTER);
	BIND_ENUM_CONSTANT(JOINT_SPINE);
	BIND_ENUM_CONSTANT(JOINT_SHOULDER_CENTER);
	BIND_ENUM_CONSTANT(JOINT_HEAD);
	BIND_ENUM_CONSTANT(JOINT_SHOULDER_LEFT);
	BIND_ENUM_CONSTANT(JOINT_ELBOW_LEFT);
	BIND_ENUM_CONSTANT(JOINT_WRIST_LEFT);
	BIND_ENUM_CONSTANT(JOINT_HAND_LEFT);
	BIND_ENUM_CONSTANT(JOINT_SHOULDER_RIGHT);
	BIND_ENUM_CONSTANT(JOINT_ELBOW_RIGHT);
	BIND_ENUM_CONSTANT(JOINT_WRIST_RIGHT);
	BIND_ENUM_CONSTANT(JOINT_HAND_RIGHT);
	BIND_ENUM_CONSTANT(JOINT_HIP_LEFT);
	BIND_ENUM_CONSTANT(JOINT_KNEE_LEFT);
	BIND_ENUM_CONSTANT(JOINT_ANKLE_LEFT);
	BIND_ENUM_CONSTANT(JOINT_FOOT_LEFT);
	BIND_ENUM_CONSTANT(JOINT_HIP_RIGHT);
	BIND_ENUM_CONSTANT(JOINT_KNEE_RIGHT);
	BIND_ENUM_CONSTANT(JOINT_ANKLE_RIGHT);
	BIND_ENUM_CONSTANT(JOINT_FOOT_RIGHT);
	BIND_ENUM_CONSTANT(JOINT_COUNT);

	// Tracking state constants
	BIND_ENUM_CONSTANT(TRACKING_NOT_TRACKED);
	BIND_ENUM_CONSTANT(TRACKING_INFERRED);
	BIND_ENUM_CONSTANT(TRACKING_TRACKED);
}

void KinectSkeleton::set_joint(int p_joint_id, const Vector3 &p_position, int p_tracking_state) {
	if (p_joint_id < 0 || p_joint_id >= JOINT_COUNT) {
		return;
	}
	joints[p_joint_id].position = p_position;
	joints[p_joint_id].tracking_state = p_tracking_state;
}

Vector3 KinectSkeleton::get_joint_position(int p_joint_id) const {
	if (p_joint_id < 0 || p_joint_id >= JOINT_COUNT) {
		return Vector3();
	}
	return joints[p_joint_id].position;
}

int KinectSkeleton::get_joint_tracking_state(int p_joint_id) const {
	if (p_joint_id < 0 || p_joint_id >= JOINT_COUNT) {
		return TRACKING_NOT_TRACKED;
	}
	return joints[p_joint_id].tracking_state;
}

bool KinectSkeleton::is_joint_tracked(int p_joint_id) const {
	return get_joint_tracking_state(p_joint_id) >= TRACKING_INFERRED;
}

Dictionary KinectSkeleton::get_all_joints() const {
	Dictionary result;
	for (int i = 0; i < JOINT_COUNT; i++) {
		Dictionary joint_data;
		joint_data["position"] = joints[i].position;
		joint_data["tracking_state"] = joints[i].tracking_state;
		result[i] = joint_data;
	}
	return result;
}

String KinectSkeleton::get_joint_name(int p_joint_id) {
	static const char *names[JOINT_COUNT] = {
		"HipCenter", "Spine", "ShoulderCenter", "Head",
		"ShoulderLeft", "ElbowLeft", "WristLeft", "HandLeft",
		"ShoulderRight", "ElbowRight", "WristRight", "HandRight",
		"HipLeft", "KneeLeft", "AnkleLeft", "FootLeft",
		"HipRight", "KneeRight", "AnkleRight", "FootRight"
	};

	if (p_joint_id < 0 || p_joint_id >= JOINT_COUNT) {
		return "Unknown";
	}
	return names[p_joint_id];
}

PackedInt32Array KinectSkeleton::get_bone_connections() {
	// Pairs of joint IDs that form bones
	static const int connections[] = {
		JOINT_HIP_CENTER, JOINT_SPINE,
		JOINT_SPINE, JOINT_SHOULDER_CENTER,
		JOINT_SHOULDER_CENTER, JOINT_HEAD,
		JOINT_SHOULDER_CENTER, JOINT_SHOULDER_LEFT,
		JOINT_SHOULDER_LEFT, JOINT_ELBOW_LEFT,
		JOINT_ELBOW_LEFT, JOINT_WRIST_LEFT,
		JOINT_WRIST_LEFT, JOINT_HAND_LEFT,
		JOINT_SHOULDER_CENTER, JOINT_SHOULDER_RIGHT,
		JOINT_SHOULDER_RIGHT, JOINT_ELBOW_RIGHT,
		JOINT_ELBOW_RIGHT, JOINT_WRIST_RIGHT,
		JOINT_WRIST_RIGHT, JOINT_HAND_RIGHT,
		JOINT_HIP_CENTER, JOINT_HIP_LEFT,
		JOINT_HIP_LEFT, JOINT_KNEE_LEFT,
		JOINT_KNEE_LEFT, JOINT_ANKLE_LEFT,
		JOINT_ANKLE_LEFT, JOINT_FOOT_LEFT,
		JOINT_HIP_CENTER, JOINT_HIP_RIGHT,
		JOINT_HIP_RIGHT, JOINT_KNEE_RIGHT,
		JOINT_KNEE_RIGHT, JOINT_ANKLE_RIGHT,
		JOINT_ANKLE_RIGHT, JOINT_FOOT_RIGHT,
	};

	PackedInt32Array result;
	for (size_t i = 0; i < sizeof(connections) / sizeof(int); i++) {
		result.push_back(connections[i]);
	}
	return result;
}

KinectSkeleton::KinectSkeleton() {
	// Initialize all joints to not tracked
	for (int i = 0; i < JOINT_COUNT; i++) {
		joints[i].position = Vector3();
		joints[i].tracking_state = TRACKING_NOT_TRACKED;
	}
}