#include <math.h>
#include <time.h>

#include <algorithm>
#include <iostream>
#include <memory>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <sstream>
#include <std_msgs/msg/int32_multi_array.hpp>
#include <string>
#include <vector>

#include "logger/logger.h"
#include "rclcpp/rclcpp.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/fu.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"
#include "sweeper_interfaces/msg/pl.hpp"
#include "sweeper_interfaces/msg/rtk.hpp"
#include "sweeper_interfaces/msg/sub.hpp"

using namespace std;
using namespace std::chrono;

// ================== RTK全局变量 ==================
system_clock::time_point last_rtk_time;
const duration<double> rtk_timeout = duration<double>(0.5);

struct struct_rtk
{
    double lon;
    double lat;
    double direction;
    double speed;
    int reliability;
};
struct_rtk g_rtk;

// ================== GPS转相对坐标的常数 ==================
#define Lat_Origin 32.045062
#define Lon_Origin 118.845200366
#define deg_rad (0.01745329252)
#define R_LATI (6378137)
#define R_LONT (5407872)

// ================== 枚举定义 ==================
const int EMPTY = 0;
const int OBSTACLE = 1;
const int VEHICLE = 2;
int is_start = 0;

// ================== 路点结构体 ==================
struct Waypoint
{
    double x, y;
    int speed;
    double lon, lat;  // 路点的绝对坐标（GPS）
    Waypoint() : x(0), y(0), speed(300), lon(0), lat(0) {}
    Waypoint(double px, double py, int s = 300, double plon = 0, double plat = 0)
        : x(px), y(py), speed(s), lon(plon), lat(plat)
    {
    }
};

// ================== 障碍物分析结构体 ==================
struct ObstacleAnalysis
{
    bool has_front_critical = false;
    bool has_front_area = false;
    bool has_left_critical = false;
    bool has_right_critical = false;

    int obstacle_distance = -1;
    int obstacle_width = 0;
    int left_edge = -1, right_edge = -1;
    int free_space_left = 0;
    int free_space_right = 0;

    int calculateSafeDistance(int speed) const { return std::max(3, speed / 400 + 2); }
    bool needEmergencyBrake(int speed) const
    {
        if (!has_front_critical) return false;
        return obstacle_distance <= calculateSafeDistance(speed);
    }
};

// ================== GPS坐标转换函数 ==================
// 计算两点间距离(米)
double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2)
{
    double x, y, length;
    x = (R_LONT) * (lonti2 - lonti1) * deg_rad;
    y = (R_LATI) * (lati2 - lati1) * deg_rad;
    length = sqrt(x * x + y * y);
    return length;
}

// GPS转相对坐标(米)
// Cur_navAngle: 当前航向角(度)
// Cur_lonti, Cur_lati: 当前位置GPS坐标
// Dest_lonti, Dest_lati: 目标位置GPS坐标
// x_diff, y_diff: 输出的相对坐标(米)
int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, double Dest_lonti, double Dest_lati,
                   double* x_diff, double* y_diff)
{
    double Navi_rad, x, y;
    float k1, k2, k3, k4;
    Navi_rad = Cur_navAngle * deg_rad;

    x = (Dest_lonti - Cur_lonti) * deg_rad * R_LONT;
    y = (Dest_lati - Cur_lati) * deg_rad * R_LATI;

    k1 = cos(Navi_rad);
    k2 = (-1) * sin(Navi_rad);
    k3 = (-1) * k2;
    k4 = k1;

    *x_diff = x * k1 + y * k2;
    *y_diff = x * k3 + y * k4;

    return 0;
}

// ================== 状态机核心类 ==================
class StateMachine
{
   public:
    enum State
    {
        NORMAL,
        WAITING,
        LATERAL_SHIFT,
        PARALLEL_MOVE
    };

    StateMachine() { current_state_ = NORMAL; }

    void setState(State new_state)
    {
        if (new_state != current_state_)
        {
            exit(current_state_);
            current_state_ = new_state;
            enter(new_state);
        }
    }

    void execute() {}

    State getCurrentState() const { return current_state_; }

    std::string getStateString() const
    {
        switch (current_state_)
        {
            case NORMAL:
                return "正常行驶";
            case WAITING:
                return "等待决策";
            case LATERAL_SHIFT:
                return "横向偏移";
            case PARALLEL_MOVE:
                return "平行前进";
            default:
                return "未知状态";
        }
    }

    ObstacleAnalysis obstacle_analysis;
    int publish_speed = 0;
    float publish_angle = 0.0f;
    int counter = 0;
    int direction = 0;
    std::vector<float> angle_history;
    rclcpp::Time start_time;
    bool start_time_set = false;

   private:
    State current_state_;

    void enter(State state)
    {
        switch (state)
        {
            case NORMAL:
                counter = 0;
                direction = 0;
                break;
            case WAITING:
                publish_speed = 0;
                publish_angle = 0.0f;
                counter = 0;
                break;
            case LATERAL_SHIFT:
                publish_speed = 300;
                counter = 0;
                break;
            case PARALLEL_MOVE:
                publish_speed = 300;
                counter = 0;
                break;
            default:
                break;
        }
    }

    void exit(State state) {}
};

