#include"rclcpp/rclcpp.hpp"
#include"motor_dev/rs485_driver.hpp"
#include"interfaces/msg/motor_cmd.hpp"
#include"interfaces/msg/motor_pos.hpp"
#include"interfaces/srv/motor_param.hpp"
using namespace motor_dev;
RS485Driver rs485_driver_;

int main(int argc,char* argv[]){
    rclcpp::init(argc,argv);
    auto node=rclcpp::Node::make_shared("motor_dev_node");
    ////auto pos_pub = node->create_publisher<motor_dev::msg::MotorPos>("/motor_pos", 10);
    std::cout << "open RS485 port.." << std::endl;
    if (!rs485_driver_.openPort("/dev/ttyACM0", 921600)){
        std::cout << "Failed to open RS485 port." << std::endl;
    }
    usleep(100000);
    rs485_driver_.bm_set_enable(1);
    rs485_driver_.bm_set_param(1,28,2);
    rs485_driver_.bm_set_param(2,28,2);
    rs485_driver_.bm_set_param(3,28,2);
    rs485_driver_.bm_set_param(4,28,2);
    rs485_driver_.bm_set_enable(2);

    rs485_driver_.bm_set_param(1,84,1);
    rs485_driver_.bm_set_param(2,84,1);
    rs485_driver_.bm_set_param(3,84,1);
    rs485_driver_.bm_set_param(4,84,1);
    usleep(1000000);
    rs485_driver_.bm_set_speed(8.0f,10,10,10);
    int idx=0;
    while(idx<20){
        rs485_driver_.bm_get_speed();
        usleep(100000);
        idx+=1;
    }
    ///usleep(20000000);
    rs485_driver_.bm_set_speed(0,0,0,0);
    idx=0;
    while(idx<10){
        rs485_driver_.bm_get_speed();
        usleep(100000);
        idx+=1;
    }
    //rs485_driver_.bm_set_enable(2,1);
    //rs485_driver_.bm_set_enable(3,1);
    //rs485_driver_.bm_set_enable(4,1);
    return 0;


    rs485_driver_.bm_set_param(1,84,10);
    rs485_driver_.bm_set_param(2,84,10);

    //init
    /////rs485_driver_.bm_set_mode(1,3);//位置模式
    auto rs485_sub_=node->create_subscription<interfaces::msg::MotorCmd>("/motor_cmd",10,[](const interfaces::msg::MotorCmd::SharedPtr msg){
            ///
            if (msg->target == "rs485") {
                if(msg->type=="bm"){
                    size_t n = msg->motor_id.size();
                    if(n<2){
                        printf("bm msg need two angle\n");
                        return;
                    }
                    //float angle1    =msg->motor_angle[0];
                    //float angle2    =msg->motor_angle[1];

                    float speed1=msg->motor_speed[0];
                    float speed2=msg->motor_speed[1];
                    printf("##############:%s,speed:%.1f,%.1f\n",msg->type.c_str(),speed1,speed2);
                    //rs485_driver_.bm_set_pos(angle1,angle2);
                    rs485_driver_.bm_set_speed(speed1,speed2,0,0);
                }else if(msg->type=="mt"){
                    uint8_t id=msg->motor_id[0];
                    float angle    =msg->motor_angle[0];
                    rs485_driver_.mt_set_pos(id,angle);
                }
            }
        });
    auto motor_pub=node->create_publisher<interfaces::msg::MotorPos>("/motor_pos",10);
    //读取电机位置
    rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(5000), [=]() { 
        static int pub_cnt=0;
        interfaces::msg::MotorPos motor_pos;
        //float angle1=rs485_driver_.bm_get_pos(1);
        //float angle2=rs485_driver_.bm_get_pos(2);
        #if 0
        float speed1=rs485_driver_.bm_get_speed(1);
        float speed2=rs485_driver_.bm_get_speed(2);
        ///printf("bm cur angle:%.1f\n",angle);
        ///rs485_driver_.bm_query_id();
        //rs485_driver_.mt_query_id();//3e cd 08 79 00 01 00 00 00 00 00 f8 95 
        motor_pos.motor_id.clear();
        motor_pos.motor_angle.clear();
        motor_pos.motor_speed.clear();
        ////printf("pub msg[%03d],%.1f,%.1f\n",pub_cnt,angle1,angle2);
        //if(angle1>-1&&angle2>-1)
        {
            motor_pos.source = "rs485";
            motor_pos.type = "bm";
            motor_pos.position="none";
            motor_pos.motor_id.push_back(1);
            motor_pos.motor_id.push_back(2);
            motor_pos.motor_speed.push_back(speed1);
            motor_pos.motor_speed.push_back(speed2);
            motor_pub->publish(motor_pos);
            
            //std::cout<<"pub msg["<<pub_cnt<<"]:"<<angle1<<","<<angle2<<std::endl;
            printf("[%03d]=====%.1f,%.1f\n",pub_cnt,speed1,speed2);
        }
        #endif
        pub_cnt+=1;
        #if 0
        motor_pos.motor_id.clear();
        motor_pos.motor_angle.clear();
        float angle=rs485_driver_.mt_get_pos(1);
        if(angle!=9999){
            motor_pos.type = "mt";
            motor_pos.motor_id.push_back(1);
            motor_pos.motor_angle.push_back(angle);
            motor_pub->publish(motor_pos);
        }
        #endif
    });
    auto motor_param_srv=node->create_service<interfaces::srv::MotorParam>("motor_param",[=](const std::shared_ptr<interfaces::srv::MotorParam::Request> req,std::shared_ptr<interfaces::srv::MotorParam::Response> res){
            uint8_t mid=req->motor_id;
            res->ret=-1;
            printf("recv sv:%d,%d,%d,%d\n",req->motor_id,req->max_speed,req->add_acc,req->dec_acc);
            if(mid>-1&&mid<500){
                if(req->max_speed<500&&req->add_acc<500&&req->dec_acc<500){
                    rs485_driver_.bm_set_param(mid,77,req->max_speed);
                    rs485_driver_.bm_set_param(mid,78,req->add_acc);
                    rs485_driver_.bm_set_param(mid,79,req->dec_acc);
                    res->ret=0;
                    printf("svc:%d\n",res->ret);
                }     
            }
		});



    rclcpp::spin(node);
    printf("now close rs485 port\n");
    rs485_driver_.closePort();
    rclcpp::shutdown();
}
