#include "recharge/recharge.hpp"
#include <rclcpp/executors/multi_threaded_executor.hpp>
#include <rclcpp/qos.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <chrono>
#include <functional>
#include <thread>
#define DEBUG_SCAN 1

using std::placeholders::_1;

// 全局变量
static std::string last_mode = "";
static bool detect_charging_voltage = false;
static rclcpp::Time detect_charging_voltage_time;

// 全局函数实现
bool isCharging()
{
    return last_mode == "recharge";
}

// 在全局函数部分添加
std::pair<double, double> calculateAngleBisector(double k1, double b1, double k2, double b2)
{
    // 计算两条直线的夹角
    double angle1 = atan(k1);
    double angle2 = atan(k2);

    // 计算两条直线之间的夹角
    double delta_angle = fabs(angle1 - angle2);

    // 计算中线角度
    double bisector_angle1;
    if (angle1 < angle2)
    {
        bisector_angle1 = angle1 + delta_angle / 2;
    }
    else
    {
        bisector_angle1 = angle2 + delta_angle / 2;
    }

    // 处理平行线（无交点）的情况
    if (fabs(k1 - k2) < 1e-10)
    {
        // 平行线没有交点，返回原直线
        return {k1, b1};
    }

    // 计算交点坐标
    double x_intersect = (b2 - b1) / (k1 - k2);
    double y_intersect = k1 * x_intersect + b1;

    // 计算平分线的斜率
    double k3 = tan(bisector_angle1);
    double b3 = y_intersect - k3 * x_intersect;

    return {k3, b3};
}

void setChargingVoltage(bool value)
{
    detect_charging_voltage = value;
    detect_charging_voltage_time = rclcpp::Clock().now();
}

bool getChargingVoltage()
{
    auto now = rclcpp::Clock().now();
    auto diff = now - detect_charging_voltage_time;
    if (detect_charging_voltage && diff.seconds() > 0.1)
    {
        return true;
    }
    return false;
}

void setMode(const std::string &mode)
{
    last_mode = mode;
    if (last_mode != "recharge")
    {
        setChargingVoltage(false);
    }
}

std::string getMode()
{
    return last_mode;
}

// ToolClass 实现
double ToolClass::angleBetween(const IndexPoint &p1, const IndexPoint &p2)
{
    return atan2(p2.y - p1.y, p2.x - p1.x);
}

double ToolClass::angleDiff(double a1, double a2)
{
    double diff = fabs(a1 - a2);
    return std::min(diff, 2 * M_PI - diff) * 180.0 / M_PI;
}

double ToolClass::averageAngle(const std::vector<double> &angles)
{
    double sin_sum = 0.0, cos_sum = 0.0;
    for (double a : angles)
    {
        sin_sum += sin(a);
        cos_sum += cos(a);
    }
    return atan2(sin_sum, cos_sum);
}

std::vector<std::vector<IndexPoint>> ToolClass::splitPointsToSegments(
    const std::vector<IndexPoint> &points,
    double angle_threshold_deg,
    int history_window_size)
{
    std::vector<std::vector<IndexPoint>> segments;
    if (points.size() < 2)
    {
        segments.push_back(points);
        return segments;
    }

    std::vector<IndexPoint> current_segment;
    std::deque<double> angle_history;  // 改为 deque 方便操作

    current_segment.push_back(points[0]);

    for (size_t i = 1; i < points.size(); i++)
    {
        const IndexPoint &p_prev = points[i - 1];
        const IndexPoint &p_curr = points[i];
        double curr_angle = angleBetween(p_prev, p_curr);

        // 更新方向历史
        angle_history.push_back(curr_angle);
        if (angle_history.size() > history_window_size)
        {
            angle_history.pop_front();
        }

        // 若历史方向足够，开始判断角度差
        if (angle_history.size() >= history_window_size)
        {
            // 计算历史方向平均值（排除当前角度）
            std::vector<double> hist(angle_history.begin(), angle_history.end() - 1);
            if (hist.empty())
                continue;

            double avg_angle = averageAngle(hist);
            double diff = angleDiff(avg_angle, curr_angle);

            if (diff > angle_threshold_deg)
            {
                // 发生显著拐弯，分段
                // 将 p_prev 加入当前分段
                current_segment.push_back(p_prev);
                segments.push_back(current_segment);
                
                // 开始新分段，以当前点为起点
                current_segment.clear();
                current_segment.push_back(p_curr);
                
                // 重置方向历史，仅保留当前角度
                angle_history.clear();
                angle_history.push_back(curr_angle);
                continue;  // 跳过下面的 current_segment.push_back(p_curr);
            }
        }

        // 正常情况：将当前点加入当前分段
        current_segment.push_back(p_curr);
    }

    // 处理最后一个分段（将剩余的点加入结果）
    if (!current_segment.empty())
    {
        segments.push_back(current_segment);
    }

    return segments;
}

