#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "interfaces/msg/motor_cmd.hpp"

using namespace std::chrono_literals;

using MotorCmd = interfaces::msg::MotorCmd;

class MotorCmdPublisher : public rclcpp::Node
{
public:
  MotorCmdPublisher()
  : Node("motor_test_node")
  {
    publisher_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
    
    timer_ = this->create_wall_timer(100ms, std::bind(&MotorCmdPublisher::timer_callback, this));
  }

private:
  int angle1=9999;
  int angle2=6666;
  void timer_callback()
  {
    auto message = MotorCmd();
    
    message.target = "rs485";
    message.type = "bm";
    message.position = "";
    
    message.motor_id.push_back(1);
    message.motor_id.push_back(2);
    idx+=1;
    //angle1=angle1+20;
    //angle2=angle2-20;
    /*
    if(idx%2==0){
    }else{
	angle1=-300;
	angle2=300;
    }
    */
    message.motor_angle.push_back((float)angle1);
    message.motor_angle.push_back((float)angle2);
    publisher_->publish(message);
    
    RCLCPP_INFO(this->get_logger(), "Published MotorCmd message:%d,%d",angle1,angle2);
    
    timer_->cancel();
  }
  int idx=0; 
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<MotorCmd>::SharedPtr publisher_;
};

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