// lidascan_ctrl.cpp
#include "lidascan_ctrl.hpp"
#include <rclcpp/logging.hpp>
#include <rclcpp/qos.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <cmath>
#include <limits>
#include <deque>
#include <numeric>
#include <thread>
#include <Eigen/Dense>
#include "../../common/utils/topic.h"

using namespace std::chrono_literals;
using std::placeholders::_1;

LidarScanController::LidarScanController(rclcpp::Node *node)
    : node_(node), active_(false), charging_state_(ChargingState::IDLE), detect_charging_voltage_(false), frame_index_(0), station_detected_(false), current_angle_bisector_k_(0.0),
      current_angle_bisector_b_(0.0), // 新增
      has_angle_bisector_(false)      // 新增
{
}

LidarScanController::~LidarScanController()
{
}

void LidarScanController::initialize()
{
    // 声明参数
    node_->declare_parameter<double>("distance_threshold", 0.5);
    node_->declare_parameter<int>("fit_window_size", 10);
    node_->declare_parameter<double>("fit_residual_threshold", 0.09);

    // 获取参数
    distance_threshold_ = node_->get_parameter("distance_threshold").as_double();
    fit_window_size_ = node_->get_parameter("fit_window_size").as_int();
    fit_residual_threshold_ = node_->get_parameter("fit_residual_threshold").as_double();

    // QoS配置
    auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(10))
                           .best_effort()
                           .durability_volatile();

    // 创建激光雷达订阅者
    scan_sub_ = node_->create_subscription<sensor_msgs::msg::LaserScan>(
        "/scan/back", qos_profile,
        [this](const LaserScanSharedPtr msg)
        {
            if (active_)
                processLaserScan(msg);
        });

    // 创建控制发布者
    pub_ctrl_ = node_->create_publisher<geometry_msgs::msg::Twist>(nav2ControlTopic, 10);

    // 创建可视化发布者
    pub_rviz_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>(
        "/line_marker_array", 10);

    // 创建过滤后的激光数据发布者
    pub_filtered_ = node_->create_publisher<sensor_msgs::msg::LaserScan>("/scan/out", 10);
    pub_line_ = node_->create_publisher<sensor_msgs::msg::LaserScan>("/scan/out_line", 10);

    // 创建基站检测状态发布者（使用 transient_local 确保新订阅者能收到最后一条消息）
    pub_station_detected_ = node_->create_publisher<std_msgs::msg::Bool>(
        "/station/detected",
        rclcpp::QoS(10).transient_local()); // 修改为 transient_local
    pub_charging_voltage_ = node_->create_publisher<std_msgs::msg::String>(
        "/charging/voltage_detected", 10);
    pub1_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/state",10);
    pub2_=node_->create_publisher<std_msgs::msg::Int32>("/vlago/recharge/feedback",10);
    // 初始化控制消息
    ctrl_twist_.linear.x = 0.0;
    ctrl_twist_.angular.z = 0.0;
    ctrl_twist_time_ = node_->now();

    // 创建导航定时器
    navigation_timer_ = node_->create_wall_timer(
        100ms, std::bind(&LidarScanController::navigationTimerCallback, this));

    detect_station_time_ = node_->now() - rclcpp::Duration(100.0, 0);
    detect_charging_voltage_time_ = node_->now();

    // 初始化基站检测状态
    station_detected_ = false;
    last_station_publish_time_ = node_->now();

    RCLCPP_INFO(node_->get_logger(), "激光扫描控制器已初始化");
}

void LidarScanController::resetStationDetection()
{
    // 重置内部状态
    station_detected_ = false;
    detect_charging_voltage_ = false;

    // 发布false信号
    auto false_msg = std_msgs::msg::Bool();
    false_msg.data = false;
    pub_station_detected_->publish(false_msg);

    // 重置时间戳
    last_station_publish_time_ = node_->now();

    RCLCPP_INFO(node_->get_logger(), "基站检测状态已重置: false");
}

void LidarScanController::startNavigation()
{
    active_ = true;
    charging_state_ = ChargingState::NAVIGATING;
    station_detected_ = false;
    RCLCPP_INFO(node_->get_logger(), "导航功能已启动");
}

void LidarScanController::stopNavigation()
{
    active_ = false;
    charging_state_ = ChargingState::IDLE;
    stopMotion();
    RCLCPP_INFO(node_->get_logger(), "导航功能已停止");
}

void LidarScanController::publishStationDetected(bool detected)
{
    auto now = node_->now();
    auto time_diff = (now - last_station_publish_time_).seconds();
    // if (station_detected_ == false && detected == false)
    // {
    //     auto msg = std_msgs::msg::Bool();
    //     msg.data = detected;
    //     pub_station_detected_->publish(msg);
    // }
    // 防止频繁发布，至少间隔0.5秒
    if (station_detected_ != detected)
    {
        auto msg = std_msgs::msg::Bool();
        msg.data = detected;
        pub_station_detected_->publish(msg);
        station_detected_ = detected;
    }
    last_station_publish_time_ = now;

    // if (detected)
    // {
    //     RCLCPP_INFO(node_->get_logger(), "发布基站检测成功信号: true");
    // }
    // else
    // {
    //     RCLCPP_DEBUG(node_->get_logger(), "发布基站检测信号: false");
    // }
}

