//======================================================================================================
// Copyright 2025, Rokoko Glove OptiTrack Integration
//======================================================================================================

#include "RokokoDataParser.h"
#include "RokokoData.h"
#include <sstream>
#include <iostream>
#include <cstring>
#include <cmath>

namespace RokokoIntegration
{
	bool RokokoDataParser::ParseLiveFrame(const std::string& jsonString, RokokoData::LiveFrame_v4& frame)
	{
		try {
			// nlohmann/json을 사용하여 JSON 파싱
			nlohmann::json j = nlohmann::json::parse(jsonString);
			
			// Unity JsonLiveSerializerV3.cs와 동일한 구조로 파싱
			
			// Scene 데이터 파싱
			if (j.contains("scene")) {
				auto& scene = j["scene"];
				
				// 타임스탬프
				if (scene.contains("timestamp")) {
					frame.timestamp = scene["timestamp"];
				}
				
				// Actors 파싱
				if (scene.contains("actors") && scene["actors"].is_array()) {
					auto& actors = scene["actors"];
					for (const auto& actorJson : actors) {
						RokokoData::ActorData actor;
						
						// Actor 기본 정보
						if (actorJson.contains("name")) {
							actor.name = actorJson["name"];
						}
						
						// Body 데이터 파싱
						if (actorJson.contains("body")) {
							ParseBodyFrame(actorJson["body"], actor.body);
						}
						
						frame.scene.actors.push_back(actor);
					}
				}
			}
			
			return true;
			
		} catch (const nlohmann::json::exception& e) {
			// JSON 파싱 에러 처리
			std::cerr << "JSON parsing error: " << e.what() << std::endl;
			return false;
		} catch (...) {
			// 기타 예외 처리
			std::cerr << "Unknown error during JSON parsing" << std::endl;
			return false;
		}
	}
	
