#include "rclcpp/rclcpp.hpp"
#include "interface/msg/radar.hpp"
#include "interface/msg/lidar.hpp"
#include "interface/msg/pl.hpp"
#include "interface/msg/fu.hpp"

#include <math.h>
#include "fu.hpp"
#include <iostream>
#include <time.h>
using namespace std;

int tmp_mode = 4;

const int STEERING_RATIO = 14;
int mm;
int pass = 0;
time_t start_time, end_time;

class fu_node : public rclcpp::Node
{
public:
    fu_node(std::string name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
        // 创建一个订阅者订阅话题
        msg_subscribe_radar = this->create_subscription<interface::msg::RADAR>("radar_message",
                                                                               10,
                                                                               std::bind(&fu_node::msg_callback_radar,
                                                                                         this,
                                                                                         std::placeholders::_1));
        msg_subscribe_lidar = this->create_subscription<interface::msg::Lidar>("lidar_message",
                                                                               10,
                                                                               std::bind(&fu_node::msg_callback_lidar,
                                                                                         this,
                                                                                         std::placeholders::_1));
        msg_subscribe_pl = this->create_subscription<interface::msg::PL>("pl_message",
                                                                         10,
                                                                         std::bind(&fu_node::msg_callback_pl,
                                                                                   this,
                                                                                   std::placeholders::_1));
        // 创建发布者
        msg_publisher_ = this->create_publisher<interface::msg::FU>("fu_message", 10);
        // 创建定时器，500ms为周期，定时发布
        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&fu_node::timer_callback, this));
    }