void LidarScanController::stopMotion()
{
    ctrl_twist_.linear.x = 0.0;
    ctrl_twist_.angular.z = 0.0;
    ctrl_twist_time_ = node_->now();
    pub_ctrl_->publish(ctrl_twist_);
    RCLCPP_INFO(node_->get_logger(), "停止运动");
}

void LidarScanController::navigationTimerCallback()
{
    if (!active_)
        return;

    auto now = node_->now();
    auto time_diff = (now - ctrl_twist_time_).seconds();

    if (time_diff < 2.0)
    {
        pub_ctrl_->publish(ctrl_twist_);
    }
}

void LidarScanController::processLaserScan(const LaserScanSharedPtr &msg)
{
    if (!active_)
        return;

    // 应用过滤算法
    filterScanModify0(msg);
    filterScanModify1(msg);
    filterScanModify2(msg);
    filterScanModify3(msg);

    auto [begin_index, end_index] = filterScanModify4(msg);

    if (end_index - begin_index < 2)
    {
        RCLCPP_DEBUG(node_->get_logger(), "没有可连续性的点");
        return;
    }

    // 处理分段数据
    if (processSegmentData(msg, begin_index, end_index))
    {
        detect_station_time_ = node_->now();
    }
}

bool LidarScanController::processSegmentData(const LaserScanSharedPtr &msg,
                                             int begin_index, int end_index)
{
    // 转换到XY坐标系
    auto xy_list = laserScanToXY1(begin_index, end_index, msg);
    if (xy_list.size() < 2)
    {
        RCLCPP_WARN(node_->get_logger(), "XY转换结果点数不足: %zu", xy_list.size());
        return false;
    }

    // 获取分段
    auto sections = getSections(begin_index, end_index, msg, xy_list);
    if (sections.empty())
    {
        // RCLCPP_WARN(node_->get_logger(), "没有获取到分段");
        return false;
    }

    // 选择最近的分段
    int nearest_index = -1;
    double nearest_x = 100.0;

    for (size_t i = 0; i < sections.size(); i++)
    {
        int section_start = sections[i].first;
        int section_end = sections[i].second;

        if (section_end - section_start < 10)
            continue;

        double avg_total = 0.0;
        int avg_count = 0;

        for (int index = section_start; index <= section_end; index++)
        {
            if (xy_list.find(index) == xy_list.end())
                continue;

            double x = msg->ranges[index];
            avg_count++;
            avg_total += x;
        }

        if (avg_count == 0)
            continue;

        double avg_value = avg_total / avg_count;

        if (nearest_x > avg_value)
        {
            nearest_x = avg_value;
            nearest_index = i;
        }
    }

    if (nearest_index < 0)
    {
        return false;
    }

    begin_index = sections[nearest_index].first;
    end_index = sections[nearest_index].second;

    // 重置激光数据
    auto filtered_msg = resetLaserData(msg, begin_index, end_index);

    // 发布过滤后的数据
    publishFilteredScan(filtered_msg, filtered_msg->ranges);

    // 进一步处理
    auto xy_list_full = laserScanToXY(filtered_msg);
    if (xy_list_full.size() < 2)
    {
        // publishStationDetected(false);
        return false;
    }

    // 获取分段
    auto turn_segments = getSegments(xy_list_full);
    auto filtered_segments = filterSegments(turn_segments);

    if (filtered_segments.empty())
    {
        // publishStationDetected(false);
        return false;
    }

    // 创建结果对象
    RechargeResult result;
    result.frameid = frame_index_++;

    // 处理左右点
    int segment_index = 0;
    for (const auto &seg : filtered_segments)
    {
        const auto &info = seg.second;
        int start_idx = std::get<0>(info);
        int end_idx = std::get<1>(info);

        for (int index = start_idx; index <= end_idx; index++)
        {
            if (xy_list_full.find(index) == xy_list_full.end())
                continue;

            double x = xy_list_full[index].first;
            double y = xy_list_full[index].second;
            IndexPoint item(x, y, index);

            if (segment_index == 0)
            {
                result.left_points.push_back(item);
            }
            else if (segment_index == 1)
            {
                result.right_points.push_back(item);
            }
        }
        segment_index++;

        if (segment_index >= 2)
            break; // 只需要两个分段
    }

    // 如果成功获取左右点，进行控制
    if (!result.left_points.empty() && !result.right_points.empty())
    {
        procData(result);
        return true;
    }
    // publishStationDetected(false);
    return false;
}

