#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "interface/msg/radar.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"

#include <math.h>

using namespace std;

int obs_r[80] = {0};
int obs_l[80] = {0};
int obs_f1[70] = {0};
int obs_f2[70] = {0};

int ob_r[80] = {0};
int ob_l[80] = {0};
int ob_f1[70] = {0};
int ob_f2[70] = {0};

int mid_distance;
int mm;
int min_front;
int obs_mid;

class PointCloudXYZExtractor : public rclcpp::Node
{
public:
    PointCloudXYZExtractor() : Node("extractor_node")
    {
        RCLCPP_INFO(this->get_logger(), "extractor_node节点已经启动.");
        // 创建发布者
        command_publisher_ = this->create_publisher<interface::msg::RADAR>("radar_message", 10);
        // 创建定时器_，100ms为周期，定时发布
        timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&PointCloudXYZExtractor::timer_callback, this));

        // subscription_right = this->create_subscription<sensor_msgs::msg::PointCloud2>(
        //     "lidar_right",
        //     10,
        //     std::bind(&PointCloudXYZExtractor::topic_callback_right,
        //               this,
        //               std::placeholders::_1));

        // subscription_left = this->create_subscription<sensor_msgs::msg::PointCloud2>(
        //     "lidar_left",
        //     10,
        //     std::bind(&PointCloudXYZExtractor::topic_callback_left,
        //               this,
        //               std::placeholders::_1));

        subscription_front1 = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "lidar_front1",
            10,
            std::bind(&PointCloudXYZExtractor::topic_callback_front1,
                      this,
                      std::placeholders::_1));

        // subscription_front2 = this->create_subscription<sensor_msgs::msg::PointCloud2>(
        //     "lidar_front2",
        //     10,
        //     std::bind(&PointCloudXYZExtractor::topic_callback_front2,
        //               this,
        //               std::placeholders::_1));
    }

