import numpy as np
from PIL import Image
import math

import SimpleView_SaveImage
import config
from cat import clip_and_rotate_point_cloud, merge_point_clouds
from image import detect_large_holes


def tiff_depth_to_point_clouds(tiff_paths, sn, dedup=True):
    """
    将多个 TIFF 深度图转换为点云，并拼接在一起后生成 PCD 文件

    :param tiff_paths: list of str, TIFF 深度图路径列表
    :param sn: str, 设备序列号，用于查找配置参数
    :param dedup: bool, 是否根据 (x, y) 去重，默认开启
    :return: list of [x, y, z], 合并后的点云数据
    """
    merged_points = []

    # Step 1: 遍历每个 TIFF 文件，生成对应的点云
    for tiff_path in tiff_paths:
        points = tiff_depth_to_point_cloud(tiff_path+".tiff", sn, dedup)
        merged_points.extend([points])

    # Step 2: 可选：使用 merge_point_clouds 对点云进行 x 轴拼接（若需要）
    # 注意：如果每个点云已经带有偏移，则不需要再次拼接
    has_voids = True
    if hasattr(config, 'CUT_CONFIG_MAP') and isinstance(config.CUT_CONFIG_MAP, dict) and (sn in config.CUT_CONFIG_MAP):
        merged_points, xrange = merge_point_clouds(merged_points, config.CUT_CONFIG_MAP[sn],sn)

        real_holes = detect_large_holes(merged_points, sn, xrange)
        if real_holes.__len__()==0:
            has_voids=False
        # Step 3: 写入最终的 PCD 文件
        output_pcd_path = config.save_path("pcd", f"{sn}_merged.pcd")
        write_pcd(output_pcd_path, merged_points)
    # print(f"Merged and saved to {output_pcd_path}")
    return has_voids


def tiff_depth_to_point_cloud(tiff_path,sn,  dedup=True):
    """
    将 TIFF 深度图转换为点云，基于给定的视场角计算相机内参

    :param tiff_path: str, TIFF 深度图路径
    :param hfov_degrees: float or None, 水平视场角（单位：度）
    :param vfov_degrees: float or None, 垂直视场角（单位：度）
    :param output_ply_path: str or None, 输出 Pcd 文件路径（可选）
    :param dedup: bool, 是否根据 (x, y) 去重，默认开启
    :return: list of [x, y, z] 点云数据
    """
    hfov_degrees = config.CAMERA_CONFIG_MAP[sn].get("x_angle")
    max_z = config.CAMERA_CONFIG_MAP[sn].get("max_z")
    vfov_degrees = config.CAMERA_CONFIG_MAP[sn].get("y_angle")
    # plane_scaling_ratio = config.CAMERA_CONFIG_MAP[sn].get("plane_scaling_ratio")
    # 加载 TIFF 图像
    depth_image = Image.open(tiff_path)
    depth_array = np.array(depth_image, dtype=np.float32)

    height, width = depth_array.shape

    # 根据提供的 FOV 计算 fx/fy
    if hfov_degrees is not None:
        hfov_rad = math.radians(hfov_degrees)
        fx = width / (2 * math.tan(hfov_rad / 2))
        fy = fx * height / width  # 保持像素方形比例
    else:
        vfov_rad = math.radians(vfov_degrees)
        fy = height / (2 * math.tan(vfov_rad / 2))
        fx = fy * width / height  # 保持像素方形比例

    cx = width / 2
    cy = height / 2

    print(f"Calculated intrinsics: fx={fx:.2f}, fy={fy:.2f}, cx={cx:.2f}, cy={cy:.2f}")

    points = []
    seen_xy = set()  # 用于记录已添加的 (x, y)

    # 获取裁剪配置
    clip_config = config.CUT_CONFIG_MAP.get(sn)
    if clip_config:
        min_pt = np.array(clip_config.get("min_pt", [-np.inf] * 3))
        max_pt = np.array(clip_config.get("max_pt", [np.inf] * 3))
        rotation = np.array(clip_config.get("rotation", [1,0,0,0,1,0,0,0,1])).reshape(3, 3)
    else:
        min_pt = max_pt = rotation = None

    for v in range(height):
        for u in range(width):
            z = depth_array[v, u]  # mm -> m
            if z <= 0 or z > max_z:
                continue

            x = (u - cx) * z / fx
            y = (v - cy) * z / fy

            # 构造点并旋转（保留浮点精度）
            point = np.array([x, y, z])
            if clip_config:
                rotated_point = (rotation @ point.T).T
                if not np.all(rotated_point >= min_pt) or not np.all(rotated_point <= max_pt):
                    continue
                x_final, y_final, z_final = rotated_point
            else:
                x_final, y_final, z_final = point

            # 到这里才进行去重判断
            if dedup:
                x_int = int(round(x_final))
                y_int = int(round(y_final))
                z_int = int(round(z_final))
                if (x_int, y_int) in seen_xy:
                    continue
                seen_xy.add((x_int, y_int))
                # 保留浮点精度写入结果
                points.append([x_int, y_int, z_int])


    # 可选输出PLY文件用于可视化
    if config.CAMERA_CONFIG_MAP[sn].get("save_pcd"):
        output_ply_path = config.save_path("pcd",sn+".pcd")
        write_pcd(output_ply_path, points)

    return points

def write_pcd(filename, points):
    """
    将点云写入 PCD 文件

    :param filename: str, 输出 PCD 文件路径
    :param points: list of [x, y, z], 点云数据
    """
    with open(filename, 'w') as f:
        # 写入 PCD 文件头
        f.write("# .PCD v0.7 - Point Cloud Data file format\n")
        f.write(f"VERSION 0.7\n")
        f.write("FIELDS x y z\n")
        f.write("SIZE 4 4 4\n")  # float 类型的大小是 4 字节
        f.write("TYPE F F F\n")  # 每个字段的数据类型
        f.write(f"COUNT 1 1 1\n")  # 每个字段的数量
        f.write(f"WIDTH {len(points)}\n")  # 点数量
        f.write("HEIGHT 1\n")  # 单行表示无结构点云
        f.write("VIEWPOINT 0 0 0 1 0 0 0\n")  # 默认视角参数
        f.write(f"POINTS {len(points)}\n")  # 总点数
        f.write("DATA ascii\n")  # 数据部分以 ASCII 形式存储

        # 写入点数据
        for point in points:
            f.write(f"{point[0]} {point[1]} {point[2]}\n")

    print(f"Saved point cloud to {filename}")


if __name__ == '__main__':
    paths = SimpleView_SaveImage.pic("00DA6823936")
    tiff_depth_to_point_clouds(paths, "00DA6823936")
    # tiff_depth_to_point_clouds(["D:/PycharmProjects/Hik3D/image/2025-06-24/depth/145209062_-Depth.tiff"
    #                                ,"D:/PycharmProjects/Hik3D/image/2025-06-24/depth/145209062_-Depth.tiff"
    #                                ,"D:/PycharmProjects/Hik3D/image/2025-06-24/depth/145209062_-Depth.tiff"], "00DA6823936")