	void RokokoDataParser::ParseBodyFrame(const nlohmann::json& bodyJson, RokokoData::Body& body)
	{
		// Unity JsonLiveSerializerV3.cs의 BodyFrame 구조와 동일하게 파싱
		
		// === 전신 스켈레톤 데이터 파싱 (Unity BodyFrame과 동일) ===
		
		// 몸통 파싱
		ParseFingerJoint(bodyJson, "hip", body.hip);
		ParseFingerJoint(bodyJson, "spine", body.spine);
		ParseFingerJoint(bodyJson, "chest", body.chest);
		ParseFingerJoint(bodyJson, "neck", body.neck);
		ParseFingerJoint(bodyJson, "head", body.head);
		
		// 왼쪽 팔 파싱
		ParseFingerJoint(bodyJson, "leftShoulder", body.leftShoulder);
		ParseFingerJoint(bodyJson, "leftUpperArm", body.leftUpperArm);
		ParseFingerJoint(bodyJson, "leftLowerArm", body.leftLowerArm);
		ParseFingerJoint(bodyJson, "leftHand", body.leftHand);
		
		// 오른쪽 팔 파싱
		ParseFingerJoint(bodyJson, "rightShoulder", body.rightShoulder);
		ParseFingerJoint(bodyJson, "rightUpperArm", body.rightUpperArm);
		ParseFingerJoint(bodyJson, "rightLowerArm", body.rightLowerArm);
		ParseFingerJoint(bodyJson, "rightHand", body.rightHand);
		
		// 다리 파싱 (필요시)
		ParseFingerJoint(bodyJson, "leftUpLeg", body.leftUpLeg);
		ParseFingerJoint(bodyJson, "leftLeg", body.leftLeg);
		ParseFingerJoint(bodyJson, "leftFoot", body.leftFoot);
		ParseFingerJoint(bodyJson, "leftToe", body.leftToe);
		ParseFingerJoint(bodyJson, "leftToeEnd", body.leftToeEnd);
		
		ParseFingerJoint(bodyJson, "rightUpLeg", body.rightUpLeg);
		ParseFingerJoint(bodyJson, "rightLeg", body.rightLeg);
		ParseFingerJoint(bodyJson, "rightFoot", body.rightFoot);
		ParseFingerJoint(bodyJson, "rightToe", body.rightToe);
		ParseFingerJoint(bodyJson, "rightToeEnd", body.rightToeEnd);
		
		// === 손가락 데이터 파싱 ===
		
		// 왼손 손가락 데이터 파싱
		ParseFingerJoint(bodyJson, "leftThumbProximal", body.leftThumbProximal);
		ParseFingerJoint(bodyJson, "leftThumbMedial", body.leftThumbMedial);
		ParseFingerJoint(bodyJson, "leftThumbDistal", body.leftThumbDistal);
		ParseFingerJoint(bodyJson, "leftThumbTip", body.leftThumbTip);
		
		ParseFingerJoint(bodyJson, "leftIndexProximal", body.leftIndexProximal);
		ParseFingerJoint(bodyJson, "leftIndexMedial", body.leftIndexMedial);
		ParseFingerJoint(bodyJson, "leftIndexDistal", body.leftIndexDistal);
		ParseFingerJoint(bodyJson, "leftIndexTip", body.leftIndexTip);
		
		ParseFingerJoint(bodyJson, "leftMiddleProximal", body.leftMiddleProximal);
		ParseFingerJoint(bodyJson, "leftMiddleMedial", body.leftMiddleMedial);
		ParseFingerJoint(bodyJson, "leftMiddleDistal", body.leftMiddleDistal);
		ParseFingerJoint(bodyJson, "leftMiddleTip", body.leftMiddleTip);
		
		ParseFingerJoint(bodyJson, "leftRingProximal", body.leftRingProximal);
		ParseFingerJoint(bodyJson, "leftRingMedial", body.leftRingMedial);
		ParseFingerJoint(bodyJson, "leftRingDistal", body.leftRingDistal);
		ParseFingerJoint(bodyJson, "leftRingTip", body.leftRingTip);
		
		ParseFingerJoint(bodyJson, "leftLittleProximal", body.leftLittleProximal);
		ParseFingerJoint(bodyJson, "leftLittleMedial", body.leftLittleMedial);
		ParseFingerJoint(bodyJson, "leftLittleDistal", body.leftLittleDistal);
		ParseFingerJoint(bodyJson, "leftLittleTip", body.leftLittleTip);
		
		// 오른손 손가락 데이터 파싱
		ParseFingerJoint(bodyJson, "rightThumbProximal", body.rightThumbProximal);
		ParseFingerJoint(bodyJson, "rightThumbMedial", body.rightThumbMedial);
		ParseFingerJoint(bodyJson, "rightThumbDistal", body.rightThumbDistal);
		ParseFingerJoint(bodyJson, "rightThumbTip", body.rightThumbTip);
		
		ParseFingerJoint(bodyJson, "rightIndexProximal", body.rightIndexProximal);
		ParseFingerJoint(bodyJson, "rightIndexMedial", body.rightIndexMedial);
		ParseFingerJoint(bodyJson, "rightIndexDistal", body.rightIndexDistal);
		ParseFingerJoint(bodyJson, "rightIndexTip", body.rightIndexTip);
		
		ParseFingerJoint(bodyJson, "rightMiddleProximal", body.rightMiddleProximal);
		ParseFingerJoint(bodyJson, "rightMiddleMedial", body.rightMiddleMedial);
		ParseFingerJoint(bodyJson, "rightMiddleDistal", body.rightMiddleDistal);
		ParseFingerJoint(bodyJson, "rightMiddleTip", body.rightMiddleTip);
		
		ParseFingerJoint(bodyJson, "rightRingProximal", body.rightRingProximal);
		ParseFingerJoint(bodyJson, "rightRingMedial", body.rightRingMedial);
		ParseFingerJoint(bodyJson, "rightRingDistal", body.rightRingDistal);
		ParseFingerJoint(bodyJson, "rightRingTip", body.rightRingTip);
		
		ParseFingerJoint(bodyJson, "rightLittleProximal", body.rightLittleProximal);
		ParseFingerJoint(bodyJson, "rightLittleMedial", body.rightLittleMedial);
		ParseFingerJoint(bodyJson, "rightLittleDistal", body.rightLittleDistal);
		ParseFingerJoint(bodyJson, "rightLittleTip", body.rightLittleTip);
	}
	