void LidarScanController::procData(RechargeResult &result)
{
    if (result.left_points.size() < 2 || result.right_points.size() < 2)
    {
        RCLCPP_WARN(node_->get_logger(), "点数量不足，跳过拟合");
        has_angle_bisector_ = false;
        return;
    }

    // 拟合左边直线
    auto [k1, b1] = RechargeTool::simpleFitLine(result.left_points);
    RCLCPP_DEBUG(node_->get_logger(), "左直线拟合: y = %.4fx + %.4f", k1, b1);

    // 拟合右边直线
    auto [k2, b2] = RechargeTool::simpleFitLine(result.right_points);
    RCLCPP_DEBUG(node_->get_logger(), "右直线拟合: y = %.4fx + %.4f", k2, b2);

    // 检查是否平行
    if (fabs(k1 - k2) < 1e-10)
    {
        RCLCPP_WARN(node_->get_logger(), "两直线平行，无交点");
        result.middle_point = IndexPoint(0.0, 0.0, 0);
        result.middle_line = StraightLine(0.0, 0.0);
        result.left_line = StraightLine(k1, b1);
        result.right_line = StraightLine(k2, b2);
        has_angle_bisector_ = false;
        return;
    }

    // 计算交点
    double x = (b2 - b1) / (k1 - k2);
    double y = k1 * x + b1;

    // 更新中间点
    result.middle_point = IndexPoint(x, y, 0);

    // 计算角平分线
    auto [k3, b3] = RechargeTool::calculateAngleBisector(k1, b1, k2, b2);
    current_angle_bisector_k_ = k3;
    current_angle_bisector_b_ = b3;
    has_angle_bisector_ = true;

    // 详细打印角平分线信息（用于测试对齐）
    // RCLCPP_INFO(node_->get_logger(), "========== 基站检测信息 ==========");
    // RCLCPP_INFO(node_->get_logger(), "左直线: y = %.4fx + %.4f", k1, b1);
    // RCLCPP_INFO(node_->get_logger(), "右直线: y = %.4fx + %.4f", k2, b2);
    // RCLCPP_INFO(node_->get_logger(), "角平分线: y = %.4fx + %.4f", k3, b3);
    // RCLCPP_INFO(node_->get_logger(), "交点: (%.4f, %.4f)", x, y);

    // 计算角度信息
    double left_angle = atan(k1) * 180.0 / M_PI;
    double right_angle = atan(k2) * 180.0 / M_PI;
    double bisector_angle = atan(k3) * 180.0 / M_PI;

    // RCLCPP_INFO(node_->get_logger(), "左直线角度: %.2f°", left_angle);
    // RCLCPP_INFO(node_->get_logger(), "右直线角度: %.2f°", right_angle);
    // RCLCPP_INFO(node_->get_logger(), "角平分线角度: %.2f°", bisector_angle);

    // 更新结果
    result.middle_line = StraightLine(k3, b3);
    result.left_line = StraightLine(k1, b1);
    result.right_line = StraightLine(k2, b2);

    // 运动控制
    controlRechargeMotion(result);

    // 可视化
    publishRechargeVisualization(result);
}

