# -*- coding: utf-8 -*-
"""
最终功能模块：从像素坐标到经纬度的转换

核心功能:
- pixel_to_lonlat: 输入像素坐标(u, v)和摄像头状态，输出对应的经纬度。
"""
import numpy as np
from scipy.spatial.transform import Rotation as R

# --- 核心函数 ---

def compute_hfov_by_zoom(hfov_max, zoom, camera_type="dahua", hfov_min=None):
    """
    将缩放等级(zoom)映射为水平视场角(hfov)。
    规则:
    - 最小zoom对应最大视场角(hfov_max)
    - 最大zoom对应最小视场角(hfov_max * min_zoom / max_zoom)
    """
    if zoom is None:
        return hfov_max
    camera_type = (camera_type or "dahua").lower()
    if camera_type in ("hik", "hikvision", "hk", "haikang", "海康"):
        min_zoom, max_zoom = 1.0, 56.0
    else:
        min_zoom, max_zoom = 0.4375, 56.0

    z = float(zoom)
    if z < min_zoom:
        z = min_zoom
    elif z > max_zoom:
        z = max_zoom
    if hfov_min is None:
        hfov_min = 1.5
    if hfov_min is not None:
        span = max_zoom - min_zoom
        if span <= 1e-9:
            return float(hfov_min)
        t = (z - min_zoom) / span
        return float(hfov_max) + (float(hfov_min) - float(hfov_max)) * t
    return hfov_max * (min_zoom / z)

def pixel_to_lonlat(u, v, camera_params, ptz_params):
    """
    正向投影：将像素坐标(u, v)转换为经纬度。

    Args:
        u (int): 像素横坐标
        v (int): 像素纵坐标
        camera_params (dict): 摄像头的固定物理参数 (通过标定获得)
        ptz_params (dict): 摄像头当前的云台参数

    Returns:
        tuple: (经度, 纬度) 或 (None, None) 如果计算失败
    """
    # --- 1. 解包所有已知参数 ---
    res_w, res_h = int(camera_params['resolution_w']), int(camera_params['resolution_h'])
    cam_lon, cam_lat = float(camera_params['cam_lon']), float(camera_params['cam_lat'])
    cam_height = float(camera_params['height'])
    hfov_max = float(camera_params['hfov'])
    zoom_value = ptz_params.get('z')
    if zoom_value is None:
        zoom_value = ptz_params.get('zoom')
    hfov = compute_hfov_by_zoom(
        hfov_max=hfov_max,
        zoom=zoom_value,
        camera_type=camera_params.get('camera_type', 'dahua'),
        hfov_min=camera_params.get('hfov_min')
    )
    north_p = float(camera_params['north_p_value'])
    p, t = float(ptz_params['pan']), float(ptz_params['tilt'])

    # --- 2. 相机内参模型：将像素(u,v)转换为相机坐标系下的方向向量 ---
    # (相机坐标系定义: x向右, y向下, z向前)
    f_x = res_w / (2 * np.tan(np.radians(hfov) / 2))
    f_y = f_x  # 假设像素是正方形
    cx, cy = res_w / 2, res_h / 2

    # 计算方向向量并归一化
    vec_cam = np.array([(u - cx) / f_x, (v - cy) / f_y, 1.0])
    vec_cam /= np.linalg.norm(vec_cam)

    # --- 3. 旋转变换：将相机坐标系下的向量旋转到世界坐标系 ---
    # (世界坐标系定义: ENU, 即 x向东, y向北, z向上)
    r_base = R.from_euler('x', -90, degrees=True) # 基础旋转：将(相机前)对准(世界北)
    p_corrected = p - north_p                     # 校正P值，使其以正北为0度
    r_pan = R.from_euler('z', p_corrected, degrees=True)  # 方位角旋转 (绕世界Z轴)
    r_tilt = R.from_euler('x', -t, degrees=True)          # 俯仰角旋转 (绕世界X轴)

    # 合成总旋转
    total_rotation = r_pan * r_tilt * r_base
    vec_enu = total_rotation.apply(vec_cam)

    # --- 4. 几何解算：计算光线与地面的交点 ---
    # 如果光线向量的z分量是正数或零，说明光线朝向天空或水平，无法与地面相交
    if vec_enu[2] >= -1e-9:
        return None, None

    # 摄像头在世界坐标系中的位置
    cam_pos_enu = np.array([0, 0, cam_height])

    # 求解射线参数 s
    s = -cam_height / vec_enu[2]

    # 计算交点坐标 (米)
    intersection_enu = cam_pos_enu + s * vec_enu

    # --- 5. 坐标转换：将米制坐标转回经纬度 ---
    EARTH_RADIUS = 6378137.0
    delta_lat_rad = intersection_enu[1] / EARTH_RADIUS
    delta_lon_rad = intersection_enu[0] / (EARTH_RADIUS * np.cos(np.radians(cam_lat)))

    lon = cam_lon + np.degrees(delta_lon_rad)
    lat = cam_lat + np.degrees(delta_lat_rad)

    return {"lon":lon, "lat":lat}

# ==============================================================================
# 使用示例
# ==============================================================================
if __name__ == '__main__':
    # --- 1. 定义您最终标定好的、精确的摄像头参数 ---
    # 这些是固定的物理参数，您应该保存下来
    CAMERA_PARAMS = {
        "height": 28.0000,
        "hfov": 61.0000,
        "hfov_min": 1.15,
        "resolution_w": 1280,
        "resolution_h": 720,
        "cam_lon": 112.893799,
        "cam_lat": 28.221701,
        "north_p_value": 39.0,
        "camera_type": "dahua",
    }

    # --- 2. 定义摄像头当前的云台状态 ---
    # 在您的实际应用中，这些值应该是从摄像头API实时读取的
    CURRENT_PTZ_PARAMS = {
        "pan": 271.9,
        "tilt": 26.2,
        "z": 0.4375
    }

    # --- 3. 指定您想要查询的屏幕像素坐标 ---
    pixel_u, pixel_v = 91, 276 # 使用您标定过的“黄金数据点”之一作为测试

    print(f"--- 正在将像素坐标 ({pixel_u}, {pixel_v}) 转换为经纬度 ---")

    # --- 4. 调用核心函数进行计算 ---
    predicted_lon, predicted_lat = pixel_to_lonlat(pixel_u, pixel_v, CAMERA_PARAMS, CURRENT_PTZ_PARAMS)

    # --- 5. 打印结果 ---
    if predicted_lon is not None:
        print("\n计算成功！")
        print(f"  预测经度: {predicted_lon:.8f}")
        print(f"  预测纬度: {predicted_lat:.8f}")

        # 与标定时的真实值对比 (可选，用于验证)
        true_lon, true_lat = 112.89461289520455, 28.221594983922852
        print("\n--- 与标定时的真实值对比 ---")
        print(f"  真实经度: {true_lon:.8f}")
        print(f"  真实纬度: {true_lat:.8f}")
    else:
        print("\n计算失败（可能该点指向天空）。")