//
// Created by xtkuang on 2025/5/8.
//

#ifndef CMVR_ES_ABSTRACT_DEXHAND_H
#define CMVR_ES_ABSTRACT_DEXHAND_H
#pragma once

#include "abstract_device.h"

namespace cmvr::device{
    // 单个触觉点数据（16位无符号整数）
    using TactilePoint = uint16_t;

    // 手指触觉数据结构体（支持从数组任意位置解析）
    struct FingerTactileData {
        std::vector<std::vector<TactilePoint>> data; // 触觉数据二维数组
        int rows;                                     // 行数
        int cols;                                     // 列数
        int byteSize;                                 // 总字节数（rows * cols * 2字节/点）
        std::string name;                             // 部位名称

        // 初始化数据数组
        void init() {
            data.resize(rows, std::vector<TactilePoint>(cols, 0));
        }

        // 从 std::vector<uint16_t> 中赋值数据
        void assignFromVector(const std::vector<uint16_t>& values, int startIndex = 0) {
            int index = startIndex;
            for (int i = 0; i < rows; ++i) {
                for (int j = 0; j < cols; ++j) {
                    if (index < values.size()) {
                        data[i][j] = values[index];
                        index++;
                    }
                }
            }
        }
    };

    // 手掌触觉数据结构体（支持从数组任意位置解析）
    struct PalmTactileData {
        std::vector<std::vector<TactilePoint>> data; // 触觉数据二维数组
        int rows = 8;                                 // 行数（掌心固定为8行）
        int cols = 14;                                // 列数（掌心固定为14列）
        int byteSize = 224;                           // 总字节数（8*14*2=224）
        std::string name = "掌心";                    // 部位名称

        // 初始化数据数组
        void init() {
            data.resize(rows, std::vector<TactilePoint>(cols, 0));
        }

        // 从 std::vector<uint16_t> 中赋值数据
        void assignFromVector(const std::vector<uint16_t>& values, int startIndex = 0) {
            int index = startIndex;
            for (int j = 0; j < cols; ++j) {
                for (int i = rows - 1; i >= 0; --i) {
                    if (index < values.size()) {
                        data[i][j] = values[index];
                        index++;
                    }
                }
            }
        }
    };

    // 手指枚举（方便按手指类型解析）
    enum FingerType {
        PINKY,    // 小拇指
        RING,     // 无名指
        MIDDLE,   // 中指
        INDEX,    // 食指
        THUMB     // 大拇指
    };

    // 整只手的触觉传感器数据
    struct HandTactileSensors {
        // 五指的触觉数据（每根手指包含指端、指尖、指腹）
        struct {
            FingerTactileData tip;   // 指端
            FingerTactileData finger; // 指尖
            FingerTactileData pad;    // 指腹
        } pinky, ring, middle, index; // 小拇指、无名指、中指、食指

        // 大拇指（特殊：多一个指中）
        struct {
            FingerTactileData tip;    // 指端
            FingerTactileData finger;  // 指尖
            FingerTactileData middle;  // 指中
            FingerTactileData pad;     // 指腹
        } thumb;

        // 掌心触觉数据
        PalmTactileData palm;

        // 初始化所有传感器数据（设置相对偏移量）
        void init() {
            // 小拇指（总370byte）
            pinky.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "小拇指指端"};
            pinky.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "小拇指指尖"};
            pinky.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "小拇指指腹"};

            // 无名指（总370byte）
            ring.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "无名指指端"};
            ring.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "无名指指尖"};
            ring.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "无名指指腹"};

            // 中指（总370byte）
            middle.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "中指指端"};
            middle.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "中指指尖"};
            middle.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "中指指腹"};

            // 食指（总370byte）
            index.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "食指指端"};
            index.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "食指指尖"};
            index.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "食指指腹"};

            // 大拇指（总420byte）
            thumb.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "大拇指指端"};
            thumb.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "大拇指尖"};
            thumb.middle = {.rows = 3, .cols = 3, .byteSize = 18, .name = "大拇指指中"};
            thumb.pad = {.rows = 12, .cols = 8, .byteSize = 192, .name = "大拇指指腹"};

            // 掌心（总224byte）
            palm = { .name = "掌心"};

            // 初始化所有数据数组
            pinky.tip.init();
            pinky.finger.init();
            pinky.pad.init();

            ring.tip.init();
            ring.finger.init();
            ring.pad.init();

            middle.tip.init();
            middle.finger.init();
            middle.pad.init();

            index.tip.init();
            index.finger.init();
            index.pad.init();

            thumb.tip.init();
            thumb.finger.init();
            thumb.middle.init();
            thumb.pad.init();

            palm.init();
        }
        // 按手指类型解析整个手指的数据
        void parseFinger(FingerType fingerType, const std::vector<uint16_t>& values) {
            switch (fingerType) {
                case PINKY:
                    pinky.tip.assignFromVector(values);
                    pinky.finger.assignFromVector(values, 9);
                    pinky.pad.assignFromVector(values, 9 + 96);
                    break;

                case RING:
                    ring.tip.assignFromVector(values);
                    ring.finger.assignFromVector(values, 9);
                    ring.pad.assignFromVector(values, 9 + 96);
                    break;

                case MIDDLE:
                    middle.tip.assignFromVector(values);
                    middle.finger.assignFromVector(values, 9);
                    middle.pad.assignFromVector(values, 9 + 96);
                    break;

                case INDEX:
                    index.tip.assignFromVector(values);
                    index.finger.assignFromVector(values, 9);
                    index.pad.assignFromVector(values, 9 + 96);
                    break;

                case THUMB:
                    thumb.tip.assignFromVector(values);
                    thumb.finger.assignFromVector(values, 9);
                    thumb.middle.assignFromVector(values, 9 + 96);
                    thumb.pad.assignFromVector(values, 9 + 96 + 9);
                    break;
            }
        }

        // 解析手心（掌心）数据
        void parsePalm(const std::vector<uint16_t>& values) {
            palm.assignFromVector(values);
        }

        // 获取手指名称（用于调试）
        std::string getFingerName(FingerType fingerType) {
            switch (fingerType) {
                case PINKY: return "小拇指";
                case RING: return "无名指";
                case MIDDLE: return "中指";
                case INDEX: return "食指";
                case THUMB: return "大拇指";
                default: return "未知";
            }
        }
    };
    class AbstractDexHand: public AbstractDevice{
    public:
        explicit AbstractDexHand(const XmlNode& cfg): AbstractDevice(cfg) {}
        ~AbstractDexHand() override = default;

        virtual void getState(DexHandState &state) {}

        // control
        virtual void setPositions(const std::vector<int>& finger_joint_targets) {}
        virtual void setAngles(const std::vector<int>& finger_joint_angles) {}
        virtual void setVelocities(const std::vector<int>& finger_joint_velocities) {}
        virtual void setPresetAct(int action_id) {}
        virtual void execPresetAct(int action_id) {}
        virtual void setForce(const std::vector<int>& finger_joint_force) {}
        virtual HandTactileSensors& getSensorData() { return hand_tactile_sensors_;}
    protected:
        DexHandState status_;
        HandTactileSensors hand_tactile_sensors_;
    };
}

#endif //CMVR_ES_ABSTRACT_DEXHAND_H