void LidarScanController::controlRechargeMotion(const RechargeResult &result)
{
    if (!active_) return;
    publishStationDetected(true);
    bool in_rotation_delay = node_->get_parameter("in_rotation_delay").as_bool();
    if (in_rotation_delay)
    {
        RCLCPP_INFO(node_->get_logger(), "旋转延迟期间，仅发布检测信号，不执行运动控制");
        // 清除控制命令，避免定时器发布旧命令
        // 修改：不再清除控制命令，而是继续旋转
        // 只清除线性速度，保持角速度
        double angular_velocity = 0.15; // 与RotationManager中的角速度一致

        ctrl_twist_.linear.x = 0.0;               // 线速度保持0
        ctrl_twist_.angular.z = angular_velocity; // 保持旋转速度
        ctrl_twist_time_ = node_->now();

        // 发布旋转命令
        pub_ctrl_->publish(ctrl_twist_);
        return;
    }
    // 如果检测到充电电压，停止移动
    if (getChargingVoltage())
    {
        RCLCPP_INFO(node_->get_logger(), "检测到充电电压，停止移动");
        ctrl_twist_.linear.x = 0.0;
        ctrl_twist_.angular.z = 0.0;
        ctrl_twist_time_ = node_->now();
        stopNavigation();
        return;
    }

    double dx = result.middle_point.x;
    double dy = result.middle_point.y;
    double k = result.middle_line.k;

    RCLCPP_DEBUG(node_->get_logger(), "控制参数 - dx: %.4f, dy: %.4f, k: %.4f", dx, dy, k);

    // 调整 dy
    dy = dy - 0.10;
    RCLCPP_DEBUG(node_->get_logger(), "调整后 dy: %.4f", dy);

    // 计算偏航角误差
    double yaw_diff = atan(k);
    RCLCPP_INFO(node_->get_logger(), "偏航角误差: %.4f rad (%.2f 度)",
                yaw_diff, yaw_diff * 180.0 / M_PI);

    if (checkStationAlignment(dx, dy, yaw_diff))
    {
        // 停止行车中，准备向前移动，再校准回充
        RCLCPP_WARN(node_->get_logger(), "===== 开始对准误差调整 =====");
        RCLCPP_WARN(node_->get_logger(), "当前误差 - 夹角:%.2f°, dy:%.3fm",
                    yaw_diff * 180.0 / M_PI, dy);

        // 1. 停止当前运动
        ctrl_twist_.linear.x = 0.0;
        ctrl_twist_.angular.z = 0.0;
        ctrl_twist_time_ = node_->now();
        pub_ctrl_->publish(ctrl_twist_);
        rclcpp::sleep_for(std::chrono::milliseconds(300));

        // 2. 根据误差类型进行旋转调整
        double adjust_duration = 0.0;
        double adjust_angular = 0.0;
        std::string adjust_type;

        const double ANGLE_8DEG = 5.0 * M_PI / 180.0; // 5度阈值
                                                      // const double DY_THRESHOLD = 0.05;             // 5cm阈值

        // 优先调整角度
        if (fabs(yaw_diff) > ANGLE_8DEG)
        {
            adjust_type = "角度调整";
            // 根据夹角大小计算调整时间
            adjust_duration = fabs(yaw_diff) / 0.15; // 角速度0.25 rad/s
            adjust_angular = (yaw_diff > 0) ? -0.15 : 0.15;

            RCLCPP_WARN(node_->get_logger(), "旋转 %.2f 秒", adjust_duration);
        }
        // 角度满足要求后调整dy
        // else
        // {
        //     adjust_type = "横向偏移调整";
        //     // dy为正(偏右) -> 向左转；dy为负(偏左) -> 向右转
        //     adjust_duration = fabs(dy) * 20.0;  // 每5cm误差旋转1秒
        //     adjust_angular = (dy > 0) ? 0.10 : -0.10;

        //     RCLCPP_WARN(node_->get_logger(), "旋转 %.2f 秒",adjust_duration);
        // }

        // 3. 执行旋转调整
        if (active_)
        {
            if (adjust_duration > 0.1)
            {
                geometry_msgs::msg::Twist adjust_cmd;
                adjust_cmd.linear.x = 0.0;
                adjust_cmd.angular.z = adjust_angular;

                // 按计算的时间连续旋转
                rclcpp::Time adjust_start = node_->now();
                int publish_count = 0;

                while (rclcpp::ok() && active_ &&(node_->now() - adjust_start).seconds() < adjust_duration)
                {
                    pub_ctrl_->publish(adjust_cmd);
                    publish_count++;
                    rclcpp::sleep_for(std::chrono::milliseconds(50));
                }

                RCLCPP_WARN(node_->get_logger(), "%s完成: 发布%d次命令, 实际旋转%.2f秒",
                            adjust_type.c_str(), publish_count, adjust_duration);

                // 4. 停止旋转
                adjust_cmd.angular.z = 0.0;
                pub_ctrl_->publish(adjust_cmd);
                rclcpp::sleep_for(std::chrono::milliseconds(500));
            }

            // 5. 向前走一小段距离（新增功能）
            const double FORWARD_DISTANCE = 0.80; // 向前走150cm
            const double FORWARD_SPEED = 0.09;    // 前进速度 0.08 m/s

            // 计算前进所需时间
            double forward_duration = FORWARD_DISTANCE / FORWARD_SPEED;

            RCLCPP_WARN(node_->get_logger(), "开始向前调整: 距离=%.2fm, 速度=%.2fm/s, 时间=%.2fs",
                        FORWARD_DISTANCE, FORWARD_SPEED, forward_duration);

            // 发送前进命令
            geometry_msgs::msg::Twist forward_cmd;
            forward_cmd.linear.x = FORWARD_SPEED;
            forward_cmd.angular.z = 0.0;

            rclcpp::Time forward_start = node_->now();
            int forward_count = 0;

            while (rclcpp::ok() && active_ &&(node_->now() - forward_start).seconds() < forward_duration)
            {
                pub_ctrl_->publish(forward_cmd);
                forward_count++;
                rclcpp::sleep_for(std::chrono::milliseconds(50));
            }

            // 停止前进
            forward_cmd.linear.x = 0.0;
            pub_ctrl_->publish(forward_cmd);

            RCLCPP_INFO(node_->get_logger(), "前进调整完成: 发布%d次命令, 实际前进%.2f秒",
                        forward_count, forward_duration);
        }

        // 6. 重要：等待新一帧激光数据并更新result
        // 等待激光数据更新（约100ms足够接收新数据）
        rclcpp::sleep_for(std::chrono::milliseconds(450));

        // 注意：这里无法直接更新result，但函数返回后，下一帧激光数据会触发新的processSegmentData
        // 从而得到新的result，重新进入controlRechargeMotion

        RCLCPP_WARN(node_->get_logger(), "===== 对准误差调整完成 =====");

        // 7. 返回，等待下一帧激光数据重新计算
        // 不继续执行后面的速度计算，避免使用旧数据
        return;
    }
    // 计算控制速度
    auto [linear_velocity, angular_velocity] = RechargeTool::computeDockingVelocity(dx - 0.3, dy, yaw_diff);
    RCLCPP_DEBUG(node_->get_logger(), "基础控制速度 - 线速度: %.4f, 角速度: %.4f",
                 linear_velocity, angular_velocity);
    RCLCPP_INFO(node_->get_logger(), "dx=%.4f,dy=%.4f", dx, dy);
    // 限制角速度
    if (angular_velocity > 0.0)
    {
        angular_velocity = std::min(fabs(angular_velocity), 0.09);
    }
    else
    {
        angular_velocity = -std::min(fabs(angular_velocity), 0.09);
    }

    // 根据距离调整速度系数
    double radio_x = 1.0;
    double radio_z = 1.0;

    if (dx > 1.0)
    {
        // 保持默认
    }
    else if (dx > 0.8)
    {
        radio_x = 0.7;
    }
    else if (dx > 0.7)
    {
        radio_x = 0.5;
    }
    else if (dx > 0.68)
    {
        radio_x = 0.3;
        radio_z = 1.0;
    }
    else if (dx > 0.65)
    {
        radio_x = 0.2;
        radio_z = 1.0;
    }
    else if (dx > 0.62)
    {
        radio_x = 0.1;
        radio_z = 1.0;
    }
    else if (dx > 0.2)
    {
        radio_x = 0.15;
        radio_z = 1.0;
    }

    RCLCPP_DEBUG(node_->get_logger(), "距离调整系数 - radio_x: %.2f, radio_z: %.2f", radio_x, radio_z);

    // 设置控制命令
    if (dx > 0.54)
    {
        double final_linear_vel = linear_velocity * radio_x;
        if (final_linear_vel > -0.04)
        {
            final_linear_vel = -0.04;
        }

        ctrl_twist_.linear.x = final_linear_vel;
        ctrl_twist_.angular.z = angular_velocity * radio_z;
        ctrl_twist_time_ = node_->now();

        RCLCPP_DEBUG(node_->get_logger(), "控制命令 - 线速度: %.4f, 角速度: %.4f",
                     ctrl_twist_.linear.x, ctrl_twist_.angular.z);
    }
    else
    {
        // 到达目标位置
        RCLCPP_INFO(node_->get_logger(), "到达目标位置 (dx=%.4f <= 0.54), 停止移动", dx);
        ctrl_twist_.linear.x = 0.0;
        ctrl_twist_.angular.z = 0.0;
        ctrl_twist_time_ = node_->now();
        auto msg =std_msgs::msg::Int32();
        msg.data=RECHARGE_FEEDBACK_SUCCESS_DOCKED;
        pub1_->publish(msg);
        stopNavigation();

        // 模拟检测到充电电压
        setChargingVoltage(true);
    }
}