private:
    void msg_callback_radar(const interface::msg::RADAR::SharedPtr msg)
    {
        // std::cout << "msg_callback_radar" << std::endl;

        // 接收16线激光雷达检测障碍物信息 - 急停
        // mm = msg->obstacle_mm;
        mm = 0;

        /*
        for (int i = 0; i < 80; i++)
        {
            obstacle_r[i] = msg->obstacle_r[i];
        }
        for (int i = 0; i < 80; i++)
        {
            obstacle_l[i] = msg->obstacle_l[i];
        }
        for (int i = 0; i < 70; i++)
        {
            obstacle_f1[i] = msg->obstacle_f1[i];
        }
        for (int i = 0; i < 70; i++)
        {
            obstacle_f2[i] = msg->obstacle_f2[i];
        }
        cout<<mm<<endl;
        for(int i = 0;i<70;i++)
        {
            cout<<"\t"<<obstacle_f2[i]<<"  ";
        }
        cout<<endl;
        for(int i = 0;i<70;i++)
        {
            cout<<"\t"<<obstacle_f1[i]<<"  ";
        }
        cout<<endl;

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

    void msg_callback_lidar(const interface::msg::Lidar::SharedPtr msg)
    {
        // 接收128线激光雷达检测障碍物信息 - 减速
        obstacle_m = msg->obstacle_m;
        // std::cout << obstacle_m << "分米以内检测到障碍物=========" << std::endl;
    }

    void msg_callback_pl(const interface::msg::PL::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "x:'%lf',y:'%lf'", msg->x, msg->y);
        x = msg->x;
        y = msg->y;
        door = msg->door;
        speed = (int)msg->speed;
        drive_mode = msg->drive_mode;
    }

    void timer_callback()
    {
        double seconds = 0;
        // 创建消息
        interface::msg::FU message;

        message.action = 0;
        message.angle = 0;
        message.speed = 0;
        message.drive_mode = drive_mode;
        message.door = door;

        // if (pass == 0)
        // {
        //     tmp_mode = drive_mode;
        // }
        // std::cout << "FU drive_mode: " << message.drive_mode << std::endl;

        message.angle = calculate_the_angle(x, y);
        message.speed = calculate_the_speed(obstacle_m, speed); // 根据正前方障碍物距离和PL速度计算目标车速

        // 遇障处理
        /*
        if (tmp_mode == 2) // 进出站台（不绕行）
        {
            if (speed != 0 && obstacle_m > 70)
                message.speed = 80;
            cout << "drive mode " << drive_mode << endl;
            pass = 0; // 不绕行
        }
        if (tmp_mode == 1) // 路口转弯，仅用车身前半段侧方雷达检测斜后方来车（采点时转弯应让车头先直线通过停止线，待车头超过花圃再转弯）
        {
            message.speed = 80;
            cout << "drive mode " << drive_mode << endl;
            pass = 0; // 不绕行
            if (message.angle > 90)
            {
                int rr;
                for (rr = 40; rr > 0 && obstacle_r[rr] == 0; rr--)
                    ;
                if (rr != 0)
                    message.speed = 0;
            }
            if (message.angle < -90)
            {
                int ll;
                for (ll = 40; ll > 0 && obstacle_l[ll] == 0; ll--)
                    ;
                if (ll != 0)
                    message.speed = 0;
            }
        }
        if (tmp_mode == 10) // 直行道路（跟车减速+绕障）
        {
            if (pass == 0) // 未进入绕障状态时
            {
                if (obstacle_m > 70) // 限速
                {
                    // message.speed = obstacle_m > 100 ? 100 : obstacle_m;
                    start_time = time(NULL);
                }
                else // 连续<7m 10s内 停车等待； 倒计时结束后令pass = 1  进行绕障
                {
                    end_time = time(NULL);
                    seconds = difftime(end_time, start_time);
                    cout << "wait" << (int)seconds << endl;
                    if (seconds > 10)
                    {
                        pass = 1;
                    }
                    else
                        message.speed = 0;
                }
            }
            else if (pass == 1) // 已进入绕障状态
            {
                cout << "rao zhang zhong ......." << endl;
                for (int kk = 0; kk < 70; kk++)
                {
                    obstacle_f2[kk] = obstacle_f2[kk] || obstacle_f1[kk];
                }
                if (mm == 0) // 必停区无障碍
                {
                    message.speed = 30; // 绕障限速
                    int ff;
                    for (ff = 17; ff < 39 && obstacle_f2[ff] == 0; ff++)
                        ;
                    if (ff != 39) // 正前方有障碍
                    {
                        int ll, fl;
                        int tmp_space = 0, max_space = 0, left_i = 0;
                        for (fl = 1; fl < 38; fl++)
                        {
                            if (obstacle_f2[fl] == 0 && obstacle_f2[fl - 1] == 0)
                            {
                                tmp_space++;
                            }
                            else
                                tmp_space = 0;
                            if (tmp_space > max_space)
                            {
                                max_space = tmp_space;
                                left_i = fl;
                            }
                        }
                        for (ll = 79; ll > 0 && obstacle_l[ll] == 0; ll--)
                            ;
                        if (ll == 0 && max_space > 25) // 左侧无障碍 && 左前空间足够 ，左转
                        {
                            message.angle = -520;
                        }
                        else // 左侧有障碍 || 左前空间不够，停等
                            message.speed = 0;
                    }
                    else // 正前方无障碍
                    {

                        int ff2;
                        for (ff2 = 38; ff2 < 62 && obstacle_f2[ff2] == 0; ff2++)
                            ;
                        int rr;
                        for (rr = 48; rr > 0 && obstacle_r[rr] == 0; rr--)
                            ;
                        if (rr != 0) // 根据右侧障碍物相对位置计算回正角度
                            message.angle = -88;
                        else
                        {
                            if (ff2 == 62)
                            {
                                pass = 0; // 右侧无障碍，退出绕障
                                cout << "out!!" << endl;
                            }
                            else
                            {
                                message.angle = -88;
                            }
                        }
                    }
                }
            }
        }
        */

        if (mm == 1)
            message.ems = 1; // 急停
        else
            message.ems = 0; // 急停复位，恢复到正常自动驾驶

        // 更新刹车状态
        if (safe_st < 50)
        {
            message.action = 50; // 表示带刹车
            ++safe_st;           // 增加刹车状态计数
        }
        else
        {
            message.action = 0; // 表示无刹车
        }

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

    // 根据目标点位置计算方向盘角度
    float calculate_the_angle(double x, double y)
    {
        // 处理x为0的情况
        if (x == 0)
        {
            return 90.0f;
        }

        // 计算目标点的角度
        float angle_f = atan2(y, x) * 180 / M_PI;

        // 计算方向盘角度
        float steering_angle;
        if (angle_f <= 90.0f)
        {
            steering_angle = (90.0f - angle_f) * STEERING_RATIO;
        }
        else
        {
            steering_angle = -((angle_f - 90.0f) * STEERING_RATIO);
        }

        // 限定在 -520~520
        return std::clamp(steering_angle, -520.0f, 520.0f);
    }

    // 根据正前方障碍物距离和PL速度计算目标车速
    int calculate_the_speed(int obstacle, int pl_speed)
    {
        int speed_t = 70; // 默认速度 -> 7 km/h

        // 如果PL速度为0，车速设置为0
        if (pl_speed == 0)
        {
            speed_t = 0;
        }
        else if (obstacle < 30) // 3米内检测到障碍物
        {
            speed_t = 0; // 完全刹停
        }
        else if (obstacle < 100) // 10米内检测到障碍物
        {
            // 距离从100降到30，速度从70降到0，所以每米减速 (70-0)/(100-30) = 70/70 = 7/7 = 1
            double diff = (double)(100 - obstacle) * 1;
            speed_t = 70 - (int)diff;
        }

        else if (drive_mode != 0) // 限速
            speed_t = 50;

        // std::cout << obstacle << "分米以内检测到障碍物" << std::endl;
        // std::cout << "限速 : " << speed_t << std::endl;
        // 限定在 0~50
        return std::clamp(speed_t, 0, 70);
    }

    double x;
    double y;
    int door = 0;
    int speed = 0;
    int reliability;
    /*0：直行，1：转弯 ，2：站内*/
    int drive_mode = 0;

    int obstacle_m = 200;

    int obstacle_r[80] = {0};
    int obstacle_l[80] = {0};
    int obstacle_f1[70] = {0};
    int obstacle_f2[70] = {0};

    int safe_st;
    // 声名定时器指针
    rclcpp::TimerBase::SharedPtr timer_;
    // 声明话题发布者指针
    rclcpp::Publisher<interface::msg::FU>::SharedPtr msg_publisher_;
    // 声明订阅者
    rclcpp::Subscription<interface::msg::RADAR>::SharedPtr msg_subscribe_radar;
    rclcpp::Subscription<interface::msg::Lidar>::SharedPtr msg_subscribe_lidar;
    rclcpp::Subscription<interface::msg::PL>::SharedPtr msg_subscribe_pl;
};
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<fu_node>("fu_node");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
// 暂时无用代码
// message.speed = (short)(55 + obstacle_length * 5); // 5.5 ~ 15 km/h
// message.speed = (short)(10 + obstacle_length * 2); // 1 ~ 4.8 km/h
// message.speed = (short)(53 + obstacle_length * 13); // 5.3 ~ 30 km/h

// if (obstacle_r == 1)
// {
//     safe_st = 0;
//     message.angle = -300;
//     message.speed = (short)300;
// }
// if (obstacle_l == 1)
// {
//     safe_st = 0;
//     message.angle = 300;
//     message.speed = (short)300;
// }

// else if (obstacle <= 150) // 15米内检测到障碍物
// {
//     // 距离从150降到70，速度从50降到0，所以每米减速 (50-0)/(150-70) = 50/80 = 5/8 = 0.625
//     double diff = (double)(150 - obstacle) * 0.625;
//     speed_t = 50 - (int)diff;
// }