#ifndef SPACE_ANALYZER_HPP
#define SPACE_ANALYZER_HPP

#include "busbar_config.hpp"
#include <vector>
#include <cstdint>
#include <cmath>
#include <unordered_set>

class SpaceAnalyzer {
private:
    const BusbarConfig& config_;
    const std::vector<bool>& obstacles_;           // 原有障碍物 (flag=0)
    const std::vector<bool>& collision_voxels_;    // 会碰撞的空闲体素 (flag=1)
    uint64_t xsize_, ysize_, zsize_;

    // 预计算的检测半径，避免每次hasSpace调用时重复计算
    int half_w_;
    int half_t_;

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

public:
    // 新构造函数：支持两种类型的体素
    SpaceAnalyzer(const BusbarConfig& config,
        const std::vector<bool>& obstacles,
        const std::vector<bool>& collision_voxels,
        uint64_t xsize, uint64_t ysize, uint64_t zsize)
        : config_(config), obstacles_(obstacles), collision_voxels_(collision_voxels),
        xsize_(xsize), ysize_(ysize), zsize_(zsize),
        half_w_(static_cast<int>(std::ceil(config.clearance()))),
        half_t_(static_cast<int>(std::ceil(config.clearance()))) {}

    // 内联热点函数：检查坐标是否在边界内
    inline bool isInBounds(int64_t x, int64_t y, int64_t z) const {
        return x >= 0 && y >= 0 && z >= 0 &&
            static_cast<uint64_t>(x) < xsize_ &&
            static_cast<uint64_t>(y) < ysize_ &&
            static_cast<uint64_t>(z) < zsize_;
    }

    inline bool isInBounds(const Point& p) const {
        return isInBounds(p.x(), p.y(), p.z());
    }

    // 内联热点函数：检查体素是否为原有障碍物 (flag=0)
    inline bool isObstacle(int64_t x, int64_t y, int64_t z) const {
        if (!isInBounds(x, y, z)) return true;
        return obstacles_[getIndex(x, y, z)];
    }

    inline bool isObstacle(const Point& p) const {
        return isObstacle(p.x(), p.y(), p.z());
    }

    // 内联热点函数：检查体素是否畅通（既不是障碍物，也不是会碰撞的空闲体素）
    inline bool isVoxelFree(const Point& p) const {
        if (!isInBounds(p)) return false;
        size_t idx = getIndex(p.x(), p.y(), p.z());
        return !obstacles_[idx] && !collision_voxels_[idx];
    }

    // 检查体素是否会碰撞（包括障碍物和会碰撞的空闲体素）
    inline bool isCollision(int64_t x, int64_t y, int64_t z) const {
        if (!isInBounds(x, y, z)) return true;
        size_t idx = getIndex(x, y, z);
        return obstacles_[idx] || collision_voxels_[idx];
    }

    inline bool isCollision(const Point& p) const {
        return isCollision(p.x(), p.y(), p.z());
    }

    // 检查体素是否为防护间隙体素（flag=1，不含障碍物）
    inline bool isCollisionVoxel(const Point& p) const {
        if (!isInBounds(p)) return false;
        size_t idx = getIndex(p.x(), p.y(), p.z());
        return collision_voxels_[idx];
    }

    // 检测参数
    bool isEndpointValid(const Point& p) const;

    // 检查某个点在给定姿态下是否有足够空间
    bool hasSpace(const Point& p, const Vector3& tangent,
        const Vector3& normal) const;

    // 优化版：检查 clearance 外壳，使用去重集合避免重复检查
    // 返回 false 如果有碰撞，true 如果通过
    bool checkClearanceShell(const Point& p, const Vector3& tangent,
        const Vector3& normal, std::unordered_set<int64_t>& checked) const;

    // 获取地图尺寸
    uint64_t xsize() const { return xsize_; }
    uint64_t ysize() const { return ysize_; }
    uint64_t zsize() const { return zsize_; }

    // 获取预计算的检测半径
    int halfWidth() const { return half_w_; }
    int halfThickness() const { return half_t_; }
};

#endif