// ScanFilterNode 实现
ScanFilterNode::ScanFilterNode() : Node("scan_filter_node")
{
    // 声明参数
    this->declare_parameter<double>("distance_threshold", 0.5);
    this->declare_parameter<int>("fit_window_size", 10);
    this->declare_parameter<double>("fit_residual_threshold", 0.09);

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

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

    // 创建订阅者和发布者
    scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
        "/scan/back", qos_profile,
        std::bind(&ScanFilterNode::scanCallback, this, _1));

    pub_filtered_ = this->create_publisher<sensor_msgs::msg::LaserScan>("/scan/out", 10);
    pub_line_ = this->create_publisher<sensor_msgs::msg::LaserScan>("/scan/out_line", 10);
    pub_ctrl_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_raw", 10);

    sub_cmd_enable_ = this->create_subscription<std_msgs::msg::Bool>(
        "/recharge/cmd_enable", 10,
        std::bind(&ScanFilterNode::onCmdEnable, this, _1));

    pub_rviz_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
        "/line_marker_array", 10);

    // 初始化控制消息
    ctrl_twist_.linear.x = 0.0;
    ctrl_twist_.angular.z = 0.0;
    ctrl_twist_time_ = this->now();

    // 创建定时器
    timer_ = this->create_wall_timer(
        100ms, std::bind(&ScanFilterNode::timerCallback, this));

    detect_station_time_ = this->now() - rclcpp::Duration(100.0, 0);
}

