#ifndef COARSE_PLANNER_HPP
#define COARSE_PLANNER_HPP

#include "point.hpp"
#include "vector3.hpp"
#include "space_analyzer.hpp"
#include "busbar_config.hpp"
#include <vector>
#include <string>

// 粗规划结果
struct CoarsePlanResult {
    bool success = false;
    std::vector<Point> path;       // 完整体素级路径
    std::vector<Point> waypoints;  // Douglas-Peucker简化航点
    std::string message;
    int voxels_explored = 0;
};

// 体素级粗规划器：1×1巴片（不考虑姿态），考虑防护间隙
class CoarsePlanner {
public:
    CoarsePlanner(const BusbarConfig& config, const SpaceAnalyzer& analyzer);

    CoarsePlanResult plan(const Point& start, const Point& goal);

private:
    const BusbarConfig& config_;
    const SpaceAnalyzer& analyzer_;
    uint64_t xsize_, ysize_, zsize_;

    inline size_t toIndex(int64_t x, int64_t y, int64_t z) const {
        return static_cast<size_t>(x + y * xsize_ + z * xsize_ * ysize_);
    }

    inline bool isPassable(int64_t x, int64_t y, int64_t z) const {
        return analyzer_.isInBounds(x, y, z) && !analyzer_.isObstacle(x, y, z);
    }

    static float heuristic(const Point& a, const Point& b);

    // Douglas-Peucker路径简化
    static std::vector<Point> simplifyPath(const std::vector<Point>& path, float epsilon);
};

#endif