#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "interfaces/msg/motor_cmd.hpp"
#include "interfaces/srv/motor_info.hpp"
#include "interfaces/srv/motor_param.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/image.hpp"
using namespace std::chrono_literals;

static auto g_program_start_time = std::chrono::steady_clock::now();

double log_printf(const char* format, ...) {
    auto now = std::chrono::steady_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(now - g_program_start_time);
    double seconds = elapsed.count();
    double minute=seconds/60;
    char buffer[1024];
    va_list args;
    va_start(args, format);
    int n = vsnprintf(buffer, sizeof(buffer), format, args);
    va_end(args);

    if (n < 0) {
        std::fprintf(stderr, "[%.3f] (log error)\n", seconds);
        return 0;
    }

    std::fprintf(stdout, "[%.3f/%.1f] %s", seconds,minute, buffer);
    std::fflush(stdout);
    return seconds;
}
using MotorCmd = interfaces::msg::MotorCmd;
using MotorInfo=interfaces::srv::MotorInfo;
using MotorParam=interfaces::srv::MotorParam;
class MotorCmdPublisher : public rclcpp::Node
{
public:
  MotorCmdPublisher()
  : Node("motor_test_node")
  {
    publisher_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
    //publisher_ = this->create_publisher<std_msgs::msg::String>("/led_cmd", 10);
     cam_sub=this->create_subscription<sensor_msgs::msg::Image>("/camera/camera/color/image_raw",10,[=](sensor_msgs::msg::Image::SharedPtr img){
		     static double last_stamp;
		   double cur_stamp= log_printf("sub img:%dx%d ",img->width,img->height);
		   double diff=cur_stamp-last_stamp;
		   last_stamp=cur_stamp;
		   log_printf("hz:%.1f\n",1.0/diff);
		     
		     });
    //client = create_client<MotorInfo>("/motor_info");
    ///client = create_client<MotorParam>("/motor_param");
    ////timer_ = this->create_wall_timer(5000ms, std::bind(&MotorCmdPublisher::timer_callback, this));
  }
 
private:
  int angle1=9999;
  int angle2=6666;
  int xcnt=0;
  //rclcpp::Client<interfaces::srv::MotorInfo>::SharedPtr client;
  rclcpp::Client<interfaces::srv::MotorParam>::SharedPtr client;
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr cam_sub;
  void timer_callback()
  {


auto request = std::make_shared<MotorParam::Request>();
request->motor_id=1;
request->max_speed=20;
request->add_acc=4;
request->dec_acc=5;
/*
auto future = client->async_send_request(request);

if (future.wait_for(5s) != std::future_status::ready) {
  RCLCPP_ERROR(this->get_logger(), "服务调用超时");
  return;
}
auto response=future.get();
printf("ret :%d\n",response->ret);
*/

using ServiceResponseFuture = rclcpp::Client<interfaces::srv::MotorParam>::SharedFuture;
    auto response_received_callback = [this](ServiceResponseFuture future) {
        auto response = future.get();
        RCLCPP_INFO(this->get_logger(), "Service response: ret = %d", response->ret);
    };

    auto future = client->async_send_request(request, response_received_callback);




#if 0
auto request = std::make_shared<MotorInfo::Request>();
request->type="bm";
request->motor_ids.clear();
request->motor_ids.push_back(static_cast<uint8_t>(1));
request->motor_ids.push_back(static_cast<uint8_t>(2));

auto future = client->async_send_request(request);

if (future.wait_for(10s) != std::future_status::ready) {
  RCLCPP_ERROR(this->get_logger(), "服务调用超时");
  return;
}

auto response = future.get();
printf("ret angle:%.1f,%.1f\n",response->motor_angles[0],response->motor_angles[1]);
#endif
#if 0
    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);
#endif 
#if 0  
    auto message=std_msgs::msg::String();
    xcnt+=1;
    if(xcnt%2==0)
    	message.data="green,10";
    else
	message.data="blue,10";
    publisher_->publish(message);

    RCLCPP_INFO(this->get_logger(), "Published LedCmd message:%s",message.data.c_str());
#endif
    //timer_->cancel();
  }
  int idx=0; 
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<MotorCmd>::SharedPtr publisher_;
  //rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
};

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