Eigen::MatrixXd ScanFilterNode::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 ScanFilterNode::procData(Result &result)
{
    RCLCPP_INFO(get_logger(), "处理数据中");
    if (result.left_points.size() < 2 || result.right_points.size() < 2)
    {
        return;
    }

    // 获取点集矩阵
    Eigen::MatrixXd point_list_left = getPointList(result.left_points);
    Eigen::MatrixXd point_list_right = getPointList(result.right_points);

    // 拟合左边直线
    auto [k1, b1] = fitLineLeastSquares(point_list_left);

    // 拟合右边直线
    auto [k2, b2] = fitLineLeastSquares(point_list_right);

    // 计算交点
    if (fabs(k1 - k2) > 1e-10)
    {
        double x = (b2 - b1) / (k1 - k2);
        double y = k1 * x + b1;
        result.middle_point = IndexPoint(x, y, 0);
    }
    else
    {
        // 平行线情况，取平均值
        result.middle_point = IndexPoint(0.0, 0.0, 0);
    }

    // 计算角平分线
    auto [k3, b3] = calculateAngleBisector(k1, b1, k2, b2);

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

void ScanFilterNode::ctrlProc(const std::vector<IndexPoint> &left_points,
                              const std::vector<IndexPoint> &right_points)
{
    RCLCPP_INFO(get_logger(), "进入控制模块");
    if (getChargingVoltage())
    {
        ctrl_twist_.linear.x = 0.0;
        ctrl_twist_.angular.z = 0.0;
        ctrl_twist_time_ = this->now();
        return;
    }

    // 创建临时Result对象
    Result result;
    result.frameid = frame_index_;
    result.left_points = left_points;
    result.right_points = right_points;

    // 处理数据（拟合直线、计算交点等）
    procData(result);
    RCLCPP_INFO(get_logger(), "数据处理完成");
    double dx = result.middle_point.x;
    double dy = result.middle_point.y;
    double k = result.middle_line.k;

    // 调整dy
    dy = dy - 0.10;

    // 计算偏航角误差
    double yaw_error = atan(k);

    // 计算控制速度
    auto [linear_velocity, angular_velocity] = computeDockingVelocity(dx - 0.3, dy, yaw_error);

    // 限制角速度
    if (angular_velocity > 0.0)
    {
        angular_velocity = std::min(fabs(angular_velocity), 0.09);
    }
    else
    {
        angular_velocity = -std::min(fabs(angular_velocity), 0.09);
    }

    // 更新检测时间
    detect_station_time_ = this->now();

    // 根据距离调整速度系数
    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_INFO(get_logger(), "设置控制命令");
    // 设置控制命令
    if (dx > 0.54)
    {
        ctrl_twist_.linear.x = std::min(linear_velocity * radio_x, -0.04);
        ctrl_twist_.angular.z = angular_velocity * radio_z;
        ctrl_twist_time_ = this->now();
    }
    else
    {
        // 到达充电位置
        ctrl_twist_.linear.x = 0.0;
        ctrl_twist_.angular.z = 0.0;
        ctrl_twist_time_ = this->now();
    }
    frame_index_++;
}

std::vector<std::pair<int, int>> ScanFilterNode::getSections(
    int begin_index, int end_index,
    const sensor_msgs::msg::LaserScan::SharedPtr 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>> 
ScanFilterNode::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);
    }

    // 使用ToolClass进行分段
    auto segments = ToolClass::splitPointsToSegments(path, 10.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_next.x - s_prev.x;
        double dy = s_next.y - s_prev.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;

        double value_x = s_prev.x;
        double value_y = s_prev.y;

        if (distance > 0.14 && (index_end - index_start) > 3)
        {
            // 添加一个额外的 int 参数（这里用0作为占位符）
            turn_segments[index_start] = std::make_tuple(
                index_start, index_end, 0.0, value_x, value_y,
                distance, line_angle, 0);  // 添加额外的 int 参数
        }
    }
    
    return turn_segments;
}

std::map<int, std::tuple<int, int, double, double, double, double, double, int>>
ScanFilterNode::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;
    RCLCPP_INFO(get_logger(), "提取分段处理中");
    if (turn_segments.empty())
    {
        return new_segments;
    }

    double last_line_angle = 0.0;
    double last_distance = 0.0;
    std::tuple<int, int, double, double, double, double, double, int> last_seg;

    std::tuple<int, int, double, double, double, double, double, int> left_seg;
    std::tuple<int, int, double, double, double, double, double, int> right_seg;
    bool left_found = false, right_found = false;

    for (const auto &seg : turn_segments)
    {
        const auto &info = seg.second;
        double distance = std::get<5>(info);
        double line_angle = std::get<6>(info);

        if (last_line_angle != 0.0)
        {
            double diff_angle = line_angle - last_line_angle;

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

            if (diff_angle > (target_angle - 0.2) && diff_angle < (target_angle + 0.2))
            {
                left_seg = last_seg;
                right_seg = seg.second;

                if (std::get<0>(right_seg) - std::get<1>(left_seg) < 3)
                {
                    new_segments[seg.first] = seg.second;
                    new_segments[seg.first - 1] = last_seg;
                    break;
                }
            }
        }

        last_line_angle = line_angle;
        last_distance = distance;
        last_seg = seg.second;
    }

    return new_segments;
}

void ScanFilterNode::onPointPartition(const Result &result)
{
    RCLCPP_INFO(get_logger(), "开始处理点分区");
    if (result.left_points.size() == 0 || result.right_points.size() == 0)
    {
        RCLCPP_ERROR(get_logger(), "左右两侧没有两个以上的点模拟线");
        RCLCPP_ERROR(get_logger(), "左侧点数: %zu, 右侧点数: %zu",
                     result.left_points.size(), result.right_points.size());
        return;
    }

    // 调用控制处理
    ctrlProc(result.left_points, result.right_points);

    // 显示结果（RViz可视化）
    rvizShowResult(result);
    RCLCPP_INFO(get_logger(), "已发布可视化标记");
}

