import numpy as np
import cv2
import matplotlib.pyplot as plt

from scipy.ndimage import label

import config


def point_cloud_to_2d_image(points, resolution=1.0, x_range=(17, 275), y_range=(-129, -1227)):
    """
    将点云转换为 2D 图像（X-Y 平面），每个像素值表示对应位置的 Z 值平均值
    """
    if not isinstance(points, np.ndarray):
        points = np.array(points)

    if len(points) == 0:
        raise ValueError("点云为空")

    x_min, x_max = x_range
    y_min, y_max = y_range

    width = int((x_max - x_min) / resolution) + 1
    height = int((y_max - y_min) / resolution) + 1

    image = np.zeros((height, width), dtype=np.float32)
    count_map = np.zeros((height, width), dtype=np.int32)

    for x, y, z in points:
        xi = int((x - x_min) / resolution)
        yi = int((y - y_min) / resolution)

        if 0 <= xi < width and 0 <= yi < height:
            image[yi, xi] += z
            count_map[yi, xi] += 1

    # 防止除零错误
    count_map[count_map == 0] = 1
    image /= count_map

    return image, (x_min, y_min)


def detect_holes_by_density(density_map, density_threshold_ratio=0.5, min_area=100):
    """
    基于点云密度图识别空洞区域

    :param density_map: 2D numpy array, 每个像素表示该位置点云密度
    :param density_threshold_ratio: 密度低于均值的 ratio 倍时视为空洞候选
    :param min_area: 最小空洞面积（像素数）
    :return: list of ((cx, cy), area)，空洞中心和面积（图像坐标）
    """
    # 计算邻域平均密度（3x3窗口）
    avg_density = np.zeros_like(density_map)
    for i in range(density_map.shape[0]):
        for j in range(density_map.shape[1]):
            # 取 3x3 邻域
            neighbors = density_map[
                        max(0, i - 1):min(i + 2, density_map.shape[0]),
                        max(0, j - 1):min(j + 2, density_map.shape[1])
                        ]
            avg_density[i, j] = np.mean(neighbors)

    # 构建空洞候选区：密度低于邻域平均值的 50%
    binary_map = (density_map < avg_density * density_threshold_ratio).astype(np.uint8)

    # 连通域分析
    structure = np.array([[1, 1, 1],
                          [1, 1, 1],
                          [1, 1, 1]])
    labeled_map, num_features = label(binary_map, structure=structure)

    holes = []
    for label_id in range(1, num_features + 1):
        coords = np.where(labeled_map == label_id)
        hole_pixel_count = len(coords[0])

        if hole_pixel_count >= min_area:
            cx = np.mean(coords[1])  # x 坐标（列）
            cy = np.mean(coords[0])  # y 坐标（行）
            area = hole_pixel_count
            holes.append(((cx, cy), area))

    return holes


def detect_black_regions(binary_mask, min_area=10):
    """
    检测图像中的黑色连通区域（值为 0 的区域）

    :param binary_mask: 2D numpy array, 二值图（0 表示黑色区域）
    :param min_area: int, 最小面积阈值，小于该值的区域会被忽略
    :return: list of dict, 包含每个区域的信息:
             [
                 {
                     'id': int,
                     'center': (cx, cy),
                     'area': int,
                     'coordinates': [(x1,y1), (x2,y2), ...]
                 },
                 ...
             ]
    """
    # 确保输入是二值图像（0 是黑）
    binary = (binary_mask == 0).astype(np.uint8)

    # 使用连通域分析
    structure = np.array([[1, 1, 1],
                          [1, 1, 1],
                          [1, 1, 1]])
    labeled_map, num_features = label(binary, structure=structure)

    regions = []

    for label_id in range(1, num_features + 1):
        coords = np.where(labeled_map == label_id)
        area = len(coords[0])

        if area >= min_area:
            cx = np.mean(coords[1])  # x 坐标（列）
            cy = np.mean(coords[0])  # y 坐标（行）

            regions.append(((cx, cy),area))


    return regions
def convert_image_holes_to_real(holes, offset, resolution):
    real_holes = []
    x_min, y_min = offset
    for ((cx, cy), area) in holes:
        real_x = x_min + cx * resolution
        real_y = y_min + cy * resolution
        real_holes.append(((real_x, real_y), area * resolution ** 2))
    return real_holes


def visualize_holes_on_image(image, holes, output_path=None):
    """
    在图像上画出检测到的空洞中心和轮廓
    """
    # 彩色化灰度图用于可视化
    color_image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)

    for (cx, cy), _ in holes:
        # 绘制圆形标注空洞中心
        cv2.circle(color_image, (int(cx), int(cy)), radius=5, color=(0, 0, 255), thickness=-1)

    # 显示图像
    # plt.figure(figsize=(10, 8))
    plt.imshow(cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB))
    # plt.title("Detected Holes")
    plt.axis("off")
    output_path = config.save_path("image", "_holes.png")
    if output_path:
        plt.savefig(output_path, bbox_inches='tight', dpi=200)
        print(f"Saved visualization to {output_path}")
    # plt.show()

def read_pcd_points(pcd_path):
    """
    从 .pcd 文件中提取点云坐标 (x, y, z)

    :param pcd_path: str, PCD 文件路径
    :return: list of [x, y, z], 点云坐标列表
    """
    points = []
    data_started = False

    with open(pcd_path, 'r') as f:
        for line in f:
            if line.startswith("DATA"):
                data_started = True
                continue
            if data_started:
                parts = line.strip().split()
                if len(parts) >= 3:
                    try:
                        x = float(parts[0])
                        y = float(parts[1])
                        z = float(parts[2])
                        points.append([x, y, z])
                    except ValueError:
                        continue
    return points

def detect_large_holes(points,sn, x_max):

    # 2. 生成 2D 图像
    x_range = (config.CUT_CONFIG_MAP[sn]["min_pt"][0],x_max)       # 手动指定 X 范围
    y_range = (config.CUT_CONFIG_MAP[sn]["min_pt"][1], config.CUT_CONFIG_MAP[sn]["max_pt"][1])  # 注意：这里要保证 y_min < y_max，否则反转一下
    resolution = config.CAMERA_CONFIG_MAP[sn].get("resolution")
    image, offset = point_cloud_to_2d_image(points, resolution=resolution, x_range=x_range, y_range=y_range)

    # 3. 图像归一化用于可视化
    normalized_image = cv2.normalize(image, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8)

    # 4. 检测空洞
    holes = detect_black_regions(normalized_image, min_area=20)

    # 5. 可视化空洞
    visualize_holes_on_image(normalized_image, holes)

    # 6. 输出真实世界坐标
    real_holes = convert_image_holes_to_real(holes, offset, resolution)
    return real_holes

if __name__ == '__main__':
    points = read_pcd_points("D:/PycharmProjects/Hik3D/image/2025-06-25/pcd/182109899_00DA6823936_merged.pcd")
    sn = "00DA6823936"
    x_max = 326
    detect_large_holes(points,sn, x_max)