bool LidarScanController::checkStationAlignment(double dx, double dy, double yaw_diff)
{
    bool isstop = false;
    const double ALIGNMENT_DISTANCE_THRESHOLD = 0.80;            // 80cm
    const double ALIGNMENT_ANGLE_THRESHOLD = 5.0 * M_PI / 180.0; // 5度

    if (dx <= ALIGNMENT_DISTANCE_THRESHOLD)
    {
        RCLCPP_INFO(node_->get_logger(), "进入对准区域 - 距离基站: %.4f m, 夹角: %.2f°",
                    dx, yaw_diff * 180.0 / M_PI);

        // 判断是否对准（夹角小于10度）
        if (fabs(yaw_diff) <= ALIGNMENT_ANGLE_THRESHOLD)
        {
            RCLCPP_INFO(node_->get_logger(), "✓ 已对准基站，继续行驶 - 夹角: %.2f°,dx=%.2f,dy=%.2f",
                        yaw_diff * 180.0 / M_PI, dx, dy);

            RCLCPP_DEBUG(node_->get_logger(), "对准行驶 - 线速度: %.4f", ctrl_twist_.linear.x);
        }
        else
        {
            RCLCPP_WARN(node_->get_logger(), "✗ 未对准基站，停止行驶 - 夹角: %.2f° > 5°,dx=%.2f,dy=%.2f",
                        yaw_diff * 180.0 / M_PI, dx, dy);
            isstop = true;
            // RCLCPP_INFO(node_->get_logger(), "自动调整方向 - 旋转角速度: %.4f", adjust_angular);
        }
    }
    return isstop;
}

// ==================== 过滤函数实现 ====================
void LidarScanController::filterScanModify0(LaserScanSharedPtr msg)
{
    int fixed = getFixed();
    double diff_yaw = msg->angle_increment * fixed;
    msg->angle_min -= diff_yaw;
    msg->angle_max -= diff_yaw;

    std::vector<float> ranges = msg->ranges;
    for (size_t i = 0; i < msg->ranges.size(); i++)
    {
        int index = getFixedIndex(msg, i);
        ranges[index] = msg->ranges[i];
    }
    msg->ranges = ranges;
}

void LidarScanController::filterScanModify1(LaserScanSharedPtr msg)
{
    // 过滤角度，只接受后面90度
    int min_index = param_.min_index;
    int max_index = param_.max_index;

    for (size_t i = 0; i < msg->ranges.size(); i++)
    {
        if (i < min_index || i > max_index)
        {
            msg->ranges[i] = std::numeric_limits<float>::infinity();
        }
    }
}

void LidarScanController::filterScanModify2(LaserScanSharedPtr msg)
{
    // 过滤距离
    for (size_t i = 0; i < msg->ranges.size(); i++)
    {
        if (!isValid(msg->ranges[i]))
            continue;

        if (msg->ranges[i] < param_.min_distance ||
            msg->ranges[i] > param_.max_distance)
        {
            msg->ranges[i] = std::numeric_limits<float>::infinity();
        }
    }
}

void LidarScanController::filterScanModify3(LaserScanSharedPtr msg)
{
    // 过滤强度
    for (size_t i = 0; i < msg->ranges.size(); i++)
    {
        if (!isValid(msg->ranges[i]))
            continue;

        float value = msg->intensities[i];
        if (value < param_.min_intensities || value > param_.max_intensities)
        {
            msg->ranges[i] = std::numeric_limits<float>::infinity();
        }

        // 再次检查距离
        if (msg->ranges[i] < param_.min_distance ||
            msg->ranges[i] > param_.max_distance)
        {
            msg->ranges[i] = std::numeric_limits<float>::infinity();
        }
    }
}