void ScanFilterNode::rvizShowResult(const Result &result)
{
    RCLCPP_INFO(get_logger(), "发布可视化标记 - 坐标系: laser_frame_back1");
    RCLCPP_INFO(get_logger(), "中间点坐标: x=%.3f, y=%.3f",
                result.middle_point.x, result.middle_point.y);
    RCLCPP_INFO(get_logger(), "中线参数: k=%.3f, b=%.3f",
                result.middle_line.k, result.middle_line.b);
    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;
    }
    double y1 = result.middle_line.k * x1 + result.middle_line.b;
    double y2 = result.middle_line.k * x2 + result.middle_line.b;
    RCLCPP_INFO(get_logger(), "线段端点: (%.3f, %.3f) -> (%.3f, %.3f)",
                x1, y1, x2, y2);
    // 创建标记数组
    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 = this->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;

    geometry_msgs::msg::Point p1, p2;
    p1.x = x1;
    p1.y = y1;
    p1.z = 0.0;
    p2.x = x2;
    p2.y = y2;
    p2.z = 0.0;
    line_marker.points.push_back(p1);
    line_marker.points.push_back(p2);

    marker_array->markers.push_back(line_marker);

    // 添加左点标记
    if (!result.left_points.empty())
    {
        visualization_msgs::msg::Marker left_marker;
        left_marker.header.frame_id = "laser_frame_back1";
        left_marker.header.stamp = this->now();
        left_marker.ns = "left_point";
        left_marker.id = 1;
        left_marker.type = visualization_msgs::msg::Marker::SPHERE;
        left_marker.action = visualization_msgs::msg::Marker::ADD;
        left_marker.pose.orientation.w = 1.0;
        left_marker.scale.x = 0.03;
        left_marker.scale.y = 0.03;
        left_marker.color.r = 1.0;
        left_marker.color.a = 1.0;

        left_marker.pose.position.x = result.left_points[0].x;
        left_marker.pose.position.y = result.left_points[0].y;
        left_marker.pose.position.z = 0.0;

        marker_array->markers.push_back(left_marker);
    }

    // 添加右点标记
    if (!result.right_points.empty())
    {
        visualization_msgs::msg::Marker right_marker;
        right_marker.header.frame_id = "laser_frame_back1";
        right_marker.header.stamp = this->now();
        right_marker.ns = "right_point";
        right_marker.id = 2;
        right_marker.type = visualization_msgs::msg::Marker::SPHERE;
        right_marker.action = visualization_msgs::msg::Marker::ADD;
        right_marker.pose.orientation.w = 1.0;
        right_marker.scale.x = 0.03;
        right_marker.scale.y = 0.03;
        right_marker.color.g = 1.0;
        right_marker.color.a = 1.0;

        right_marker.pose.position.x = result.right_points.back().x;
        right_marker.pose.position.y = result.right_points.back().y;
        right_marker.pose.position.z = 0.0;

        marker_array->markers.push_back(right_marker);
    }

    // 添加中间点标记
    visualization_msgs::msg::Marker middle_marker;
    middle_marker.header.frame_id = "laser_frame_back1";
    middle_marker.header.stamp = this->now();
    middle_marker.ns = "middle_point";
    middle_marker.id = 3;
    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);
}

void ScanFilterNode::onCmdEnable(const std_msgs::msg::Bool::SharedPtr msg)
{
    cmd_enable_ = msg->data;
}

void ScanFilterNode::timerCallback()
{
    if (!isCharging())
        return;

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

    if (time_diff < 2.0)
    {
        double radio_x = 1.0;
        double radio_z = 1.0;
        geometry_msgs::msg::Twist msg;
        msg.linear.x = ctrl_twist_.linear.x * radio_x;
        msg.angular.z = ctrl_twist_.angular.z * radio_z;

        if (cmd_enable_)
        {
            pub_ctrl_->publish(msg);
        }
    }
}

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

int ScanFilterNode::getFixed()
{
    return 0;
}