private:
    void timer_callback()
    {
        cout << "mm  " << mm << endl;
        // cout << "mid_distance  " << (int)(((float)mid_distance) * 0.927) << endl;
        /*
        cout << "   ";
        for (int i = 0; i < 70; i++)
        {
            if (i >= 10)
                cout << i;
            else
            {
                cout << " " << i;
            }
        }
        cout << endl;
        cout << "   ";
        for (int i = 0; i < 70; i++)
        {
            cout << " " << ob_f2[i];
        }
        cout << endl;
        cout << "   ";
        for (int i = 0; i < 70; i++)
        {
            cout << " " << ob_f1[i];
        }
        cout << endl;

        for (int i = 0; i < 80; i++)
        {
            if (i % 4 == 0)
                cout << ob_l[i] << "\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t  " << ob_r[i] << endl;
        }
        cout << endl;
        */

        // 创建消息
        auto message = interface::msg::RADAR();
        /*
        message.obstacle_r.resize(80);
        message.obstacle_r.assign(ob_r, ob_r + 80);

        message.obstacle_l.resize(80);
        message.obstacle_l.assign(ob_l, ob_l + 80);

        message.obstacle_f1.resize(70);
        message.obstacle_f1.assign(ob_f1, ob_f1 + 70);

        message.obstacle_f2.resize(70);
        message.obstacle_f2.assign(ob_f2, ob_f2 + 70);
        */

        // 激光雷达
        // message.obstacle_m = mid_distance * cos(22 / 180 * M_1_PI);
        // message.obstacle_m = (int)(((float)mid_distance) * 0.927);
        message.obstacle_mm = mm;

        // 日志打印
        // RCLCPP_INFO(this->get_logger(), "lat:'%lf',lon:'%lf',head:'%lf',speed:'%lf'",
        //             message.lat, message.lon, message.head, message.speed);

        // 发布消息
        command_publisher_->publish(message);
    }

    /*
    void topic_callback_right(const sensor_msgs::msg::PointCloud2::SharedPtr msg) const
    {
        // cout<<(msg->header.frame_id == "laser_link0")<<endl;
        memset(obs_r, 0, sizeof(obs_r));
        // int count_ob = 0;
        sensor_msgs::PointCloud2Iterator<float> iter_x_r(*msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y_r(*msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z_r(*msg, "z");
        if (msg->header.frame_id == "laser_link0")
        {
            for (size_t i = 0; i < msg->width * msg->height; ++i, ++iter_x_r, ++iter_y_r, ++iter_z_r)
            {
                float x = *iter_x_r * 10;
                float y = *iter_y_r * 10;
                float z = *iter_z_r * 10;
                // std::cout<<"x : "<<x<<std::endl;
                if (x != NAN && y != NAN && z != NAN && z > 0.3 && z < 15 && x < 0 && x > -25)
                {
                    for (int k = -30; k < 50; k++)
                    {
                        if (y > k && y <= k + 1)
                        {
                            obs_r[k + 30] = 1;
                        }
                    }
                }
            }

            memcpy(ob_r, obs_r, sizeof(obs_r));
            // for(int i = 0;i<8;i++)
            // {
            //     cout<<obs_r[i]<<"\t";
            // }
            // cout<<endl;
        }
    }
    void topic_callback_left(const sensor_msgs::msg::PointCloud2::SharedPtr msg) const
    {
        memset(obs_l, 0, sizeof(obs_l));
        // int count_ob = 0;
        sensor_msgs::PointCloud2Iterator<float> iter_x_l(*msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y_l(*msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z_l(*msg, "z");
        if (msg->header.frame_id == "laser_link3")
        {
            for (size_t i = 0; i < msg->width * msg->height; ++i, ++iter_x_l, ++iter_y_l, ++iter_z_l)
            {
                float x = *iter_x_l * 10;
                float y = *iter_y_l * 10;
                float z = *iter_z_l * 10;
                // std::cout<<"x : "<<x<<std::endl;
                if (x != NAN && y != NAN && z != NAN && z > 0.3 && z < 15 && x < 0 && x > -25)
                {
                    for (int k = -50; k < 30; k++)
                    {
                        if (y > k && y <= k + 1)
                        {
                            obs_l[k + 50] = 1;
                        }
                    }
                }
            }
            for (int k = 0; k < 40; k++)
            {
                int temp = obs_l[k];
                obs_l[k] = obs_l[79 - k];
                obs_l[79 - k] = temp;
            }
            memcpy(ob_l, obs_l, sizeof(obs_l));
        }

        // for(int i = 0;i<8;i++)
        // {
        //     cout<<obs_l[i]<<"\t";
        // }
        // cout<<endl;
    }
    */

    /*
    void topic_callback_front1(const sensor_msgs::msg::PointCloud2::SharedPtr msg) const
    {
        // memset(obs_f1, 0, sizeof(obs_f1));
        // int count_ob = 0;
        sensor_msgs::PointCloud2Iterator<float> iter_x_f1(*msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y_f1(*msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z_f1(*msg, "z");
        if (msg->header.frame_id == "laser_link1")
        {
            mm = 0;
            for (size_t i = 0; i < msg->width * msg->height; ++i, ++iter_x_f1, ++iter_y_f1, ++iter_z_f1)
            {
                float x = *iter_x_f1 * 10;
                float y = *iter_y_f1 * 10;
                float z = *iter_z_f1 * 10;

                if (x != NAN && y != NAN && z != NAN && x < 11 && x > -11 && y > -10 && y < 0)
                {
                    mm = 1;
                }
                // if (x != NAN && y != NAN && z != NAN && z > 0 && z < 17 && x < -4 && x > -29)
                // {
                //     for (int k = -30; k < 40; k++)
                //     {
                //         if (y > k && y <= k + 1)
                //         {
                //             obs_f1[k + 30] = 1;
                //         }
                //     }
                // }
            }
            // memcpy(ob_f1, obs_f1, sizeof(obs_f1));
        }

        // for(int i = 0;i<8;i++)
        // {
        //     cout<<obs_f1[i]<<"\t";
        // }
        // cout<<endl;
    }
    */

    void topic_callback_front1(const sensor_msgs::msg::PointCloud2::SharedPtr msg) const
    {
        static int mm_no_obstacle_count = 0;
        static const int MM_CLEAR_THRESHOLD = 3;

        bool has_obstacle = false; // 当前帧是否检测到障碍物
        sensor_msgs::PointCloud2Iterator<float> iter_x_f1(*msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y_f1(*msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z_f1(*msg, "z");

        if (msg->header.frame_id == "laser_link1")
        {
            for (size_t i = 0; i < msg->width * msg->height; ++i, ++iter_x_f1, ++iter_y_f1, ++iter_z_f1)
            {
                float x = *iter_x_f1 * 10;
                float y = *iter_y_f1 * 10;
                float z = *iter_z_f1 * 10;
                if (x != NAN && y != NAN && z != NAN && x < 11 && x > -11 && y > -10 && y < 0)
                {
                    has_obstacle = true;
                    break;
                }
            }
        }

        if (has_obstacle)
        {
            mm = 1;
            mm_no_obstacle_count = 0;
        }
        else
        {
            mm_no_obstacle_count++;
            if (mm_no_obstacle_count >= MM_CLEAR_THRESHOLD)
            {
                mm = 0; // 连续3帧没有障碍物才设为0
            }
        }
    }

    /*
    void topic_callback_front2(const sensor_msgs::msg::PointCloud2::SharedPtr msg) const
    {
        min_front = 101;
        memset(obs_f2, 0, sizeof(obs_f2));
        // int count_ob = 0;
        sensor_msgs::PointCloud2Iterator<float> iter_x_f2(*msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y_f2(*msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z_f2(*msg, "z");

        if (msg->header.frame_id == "laser_link2")
        {
            for (size_t i = 0; i < msg->width * msg->height; ++i, ++iter_x_f2, ++iter_y_f2, ++iter_z_f2)
            {
                float x = *iter_x_f2 * 10;
                float y = *iter_y_f2 * 10;
                float z = *iter_z_f2 * 10;

                if (x != NAN && y != NAN && z != NAN && z > 8 && x < -30 && x > -180 && y < 10 && y > -12)
                {
                    obs_mid = -((int)x); //??????????????
                    if (obs_mid < min_front)
                    {
                        min_front = obs_mid;
                    }
                }

                if (x != NAN && y != NAN && z != NAN && z > 1 && z < 20 && x < -18 && x > -63)
                {
                    for (int k = -30; k < 40; k++)
                    {
                        if (y > k && y <= k + 1)
                        {
                            obs_f2[k + 30] = 1;
                        }
                    }
                }
            }
            memcpy(ob_f2, obs_f2, sizeof(obs_f2));
        }
        if (min_front != 101)
        {
            mid_distance = min_front;
        }

        // std::cout<<"mid_distance : "<<mid_distance<<std::endl;
        // for(int i = 0;i<8;i++)
        // {
        //     cout<<obs_f2[i]<<"\t";
        // }
        // cout<<endl;
    }
    */

    // rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_right;
    // rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_left;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_front1;
    // rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_front2;

    // 声名定时器指针
    rclcpp::TimerBase::SharedPtr timer_;
    // 声明话题发布者指针
    rclcpp::Publisher<interface::msg::RADAR>::SharedPtr command_publisher_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PointCloudXYZExtractor>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}