std::pair<int, int> LidarScanController::filterScanModify4(LaserScanSharedPtr msg)
{
    // 过滤连续性
    int begin_index = 0;
    int end_index = 0;
    std::vector<float> ranges = msg->ranges;

    // 顺序找起点
    for (int i = param_.min_index + 1; i < param_.max_index; i++)
    {
        if (!isValid(ranges[i]))
            continue;
        if (!isValid(ranges[i - 1]))
        {
            begin_index = i;
            break;
        }
    }

    if (static_cast<int>(msg->ranges.size()) - begin_index < 10)
    {
        RCLCPP_ERROR(node_->get_logger(), "数据太少无法继续处理");
        return {0, 0};
    }

    // 倒序找终点
    for (int i = param_.max_index - 2; i > 0; i--)
    {
        if (!isValid(ranges[i]))
            continue;
        if (!isValid(ranges[i + 1]))
        {
            end_index = i;
            break;
        }
    }

    if (end_index < 10)
    {
        return {0, 0};
    }

    // 顺序检查连续性
    for (int i = begin_index + 1; i < end_index - 1; i++)
    {
        float prev = ranges[i - 1];
        float curr = ranges[i];
        float next = ranges[i + 1];

        if (!isValid(prev) && !isValid(curr) && !isValid(next))
        {
            if (i - begin_index < param_.min_count)
            {
                begin_index = i + 1;
            }
            else
            {
                break;
            }
        }
        else
        {
            float low = 100.0;
            float high = 0.0;
            for (int j = i - 1; j <= i + 1; j++)
            {
                if (isValid(ranges[j]))
                {
                    if (ranges[j] > 0.03 && ranges[j] < low)
                    {
                        low = ranges[j];
                    }
                    if (ranges[j] > high)
                    {
                        high = ranges[j];
                    }
                }
            }
            if (high - low > 0.08)
            {
                if (i - begin_index < param_.min_count)
                {
                    begin_index = i + 1;
                }
                else
                {
                    break;
                }
            }
        }
    }

    // 倒序检查连续性
    for (int i = end_index - 1; i > begin_index + 1; i--)
    {
        float prev = ranges[i - 1];
        float curr = ranges[i];
        float next = ranges[i + 1];

        if (!isValid(prev) && !isValid(curr) && !isValid(next))
        {
            if (end_index - i < param_.min_count)
            {
                end_index = i - 1;
            }
            else
            {
                break;
            }
        }
        else
        {
            if (isValid(next) && isValid(prev))
            {
                if (next - curr > 0.07)
                {
                    if (end_index - i < param_.min_count)
                    {
                        end_index = i - 1;
                    }
                    else
                    {
                        break;
                    }
                }
            }
        }
    }

    // 设置无效区域
    for (int i = param_.min_index; i < begin_index - 1; i++)
    {
        msg->ranges[i] = std::numeric_limits<float>::infinity();
    }
    for (int i = end_index + 1; i <= param_.max_index; i++)
    {
        msg->ranges[i] = std::numeric_limits<float>::infinity();
    }

    return {begin_index, end_index};
}

// ==================== 坐标转换函数 ====================
std::map<int, std::pair<double, double>> LidarScanController::laserScanToXY(
    const LaserScanSharedPtr &scan_msg)
{
    std::map<int, std::pair<double, double>> xy_points_map;
    double angle = scan_msg->angle_min;

    for (size_t i = 0; i < scan_msg->ranges.size(); i++)
    {
        float range_val = scan_msg->ranges[i];

        // 跳过无效测量值
        if (range_val >= scan_msg->range_min && range_val <= scan_msg->range_max)
        {
            // 极坐标转笛卡尔坐标
            double x1 = range_val * cos(angle);
            double y1 = range_val * sin(angle);
            double x = -y1;
            double y = x1;
            xy_points_map[i] = {x, y};
        }
        angle += scan_msg->angle_increment;
    }
    return xy_points_map;
}

std::map<int, std::pair<double, double>> LidarScanController::laserScanToXY1(
    int begin_index, int end_index,
    const LaserScanSharedPtr &scan_msg)
{
    std::map<int, std::pair<double, double>> xy_points_map;
    double angle = scan_msg->angle_min;

    for (int i = begin_index - 1; i <= end_index; i++)
    {
        float range_val = scan_msg->ranges[i];

        if (range_val >= scan_msg->range_min && range_val <= scan_msg->range_max)
        {
            double x1 = range_val * cos(angle);
            double y1 = range_val * sin(angle);
            double x = -y1;
            double y = x1;
            xy_points_map[i] = {x, y};
        }
        angle += scan_msg->angle_increment;
    }
    return xy_points_map;
}

