#include"rclcpp/rclcpp.hpp"
#include"motor_dev/rs485_driver.hpp"
#include"interfaces/msg/motor_cmd.hpp"
#include"interfaces/msg/motor_pos.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/ttyS1", 115200)){
        std::cout << "Failed to open RS485 port." << std::endl;
    }
    
    rs485_driver_.bm_set_mode(1,4);
    rs485_driver_.bm_set_enable(1,2);
    rs485_driver_.bm_set_mode(2,4);
    rs485_driver_.bm_set_enable(2,2);
    usleep(500000);
    rs485_driver_.bm_set_param(1,77,10);
    rs485_driver_.bm_set_param(1,78,10);
    rs485_driver_.bm_set_param(1,79,10);
    //rs485_driver_.bm_set_param(1,28,4);
    rs485_driver_.bm_set_param(2,77,10);
    rs485_driver_.bm_set_param(2,78,10);
    rs485_driver_.bm_set_param(2,79,10);
    //rs485_driver_.mt_set_pos(1,-5);
    //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];
                    printf("##############:%s,%.1f,%.1f\n",msg->type.c_str(),angle1,angle2);
                    rs485_driver_.bm_set_pos(angle1,angle2);
                }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);
                }
            }else if(msg->target=="ethercat"){
                //TODO
            }
        });
    auto motor_pub=node->create_publisher<interfaces::msg::MotorPos>("/motor_pos",10);
    //读取电机位置
    rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(2000), [=]() { 
        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);
        ///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();
        ////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_angle.push_back(angle1);
            motor_pos.motor_angle.push_back(angle2);
            motor_pub->publish(motor_pos);
            
            //std::cout<<"pub msg["<<pub_cnt<<"]:"<<angle1<<","<<angle2<<std::endl;
            printf("[%03d]=====%.1f,%.1f\n",pub_cnt,angle1,angle2);
        }
        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
    });
    
    ////rs485_driver_.closePort();
    rclcpp::spin(node);
    rclcpp::shutdown();
}
