#include "LinkerHandApi.h"
#include "ErrorCode.h"
#include <algorithm>
#include <chrono>
#include <cstdint>
#include <exception>
#include <iostream>
#include <thread>
#include <vector>

namespace {

constexpr size_t kThumbFingerIndex = 0;
constexpr size_t kIndexFingerIndex = 1;
constexpr size_t kNormalForceSensorIndex = 0;
constexpr uint8_t kReleaseForceThreshold = 80;
constexpr auto kForcePollInterval = std::chrono::milliseconds(100);
constexpr auto kForceWaitTimeout = std::chrono::seconds(8);

uint8_t maxSensorValue(const std::vector<std::vector<std::vector<uint8_t>>> &force_data,
                       size_t finger_index,
                       size_t sensor_type_index) {
    if (finger_index >= force_data.size()) {
        return 0;
    }
    if (sensor_type_index >= force_data[finger_index].size()) {
        return 0;
    }

    const auto &sensor_values = force_data[finger_index][sensor_type_index];
    if (sensor_values.empty()) {
        return 0;
    }

    return *std::max_element(sensor_values.begin(), sensor_values.end());
}

} // namespace

int main() {

    // 调用API接口
    LinkerHandApi hand(LINKER_HAND::O6, HAND_TYPE::RIGHT, "can0", 1000000);

    // 获取版本信息
    std::cout << hand.getVersion() << std::endl;

    // // 握拳
    // std::vector<uint8_t> fist_pose = {255, 255, 0, 0, 0, 0};
    // hand.fingerMove(fist_pose);
    // std::this_thread::sleep_for(std::chrono::seconds(1));
    //
    // // 张开
    // std::vector<uint8_t> open_pose = {255, 255, 255, 255, 255, 255};
    // hand.fingerMove(open_pose);
    // std::this_thread::sleep_for(std::chrono::seconds(1));
 // O6/L6 关节常用映射（6 维关节向量）:
    // joint1: 拇指对掌/侧摆
    // joint2: 拇指根部弯曲
    // joint3: 食指弯曲
    // joint4: 中指弯曲
    // joint5: 无名指弯曲
    // joint6: 小指弯曲
    //
    // 你说的“五个”一般指五指通道：拇/食/中/无名/小指。
    // 在 O6 中，拇指通常有 2 个关节自由度，所以关节总数是 6。

    // 仅使用拇指和食指: 让中/无名/小指始终保持收拢（joint4~6 固定较小值）
    // 注意: 0~255 的方向可能因机械装配有差异，以下值是可调模板。
    std::vector<uint8_t> pre_grasp = {220, 170, 230, 20, 20, 20}; // 接近前，拇食张开
    std::vector<uint8_t> pinch = {120, 90, 80, 20, 20, 20};       // 拇食闭合夹住
    std::vector<uint8_t> hold = {120, 90, 90, 20, 20, 20};        // 保持抓取
    std::vector<uint8_t> release = {220, 170, 230, 20, 20, 20};   // 松开
    std::vector<uint8_t> safe_open = {255, 255, 255, 255, 255, 255};

    // 可选：速度/力矩（如果你的固件支持该型号）
    // 对 O6/L6 推荐使用 6 维；若你的设备固件要求 5 维，请按实际固件改。
    std::vector<uint8_t> speed = {90, 90, 90, 90, 90, 90};
    std::vector<uint8_t> torque = {255, 255, 255, 255, 255, 255};
    hand.setSpeed(speed);
    hand.setTorque(torque);

    // 1) 预抓取：拇指+食指张开，其他三指保持收拢
    hand.fingerMove(pre_grasp);
    std::this_thread::sleep_for(std::chrono::seconds(1));
//    if (hasFault(hand.getFaultCode())) return 1;

    // 2) 闭合夹取“水平盖”形状物体（两指对夹）
    hand.fingerMove(pinch);
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
//    if (hasFault(hand.getFaultCode())) return 1;

    // 3) 保持并持续检测拇指/食指法向压力，超过阈值立即松手
    hand.fingerMove(hold);
    bool released_by_force = false;
    const auto wait_deadline = std::chrono::steady_clock::now() + kForceWaitTimeout;

    while (std::chrono::steady_clock::now() < wait_deadline) {
        try {

            const auto force_data = hand.getForce();
            const uint8_t thumb_force = maxSensorValue(force_data, kThumbFingerIndex, kNormalForceSensorIndex);
            const uint8_t index_force = maxSensorValue(force_data, kIndexFingerIndex, kNormalForceSensorIndex);

            std::cout << "Thumb normal force: " << static_cast<int>(thumb_force)
                      << ", index normal force: " << static_cast<int>(index_force) << std::endl;

            if (thumb_force >= kReleaseForceThreshold || index_force >= kReleaseForceThreshold) {
                std::cout << "Force threshold reached, releasing object." << std::endl;
                hand.fingerMove(release);
                released_by_force = true;
                break;
            }
        } catch (const linkerhand::UnsupportedFeatureException &e) {
            std::cerr << "Force feedback is not supported on this hand: " << e.what() << std::endl;
            break;
        } catch (const std::exception &e) {
            std::cerr << "Failed to read force data: " << e.what() << std::endl;
            break;
        }

        std::this_thread::sleep_for(kForcePollInterval);
    }

    if (!released_by_force) {
        std::cout << "Force threshold was not reached before timeout, releasing object." << std::endl;
        hand.fingerMove(release);
    }
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // 4) 回到安全张开位
    hand.fingerMove(safe_open);
    std::this_thread::sleep_for(std::chrono::seconds(1));

    std::cout << "Done: thumb-index pick and force-triggered release sequence finished." << std::endl;
    return 0;
}
