creep-flight-distance-calculation/include/busbar_planner.hpp

#ifndef BUSBAR_PLANNER_HPP
#define BUSBAR_PLANNER_HPP

#include "segment_generator.hpp"
#include "coarse_planner.hpp"
#include <unordered_map>
#include <vector>
#include <memory>
#include <unordered_set>
#include <functional>

// 规划结果
struct BusbarPlanResult {
    bool success = false;
    std::vector<std::shared_ptr<BusbarSegment>> segments;
    std::vector<std::shared_ptr<BusbarSegment>> merged_segments;  // 合并后、简化前的路
    std::vector<std::shared_ptr<BusbarSegment>> raw_segments;     // 原始A*路径（合并前
    float total_cost = 0.0f;
    std::string message;
    int states_explored = 0;       // 实际探索的状态数
    int states_generated = 0;      // 生成的总状态数
    int states_pruned = 0;         // 被剪枝的状态数
    std::vector<Point> coarse_path;      // 粗规划完整体素路
    std::vector<Point> coarse_waypoints; // 粗规划简化航
};

// 通道数据：与地图等大的bool网格，标记哪些体素在通道
struct CorridorData {
    std::vector<bool> grid;   // 与体素地图等
    uint64_t xsize, ysize, zsize;
    bool enabled = false;     // 是否启用通道剪枝

    CorridorData() : xsize(0), ysize(0), zsize(0), enabled(false) {}

    // 检查体素是否在通道
    inline bool isInCorridor(const Point& p) const {
        if (!enabled) return true;  // 未启用时全部通过
        int64_t x = p.x(), y = p.y(), z = p.z();
        if (x < 0 || y < 0 || z < 0 ||
            static_cast<uint64_t>(x) >= xsize ||
            static_cast<uint64_t>(y) >= ysize ||
            static_cast<uint64_t>(z) >= zsize) {
            return false;
        }
        return grid[static_cast<size_t>(x + y * xsize + z * xsize * ysize)];
    }
};

class BusbarPlanner {
private:
    const BusbarConfig& config_;
    const SpaceAnalyzer& analyzer_;
    float heuristic_weight_;

    // 粗路径引导数据（mutable因为在const plan()中设置）
    mutable std::vector<Point> coarse_waypoints_;
    mutable std::vector<float> coarse_cumulative_dist_;
    mutable bool coarse_path_available_ = false;

    // 基于粗路径的启发式：返回沿粗路径的剩余距偏离距离
    float coarsePathHeuristic(const Point& pos) const;

    // 启发式函
    float heuristic(const SegmentEndState& state,
        const SegmentEndState& goal) const;

    // 路径重建
    std::vector<std::shared_ptr<BusbarSegment>> reconstructPath(
        const std::unordered_map<SegmentEndState, SegmentEndState>& parents,
        const std::unordered_map<SegmentEndState, std::shared_ptr<BusbarSegment>>& edges,
        const SegmentEndState& goal) const;

    // 合并连续直段
    std::vector<std::shared_ptr<BusbarSegment>> mergeStraightSegments(
        const SegmentGenerator& generator,
        const std::vector<std::shared_ptr<BusbarSegment>>& segments) const;

    // 检查两段是否可以合
    bool canMergeSegments(const BusbarSegment& seg1,
        const BusbarSegment& seg2) const;

    // 后处理拉直：将连续的小弯+直段序列替换为单根长直段
    std::vector<std::shared_ptr<BusbarSegment>> straightenPath(
        const SegmentGenerator& generator,
        const std::vector<std::shared_ptr<BusbarSegment>>& segments) const;

    // 从原始路径做两节点拉直：同法向同高度时尝试替换为 直段+立弯+直段
    std::vector<std::shared_ptr<BusbarSegment>> straightenRawPath(
        const SegmentGenerator& generator,
        const std::vector<std::shared_ptr<BusbarSegment>>& segments) const;

    // 生成所有可能的初始方向
    std::vector<SegmentCandidate> generateInitialCandidates(
        const SegmentGenerator& generator,
        const Point& start_pos, const Vector3& start_normal,
        const Point& goal_pos,
        const Vector3& start_tangent) const;