// ================== FU节点类 ==================
class fu_node : public rclcpp::Node
{
   public:
    fu_node(std::string name) : Node(name), state_machine_(std::make_unique<StateMachine>())
    {
        LOG_INFO("%s节点已经启动.", name.c_str());
        // 初始化最后接收时间为启动时间（避免初始状态直接超时）
        last_rtk_time = system_clock::now();

        // ======== 参数声明 ========
        this->declare_parameter("enable_obstacle_stop", true);
        this->declare_parameter("enable_obstacle_avoid", true);
        this->declare_parameter("enable_visualize_grid", true);
        this->declare_parameter("FRONT_STOP_ZONE_ROWS", 3);
        this->declare_parameter("REAR_STOP_ZONE_ROWS", 1);
        this->declare_parameter("LEFT_STOP_ZONE_COLS", 3);
        this->declare_parameter("RIGHT_STOP_ZONE_COLS", 3);
        this->declare_parameter("front_area_extend", 7);
        this->declare_parameter("left_area_extend", 2);
        this->declare_parameter("right_area_extend", 2);
        this->declare_parameter("waypoint_tolerance", 2.0);
        this->declare_parameter("lateral_offset", 2);
        this->declare_parameter("parallel_distance", 3);
        this->declare_parameter("avoid_able_width", 6);
        this->declare_parameter("min_free_space", 6);

        // ======== 参数读取 ========
        this->get_parameter("enable_obstacle_stop", enable_obstacle_stop_);
        this->get_parameter("enable_obstacle_avoid", enable_obstacle_avoid_);
        this->get_parameter("enable_visualize_grid", enable_visualize_grid_);
        this->get_parameter("FRONT_STOP_ZONE_ROWS", FRONT_STOP_ZONE_ROWS_);
        this->get_parameter("REAR_STOP_ZONE_ROWS", REAR_STOP_ZONE_ROWS_);
        this->get_parameter("LEFT_STOP_ZONE_COLS", LEFT_STOP_ZONE_COLS_);
        this->get_parameter("RIGHT_STOP_ZONE_COLS", RIGHT_STOP_ZONE_COLS_);
        this->get_parameter("front_area_extend", front_area_extend_);
        this->get_parameter("left_area_extend", left_area_extend_);
        this->get_parameter("right_area_extend", right_area_extend_);
        this->get_parameter("waypoint_tolerance", waypoint_tolerance_);
        this->get_parameter("lateral_offset", lateral_offset_);
        this->get_parameter("parallel_distance", parallel_distance_);
        this->get_parameter("avoid_able_width", avoid_able_width_);
        this->get_parameter("min_free_space", min_free_space_);

        parameter_callback_handle_ =
            this->add_on_set_parameters_callback(std::bind(&fu_node::parameter_callback, this, std::placeholders::_1));

        // ======== 订阅者初始化 ========
        subscription_grid = this->create_subscription<std_msgs::msg::Int32MultiArray>(
            "grid_raw", 10, std::bind(&fu_node::gridCallback, this, std::placeholders::_1));
        msg_subscribe_pl = this->create_subscription<sweeper_interfaces::msg::Pl>(
            "pl_message", 10, std::bind(&fu_node::msg_callback_pl, this, std::placeholders::_1));
        msg_subscribe_rtk = this->create_subscription<sweeper_interfaces::msg::Rtk>(
            "rtk_message", 10, std::bind(&fu_node::msg_callback_rtk, this, std::placeholders::_1));

        // ======== 发布者初始化 ========
        pub_mc = this->create_publisher<sweeper_interfaces::msg::McCtrl>("auto_mc_ctrl", 10);

        // ======== 定时器初始化 ========
        timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&fu_node::timer_callback, this));

        // ======== 初始化 ========
        avoiding_obstacle_ = false;
        current_waypoint_idx_ = 0;
        vehicle_min_x_ = -1;
        vehicle_max_x_ = -1;
        vehicle_min_y_ = -1;
        vehicle_max_y_ = -1;
        node_clock_ = this->get_clock();

        front_stop_zone_ = {0, 0, 0, 0};
        rear_stop_zone_ = {0, 0, 0, 0};
        left_stop_zone_ = {0, 0, 0, 0};
        right_stop_zone_ = {0, 0, 0, 0};

        LOG_INFO(
            "\n---------- FU节点参数配置 ----------\n  遇障停车: %s\n  绕障: %s\n  栅格可视化: %s\n"
            "  路点容差: %.2f米\n  横向偏移: %d栅格\n  平行距离: %d栅格\n"
            "  可绕宽度: %d栅格\n  最小空闲: %d栅格\n--------------------------------------------\n",
            (enable_obstacle_stop_ ? "开启" : "关闭"), (enable_obstacle_avoid_ ? "开启" : "关闭"),
            (enable_visualize_grid_ ? "是" : "否"), waypoint_tolerance_, lateral_offset_, parallel_distance_,
            avoid_able_width_, min_free_space_);
    }

   private:
    std::unique_ptr<StateMachine> state_machine_;
    rclcpp::Clock::SharedPtr node_clock_;

    // ======== 配置参数 ========
    bool enable_obstacle_stop_, enable_obstacle_avoid_, enable_visualize_grid_;
    int FRONT_STOP_ZONE_ROWS_, REAR_STOP_ZONE_ROWS_;
    int LEFT_STOP_ZONE_COLS_, RIGHT_STOP_ZONE_COLS_;
    int front_area_extend_, left_area_extend_, right_area_extend_;
    double waypoint_tolerance_;
    int lateral_offset_, parallel_distance_, avoid_able_width_, min_free_space_;

    rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;

    // ======== 规划层消息缓存 ========
    double pl_x_ = 0.0, pl_y_ = 0.0;
    int pl_speed_ = 0;

    // ======== 栅格检测结果 ========
    bool obstacle_in_front_ = false, obstacle_in_rear_ = false;
    bool obstacle_in_left_ = false, obstacle_in_right_ = false;
    bool obstacle_in_front_area_ = false;
    int obstacle_left_edge_ = -1, obstacle_right_edge_ = -1;
    int obstacle_max_x_ = 100;
    int free_space_left_ = 0, free_space_right_ = 0;

    // ======== 自车位置 ========
    int vehicle_min_x_, vehicle_max_x_, vehicle_min_y_, vehicle_max_y_;

    // ======== 安全区结构体 ========
    struct SafetyZone
    {
        int min_x, max_x, min_y, max_y;
    };
    SafetyZone front_stop_zone_, rear_stop_zone_, left_stop_zone_, right_stop_zone_;

    // ======== 避障相关 ========
    bool avoiding_obstacle_;
    std::vector<Waypoint> avoidance_waypoints_;
    int current_waypoint_idx_;
    std::vector<std::vector<int>> cached_grid_;
    bool grid_data_valid_ = false;
    rclcpp::Time last_grid_time_;

    // ======== 发布者和订阅者 ========
    rclcpp::Publisher<sweeper_interfaces::msg::McCtrl>::SharedPtr pub_mc;
    rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr subscription_grid;
    rclcpp::Subscription<sweeper_interfaces::msg::Pl>::SharedPtr msg_subscribe_pl;
    rclcpp::Subscription<sweeper_interfaces::msg::Rtk>::SharedPtr msg_subscribe_rtk;
    rclcpp::TimerBase::SharedPtr timer_;

    // ======== 参数回调 ========
    rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector<rclcpp::Parameter>& parameters)
    {
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true;
        for (const auto& param : parameters)
        {
            if (param.get_name() == "enable_obstacle_stop")
            {
                enable_obstacle_stop_ = param.as_bool();
                LOG_INFO("[参数更新] 遇障停车: %s", enable_obstacle_stop_ ? "开启" : "关闭");
            }
            else if (param.get_name() == "enable_obstacle_avoid")
            {
                enable_obstacle_avoid_ = param.as_bool();
                LOG_INFO("[参数更新] 自主绕障: %s", enable_obstacle_avoid_ ? "开启" : "关闭");
                if (!enable_obstacle_avoid_)
                {
                    state_machine_->setState(StateMachine::NORMAL);
                    avoiding_obstacle_ = false;
                    avoidance_waypoints_.clear();
                }
            }
            else if (param.get_name() == "waypoint_tolerance")
                waypoint_tolerance_ = param.as_double();
            else if (param.get_name() == "lateral_offset")
                lateral_offset_ = param.as_int();
            else if (param.get_name() == "parallel_distance")
                parallel_distance_ = param.as_int();
            else if (param.get_name() == "avoid_able_width")
                avoid_able_width_ = param.as_int();
            else if (param.get_name() == "min_free_space")
                min_free_space_ = param.as_int();
        }
        return result;
    }

    // ======== RTK回调 ========
    void msg_callback_rtk(const sweeper_interfaces::msg::Rtk::SharedPtr msg)
    {
        last_rtk_time = system_clock::now();

        g_rtk.reliability = (msg->p_quality == 4 && msg->h_quality == 4) ? 1 : 0;
        g_rtk.lat = msg->lat;
        g_rtk.lon = msg->lon;
        g_rtk.direction = msg->head;
    }

    // ======== 角度计算 ========
    float calculate_the_angle(double target_x, double target_y)
    {
        if (target_x == 0) return 90.0f;
        float angle_f = atan2(target_y, target_x) * 180 / M_PI;
        float steering_angle = (angle_f <= 90.0f) ? 90.0f - angle_f : -(angle_f - 90.0f);
        // 计算阶段就限制角度范围
        return std::clamp(steering_angle, -50.0f, 50.0f);
    }

    // ======== 自车位置检测 ========
    void findVehicleRectangle(const std::vector<std::vector<int>>& grid)
    {
        vehicle_min_x_ = vehicle_max_x_ = vehicle_min_y_ = vehicle_max_y_ = -1;
        for (size_t i = 0; i < grid.size(); ++i)
        {
            for (size_t j = 0; j < grid[i].size(); ++j)
            {
                if (grid[i][j] == VEHICLE)
                {
                    if (vehicle_min_x_ == -1 || (int)i < vehicle_min_x_) vehicle_min_x_ = i;
                    if (vehicle_max_x_ == -1 || (int)i > vehicle_max_x_) vehicle_max_x_ = i;
                    if (vehicle_min_y_ == -1 || (int)j < vehicle_min_y_) vehicle_min_y_ = j;
                    if (vehicle_max_y_ == -1 || (int)j > vehicle_max_y_) vehicle_max_y_ = j;
                }
            }
        }
        if (vehicle_min_x_ != -1 && vehicle_max_x_ != -1 && vehicle_min_y_ != -1 && vehicle_max_y_ != -1)
        {
            calculateSafetyZones(grid.size(), grid[0].size());
        }
    }

    // ======== 安全区计算 ========
    void calculateSafetyZones(int grid_height, int grid_width)
    {
        front_stop_zone_ = {max(0, vehicle_min_x_ - FRONT_STOP_ZONE_ROWS_), vehicle_min_x_ - 1, vehicle_min_y_,
                            vehicle_max_y_};
        rear_stop_zone_ = {vehicle_max_x_ + 1, min(grid_height - 1, vehicle_max_x_ + REAR_STOP_ZONE_ROWS_),
                           vehicle_min_y_, vehicle_max_y_};
        left_stop_zone_ = {vehicle_min_x_, vehicle_max_x_, max(0, vehicle_min_y_ - LEFT_STOP_ZONE_COLS_),
                           vehicle_min_y_ - 1};
        right_stop_zone_ = {vehicle_min_x_, vehicle_max_x_, vehicle_max_y_ + 1,
                            min(grid_width - 1, vehicle_max_y_ + RIGHT_STOP_ZONE_COLS_)};
    }

    // ======== 栅格回调 ========
    void gridCallback(const std_msgs::msg::Int32MultiArray::SharedPtr msg)
    {
        last_grid_time_ = node_clock_->now();

        if (msg->layout.dim.size() != 2)
        {
            LOG_ERROR("栅格地图消息维度无效！");
            return;
        }

        size_t height = msg->layout.dim[0].size;
        size_t width = msg->layout.dim[1].size;

        if (msg->data.size() != height * width)
        {
            LOG_ERROR("栅格地图数据长度不匹配！期望%zu，实际%zu", height * width, msg->data.size());
            return;
        }

        std::vector<std::vector<int>> grid(height, std::vector<int>(width));
        for (size_t i = 0; i < height; ++i)
            for (size_t j = 0; j < width; ++j) grid[i][j] = msg->data[i * width + j];

        findVehicleRectangle(grid);
        analyzeObstacles(grid);
        if (enable_visualize_grid_) visualizeGridInTerminal(grid);
        cacheGridData(grid);

        // 立即执行紧急停车检查（高优先级）
        updateObstacleAnalysis();
        if (enable_obstacle_stop_ && is_start)
        {
            if (state_machine_->obstacle_analysis.has_front_critical)
            {
                state_machine_->publish_speed = 0;
                state_machine_->publish_angle = 0.0f;
                state_machine_->setState(StateMachine::WAITING);
                publishControlCommand(true);  // 立即发布带刹车的停车指令
                LOG_WARN("[紧急停车] 栅格回调中检测到前方障碍物，立即停车！");
            }
        }
    }

    // ======== 栅格缓存 ========
    void cacheGridData(const std::vector<std::vector<int>>& grid)
    {
        cached_grid_ = grid;
        grid_data_valid_ = true;
    }

    // ======== 障碍物分析 ========
    void analyzeObstacles(const std::vector<std::vector<int>>& grid)
    {
        obstacle_in_front_ = obstacle_in_rear_ = obstacle_in_left_ = obstacle_in_right_ = false;
        obstacle_in_front_area_ = false;
        obstacle_left_edge_ = obstacle_right_edge_ = -1;
        free_space_left_ = free_space_right_ = 0;
        obstacle_max_x_ = 100;
        if (vehicle_min_x_ == -1) return;

        // 检测前方必停区障碍物
        bool found_front_obstacle = false;
        for (int i = front_stop_zone_.max_x; i >= front_stop_zone_.min_x && !found_front_obstacle; --i)
            for (int j = front_stop_zone_.min_y; j <= front_stop_zone_.max_y; ++j)
                if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE)
                {
                    obstacle_in_front_ = true;
                    found_front_obstacle = true;
                    break;
                }

        // 检测后方必停区障碍物
        for (int i = rear_stop_zone_.min_x; i <= rear_stop_zone_.max_x; ++i)
            for (int j = rear_stop_zone_.min_y; j <= rear_stop_zone_.max_y; ++j)
                if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE)
                {
                    obstacle_in_rear_ = true;
                    break;
                }

        // 检测左侧必停区障碍物
        for (int i = left_stop_zone_.min_x; i <= left_stop_zone_.max_x; ++i)
            for (int j = left_stop_zone_.min_y; j <= left_stop_zone_.max_y; ++j)
                if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE)
                {
                    obstacle_in_left_ = true;
                    break;
                }

        // 检测右侧必停区障碍物
        for (int i = right_stop_zone_.min_x; i <= right_stop_zone_.max_x; ++i)
            for (int j = right_stop_zone_.min_y; j <= right_stop_zone_.max_y; ++j)
                if ((size_t)i < grid.size() && (size_t)j < grid[0].size() && grid[i][j] == OBSTACLE)
                {
                    obstacle_in_right_ = true;
                    break;
                }

        // 检测前方扩展区域障碍物
        int front_area_min_x = max(0, vehicle_min_x_ - front_area_extend_);
        int front_area_max_x = vehicle_min_x_ - 1;
        int front_area_min_y = max(0, vehicle_min_y_ - left_area_extend_);
        int front_area_max_y = min((int)grid[0].size() - 1, vehicle_max_y_ + right_area_extend_);
        int leftmost_obstacle = -1, rightmost_obstacle = -1;

        for (size_t i = front_area_min_x; i <= (size_t)front_area_max_x; ++i)
        {
            for (size_t j = front_area_min_y; j <= (size_t)front_area_max_y; ++j)
            {
                if (grid[i][j] == OBSTACLE)
                {
                    obstacle_in_front_area_ = true;
                    if (leftmost_obstacle == -1 || (int)j < leftmost_obstacle) leftmost_obstacle = j;
                    if (rightmost_obstacle == -1 || (int)j > rightmost_obstacle) rightmost_obstacle = j;
                    int distance = vehicle_min_x_ - (int)i;
                    if (distance < obstacle_max_x_) obstacle_max_x_ = distance;
                }
            }
        }

        // 计算绕障空间
        calculateAvoidanceSpace(grid, leftmost_obstacle, rightmost_obstacle);
    }

    // ======== 绕障空间计算 ========
    void calculateAvoidanceSpace(const std::vector<std::vector<int>>& grid, int core_left_obstacle,
                                 int core_right_obstacle)
    {
        if (!obstacle_in_front_area_)
        {
            free_space_left_ = left_area_extend_ + (vehicle_max_y_ - vehicle_min_y_ + 1);
            free_space_right_ = right_area_extend_ + (vehicle_max_y_ - vehicle_min_y_ + 1);
            obstacle_left_edge_ = obstacle_right_edge_ = -1;
            return;
        }

        obstacle_left_edge_ = core_left_obstacle;
        obstacle_right_edge_ = core_right_obstacle;
        size_t avoidance_min_x = (size_t)max(0, vehicle_min_x_ - front_area_extend_);
        size_t avoidance_max_x = (size_t)vehicle_min_x_ - 1;

        // 计算左侧空闲空间
        int left_check_start = max(0, vehicle_min_y_ - left_area_extend_);
        int left_check_end = obstacle_left_edge_ - 1;
        free_space_left_ =
            calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, left_check_start, left_check_end);

        // 计算右侧空闲空间
        int right_check_start = obstacle_right_edge_ + 1;
        int right_check_end = min((int)grid[0].size() - 1, vehicle_max_y_ + right_area_extend_);
        free_space_right_ =
            calculateSpaceInDirection(grid, avoidance_min_x, avoidance_max_x, right_check_start, right_check_end);
    }

    // ======== 方向空闲空间计算 ========
    int calculateSpaceInDirection(const std::vector<std::vector<int>>& grid, size_t min_x, size_t max_x,
                                  int check_start_y, int check_end_y)
    {
        if (check_start_y > check_end_y) return 0;
        int available_space = 0;
        for (int j = check_start_y; j <= check_end_y; ++j)
        {
            if (j < 0 || j >= (int)grid[0].size()) break;
            bool column_clear = true;
            for (size_t i = min_x; i <= max_x; ++i)
            {
                if (i >= grid.size() || grid[i][j] == OBSTACLE)
                {
                    column_clear = false;
                    break;
                }
            }
            if (column_clear)
                available_space++;
            else
                break;
        }
        return available_space;
    }

    // ======== 栅格可视化 ========
    void visualizeGridInTerminal(const std::vector<std::vector<int>>& grid)
    {
        std::stringstream ss;
        ss << "  ----------栅格地图可视化----------   " << endl;
        ss << "  " << string(grid[0].size(), '-') << endl;
        ss << "   ";
        for (size_t i = 0; i < grid[0].size(); i++) ss << (i % 10);
        ss << endl;
        for (size_t i = 0; i < grid.size(); i++)
        {
            ss << (i < 10 ? " " : "") << i << "|";
            for (size_t j = 0; j < grid[i].size(); j++)
            {
                bool in_front = (i >= front_stop_zone_.min_x && i <= front_stop_zone_.max_x &&
                                 j >= front_stop_zone_.min_y && j <= front_stop_zone_.max_y);
                bool in_left = (i >= left_stop_zone_.min_x && i <= left_stop_zone_.max_x &&
                                j >= left_stop_zone_.min_y && j <= left_stop_zone_.max_y);
                bool in_right = (i >= right_stop_zone_.min_x && i <= right_stop_zone_.max_x &&
                                 j >= right_stop_zone_.min_y && j <= right_stop_zone_.max_y);

                if (grid[i][j] == OBSTACLE)
                    ss << "@";
                else if (grid[i][j] == VEHICLE)
                    ss << "V";
                else if (in_front)
                    ss << "F";
                else if (in_left)
                    ss << "L";
                else if (in_right)
                    ss << "R";
                else
                    ss << ".";
            }
            ss << "|" << endl;
        }
        ss << "  " << string(grid[0].size(), '-') << endl;
        LOG_INFO("%s", ss.str().c_str());
    }

    // ======== 规划层消息回调 ========
    void msg_callback_pl(const sweeper_interfaces::msg::Pl::SharedPtr msg)
    {
        pl_x_ = msg->x;
        pl_y_ = msg->y;
        pl_speed_ = (int)msg->speed;
        is_start = msg->is_start;

        LOG_INFO("[规划层消息回调] 目标点位置： x -- %.2f , y -- %.2f", pl_x_, pl_y_);
    }

    // ======== 绕障路点规划 ========
    void planAvoidanceWaypoints(int avoid_direction)
    {
        avoidance_waypoints_.clear();

        const double GRID_RESOLUTION = 0.3;
        const int SAFE_GRID_GAP = 1;
        const int AVOID_SPEED = 300;

        Waypoint point_lateral_shift;
        Waypoint point_parallel_forward;
        double actual_lateral_offset_m = 0.0;
        int lateral_offset_grid_num = 0;

        const int stage1_forward_grid = state_machine_->obstacle_analysis.obstacle_distance;
        const int stage2_forward_grid = parallel_distance_;
        const double stage1_forward_m = stage1_forward_grid * GRID_RESOLUTION;
        const double stage2_forward_m = stage2_forward_grid * GRID_RESOLUTION;

        // 计算横向偏移量
        if (avoid_direction == -1)
        {
            lateral_offset_grid_num = (vehicle_max_y_ - state_machine_->obstacle_analysis.left_edge) + SAFE_GRID_GAP;
            actual_lateral_offset_m = -1 * lateral_offset_grid_num * GRID_RESOLUTION;
        }
        else if (avoid_direction == 1)
        {
            lateral_offset_grid_num = (state_machine_->obstacle_analysis.right_edge - vehicle_min_y_) + SAFE_GRID_GAP;
            actual_lateral_offset_m = 1 * lateral_offset_grid_num * GRID_RESOLUTION;
        }

        // 相对坐标
        double relative_x_1 = actual_lateral_offset_m;
        double relative_y_1 = stage1_forward_m;
        double relative_x_2 = actual_lateral_offset_m;
        double relative_y_2 = stage2_forward_m;

        // 转换为绝对GPS坐标
        double abs_lon_1, abs_lat_1, abs_lon_2, abs_lat_2;
        rtk_relative_to_gps(relative_x_1, relative_y_1, g_rtk.direction, g_rtk.lon, g_rtk.lat, &abs_lon_1, &abs_lat_1);
        rtk_relative_to_gps(relative_x_2, relative_y_2, g_rtk.direction, g_rtk.lon, g_rtk.lat, &abs_lon_2, &abs_lat_2);

        // 构建路点
        point_lateral_shift = Waypoint(relative_x_1, relative_y_1, AVOID_SPEED, abs_lon_1, abs_lat_1);
        point_parallel_forward = Waypoint(relative_x_2, relative_y_2, AVOID_SPEED, abs_lon_2, abs_lat_2);

        avoidance_waypoints_.push_back(point_lateral_shift);
        avoidance_waypoints_.push_back(point_parallel_forward);

        current_waypoint_idx_ = 0;
        avoiding_obstacle_ = true;
        state_machine_->setState(StateMachine::LATERAL_SHIFT);

        LOG_INFO("[绕障规划] 方向：%s | 路点1 GPS(%.6f,%.6f) | 路点2 GPS(%.6f,%.6f)",
                 avoid_direction == -1 ? "左" : "右", abs_lon_1, abs_lat_1, abs_lon_2, abs_lat_2);
    }

    // ======== 相对坐标转GPS绝对坐标 ========
    void rtk_relative_to_gps(double rel_x, double rel_y, double cur_heading, double cur_lon, double cur_lat,
                             double* dest_lon, double* dest_lat)
    {
        double Navi_rad = cur_heading * deg_rad;

        // 逆向旋转矩阵：从车体坐标系转回地理坐标系
        float k1 = cos(Navi_rad);
        float k2 = sin(Navi_rad);
        float k3 = -sin(Navi_rad);
        float k4 = cos(Navi_rad);

        // 地理坐标增量
        double delta_x = rel_x * k1 + rel_y * k3;  // 东向增量
        double delta_y = rel_x * k2 + rel_y * k4;  // 北向增量

        // 转换为GPS坐标增量
        double delta_lon = delta_x / (deg_rad * R_LONT);
        double delta_lat = delta_y / (deg_rad * R_LATI);

        *dest_lon = cur_lon + delta_lon;
        *dest_lat = cur_lat + delta_lat;

        LOG_INFO("[相对转GPS] 相对(%.2f,%.2f) -> GPS增量(%.8f,%.8f) -> 目标(%.6f,%.6f)", rel_x, rel_y, delta_lon,
                 delta_lat, *dest_lon, *dest_lat);
    }

    // ======== 路点到达判断 ========
    // 返回值：1=到达 0=未到达 -1=RTK信号丢失(需要停车)
    int waypointReached(const Waypoint& target)
    {
        // 验证RTK信号质量
        auto now = system_clock::now();
        duration<double> time_since_last = now - last_rtk_time;

        if (g_rtk.reliability != 0 && time_since_last <= rtk_timeout)
        {
            // RTK信号良好：使用绝对坐标判断
            double distance = ntzx_GPS_length(g_rtk.lon, g_rtk.lat, target.lon, target.lat);

            LOG_INFO("[路点判断] RTK信号良好 | 当前GPS(%.6f,%.6f) | 目标GPS(%.6f,%.6f) | 距离: %.2f m | 容差: %.2f m",
                     g_rtk.lon, g_rtk.lat, target.lon, target.lat, distance, waypoint_tolerance_);

            return distance < waypoint_tolerance_ ? 1 : 0;
        }
        else
        {
            // RTK信号丢失：无法安全判断
            if (time_since_last > rtk_timeout)
            {
                LOG_ERROR("[路点判断] RTK超时(%.2fs)，信号丢失，停车退出绕障！", time_since_last.count());
            }
            else
            {
                LOG_ERROR("[路点判断] RTK信号差(可靠性=%d)，停车退出绕障！", g_rtk.reliability);
            }

            return -1;
        }
    }

    // ======== 路点跟踪执行 ========
    void executeWaypointTracking()
    {
        if (current_waypoint_idx_ >= (int)avoidance_waypoints_.size())
        {
            avoiding_obstacle_ = false;
            current_waypoint_idx_ = 0;
            avoidance_waypoints_.clear();
            state_machine_->setState(StateMachine::NORMAL);
            LOG_INFO("[绕障完成] 所有路点跟踪完毕，恢复全局规划控制");
            return;
        }

        Waypoint& target = avoidance_waypoints_[current_waypoint_idx_];

        // 计算目标方向角：
        float target_angle = calculate_the_angle(target.x, target.y);
        state_machine_->publish_angle = target_angle;
        state_machine_->publish_speed = target.speed;

        LOG_INFO("[路点跟踪] 路点%d/%d | 相对坐标(%.2f,%.2f) | 目标方向: %.2f度", current_waypoint_idx_ + 1,
                 (int)avoidance_waypoints_.size(), target.x, target.y, target_angle);

        // 检查路点到达情况
        int reached = waypointReached(target);

        if (reached == -1)
        {
            // RTK信号丢失，立即停车并退出绕障
            state_machine_->publish_speed = 0;
            state_machine_->publish_angle = 0.0f;
            state_machine_->setState(StateMachine::WAITING);

            avoiding_obstacle_ = false;
            current_waypoint_idx_ = 0;
            avoidance_waypoints_.clear();

            LOG_ERROR("[绕障中止] RTK信号丢失，停车并退出绕障！");
            return;
        }
        else if (reached == 1)
        {
            // 已到达路点，切换到下一个
            current_waypoint_idx_++;

            if (current_waypoint_idx_ < (int)avoidance_waypoints_.size())
            {
                state_machine_->setState(StateMachine::PARALLEL_MOVE);
                LOG_INFO("[路点跟踪] 已到达路点%d，切换到路点%d", current_waypoint_idx_, current_waypoint_idx_ + 1);
            }
            else
            {
                avoiding_obstacle_ = false;
                current_waypoint_idx_ = 0;
                avoidance_waypoints_.clear();
                state_machine_->setState(StateMachine::NORMAL);
                LOG_INFO("[绕障完成] 所有路点跟踪完毕，恢复全局规划控制");
                return;
            }
        }
        else
        {
            // 未到达路点，继续前往
            state_machine_->setState(StateMachine::LATERAL_SHIFT);
        }

        // 安全检查：前方必停区有障碍物则立即停车
        if (state_machine_->obstacle_analysis.has_front_critical)
        {
            state_machine_->publish_speed = 0;
            state_machine_->publish_angle = 0.0f;
            state_machine_->setState(StateMachine::WAITING);
            LOG_WARN("[绕障安全] 路点跟踪中检测到前方必停区障碍物，立即停车");

            // 立即发布停车指令（带刹车）
            publishControlCommand(true);
            avoiding_obstacle_ = false;
            current_waypoint_idx_ = 0;
            avoidance_waypoints_.clear();
            return;
        }
    }

    // ======== 可绕障判断 ========
    bool canAvoidObstacle() const
    {
        if (state_machine_->obstacle_analysis.obstacle_width <= 0 ||
            state_machine_->obstacle_analysis.obstacle_width > avoid_able_width_)
        {
            LOG_INFO("[绕障判断] 障碍物宽度%d栅格，超过可绕最大宽度%d栅格，不可绕",
                     state_machine_->obstacle_analysis.obstacle_width, avoid_able_width_);
            return false;
        }
        bool left_ok = state_machine_->obstacle_analysis.free_space_left >= min_free_space_;
        bool right_ok = state_machine_->obstacle_analysis.free_space_right >= min_free_space_;
        LOG_INFO("[绕障判断] 左侧空闲%d栅格，右侧空闲%d栅格，最小需求%d栅格 | 左可绕：%s，右可绕：%s",
                 state_machine_->obstacle_analysis.free_space_left, state_machine_->obstacle_analysis.free_space_right,
                 min_free_space_, left_ok ? "是" : "否", right_ok ? "是" : "否");
        return left_ok || right_ok;
    }

    // ======== 绕障方向选择 ========
    int selectOptimalAvoidDirection() const
    {
        if (state_machine_->obstacle_analysis.free_space_left > state_machine_->obstacle_analysis.free_space_right)
            return -1;
        else if (state_machine_->obstacle_analysis.free_space_right > state_machine_->obstacle_analysis.free_space_left)
            return 1;
        else
            return (pl_x_ > 0) ? 1 : -1;
    }

    // ======== 更新障碍物分析 ========
    void updateObstacleAnalysis()
    {
        state_machine_->obstacle_analysis.has_front_critical = obstacle_in_front_;
        state_machine_->obstacle_analysis.has_front_area = obstacle_in_front_area_;
        state_machine_->obstacle_analysis.has_left_critical = obstacle_in_left_;
        state_machine_->obstacle_analysis.has_right_critical = obstacle_in_right_;
        state_machine_->obstacle_analysis.obstacle_distance = (obstacle_max_x_ > 0) ? obstacle_max_x_ : -1;
        state_machine_->obstacle_analysis.left_edge = obstacle_left_edge_;
        state_machine_->obstacle_analysis.right_edge = obstacle_right_edge_;
        state_machine_->obstacle_analysis.free_space_left = free_space_left_;
        state_machine_->obstacle_analysis.free_space_right = free_space_right_;

        if (obstacle_left_edge_ != -1 && obstacle_right_edge_ != -1)
        {
            state_machine_->obstacle_analysis.obstacle_width = obstacle_right_edge_ - obstacle_left_edge_ + 1;
        }

        LOG_INFO("[障碍物分析] 距离：%d栅格 | 宽度：%d栅格 | 左边缘：%d | 右边缘：%d",
                 state_machine_->obstacle_analysis.obstacle_distance, state_machine_->obstacle_analysis.obstacle_width,
                 state_machine_->obstacle_analysis.left_edge, state_machine_->obstacle_analysis.right_edge);
    }

    // ======== 紧急停车执行 ========
    bool executeEmergencyStop()
    {
        if (!enable_obstacle_stop_) return false;

        // 检查栅格数据新鲜度（超过500ms认为数据过期）
        auto now = node_clock_->now();
        auto grid_age = (now - last_grid_time_).seconds();
        if (grid_age > 0.5)
        {
            LOG_WARN("[紧急停车] 栅格数据过期(%.3fs)，执行安全停车！", grid_age);
            state_machine_->publish_speed = 0;
            state_machine_->publish_angle = 0.0f;
            state_machine_->setState(StateMachine::WAITING);
            return true;
        }

        if (state_machine_->obstacle_analysis.has_front_critical)
        {
            state_machine_->publish_speed = 0;
            state_machine_->publish_angle = 0.0f;
            state_machine_->setState(StateMachine::WAITING);
            LOG_WARN("[紧急停车] 前方必停区检测到障碍物，执行紧急停车");
            return true;
        }
        return false;
    }

    // ======== 定时器回调（主逻辑） ========
    void timer_callback()
    {
        // 更新障碍物分析
        updateObstacleAnalysis();

        // 开始清扫任务
        if (is_start)
        {
            // 紧急停车优先级最高
            if (executeEmergencyStop())
            {
                publishControlCommand(true);  // 紧急停车使用刹车
                return;
            }

            // PL速度为0，停车
            if (pl_speed_ == 0)
            {
                state_machine_->publish_speed = 0;
                state_machine_->publish_angle = 0;
                LOG_INFO("PL速度为0，已停车");
                publishControlCommand();
                return;
            }

            // 若之前处于WAITING状态，且现在没有障碍物且RTK信号良好，恢复到NORMAL状态
            auto now = system_clock::now();
            duration<double> time_since_last_rtk = now - last_rtk_time;
            bool rtk_signal_good = (g_rtk.reliability != 0 && time_since_last_rtk <= rtk_timeout);

            if (state_machine_->getCurrentState() == StateMachine::WAITING &&
                !state_machine_->obstacle_analysis.has_front_critical &&
                rtk_signal_good)
            {
                state_machine_->setState(StateMachine::NORMAL);
                LOG_INFO("[状态恢复] 障碍物已消失且RTK信号良好，恢复正常行驶状态");
            }

            // 绕障逻辑
            if (enable_obstacle_avoid_)
            {
                if (avoiding_obstacle_)
                {
                    // 正在绕障，执行路点跟踪
                    executeWaypointTracking();
                }
                else if (state_machine_->obstacle_analysis.has_front_area && canAvoidObstacle())
                {
                    // 需要开始绕障
                    int avoid_dir = selectOptimalAvoidDirection();
                    planAvoidanceWaypoints(avoid_dir);
                    LOG_INFO("[启动绕障] 开始%s侧绕障", avoid_dir == -1 ? "左" : "右");
                }
                else if (state_machine_->getCurrentState() != StateMachine::NORMAL)
                {
                    // 无需绕障，恢复正常状态
                    state_machine_->setState(StateMachine::NORMAL);
                }
            }

            // 正常行驶逻辑
            if (state_machine_->getCurrentState() == StateMachine::NORMAL)
            {
                state_machine_->publish_speed = 800;
                state_machine_->publish_angle = calculate_the_angle(pl_x_, pl_y_);
            }

            // 发布控制指令
            publishControlCommand();
        }
    }

    // ======== 发布控制指令 ========
    void publishControlCommand(bool emergency_brake = false)
    {
        auto message = sweeper_interfaces::msg::McCtrl();
        message.sweep = true;
        message.brake = emergency_brake ? 1 : 0;  // 紧急停车时启用刹车
        message.gear = 2;
        message.angle_speed = 800;

        // 转速限制
        message.rpm = std::clamp(state_machine_->publish_speed, 0, 800);
        message.angle = std::clamp(state_machine_->publish_angle, -50.0f, 50.0f);

        pub_mc->publish(message);

        if (emergency_brake)
        {
            LOG_WARN("[控制发布] 紧急刹车！状态：%s | 转速：%d | 转向角度：%.1f° | 刹车：%d",
                     state_machine_->getStateString().c_str(), message.rpm, message.angle, message.brake);
        }
        else
        {
            LOG_INFO("[控制发布] 状态：%s | 转速：%d | 转向角度：%.1f°", state_machine_->getStateString().c_str(),
                     message.rpm, message.angle);
        }
    }
};

// ======== 主函数 ========
int main(int argc, char** argv)
{
    logger::Logger::Init("fu", "./nodes_log");

    rclcpp::init(argc, argv);
    auto node = std::make_shared<fu_node>("fu_node");
    rclcpp::spin(node);
    rclcpp::shutdown();

    logger::Logger::Shutdown();

    return 0;
}