#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "serialPort/SerialPort.h"
#include "unitreeMotor/unitreeMotor.h"

using namespace std::chrono_literals;

class MotorControlNode : public rclcpp::Node
{
public:
    MotorControlNode()
        : Node("goM8010_6_motor"), serial_("/dev/ttyACM0")
    {
        timer_ = this->create_wall_timer(
            1ms, std::bind(&MotorControlNode::control_motor, this));
    }

private:
    void control_motor()
    {
        MotorCmd cmd;
        MotorData data;

        cmd.motorType = MotorType::GO_M8010_6;
        data.motorType = MotorType::GO_M8010_6;
        cmd.mode = queryMotorMode(MotorType::GO_M8010_6, MotorMode::FOC);
        cmd.id = 0;
        cmd.q = 0.0; 
        cmd.dq = 0.0;
        cmd.kp = 0.0;
        cmd.kd = 0.0;
        cmd.tau = 0.0;

        serial_.sendRecv(&cmd, &data);

        RCLCPP_INFO(this->get_logger(), "\nMotor.q: %f\nMotor.temp: %d\nMotor.W: %f\nMotor.tau: %f\n",
                    data.q, data.temp, data.dq, data.tau);
    }

    SerialPort serial_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MotorControlNode>());
    rclcpp::shutdown();
    return 0;
}