import numpy as np
import config as con

def clip_and_rotate_point_cloud(points, config):
    """
    对点云先旋转，再裁剪

    :param points: list of [x, y, z], 原始点云数据
    :param config: dict, 包含 min_pt, max_pt, rotation 的配置
    :return: list of [x, y, z], 旋转并裁剪后的点云
    """

    # 提取配置参数
    min_pt = np.array(config.get("min_pt", [-np.inf] * 3))
    max_pt = np.array(config.get("max_pt", [np.inf] * 3))
    rotation = np.array(config.get("rotation", [1, 0, 0, 0, 1, 0, 0, 0, 1])).reshape(3, 3)

    # 转换为 NumPy 数组
    points = np.array(points, dtype=np.float64)

    # 1. 先旋转：应用旋转矩阵
    rotated_points = (rotation @ points.T).T

    # 2. 再裁剪：保留落在 min_pt ~ max_pt 范围内的点
    mask = np.all((rotated_points >= min_pt) & (rotated_points <= max_pt), axis=1)
    clipped_points = rotated_points[mask]

    return clipped_points.tolist()




def merge_point_clouds(clouds, config,sn):
    """
    将多个点云沿 x 轴依次拼接在一起

    :param clouds: list of (list of [x, y, z]), 多个点云
    :return: list of [x, y, z], 合并后的点云
    """

    min_pt = config.get("min_pt", [0, 0, 0])
    max_pt = config.get("max_pt", [0, 0, 0])
      # 返回 x 方向的区域宽度

    merged = []
    current_x_offset = 0

    reverse_order = con.CAMERA_CONFIG_MAP[sn].get("reverse_order", False)
    for cloud in reversed(clouds) if reverse_order else clouds:
        if not cloud:
            continue
        # 添加偏移后写入结果
        offset_cloud = [[p[0] + current_x_offset, p[1], p[2]] for p in cloud]
        merged.extend(offset_cloud)

        # 获取当前相机采集区域的 x 宽度
        x_width =  float(max_pt[0] - min_pt[0])

        # 更新下一个点云的偏移量（当前偏移 + 当前区域宽度）
        # 在下一次删除
        current_x_offset += x_width


    return merged,current_x_offset+min_pt[0]