int ScanFilterNode::getFixedIndex(const sensor_msgs::msg::LaserScan::SharedPtr msg, int i)
{
    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 ScanFilterNode::filterScanModify0(sensor_msgs::msg::LaserScan::SharedPtr msg)
{
#if DEBUG_SCAN
    RCLCPP_INFO(get_logger(), "已进入角度偏移校正过滤");
#endif
    int fixed = getFixed();
    double diff_yaw = msg->angle_increment * fixed;
    msg->angle_min -= diff_yaw;
    msg->angle_max -= diff_yaw;
    RCLCPP_INFO(get_logger(), "已进入角度偏移校正过滤:%.2lf", diff_yaw);
    RCLCPP_INFO(get_logger(), "已进入角度偏移校正过滤:%.2lf", msg->angle_min);
    RCLCPP_INFO(get_logger(), "已进入角度偏移校正过滤:%.2lf", msg->angle_max);
    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 ScanFilterNode::filterScanModify1(sensor_msgs::msg::LaserScan::SharedPtr msg)
{
#if DEBUG_SCAN
    RCLCPP_INFO(get_logger(), "已进入角度范围过滤");
#endif
    // 过滤角度，只接受后面90度
    int min_index = 0;

    int max_index = 180;
    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 ScanFilterNode::filterScanModify2(sensor_msgs::msg::LaserScan::SharedPtr msg)
{
    RCLCPP_INFO(get_logger(), "已进入距离过滤");
    // 过滤距离
    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 ScanFilterNode::filterScanModify3(sensor_msgs::msg::LaserScan::SharedPtr msg)
{
#if DEBUG_SCAN
    RCLCPP_INFO(get_logger(), "已进入反射强度过滤");
#endif
    // 过滤强度
    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> ScanFilterNode::filterScanModify4(sensor_msgs::msg::LaserScan::SharedPtr msg)
{
#if DEBUG_SCAN
    RCLCPP_INFO(get_logger(), "已进入点的连续性过滤");
#endif
    // 过滤连续性
    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 (msg->ranges.size() - begin_index < 10)
    {
        RCLCPP_ERROR(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; 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>> ScanFilterNode::laserScanToXY(
    const sensor_msgs::msg::LaserScan::SharedPtr 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>> ScanFilterNode::laserScanToXY1(
    int begin_index, int end_index,
    const sensor_msgs::msg::LaserScan::SharedPtr scan_msg)
{
    RCLCPP_INFO(get_logger(), "进入xy坐标系转换");
    std::map<int, std::pair<double, double>> xy_points_map;
    double angle = scan_msg->angle_min + begin_index * scan_msg->angle_increment;

    for (int i = begin_index; 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::pair<double, double> ScanFilterNode::fitLineLeastSquares(const Eigen::MatrixXd &points)
{
    int n = points.rows();
    Eigen::VectorXd x = points.col(0);
    Eigen::VectorXd y = points.col(1);

    // 最小二乘法拟合 y = kx + b
    double x_mean = x.mean();
    double y_mean = y.mean();

    double numerator = 0.0;
    double denominator = 0.0;

    for (int i = 0; i < n; i++)
    {
        numerator += (x(i) - x_mean) * (y(i) - y_mean);
        denominator += (x(i) - x_mean) * (x(i) - x_mean);
    }

    double k = numerator / denominator;
    double b = y_mean - k * x_mean;

    return {k, b};
}

std::pair<double, double> ScanFilterNode::computeDockingVelocity(double dx, double dy, double yaw_error)
{
    double max_linear_speed = 0.12;
    double max_angular_speed = 0.16;

    double Kp_linear = 0.5;
    double Kp_angular = 0.5;
    double Kp_lateral = 0.6;

    // 线速度控制
    double linear_velocity = Kp_linear * std::max(0.1, dx - 0.6);
    linear_velocity = std::clamp(linear_velocity, -max_linear_speed, max_linear_speed);
    linear_velocity = std::max(0.08, linear_velocity);

    // 角速度控制
    double angular_velocity = Kp_angular * yaw_error + Kp_lateral * dy;
    angular_velocity = std::clamp(angular_velocity, -max_angular_speed, max_angular_speed);

    return {-linear_velocity, -angular_velocity};
}

void ScanFilterNode::scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
#if DEBUG_SCAN
    RCLCPP_INFO(get_logger(), "已进入scan回调函数");
#else
    if (!isCharging())
        return;
#endif
    // 应用各种过滤器
    filterScanModify0(msg);
    filterScanModify1(msg);
    filterScanModify2(msg);
    filterScanModify3(msg);

    auto [begin_index, end_index] = filterScanModify4(msg);
    RCLCPP_INFO(get_logger(), "begin:%d,end:%d", begin_index, end_index);
    if (end_index - begin_index < 2)
    {
        RCLCPP_INFO(get_logger(), "没有可连续性的点");
        return;
    }
    // 发布过滤后的数据
    publishFilteredScan(msg, msg->ranges);
    // 进一步处理激光数据
    scanCallback1(msg, begin_index, end_index);
}

void ScanFilterNode::scanCallback1(
    const sensor_msgs::msg::LaserScan::SharedPtr msg,
    int begin_index, int end_index)
{
#if DEBUG_SCAN
    RCLCPP_INFO(get_logger(), "进一步处理过滤数据");
#else
    if (!isCharging())
        return;
#endif

    // 检查索引有效性
    if (begin_index < 0 || end_index >= static_cast<int>(msg->ranges.size()) || begin_index >= end_index)
    {
        return;
    }

    // 根据距离调整结束索引
    if (end_index > 0 && end_index < static_cast<int>(msg->ranges.size()) && msg->ranges[end_index - 1] > 0.9)
    {
        int count = end_index - begin_index;
        end_index = end_index - static_cast<int>(count * 0.16);
    }

    // 转换到XY坐标系
    auto xy_list = laserScanToXY1(begin_index, end_index, msg);
    if (xy_list.size() < 2)
    {
        RCLCPP_ERROR(get_logger(), "xy坐标系没有转换成功");
        return;
    }
    RCLCPP_INFO(get_logger(), "xy坐标系成功转换");
    RCLCPP_INFO(get_logger(), "XY转换结果: %zu 个点", xy_list.size());
    // 获取分段
    auto turn_segments = getSegments(xy_list);
    // 在scanCallback1中，获取分段后添加：
    RCLCPP_INFO(get_logger(), "原始分段数: %zu", turn_segments.size());

    // 打印每个分段的信息
    for (const auto &seg : turn_segments)
    {
        const auto &info = seg.second;
        int start_idx = std::get<0>(info);
        int end_idx = std::get<1>(info);
        double distance = std::get<5>(info);
        double angle = std::get<6>(info);

        RCLCPP_INFO(get_logger(), "分段 %d: 点数=%d, 距离=%.3f, 角度=%.3f度",
                    seg.first, end_idx - start_idx + 1, distance, angle * 180.0 / M_PI);
    }
    turn_segments = filterSegments(turn_segments);
    RCLCPP_INFO(get_logger(), "提取分段成功");
    RCLCPP_INFO(get_logger(), "过滤后分段数: %zu", turn_segments.size());
    // 创建结果对象
    Result result;
    result.frameid = frame_index_;
    result.laserScanData = msg;

    // 处理每个分段
    int segment_index = 0;
    for (const auto &seg : turn_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.find(index) == xy_list.end())
                continue;

            double x = xy_list[index].first;
            double y = xy_list[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++;
    }

    // 处理点分区
    onPointPartition(result);
}

sensor_msgs::msg::LaserScan::SharedPtr ScanFilterNode::resetLaserData(
    sensor_msgs::msg::LaserScan::SharedPtr 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;
}

void ScanFilterNode::publishFilteredScan(
    const sensor_msgs::msg::LaserScan::SharedPtr 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_INFO(get_logger(), "已成功发布过滤后的数据");
}

// ChargingManager 实现
ChargingManager::ChargingManager() : Node("scan_filter")
{
    sub_ctrl_ = this->create_subscription<std_msgs::msg::String>(
        "/recharge/ctrl_1", 10,
        std::bind(&ChargingManager::onCtrl, this, _1));

    state_ = ChargingState::IDLE;
    last_station_detection_time_ = this->now();
    last_voltage_detection_time_ = this->now();
    setChargingVoltage(false);
}

bool ChargingManager::detectStation()
{
    // 这里需要实现基站检测逻辑
    // 暂时使用简单实现
    auto now = this->now();
    // 假设有一个全局的 ScanFilterNode 实例
    // 在实际中，需要通过节点间通信获取这个信息
    return false; // 需要根据实际情况实现
}

void ChargingManager::onCtrl(const std_msgs::msg::String::SharedPtr msg)
{
    if (msg->data == "start")
    {
        setChargingVoltage(false);
        startCharging();
    }
    else if (msg->data == "stop")
    {
        stopCharging();
    }
}

void ChargingManager::startCharging()
{
    if (state_ != ChargingState::IDLE)
    {
        RCLCPP_WARN(this->get_logger(), "当前状态无法启动回充");
        return;
    }

    changeState(ChargingState::STARTING);
    last_station_detection_time_ = this->now();

    // 检测基站
    bool station_detected = false;
    for (int i = 0; i < 80; i++)
    {
        if (detectStation())
        {
            station_detected = true;
            break;
        }
        rclcpp::sleep_for(100ms);
    }

    if (!station_detected)
    {
        changeState(ChargingState::UNAVAILABLE);
        return;
    }

    // 进入回充控制流程
    executeChargingControl();
    changeState(ChargingState::CHARGING);
}

void ChargingManager::stopCharging()
{
    if (state_ == ChargingState::IDLE)
    {
        RCLCPP_WARN(this->get_logger(), "当前状态无法停止回充");
        return;
    }
    changeState(ChargingState::IDLE);
}

void ChargingManager::executeChargingControl()
{
    changeState(ChargingState::NAVIGATING);
    last_station_detection_time_ = this->now();

    while (state_ == ChargingState::NAVIGATING)
    {
        // 检测基站
        if (detectStation())
        {
            last_station_detection_time_ = this->now();
        }
        else
        {
            auto now = this->now();
            auto diff = (now - last_station_detection_time_).seconds();
            if (diff > station_lost_timeout_)
            {
                changeState(ChargingState::UNAVAILABLE);
                return;
            }
        }

        // 检测充电电压
        if (detectChargingVoltage())
        {
            changeState(ChargingState::CHARGING);
            monitorCharging();
            return;
        }

        // 生成并应用控制命令
        auto [linear_vel, angular_vel] = generateControlCommand();
        applyControlCommand(linear_vel, angular_vel);

        rclcpp::sleep_for(std::chrono::milliseconds(int(control_interval_ * 1000)));
    }
}

void ChargingManager::monitorCharging()
{
    last_voltage_detection_time_ = this->now();

    while (state_ == ChargingState::CHARGING)
    {
        if (!detectChargingVoltage())
        {
            auto now = this->now();
            auto diff = (now - last_voltage_detection_time_).seconds();
            if (diff > station_lost_timeout_)
            {
                changeState(ChargingState::COMPLETED);
                return;
            }
        }
        else
        {
            last_voltage_detection_time_ = this->now();
        }
        rclcpp::sleep_for(std::chrono::milliseconds(int(control_interval_ * 1000)));
    }
}

void ChargingManager::changeState(ChargingState new_state)
{
    state_ = new_state;
    reportState(new_state);
}

void ChargingManager::reportState(ChargingState state)
{
    std::string state_str;
    int other_status = 0;

    switch (state)
    {
    case ChargingState::IDLE:
        state_str = "空闲";
        other_status = 5; // 注意：需要根据实际情况调整
        break;
    case ChargingState::STARTING:
        state_str = "启动中";
        other_status = 1;
        break;
    case ChargingState::NAVIGATING:
        state_str = "回充行驶";
        other_status = 3;
        break;
    case ChargingState::CHARGING:
        state_str = "充电中";
        other_status = 5;
        break;
    case ChargingState::UNAVAILABLE:
        state_str = "回充不可用";
        other_status = 2;
        break;
    case ChargingState::COMPLETED:
        state_str = "充电结束";
        other_status = 5;
        break;
    }

    RCLCPP_INFO(this->get_logger(), "状态上报: %s", state_str.c_str());
}

bool ChargingManager::detectChargingVoltage()
{
    return getChargingVoltage();
}

std::pair<double, double> ChargingManager::generateControlCommand()
{
    // 这里需要实现控制命令生成逻辑
    return {0.2, 0.0};
}

void ChargingManager::applyControlCommand(double linear_vel, double angular_vel)
{
    // 应用控制命令
    // 发布到控制主题
    auto msg = geometry_msgs::msg::Twist();
    msg.linear.x = linear_vel;
    msg.angular.z = angular_vel;

    // 需要有一个发布者，或者通过其他方式传递控制命令
    // 例如: pub_ctrl_->publish(msg);
}

RechargerRecv::RechargerRecv() : Node("RechargerRecv")
{
    setChargingVoltage(false);

    last_report_status_ = -1;
    last_status_ = 1;
    running_ = false;

    // 创建发布者和订阅者
    pub_feed_ = this->create_publisher<std_msgs::msg::Int32>("/recharge/feed", 10);
    client_ = this->create_client<interface::srv::SceneControls>("/feed/recharge/ctrl");

    getcharging_sub_ = this->create_subscription<std_msgs::msg::Int32>(
        "/sensor/getcharging", 10,
        std::bind(&RechargerRecv::onGetCharging, this, _1));

    sub_mode_change_ = this->create_subscription<std_msgs::msg::String>(
        "/scene_services/mode", 10,
        std::bind(&RechargerRecv::onModeChange, this, _1));

    // 创建定时器
    auto timer_callback = [this]() -> void
    {
        this->reportLoop();
    };

    auto report_timer = this->create_wall_timer(
        std::chrono::seconds(2), timer_callback);

    // 启动线程
    thread_ = std::thread(&RechargerRecv::reportLoop, this);
}

RechargerRecv::~RechargerRecv()
{
    running_ = false;
    if (thread_.joinable())
    {
        thread_.join();
    }
}

void RechargerRecv::onGetCharging(const std_msgs::msg::Int32::SharedPtr msg)
{
    int status = msg->data;
    bool local_detect_charging_voltage = false;

    if (status == 0)
    {
        RCLCPP_INFO(this->get_logger(), "设备接触到充电区");
        local_detect_charging_voltage = true;
    }
    else
    {
        local_detect_charging_voltage = false;
    }

    setChargingVoltage(local_detect_charging_voltage);
}

void RechargerRecv::onModeChange(const std_msgs::msg::String::SharedPtr msg)
{
    std::string mode = msg->data;
    std::string last_mode_str = last_mode;

    if (last_mode_str != mode)
    {
        setMode(mode); // 需要添加这个函数
        RCLCPP_INFO(this->get_logger(), "新模式: %s", mode.c_str());
    }
}

void RechargerRecv::reportLoop()
{
    while (running_)
    {
        if (!isCharging())
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            continue;
        }

        // 报告状态
        if (last_report_status_ != last_status_)
        {
            callService(last_status_);
            last_report_status_ = last_status_;
        }

        std::this_thread::sleep_for(std::chrono::seconds(2));
    }
}

void RechargerRecv::callService(int start)
{
    // 发布到主题
    auto msg = std_msgs::msg::Int32();
    msg.data = start;

    RCLCPP_INFO(this->get_logger(), "发布回充反馈: %d", start);
    if (start != 3)
    {
        pub_feed_->publish(msg);
    }

    // 或者调用服务（如果使用服务）
    // auto request = std::make_shared<interface::srv::SceneControls::Request>();
    // request->start = start;
    // auto future = client_->async_send_request(request);
}

void RechargerRecv::changeStatus(int status)
{
    if (last_status_ != status)
    {
        RCLCPP_INFO(this->get_logger(), "回充状态变更 %d --> %d", last_status_, status);
        callService(status);
        last_status_ = status;
    }
}
