//======================================================================================================
// Copyright 2025, Rokoko Glove OptiTrack Integration
//======================================================================================================
/**
 * RokokoDataConverter class provides data conversion functionality from Rokoko format to OptiTrack format.
 * This converter handles the mapping of 20 Rokoko finger joints to 15 OptiTrack finger joints.
 */

#pragma once
#include <vector>
#include <memory>
#include "GloveDataFormat.h"

// Forward declarations
namespace RokokoData
{
    struct Vector3Frame;
    struct Vector4Frame;
    struct ActorJointFrame;
    struct ActorData;
    struct LiveFrame_v4;
}

namespace RokokoIntegration
{
    class RokokoDataConverter
    {
    public:
        /**
         * Converts Rokoko LiveFrame_v4 data to OptiTrack glove data format
         * @param rokokoFrame Input Rokoko frame data
         * @return Converted OptiTrack glove data
         */
        static sGloveDeviceData ConvertRokokoToOptiTrack(const RokokoData::LiveFrame_v4& rokokoFrame);
        
        /**
         * Converts Rokoko finger joint data to OptiTrack finger node format
         * @param rokokoJoint Input Rokoko joint data
         * @param optiTrackNode Output OptiTrack node data
         * @return true if conversion successful
         */
        static bool ConvertJoint(const RokokoData::ActorJointFrame& rokokoJoint, sFingerNode& optiTrackNode);
        
        /**
         * Validates converted OptiTrack data
         * @param gloveData Data to validate
         * @return true if data is valid
         */
        static bool ValidateOptiTrackData(const sGloveDeviceData& gloveData);
        
        /**
         * Gets the mapping information for debugging
         * @return String containing mapping details
         */
        static std::string GetMappingInfo();
        
    private:
        /**
         * Maps Rokoko finger joints to OptiTrack format
         * Maps 20 Rokoko joints (4 per finger) to 15 OptiTrack joints (3 per finger)
         * Removes proximal joints and keeps medial, distal, tip
         */
        static void MapFingerJoints(const std::vector<RokokoData::ActorJointFrame>& rokokoFingers,
                                   std::vector<sFingerNode>& optiTrackNodes);
        
        /**
         * Converts quaternion from Rokoko format to OptiTrack format
         * @param rokokoQuat Input Rokoko quaternion
         * @param optiTrackQuat Output OptiTrack quaternion
         */
        static void ConvertQuaternion(const RokokoData::Vector4Frame& rokokoQuat, 
                                    float& quat_w, float& quat_x, float& quat_y, float& quat_z);
        
        /**
         * Normalizes quaternion values
         * @param w, x, y, z Quaternion components (modified in place)
         */
        static void NormalizeQuaternion(float& w, float& x, float& y, float& z);
        
        /**
         * Validates quaternion values
         * @param w, x, y, z Quaternion components
         * @return true if quaternion is valid
         */
        static bool ValidateQuaternion(float w, float x, float y, float z);
        
        /**
         * Applies coordinate system conversion for left/right hands
         * @param isLeftHand true if left hand, false if right hand
         * @param x, y, z Position coordinates (modified in place)
         */
        static void ApplyHandCoordinateSystem(bool isLeftHand, float& x, float& y, float& z);
        
        // Finger mapping constants
        static inline const int ROKOKO_JOINTS_PER_FINGER = 4;  // Proximal, Medial, Distal, Tip
        static inline const int OPTITRACK_JOINTS_PER_FINGER = 3;  // MP, PIP, DIP
        static inline const int TOTAL_FINGERS = 5;  // Thumb, Index, Middle, Ring, Little
        static inline const int TOTAL_ROKOKO_JOINTS = TOTAL_FINGERS * ROKOKO_JOINTS_PER_FINGER;  // 20
        static inline const int TOTAL_OPTITRACK_JOINTS = TOTAL_FINGERS * OPTITRACK_JOINTS_PER_FINGER;  // 15
        
        // Joint mapping table: Rokoko index -> OptiTrack index
        // Removes proximal joints (index 0, 4, 8, 12, 16)
        static inline const int JOINT_MAPPING[15] = {
            // Thumb: Medial(1), Distal(2), Tip(3) -> MP(0), PIP(1), DIP(2)
            1, 2, 3,
            // Index: Medial(5), Distal(6), Tip(7) -> MP(3), PIP(4), DIP(5)
            5, 6, 7,
            // Middle: Medial(9), Distal(10), Tip(11) -> MP(6), PIP(7), DIP(8)
            9, 10, 11,
            // Ring: Medial(13), Distal(14), Tip(15) -> MP(9), PIP(10), DIP(11)
            13, 14, 15,
            // Little: Medial(17), Distal(18), Tip(19) -> MP(12), PIP(13), DIP(14)
            17, 18, 19
        };
    };
}