// ==================== 分段处理函数 ====================
std::vector<std::pair<int, int>> LidarScanController::getSections(
    int begin_index, int end_index,
    const LaserScanSharedPtr &msg,
    const std::map<int, std::pair<double, double>> &xy_list)
{
    std::vector<std::pair<int, int>> sections;

    int section_start = begin_index + 1;
    int section_end = 0;

    int last_index = -1;
    double last_x = 0.0, last_y = 0.0;

    for (int i = begin_index + 1; i < end_index - 1; i++)
    {
        if (xy_list.find(i) == xy_list.end())
        {
            continue;
        }

        auto curr = xy_list.at(i);
        if (last_index < 0)
        {
            last_index = i;
            last_x = curr.first;
            last_y = curr.second;
            continue;
        }

        double dx = curr.first - last_x;
        double dy = curr.second - last_y;
        double dist = sqrt(dx * dx + dy * dy);

        if (dist > 0.05 || (i - last_index) > 2)
        {
            section_end = i;
            if ((section_end - section_start) >= param_.min_count)
            {
                sections.emplace_back(section_start, section_end);
            }
            section_start = i;
        }

        last_index = i;
        last_x = curr.first;
        last_y = curr.second;
    }

    if (last_index > section_end + 10)
    {
        section_end = last_index;
        if ((section_end - section_start) > param_.min_count)
        {
            sections.emplace_back(section_start, section_end);
        }
    }

    return sections;
}

std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
LidarScanController::getSegments(const std::map<int, std::pair<double, double>> &xy_points_map)
{
    std::map<int, std::tuple<int, int, double, double, double, double, double, int>> turn_segments;

    if (xy_points_map.empty())
    {
        return turn_segments;
    }

    // 将点转换为路径
    std::vector<IndexPoint> path;
    for (const auto &point : xy_points_map)
    {
        path.emplace_back(point.second.first, point.second.second, point.first);
    }

    // 使用相同的参数进行分段
    auto segments = RechargeTool::splitPointsToSegments(path, 24.0, 4);

    // 处理每个分段
    for (size_t i = 0; i < segments.size(); i++)
    {
        if (segments[i].size() < 2)
            continue;

        // 获取分段的前两个点
        const auto &s_prev = segments[i][0];
        const auto &s_next = segments[i][1];

        // 计算距离和角度
        double dx = s_prev.x - s_next.x;
        double dy = s_prev.y - s_next.y;
        double distance = sqrt(dx * dx + dy * dy);
        double line_angle = atan2(dy, dx);

        int index_start = s_prev.index;
        int index_end = s_next.index;

        // 应用过滤条件
        if (distance > 0.14 && (index_end - index_start) > 3)
        {
            turn_segments[index_start] = std::make_tuple(
                index_start, // 0: 开始索引
                index_end,   // 1: 结束索引
                0.0,         // 2: 0.0
                s_prev.x,    // 3: x坐标
                s_prev.y,    // 4: y坐标
                distance,    // 5: 距离
                line_angle,  // 6: 线角度
                1            // 7: 1
            );
        }
    }

    return turn_segments;
}

std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
LidarScanController::filterSegments(
    const std::map<int, std::tuple<int, int, double, double, double, double, double, int>> &turn_segments)
{
    std::map<int, std::tuple<int, int, double, double, double, double, double, int>> new_segments;

    if (turn_segments.empty() || turn_segments.size() < 2)
    {
        return new_segments;
    }

    bool last_line_angle_valid = false;
    double last_line_angle = 0.0;
    std::tuple<int, int, double, double, double, double, double, int> last_seg;
    int last_key = -1;

    // 遍历所有分段
    for (const auto &seg : turn_segments)
    {
        const auto &info = seg.second;
        double line_angle = std::get<6>(info);

        if (last_line_angle_valid)
        {
            // 计算角度差
            double diff_angle = line_angle - last_line_angle;

            // 目标角度计算
            double target_angle = 0.5 * M_PI * (120.0 / 90.0);
            target_angle = M_PI - target_angle;
            double tolerance = 0.2;

            // 角度条件判断
            if (diff_angle > (target_angle - tolerance) && diff_angle < (target_angle + tolerance))
            {
                // 索引检查
                int right_start_idx = std::get<0>(info);
                int left_end_idx = std::get<1>(last_seg);

                if (right_start_idx - left_end_idx < 3)
                {
                    // 添加到结果
                    new_segments[last_key] = last_seg;
                    new_segments[seg.first] = info;
                    break; // 找到一对就跳出循环
                }
            }
        }

        // 更新上一个分段信息
        last_line_angle = line_angle;
        last_line_angle_valid = true;
        last_seg = info;
        last_key = seg.first;
    }

    return new_segments;
}

