import numpy as np
from pyproj import Transformer

def lidar_2d_to_wgs84(lidar_points_xy, origin_lon, origin_lat, rotation_angle_deg, fixed_z=0):
    """
    将激光雷达的2D坐标(X, Y)转换为WGS84地理坐标(纬度, 经度)。
    
    参数:
    - lidar_points_xy: (N, 2) 的 NumPy 数组，表示 N 个激光雷达点的 x, y 坐标。
    - origin_lon: 激光雷达坐标系原点的经度。
    - origin_lat: 激光雷达坐标系原点的纬度。
    - rotation_angle_deg: 激光雷达坐标系相对于地理北向的旋转角度（以度为单位）。
    - fixed_z: 所有点的固定高度，默认为0。
    
    返回:
    - latitudes: (N,) 的 NumPy 数组，表示转换后的纬度。
    - longitudes: (N,) 的 NumPy 数组，表示转换后的经度。
    """

    # 将角度从度转换为弧度
    rotation_angle_rad = np.radians(rotation_angle_deg)

    # 创建旋转矩阵
    rotation_matrix = np.array([
        [np.cos(rotation_angle_rad), -np.sin(rotation_angle_rad)],
        [np.sin(rotation_angle_rad), np.cos(rotation_angle_rad)]
    ])

    # 对每个点应用旋转
    rotated_points = np.dot(lidar_points_xy, rotation_matrix.T)

    # 添加固定的高度
    earth_points = np.column_stack((rotated_points, np.full(rotated_points.shape[0], fixed_z)))

    # 使用 UTM 投影进行转换，首先找到对应的 UTM 区域
    transformer_to_utm = Transformer.from_crs("EPSG:4326", "+proj=utm +zone=33 +ellps=WGS84 +datum=WGS84 +units=m +no_defs")
    utm_x_origin, utm_y_origin = transformer_to_utm.transform(origin_lat, origin_lon)

    # 将激光雷达坐标转换为 UTM 坐标
    utm_points = earth_points.copy()
    utm_points[:, 0] += utm_x_origin
    utm_points[:, 1] += utm_y_origin

    # 再将 UTM 坐标转换回 WGS84
    transformer_to_wgs84 = Transformer.from_crs("+proj=utm +zone=33 +ellps=WGS84 +datum=WGS84 +units=m +no_defs", "EPSG:4326")
    latitudes, longitudes = transformer_to_wgs84.transform(utm_points[:, 0], utm_points[:, 1])

    return latitudes, longitudes

# 示例：一些激光雷达点云数据 (X, Y)
lidar_points_xy = np.array([[100, 200], [300, 400]])

# 示例参数
origin_lon = 12.3456  # 激光雷达坐标系原点的经度
origin_lat = 54.3210  # 激光雷达坐标系原点的纬度
rotation_angle_deg = 0  # 激光雷达坐标系相对于地理北向的旋转角度（度）

# 执行转换
latitudes, longitudes = lidar_2d_to_wgs84(lidar_points_xy, origin_lon, origin_lat, rotation_angle_deg)

print("Latitude:", latitudes)
print("Longitude:", longitudes)