#include "arm_driver_define.hpp"
#include "rm_arm_node.hpp"
#include <thread>

std::thread publicThread_;

void PublicThread(ArmDriverDefault *armDriver)
{
    while (rclcpp::ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        armDriver->PublicThreadUpdate();
    }
}

void custom_api_log(const char* message, va_list args)
{
    if (!message) {
        fprintf(stderr, "Error: message is a null pointer");
        return;
    }

    char buffer[1024];
    vsnprintf(buffer, sizeof(buffer), message, args);
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), " %s",  buffer);
}

int InitRmDriver2(RmArmDriverAndKinematics **armDriver, RmArmDriverAndKinematics **armKinematics) 
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Initializing rm arm driver...\n");
    *armDriver = new RmArmDriverAndKinematics();
    (*armDriver)->SetCustomApiLog(custom_api_log);
    (*armDriver)->RegArmStatusCallback(RmArmNode::CallbackRealtimeArmJointState);
    int initResult = (*armDriver)->Init();
    if (initResult != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize rm arm driver. %d", initResult);
        return UNKNOWN_ERR;
    }

    *armKinematics = *armDriver;
    return 0;
}

int InitSimulatorDriver(ArmDriverDefault **armDriver, TrapezoidalTrajectoryKinematics **armKinematics)
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Initializing rm arm driver...\n");
    *armDriver = new ArmDriverDefault();
    (*armDriver)->RegArmStatusCallback(RmArmNode::CallbackRealtimeArmJointState);
    int initResult = (*armDriver)->Init();
    if (initResult != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize arm driver. %d", initResult);
        return UNKNOWN_ERR;
    }
    *armKinematics = new TrapezoidalTrajectoryKinematics();

    std::thread(PublicThread, *armDriver).detach();
    return 0;
}

int InitRmArmDriver(ArmDriverDefault **armDriver, TrapezoidalTrajectoryKinematics **armKinematics) 
{
    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Initializing rm arm driver...\n");
    auto rmArmDriver = new RmArmDriver();
    rmArmDriver->SetCustomApiLog(custom_api_log);
    rmArmDriver->RegArmStatusCallback(ArmDriverDefault::CallArmStatusCallback);
    int initResult = rmArmDriver->Init();
    if (initResult != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize rm arm driver. %d", initResult);
        return UNKNOWN_ERR;
    }

    *armDriver = new ArmDriverDefault();
    (*armDriver)->RegArmStatusCallback(RmArmNode::CallbackRealtimeArmJointState);
    (*armDriver)->RegArmMoveCallback(RmArmDriver::ArmMove);
    initResult = (*armDriver)->Init();
    if (initResult != 0) {
        RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize arm driver. %d", initResult);
        return UNKNOWN_ERR;
    }

    *armKinematics = new TrapezoidalTrajectoryKinematics(rmArmDriver->rmService_);
    return 0;
}