#include "arm_driver.hpp"
#include <cstring>

ArmStatusCallbackData ArmDriverDefault::leftCallbackData_;
ArmStatusCallbackData ArmDriverDefault::rightCallbackData_;
std::shared_mutex ArmDriverDefault::default_update_status_mutex_;
ArmDriverDefault::ArmStatusCallbackFun ArmDriverDefault::armStatusCallback_ = nullptr;

ArmDriverDefault::ArmDriverDefault()
{
    armStatusCallback_ = nullptr;
    armMoveFun_ = nullptr;
    initFlag_ = ARM_MOVE_STATUS_UNINIT;
}

ArmDriverDefault::~ArmDriverDefault()
{
}

void ArmDriverDefault::PublicThreadUpdate()
{
    std::unique_lock lock(default_update_status_mutex_);
    armStatusCallback_(&leftCallbackData_);
    armStatusCallback_(&rightCallbackData_);
}

int ArmDriverDefault::DefaultArmMove(const float points[USED_ARM_DOF], int armId)
{
    ArmStatusCallbackData *callbackData;
    if (armId == LEFT_ARM) {
        callbackData = &leftCallbackData_;
    } else if (armId == RIGHT_ARM) {
        callbackData = &rightCallbackData_;
    } else {
        return UNKNOWN_ERR;
    }
    std::shared_lock lock(default_update_status_mutex_);
    callbackData->errCode = 0;
    for (size_t i = 0; i < USED_ARM_DOF; i++) {
        callbackData->jointAngle[i] = points[i];
    }
    armStatusCallback_(callbackData);
    return 0;
}

int ArmDriverDefault::Init()
{
    if (armStatusCallback_ == nullptr) {
        return UNKNOWN_ERR;
    }
    if (armMoveFun_ == nullptr) {
        armMoveFun_ = DefaultArmMove;
        memset(&leftCallbackData_, 0, sizeof(ArmStatusCallbackData));
        memset(&rightCallbackData_, 0, sizeof(ArmStatusCallbackData));
        rightCallbackData_.armId = RIGHT_ARM;
        leftCallbackData_.armId = LEFT_ARM;
        for (size_t i = 0; i < USED_ARM_DOF; i++) {
            leftCallbackData_.jointEnFlag[i] = true;
            rightCallbackData_.jointEnFlag[i] = true;
            leftCallbackData_.jointTemp[i] = 0.0f;
            rightCallbackData_.jointTemp[i] = 0.0f;
            leftCallbackData_.jointVoltage[i] = 0.0f;
            rightCallbackData_.jointVoltage[i] = 0.0f;
            leftCallbackData_.jointCurrent[i] = 0.0f;
            rightCallbackData_.jointCurrent[i] = 0.0f;
            leftCallbackData_.jointSpeed[i] = 0.0f;
            rightCallbackData_.jointSpeed[i] = 0.0f;
            leftCallbackData_.jointTorque[i] = 0.0f;
            rightCallbackData_.jointTorque[i] = 0.0f;
            leftCallbackData_.jointErrCode[i] = 0;
            rightCallbackData_.jointErrCode[i] = 0;
            leftCallbackData_.jointAngle[i] = 0.0f;
            rightCallbackData_.jointAngle[i] = 0.0f;
        }
    }
    initFlag_ = ARM_MOVE_STATUS_READY;
    return OK;
}

