#ifndef ARM_DRIVER_HPP
#define ARM_DRIVER_HPP

#include <memory>
#include <string>
#include <vector>
#include "arm_define.h"
#include <shared_mutex>
#include <mutex>


struct ArmStatusCallbackData {
    int errCode;                        ///< 数据解析错误码，-3为数据解析错误，代表推送的数据不完整或格式不正确
    int armId;                          ///< 机械臂ID，0为左臂，1为右臂
    float jointCurrent[USED_ARM_DOF];   ///< 关节电流，单位mA，精度：0.001mA
    bool jointEnFlag[USED_ARM_DOF];     ///< 当前关节使能状态 ，1为上使能，0为掉使能
    float jointAngle[USED_ARM_DOF];     ///< 关节角度，单位度，精度：0.001度
    float jointSpeed[USED_ARM_DOF];     ///< 关节速度，单位度/秒，精度：0.001度/秒
    float jointTemp[USED_ARM_DOF];      ///< 关节温度，单位摄氏度，精度：0.1摄氏度
    float jointVoltage[USED_ARM_DOF];   ///< 关节电压，单位V，精度：0.01V
    float jointTorque[USED_ARM_DOF];    ///< 关节力矩，单位Nm，精度：0.001Nm
    int jointErrCode[USED_ARM_DOF];     ///< 当前关节错误码
};

enum ArmDriverStatus {
    ARM_MOVE_STATUS_UNINIT = 0,
    ARM_MOVE_STATUS_READY = 1,
    ARM_MOVE_STATUS_ERROR = 2
};

class ArmDriverDefault
{
public:
    ArmDriverDefault();
    ~ArmDriverDefault();

    int Init();

    using ArmStatusCallbackFun = void(*)(ArmStatusCallbackData *data);
    void RegArmStatusCallback(ArmStatusCallbackFun callback) {
        armStatusCallback_ = callback;
    }
    static void CallArmStatusCallback(ArmStatusCallbackData *data) {
        if (armStatusCallback_ != nullptr) {
            armStatusCallback_(data);
        }
    }

    using ArmMoveFun = int(*)(const float points[USED_ARM_DOF], int armId);
    void RegArmMoveCallback(ArmMoveFun callback) {
        armMoveFun_ = callback;
    }

    int ArmMove(const float points[USED_ARM_DOF], int armId) {
        if (armMoveFun_ != nullptr) {
            return armMoveFun_(points, armId);
        }
        return UNKNOWN_ERR;
    }

    void PublicThreadUpdate();

private:
    int initFlag_;

    static ArmStatusCallbackFun armStatusCallback_;
    ArmMoveFun armMoveFun_;

    static std::shared_mutex default_update_status_mutex_;
    static ArmStatusCallbackData leftCallbackData_;
    static ArmStatusCallbackData rightCallbackData_;
    static int DefaultArmMove(const float points[USED_ARM_DOF], int armId);
};

#endif // ARM_DRIVER_HPP