    // 生成初始直段候
    std::vector<SegmentCandidate> generateInitialStraightCandidates(
        const SegmentGenerator& generator,
        const Point& start_pos, const Vector3& start_normal,
        const Vector3& start_dir) const;

    // 检查是否可以用直段直达目标
    std::shared_ptr<BusbarSegment> tryDirectConnection(
        const SegmentGenerator& generator,
        const SegmentEndState& current,
        const Point& goal_pos, const Vector3& goal_normal,
        const Vector3& goal_tangent) const;

    // 尝试通过FLAT_BEND+直段到达终点(逆向推导)
    std::pair<std::shared_ptr<BusbarSegment>, std::shared_ptr<BusbarSegment>> tryFlatBendPlusDirectConnection(
        const SegmentGenerator& generator,
        const SegmentEndState& current,
        const Point& goal_pos, const Vector3& goal_normal,
        const Vector3& goal_tangent) const;

    // 尝试通过VERTICAL_BEND+直段到达终点(逆向推导，法向对齐但切向不对
    std::pair<std::shared_ptr<BusbarSegment>, std::shared_ptr<BusbarSegment>> tryVerticalBendPlusDirectConnection(
        const SegmentGenerator& generator,
        const SegmentEndState& current,
        const Point& goal_pos, const Vector3& goal_normal,
        const Vector3& goal_tangent) const;

    // 内部规划函数：规划两点之间的路径（用于分层规划）
    BusbarPlanResult planSegment(
        const Point& start_pos, const Vector3& start_normal, const Vector3& start_tangent,
        const Point& goal_pos, const Vector3& goal_normal, const Vector3& goal_tangent,
        float heuristic_weight, int max_iterations) const;

    // 从体素路径构建通道
    CorridorData buildCorridor(const std::vector<Point>& voxel_path, int corridor_radius) const;

    std::function<void(const std::string&)> log_callback_;

public:
    BusbarPlanner(const BusbarConfig& config, const SpaceAnalyzer& analyzer, float heuristic_weight = 3.0f)
        : config_(config), analyzer_(analyzer), heuristic_weight_(heuristic_weight), log_callback_(nullptr) {}

    void setLogCallback(std::function<void(const std::string&)> callback) {
        log_callback_ = callback;
    }

    // 必经区域结构（AABB包围盒）
    struct MandatoryRegion {
        Point min_point;  // 区域最小坐
        Point max_point;  // 区域最大坐
        bool enabled;     // 是否启用

        MandatoryRegion() : min_point(0, 0, 0), max_point(0, 0, 0), enabled(false) {}
        MandatoryRegion(const Point& min_p, const Point& max_p)
            : min_point(min_p), max_point(max_p), enabled(true) {}

        // 检查点是否在区域内
        bool contains(const Point& p) const {
            if (!enabled) return true;  // 未启用时，视为所有点都在"区域
            return p.x() >= min_point.x() && p.x() <= max_point.x() &&
                   p.y() >= min_point.y() && p.y() <= max_point.y() &&
                   p.z() >= min_point.z() && p.z() <= max_point.z();
        }

        // 获取区域中心
        Point center() const {
            return Point(
                (min_point.x() + max_point.x()) / 2.0f,
                (min_point.y() + max_point.y()) / 2.0f,
                (min_point.z() + max_point.z()) / 2.0f
            );
        }
    };

    BusbarPlanResult plan(const Point& start_pos, const Vector3& start_normal,
        const Point& goal_pos, const Vector3& goal_normal,
        const Vector3& start_tangent = Vector3(0, 0, 0),
        const Vector3& goal_tangent = Vector3(0, 0, 0),
        const MandatoryRegion& mandatory_region = MandatoryRegion(),
        float max_time_s = 0,
        bool disable_coarse = false) const;

private:
    void log(const std::string& msg) const {
        if (log_callback_) {
            log_callback_(msg);
        }
    }
};

#endif