// ==================== 辅助函数实现 ====================
void LidarScanController::publishRechargeVisualization(const RechargeResult &result)
{
    auto marker_array = std::make_shared<visualization_msgs::msg::MarkerArray>();

    // 添加中线标记
    visualization_msgs::msg::Marker line_marker;
    line_marker.header.frame_id = "laser_frame_back1";
    line_marker.header.stamp = node_->now();
    line_marker.ns = "rotating_line";
    line_marker.id = 0;
    line_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
    line_marker.action = visualization_msgs::msg::Marker::ADD;
    line_marker.pose.orientation.w = 1.0;
    line_marker.scale.x = 0.02;
    line_marker.color.r = 1.0;
    line_marker.color.a = 1.0;

    // 计算线段端点
    double k = result.middle_line.k;
    double b = result.middle_line.b;
    double x1 = result.middle_point.x;
    double x2 = x1 - 1.0;

    if (x1 > 10.0 || x1 < -10.0)
    {
        x1 = 0.5;
        x2 = -0.5;
    }

    geometry_msgs::msg::Point p1, p2;
    p1.x = x1;
    p1.y = k * x1 + b;
    p1.z = 0.0;
    p2.x = x2;
    p2.y = k * x2 + b;
    p2.z = 0.0;

    line_marker.points.push_back(p1);
    line_marker.points.push_back(p2);
    marker_array->markers.push_back(line_marker);

    // 添加中间点标记
    visualization_msgs::msg::Marker middle_marker;
    middle_marker.header.frame_id = "laser_frame_back1";
    middle_marker.header.stamp = node_->now();
    middle_marker.ns = "middle_point";
    middle_marker.id = 1;
    middle_marker.type = visualization_msgs::msg::Marker::SPHERE;
    middle_marker.action = visualization_msgs::msg::Marker::ADD;
    middle_marker.pose.orientation.w = 1.0;
    middle_marker.scale.x = 0.03;
    middle_marker.scale.y = 0.03;
    middle_marker.color.b = 1.0;
    middle_marker.color.a = 1.0;

    middle_marker.pose.position.x = result.middle_point.x;
    middle_marker.pose.position.y = result.middle_point.y;
    middle_marker.pose.position.z = 0.0;

    marker_array->markers.push_back(middle_marker);

    // 发布标记数组
    pub_rviz_->publish(*marker_array);
}

Eigen::MatrixXd LidarScanController::getPointList(const std::vector<IndexPoint> &points)
{
    Eigen::MatrixXd pts(points.size(), 2);

    for (size_t i = 0; i < points.size(); i++)
    {
        pts(i, 0) = points[i].x;
        pts(i, 1) = points[i].y;
    }

    return pts;
}

void LidarScanController::publishFilteredScan(
    const LaserScanSharedPtr &original_msg,
    const std::vector<float> &filtered_ranges)
{
    auto msg = std::make_shared<sensor_msgs::msg::LaserScan>();
    msg->header = original_msg->header;
    msg->angle_min = original_msg->angle_min;
    msg->angle_max = original_msg->angle_max;
    msg->angle_increment = original_msg->angle_increment;
    msg->time_increment = original_msg->time_increment;
    msg->scan_time = original_msg->scan_time;
    msg->range_min = original_msg->range_min;
    msg->range_max = original_msg->range_max;
    msg->ranges = filtered_ranges;
    msg->intensities = original_msg->intensities;

    pub_filtered_->publish(*msg);
    RCLCPP_DEBUG(node_->get_logger(), "已发布过滤后的激光数据");
}

LidarScanController::LaserScanSharedPtr LidarScanController::resetLaserData(
    LaserScanSharedPtr msg,
    int b_index, int e_index)
{
    std::vector<float> filtered_ranges = msg->ranges;
    std::vector<float> intensities = msg->intensities;

    for (int i = 0; i < b_index; i++)
    {
        filtered_ranges[i] = std::numeric_limits<float>::infinity();
        intensities[i] = 0.0;
    }

    for (int i = e_index; i < static_cast<int>(msg->ranges.size()); i++)
    {
        filtered_ranges[i] = std::numeric_limits<float>::infinity();
        intensities[i] = 0.0;
    }

    msg->ranges = filtered_ranges;
    msg->intensities = intensities;

    return msg;
}

bool LidarScanController::isValid(float value) const
{
    if (fabs(value) < 0.001)
        return false;
    return !std::isinf(value) && !std::isnan(value);
}

int LidarScanController::getFixedIndex(const LaserScanSharedPtr &msg, int i) const
{
    int fixed = getFixed();
    if (fixed == 0)
        return i;

    int length = msg->ranges.size();
    int index;

    if (i >= length - fixed)
    {
        index = i - length + fixed;
    }
    else
    {
        index = i + fixed;
    }
    return index;
}

void LidarScanController::publishControl()
{
    if (active_)
    {
        pub_ctrl_->publish(ctrl_twist_);
    }
}

void LidarScanController::setChargingVoltage(bool value)
{
    detect_charging_voltage_ = value;
    detect_charging_voltage_time_ = node_->now();
    auto msg = std_msgs::msg::String();
    std::stringstream ss;
    ss << (value ? "CHARGING_VOLTAGE_DETECTED" : "CHARGING_VOLTAGE_LOST")
       << ":timestamp=" << node_->now().seconds()
       << ",voltage_status=" << (value ? "true" : "false");

    if (value)
    {
        ss << ",message=充电电压已检测到，开始充电流程";
        RCLCPP_INFO(node_->get_logger(), "检测到充电电压");
    }
    else
    {
        ss << ",message=充电电压已断开";
        RCLCPP_INFO(node_->get_logger(), "充电电压已断开");
    }
     msg.data = ss.str();
    pub_charging_voltage_->publish(msg);
}

bool LidarScanController::getChargingVoltage() const
{
    auto now = node_->now();
    auto diff = now - detect_charging_voltage_time_;

    // 如果检测到充电电压且在0.1秒内，认为有效
    // if (detect_charging_voltage_ && diff.seconds() < 0.1)
    // {
    //     return true;
    // }
    // return false;
    return detect_charging_voltage_; 
}