#include "senddata.h"

#define angleLimit 20
#define disLimit 200

namespace uwb_slam
{
    void Senddata::Run(std::shared_ptr<uwb_slam::Uwb>uwb){

        ros::Rate loop_rate(10);
        // position_pub_=nh_.advertise<nav_msgs::Odometry>("uwb_odom",50);
        cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel",50);
    	obstacles_sub_ = nh_.subscribe<pibot_msgs::dis_info_array>("ceju_info",1,boost::bind(&Senddata::stereoCB,this,_1));
        // odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this);
        while(ros::ok()){
            // publishOdometry(uwb);
            if (flag_)
            {
                cmd_vel_.linear.x = 0; 
	            cmd_vel_.angular.z = -1;
                for (int i = 0; i < 17; ++i) {  
                    cmd_vel_pub_.publish(cmd_vel_);
                    loop_rate.sleep();
                }

                cmd_vel_.linear.x = 0.2; 
	            cmd_vel_.angular.z = 0;
                for (int i = 0; i < 20; ++i) {  
                    cmd_vel_pub_.publish(cmd_vel_);
                    loop_rate.sleep();
                }

                cmd_vel_.linear.x = 0; 
	            cmd_vel_.angular.z = 1;
                for (int i = 0; i < 16; ++i) {  
                    cmd_vel_pub_.publish(cmd_vel_);
                    loop_rate.sleep();
                }

                cmd_vel_.linear.x = 0; 
	            cmd_vel_.angular.z = 0;
                flag_ = 0;
            }
            else if(abs(uwb->aoa[0]) > 180 || abs(uwb->d[0]) > 2000)
            {
                cmd_vel_.angular.z = 0;
                cmd_vel_.linear.x = 0;
            }
            else if(uwb->aoa[0] > angleLimit && uwb->aoa[0] <= 180)
            {
                float angularSpeed = (float)uwb->aoa[0] / 100 + 1;
                cmd_vel_.angular.z = angularSpeed;
                cmd_vel_.linear.x = 0;
            }
            else if(uwb->aoa[0] < -angleLimit)
            {
                float angularSpeed = (float)uwb->aoa[0] / 100 - 1;
                cmd_vel_.angular.z = angularSpeed;
                cmd_vel_.linear.x = 0;
            }
            else if(uwb->d[0] > disLimit)
            {
                float lineSpeed = (float)uwb->d[0] / 1000 + 0.1;
                cmd_vel_.angular.z = 0;
                cmd_vel_.linear.x = lineSpeed;
            }
            else if(uwb->d[0] < disLimit - 30)
            {
                cmd_vel_.angular.z = 0;
                cmd_vel_.linear.x = -0.2;
            }
            else 
            {
                cmd_vel_.angular.z = 0;
                cmd_vel_.linear.x = 0; 
            }
            cmd_vel_pub_.publish(cmd_vel_);
            ros::spinOnce();
            loop_rate.sleep();
        }


    }
    // void Senddata::odomCB(const nav_msgs::Odometry& odom){
    //     sub_odom_ = odom;
    //     return;
    // }
    void Senddata::stereoCB(const pibot_msgs::dis_info_array::ConstPtr& stereo)
    {
        for (const auto& obstacle_info :stereo->dis)
        {
            float distance = obstacle_info.distance;
            // float width = obstacle_info.width;
            // float height = obstacle_info.height;
            // float angle = obstacle_info.angle;

            // if(distance>5&&distance<45&&cmd_vel_.linear.x!=0)
            if(distance>5&&distance<45)
            {
                flag_ = 1;
            }
        }
	
	    return;
    }

    // void Senddata::publishOdometry(std::shared_ptr<uwb_slam::Uwb>uwb)
    // {

    //     std::mutex mMutexSend;
    //     ros::Time current_time = ros::Time::now();

    //     // 设置 Odometry 消息的头部信息
    //     odom_.header.stamp = current_time;
    //     odom_.header.frame_id = "odom";  // 设置坐标系为 "map"
    //     odom_.child_frame_id = "base_link";  // 设置坐标系为 "base_link"
        
    //     // 填充 Odometry 消息的位置信息
    //     odom_.pose.pose.position.x = uwb->uwb_data_.x_;
    //     odom_.pose.pose.position.y = uwb->uwb_data_.y_;
    //     odom_.pose.pose.position.z = 0.0;
        

    //     // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态）
    //     // tf2::Quaternion quat;
    //     // quat.setRPY(0, 0, uwb->theta);  // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0
    //     // odom.pose.pose.orientation.x = quat.x();
    //     // odom.pose.pose.orientation.y = quat.y();
    //     // odom.pose.pose.orientation.z = quat.z();
    //     // odom.pose.pose.orientation.w = quat.w();

    //     odom_.pose.pose.orientation.x = sub_odom_.pose.pose.orientation.x;
    //     odom_.pose.pose.orientation.y = sub_odom_.pose.pose.orientation.y;
    //     odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z;
    //     odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w;

    //     // 发布 Odometry 消息
    //     position_pub_.publish(odom_);


    // }


    


} // namespace uwb_slam