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

#include "RokokoDataConverter.h"
#include "RokokoData.h"
#include <cmath>
#include <sstream>
#include <algorithm>

namespace RokokoIntegration
{
    // 정적 상수는 헤더에서 inline으로 정의됨

    sGloveDeviceData RokokoDataConverter::ConvertRokokoToOptiTrack(const RokokoData::LiveFrame_v4& rokokoFrame)
    {
        sGloveDeviceData optiTrackData;
        
        try {
            // 기본 데이터 설정
            optiTrackData.gloveId = 1;  // 기본 장갑 ID
            optiTrackData.timestamp = 0.0;  // 타임스탬프는 나중에 설정
            
            // 15개 OptiTrack 관절 노드 초기화
            optiTrackData.nodes.resize(TOTAL_OPTITRACK_JOINTS);
            
            // 검증: actors가 존재하는지 확인
            if (rokokoFrame.scene.actors.empty()) {
                optiTrackData.nodes.clear();
                return optiTrackData;
            }
            
            const auto& actor = rokokoFrame.scene.actors[0];

            // ✅ OPTIMIZATION: Use has_value() for optional instead of implicit bool conversion
            // 손가락별 데이터 매핑 (왼손)
            // 엄지손가락 (Thumb)
            if (actor.body.leftThumbMedial.has_value()) ConvertJoint(*actor.body.leftThumbMedial, optiTrackData.nodes[0]);  // MP
            if (actor.body.leftThumbDistal.has_value()) ConvertJoint(*actor.body.leftThumbDistal, optiTrackData.nodes[1]); // PIP
            if (actor.body.leftThumbTip.has_value()) ConvertJoint(*actor.body.leftThumbTip, optiTrackData.nodes[2]);       // DIP

            // 검지손가락 (Index)
            if (actor.body.leftIndexMedial.has_value()) ConvertJoint(*actor.body.leftIndexMedial, optiTrackData.nodes[3]);  // MP
            if (actor.body.leftIndexDistal.has_value()) ConvertJoint(*actor.body.leftIndexDistal, optiTrackData.nodes[4]); // PIP
            if (actor.body.leftIndexTip.has_value()) ConvertJoint(*actor.body.leftIndexTip, optiTrackData.nodes[5]);       // DIP

            // 중지손가락 (Middle)
            if (actor.body.leftMiddleMedial.has_value()) ConvertJoint(*actor.body.leftMiddleMedial, optiTrackData.nodes[6]);  // MP
            if (actor.body.leftMiddleDistal.has_value()) ConvertJoint(*actor.body.leftMiddleDistal, optiTrackData.nodes[7]); // PIP
            if (actor.body.leftMiddleTip.has_value()) ConvertJoint(*actor.body.leftMiddleTip, optiTrackData.nodes[8]);       // DIP

            // 약지손가락 (Ring)
            if (actor.body.leftRingMedial.has_value()) ConvertJoint(*actor.body.leftRingMedial, optiTrackData.nodes[9]);    // MP
            if (actor.body.leftRingDistal.has_value()) ConvertJoint(*actor.body.leftRingDistal, optiTrackData.nodes[10]);   // PIP
            if (actor.body.leftRingTip.has_value()) ConvertJoint(*actor.body.leftRingTip, optiTrackData.nodes[11]);         // DIP

            // 새끼손가락 (Little)
            if (actor.body.leftLittleMedial.has_value()) ConvertJoint(*actor.body.leftLittleMedial, optiTrackData.nodes[12]); // MP
            if (actor.body.leftLittleDistal.has_value()) ConvertJoint(*actor.body.leftLittleDistal, optiTrackData.nodes[13]); // PIP
            if (actor.body.leftLittleTip.has_value()) ConvertJoint(*actor.body.leftLittleTip, optiTrackData.nodes[14]);       // DIP
            
            // 노드 ID 설정
            for (int i = 0; i < TOTAL_OPTITRACK_JOINTS; i++) {
                optiTrackData.nodes[i].node_id = i;
            }
            
        } catch (...) {
            // 에러 발생 시 빈 데이터 반환
            optiTrackData.nodes.clear();
        }
        
        return optiTrackData;
    }
    
    bool RokokoDataConverter::ConvertJoint(const RokokoData::ActorJointFrame& rokokoJoint, sFingerNode& optiTrackNode)
    {
        try {
            // 쿼터니언 변환
            ConvertQuaternion(rokokoJoint.rotation, 
                           optiTrackNode.quat_w, 
                           optiTrackNode.quat_x, 
                           optiTrackNode.quat_y, 
                           optiTrackNode.quat_z);
            
            // 쿼터니언 정규화
            NormalizeQuaternion(optiTrackNode.quat_w, 
                              optiTrackNode.quat_x, 
                              optiTrackNode.quat_y, 
                              optiTrackNode.quat_z);
            
            // 쿼터니언 유효성 검사
            if (!ValidateQuaternion(optiTrackNode.quat_w, 
                                  optiTrackNode.quat_x, 
                                  optiTrackNode.quat_y, 
                                  optiTrackNode.quat_z)) {
                return false;
            }
            
            return true;
            
        } catch (...) {
            return false;
        }
    }
    