	void RokokoDataParser::ParseFingerJoint(const nlohmann::json& bodyJson, const std::string& jointName, std::shared_ptr<RokokoData::ActorJointFrame>& joint)
	{
		if (bodyJson.contains(jointName)) {
			auto& jointJson = bodyJson[jointName];
			
			joint = std::make_shared<RokokoData::ActorJointFrame>();
			
			// Position 파싱
			if (jointJson.contains("position")) {
				auto& pos = jointJson["position"];
				joint->position.x = pos.value("x", 0.0f);
				joint->position.y = pos.value("y", 0.0f);
				joint->position.z = pos.value("z", 0.0f);
			}
			
			// Rotation 파싱
			if (jointJson.contains("rotation")) {
				auto& rot = jointJson["rotation"];
				joint->rotation.x = rot.value("x", 0.0f);
				joint->rotation.y = rot.value("y", 0.0f);
				joint->rotation.z = rot.value("z", 0.0f);
				joint->rotation.w = rot.value("w", 1.0f);
				
				// 쿼터니언 정규화
				NormalizeQuaternion(joint->rotation.x, joint->rotation.y, joint->rotation.z, joint->rotation.w);
			}
		}
	}
	
	bool RokokoDataParser::ValidateFrameData(const RokokoData::LiveFrame_v4& frame)
	{
		try {
			// 기본 검증
			if (frame.scene.actors.empty()) {
				return false;
			}
			
			const auto& actor = frame.scene.actors[0];
			
			// 손가락 데이터 검증 (왼손 엄지손가락만 체크)
			if (!actor.body.leftThumbMedial || !actor.body.leftThumbDistal || !actor.body.leftThumbTip) {
				return false;
			}
			
			return true;
			
		} catch (...) {
			return false;
		}
	}
	