    bool RokokoDataConverter::ValidateOptiTrackData(const sGloveDeviceData& gloveData)
    {
        // 기본 검증
        if (gloveData.nodes.size() != TOTAL_OPTITRACK_JOINTS) {
            return false;
        }
        
        // 각 노드 검증
        for (const auto& node : gloveData.nodes) {
            if (!ValidateQuaternion(node.quat_w, node.quat_x, node.quat_y, node.quat_z)) {
                return false;
            }
        }
        
        return true;
    }
    
    std::string RokokoDataConverter::GetMappingInfo()
    {
        std::ostringstream oss;
        oss << "Rokoko to OptiTrack Finger Joint Mapping:\n";
        oss << "Total Rokoko joints: " << TOTAL_ROKOKO_JOINTS << " (4 per finger)\n";
        oss << "Total OptiTrack joints: " << TOTAL_OPTITRACK_JOINTS << " (3 per finger)\n";
        oss << "Mapping removes proximal joints and keeps:\n";
        oss << "  - Medial (MP): Metacarpophalangeal\n";
        oss << "  - Distal (PIP): Proximal Interphalangeal\n";
        oss << "  - Tip (DIP): Distal Interphalangeal\n";
        
        return oss.str();
    }
    
    void RokokoDataConverter::MapFingerJoints(const std::vector<RokokoData::ActorJointFrame>& rokokoFingers,
                                             std::vector<sFingerNode>& optiTrackNodes)
    {
        if (rokokoFingers.size() != TOTAL_ROKOKO_JOINTS || optiTrackNodes.size() != TOTAL_OPTITRACK_JOINTS) {
            return;
        }
        
        // 매핑 테이블을 사용하여 관절 변환
        for (int i = 0; i < TOTAL_OPTITRACK_JOINTS; i++) {
            int rokokoIndex = JOINT_MAPPING[i];
            if (rokokoIndex >= 0 && rokokoIndex < TOTAL_ROKOKO_JOINTS) {
                ConvertJoint(rokokoFingers[rokokoIndex], optiTrackNodes[i]);
            }
        }
    }
    
    void RokokoDataConverter::ConvertQuaternion(const RokokoData::Vector4Frame& rokokoQuat,
                                               float& quat_w, float& quat_x, float& quat_y, float& quat_z)
    {
        // Rokoko 쿼터니언을 OptiTrack 형식으로 복사
        quat_w = rokokoQuat.w;
        quat_x = rokokoQuat.x;
        quat_y = rokokoQuat.y;
        quat_z = rokokoQuat.z;
    }
    
    void RokokoDataConverter::NormalizeQuaternion(float& w, float& x, float& y, float& z)
    {
        float length = std::sqrt(w * w + x * x + y * y + z * z);
        
        if (length > 0.0f) {
            float invLength = 1.0f / length;
            w *= invLength;
            x *= invLength;
            y *= invLength;
            z *= invLength;
        } else {
            // 유효하지 않은 쿼터니언인 경우 기본값 설정
            w = 1.0f;
            x = 0.0f;
            y = 0.0f;
            z = 0.0f;
        }
    }
    
    bool RokokoDataConverter::ValidateQuaternion(float w, float x, float y, float z)
    {
        // NaN 체크
        if (std::isnan(w) || std::isnan(x) || std::isnan(y) || std::isnan(z)) {
            return false;
        }
        
        // 무한대 체크
        if (std::isinf(w) || std::isinf(x) || std::isinf(y) || std::isinf(z)) {
            return false;
        }
        
        // 쿼터니언 길이 체크 (정규화된 경우 1.0에 가까워야 함)
        float length = std::sqrt(w * w + x * x + y * y + z * z);
        if (std::abs(length - 1.0f) > 0.1f) {
            return false;
        }
        
        return true;
    }
    
    void RokokoDataConverter::ApplyHandCoordinateSystem(bool isLeftHand, float& x, float& y, float& z)
    {
        // 좌우손 좌표계 변환
        if (isLeftHand) {
            // 왼손: +X축이 손가락 끝 방향
            // 변환 없음 (기본값)
        } else {
            // 오른손: +X축이 손목/몸 방향
            x = -x;  // X축 반전
        }
    }
}