	bool RokokoDataParser::ExtractFingerData(const RokokoData::LiveFrame_v4& frame,
											std::vector<RokokoData::ActorJointFrame>& leftHandFingers,
											std::vector<RokokoData::ActorJointFrame>& rightHandFingers)
	{
		try {
			leftHandFingers.clear();
			rightHandFingers.clear();
			
			if (frame.scene.actors.empty()) {
				return false;
			}
			
			const auto& actor = frame.scene.actors[0];
			
			// 왼손 데이터 추출 (20개 관절)
			if (actor.body.leftThumbProximal) leftHandFingers.push_back(*actor.body.leftThumbProximal);
			if (actor.body.leftThumbMedial) leftHandFingers.push_back(*actor.body.leftThumbMedial);
			if (actor.body.leftThumbDistal) leftHandFingers.push_back(*actor.body.leftThumbDistal);
			if (actor.body.leftThumbTip) leftHandFingers.push_back(*actor.body.leftThumbTip);
			
			if (actor.body.leftIndexProximal) leftHandFingers.push_back(*actor.body.leftIndexProximal);
			if (actor.body.leftIndexMedial) leftHandFingers.push_back(*actor.body.leftIndexMedial);
			if (actor.body.leftIndexDistal) leftHandFingers.push_back(*actor.body.leftIndexDistal);
			if (actor.body.leftIndexTip) leftHandFingers.push_back(*actor.body.leftIndexTip);
			
			if (actor.body.leftMiddleProximal) leftHandFingers.push_back(*actor.body.leftMiddleProximal);
			if (actor.body.leftMiddleMedial) leftHandFingers.push_back(*actor.body.leftMiddleMedial);
			if (actor.body.leftMiddleDistal) leftHandFingers.push_back(*actor.body.leftMiddleDistal);
			if (actor.body.leftMiddleTip) leftHandFingers.push_back(*actor.body.leftMiddleTip);
			
			if (actor.body.leftRingProximal) leftHandFingers.push_back(*actor.body.leftRingProximal);
			if (actor.body.leftRingMedial) leftHandFingers.push_back(*actor.body.leftRingMedial);
			if (actor.body.leftRingDistal) leftHandFingers.push_back(*actor.body.leftRingDistal);
			if (actor.body.leftRingTip) leftHandFingers.push_back(*actor.body.leftRingTip);
			
			if (actor.body.leftLittleProximal) leftHandFingers.push_back(*actor.body.leftLittleProximal);
			if (actor.body.leftLittleMedial) leftHandFingers.push_back(*actor.body.leftLittleMedial);
			if (actor.body.leftLittleDistal) leftHandFingers.push_back(*actor.body.leftLittleDistal);
			if (actor.body.leftLittleTip) leftHandFingers.push_back(*actor.body.leftLittleTip);
			
			// 오른손 데이터 추출 (20개 관절)
			if (actor.body.rightThumbProximal) rightHandFingers.push_back(*actor.body.rightThumbProximal);
			if (actor.body.rightThumbMedial) rightHandFingers.push_back(*actor.body.rightThumbMedial);
			if (actor.body.rightThumbDistal) rightHandFingers.push_back(*actor.body.rightThumbDistal);
			if (actor.body.rightThumbTip) rightHandFingers.push_back(*actor.body.rightThumbTip);
			
			if (actor.body.rightIndexProximal) rightHandFingers.push_back(*actor.body.rightIndexProximal);
			if (actor.body.rightIndexMedial) rightHandFingers.push_back(*actor.body.rightIndexMedial);
			if (actor.body.rightIndexDistal) rightHandFingers.push_back(*actor.body.rightIndexDistal);
			if (actor.body.rightIndexTip) rightHandFingers.push_back(*actor.body.rightIndexTip);
			
			if (actor.body.rightMiddleProximal) rightHandFingers.push_back(*actor.body.rightMiddleProximal);
			if (actor.body.rightMiddleMedial) rightHandFingers.push_back(*actor.body.rightMiddleMedial);
			if (actor.body.rightMiddleDistal) rightHandFingers.push_back(*actor.body.rightMiddleDistal);
			if (actor.body.rightMiddleTip) rightHandFingers.push_back(*actor.body.rightMiddleTip);
			
			if (actor.body.rightRingProximal) rightHandFingers.push_back(*actor.body.rightRingProximal);
			if (actor.body.rightRingMedial) rightHandFingers.push_back(*actor.body.rightRingMedial);
			if (actor.body.rightRingDistal) rightHandFingers.push_back(*actor.body.rightRingDistal);
			if (actor.body.rightRingTip) rightHandFingers.push_back(*actor.body.rightRingTip);
			
			if (actor.body.rightLittleProximal) rightHandFingers.push_back(*actor.body.rightLittleProximal);
			if (actor.body.rightLittleMedial) rightHandFingers.push_back(*actor.body.rightLittleMedial);
			if (actor.body.rightLittleDistal) rightHandFingers.push_back(*actor.body.rightLittleDistal);
			if (actor.body.rightLittleTip) rightHandFingers.push_back(*actor.body.rightLittleTip);
			
			return true;
			
		} catch (...) {
			return false;
		}
	}
	
	bool RokokoDataParser::ParseActorData(const void* actorJson, RokokoData::ActorData& actor)
	{
		// 현재는 기본 구현만 제공
		// 실제 JSON 파싱 라이브러리 사용 시 구현
		return true;
	}
	
	bool RokokoDataParser::ParseFingerData(const void* fingerJson, RokokoData::ActorJointFrame& finger)
	{
		// 현재는 기본 구현만 제공
		// 실제 JSON 파싱 라이브러리 사용 시 구현
		return true;
	}
	
	bool RokokoDataParser::ValidateQuaternion(float x, float y, float z, float w)
	{
		// NaN 체크
		if (std::isnan(x) || std::isnan(y) || std::isnan(z) || std::isnan(w)) {
			return false;
		}
		
		// 무한대 체크
		if (std::isinf(x) || std::isinf(y) || std::isinf(z) || std::isinf(w)) {
			return false;
		}
		
		// 쿼터니언 길이 체크 (정규화된 경우 1.0에 가까워야 함)
		float length = std::sqrt(x * x + y * y + z * z + w * w);
		if (length < 0.1f || length > 10.0f) {
			return false;
		}
		
		return true;
	}
	
	void RokokoDataParser::NormalizeQuaternion(float& x, float& y, float& z, float& w)
	{
		float length = std::sqrt(x * x + y * y + z * z + w * w);
		if (length > 0.0001f) {
			x /= length;
			y /= length;
			z /= length;
			w /= length;
		} else {
			// 기본값 설정
			x = 0.0f;
			y = 0.0f;
			z = 0.0f;
			w = 1.0f;
		}
	}
}