import math
import numpy as np
import cameramodel

def calc_distance(cameraModel, x: float, y: float) -> tuple[float, float]:
    """计算像素坐标 (x, y) 对应的世界坐标 (Xw, Yw)。

    参数:
        x, y: 像素坐标
        cameraModel: 包含相机参数的类实例，需有以下属性:
            principal_point: 主点坐标 (cx, cy)
            pixel_size_x, pixel_size_y: 像素尺寸
            rotation_camera: 相机旋转角度
            focal_length: 焦距
            rotation_alpha, rotation_beta: 旋转角度
            height: 相机高度

    返回:
        世界坐标系下的 (Xw, Yw)
    """
    # 1. 像素坐标转归一化平面坐标（考虑主点和旋转）
    Xp = (x - cameraModel.principal_point[0]) * cameraModel.pixel_size_x
    Yp = -(y - cameraModel.principal_point[1]) * cameraModel.pixel_size_y

    # 2. 应用相机旋转
    cos_rot = math.cos(cameraModel.rotation_camera)
    sin_rot = math.sin(cameraModel.rotation_camera)
    Xp_rotation = Xp * cos_rot + Yp * sin_rot
    Yp_rotation = -Xp * sin_rot + Yp * cos_rot

    # 3. 预计算常用中间变量
    focal_sq = cameraModel.focal_length ** 2
    Yp_sq = Yp_rotation ** 2
    sqrt_focal_Yp = math.sqrt(focal_sq + Yp_sq)

    # 4. 计算角度 eta 和 seta
    eta = math.atan2(Yp_rotation, cameraModel.focal_length)  # 改用 atan2 避免除零
    tan_alpha = math.tan(cameraModel.rotation_alpha)
    tan_beta = math.tan(cameraModel.rotation_beta)
    seta = math.atan2(1, math.sqrt(tan_beta ** 2 + 1 / tan_alpha ** 2))

    # 5. 计算距离 d 和世界坐标
    seta_eta = seta + eta
    OM = cameraModel.height / math.tan(seta_eta)
    MP = cameraModel.height * abs(Xp_rotation) / (sqrt_focal_Yp * math.sin(seta_eta))
    # epsilon = math.atan2(MP, OM)  # 改用 atan2 提高数值稳定性
    epsilon = math.atan((cameraModel.height * Xp_rotation / (sqrt_focal_Yp * math.sin(seta_eta))) / OM)
    d = math.hypot(MP, OM)  # 等价于 sqrt(MP**2 + OM**2)，但更高效
    gamma = math.atan2(1, tan_alpha * tan_beta)

    # 6. 计算最终世界坐标
    Xw = d * math.cos(gamma + epsilon)
    Yw = -d * math.sin(gamma + epsilon)

    return Xw*cameraModel.zoom, Yw*cameraModel.zoom


def calc_distance2(cameraModel, Xw: float, Yw: float) -> tuple[int, int]:
    """从世界坐标 (Xw, Yw) 反推像素坐标 (x, y)。

    参数:
        Xw, Yw: 世界坐标系下的坐标
        cameraModel: 包含相机参数的类实例，需有以下属性:
            rotation_alpha, rotation_beta: 旋转角度
            focal_length (f): 焦距
            height (H): 相机高度
            rotation_camera (q): 相机旋转角度
            pixel_size_x (dx), pixel_size_y (dy): 像素尺寸
            principal_point (u0, v0): 主点坐标

    返回:
        像素坐标 (x, y)，类型为整数
    """
    # 1. 计算距离 d 和角度 seta、gamma
    d = math.hypot(Xw/cameraModel.zoom, Yw/cameraModel.zoom)
    tan_alpha = math.tan(cameraModel.rotation_alpha)
    tan_beta = math.tan(cameraModel.rotation_beta)
    seta = math.atan2(1, math.sqrt(tan_beta ** 2 + 1 / tan_alpha ** 2))
    gamma = math.atan2(1, tan_alpha * tan_beta)  # 用 atan2 避免除零错误

    # 2. 计算组合角度 epsilon（调整到 [-π, π]）
    angle = math.atan2(-Yw, Xw)  # 使用 atan2 处理所有象限
    epsilon = (angle - gamma + math.pi) % (2 * math.pi) - math.pi  # 规范化角度

    # 3. 计算 OM 和 MP
    OM = abs(d * math.cos(epsilon))
    MP = abs(d * math.sin(epsilon))

    # 4. 计算 eta 和 Yp1、Xp1
    eta = math.atan2(cameraModel.height, OM) - seta  # 替换 H
    Yp_rotation = math.tan(eta) * cameraModel.focal_length  # 替换 f
    sqrt_term = math.sqrt(cameraModel.focal_length ** 2 + Yp_rotation ** 2)
    sin_term = math.sin(seta + eta)
    Xp_rotation = MP * sqrt_term * sin_term / cameraModel.height  # 替换 H
    Xp_rotation = Xp_rotation if epsilon > 0 else -Xp_rotation  # 简化条件判断

    # 5. 应用相机旋转（逆旋转）
    cos_q = math.cos(-cameraModel.rotation_camera)  # 替换 q
    sin_q = math.sin(-cameraModel.rotation_camera)
    Xp = Xp_rotation * cos_q + Yp_rotation * sin_q
    Yp = -Xp_rotation * sin_q + Yp_rotation * cos_q

    # 6. 转换为像素坐标
    x = int(Xp / cameraModel.pixel_size_x + cameraModel.principal_point[0])  # 替换 dx, u0
    y = int(-Yp / cameraModel.pixel_size_y + cameraModel.principal_point[1])  # 替换 dy, v0

    return x, y


def calc_height(
        cameraModel,  # 包含相机参数的类实例
        x_bot: float,
        y_bot: float,
        x_top: float,
        y_top: float,
) -> float:
    """计算物体高度 Zw。

    参数:
        x_bot, y_bot: 物体底部像素坐标
        x_top, y_top: 物体顶部像素坐标
        cameraModel: 包含相机参数的类实例，需有以下属性:
            rotation_alpha, rotation_beta: 旋转角度
            principal_point: 主点坐标 (u0, v0)
            pixel_size_x, pixel_size_y: 像素尺寸 (dx, dy)
            rotation_camera: 相机旋转角度 (q)
            focal_length: 焦距 (f)
            height: 相机高度 (H)

    返回:
        物体高度 Zw
    """
    # 预计算常用值和三角函数
    cos_q = math.cos(cameraModel.rotation_camera)
    sin_q = math.sin(cameraModel.rotation_camera)
    tan_alpha = math.tan(cameraModel.rotation_alpha)
    tan_beta = math.tan(cameraModel.rotation_beta)

    # 1. 底部点计算
    # 像素坐标转归一化坐标
    Xp_bot = (x_bot - cameraModel.principal_point[0]) * cameraModel.pixel_size_x
    Yp_bot = -(y_bot - cameraModel.principal_point[1]) * cameraModel.pixel_size_y

    # 应用相机旋转
    Xp_bot_rotation = Xp_bot * cos_q + Yp_bot * sin_q
    Yp_bot_rotation = -Xp_bot * sin_q + Yp_bot * cos_q

    # 计算角度和距离
    eta_bot = math.atan2(Yp_bot_rotation, cameraModel.focal_length)
    seta = math.atan2(1, math.sqrt(tan_beta ** 2 + 1 / tan_alpha ** 2))

    # 计算OM
    OM = cameraModel.height / math.tan(seta + eta_bot)

    # 2. 顶部点计算
    # 像素坐标转归一化坐标
    Xp_top = (x_top - cameraModel.principal_point[0]) * cameraModel.pixel_size_x
    Yp_top = -(y_top - cameraModel.principal_point[1]) * cameraModel.pixel_size_y

    # 应用相机旋转
    Xp_top_rotation = Xp_top * cos_q + Yp_top * sin_q
    Yp_top_rotation = -Xp_top * sin_q + Yp_top * cos_q

    # 计算角度
    eta_top = math.atan2(Yp_top_rotation, cameraModel.focal_length)

    # 3. 计算高度
    Zw = cameraModel.height - OM * math.tan(seta + eta_top)

    return Zw

"""
计算图像中车辆坐标系Xw、Yw所对应的轴线
参数：
返回值：轴线上点在图像中的二维坐标x、y
"""
def calc_zeros_yto0(cameraModel, val):
    # 定义搜索范围和步长
    x_range = np.linspace(0, 1280, 2560)
    y_range = np.linspace(0, 960, 1920)

    # 存储解
    solutions = []
    tolerance = 1e-3  # 容忍误差

    # 网格搜索
    for x in x_range:
        for y in y_range:
            error = equations_yto0(cameraModel, x, y, val)
            if error < tolerance:
                solutions.append((x, y))

    # 去除近似重复的解
    unique_solutions = []
    for sol in solutions:
        # 检查是否与已找到的解近似
        is_unique = True
        for usol in unique_solutions:
            if np.allclose(sol, usol, atol=1):
                is_unique = False
                break
        if is_unique:
            unique_solutions.append(sol)
    x = []
    y = []
    # print("找到的解:")
    for sol in unique_solutions:
        # print(f"x = {sol[0]:.4f}, y = {sol[1]:.4f}")
        x.append(sol[0])
        y.append(sol[1])
    return x, y

def equations_yto0(cameraModel, x, y, val):
    Xw, Yw = calc_distance(cameraModel, x, y)
    return (Yw - val)**2   # 误差平方和

def calc_zeros_xto0(cameraModel, val):
    # 定义搜索范围和步长
    x_range = np.linspace(0, 1280, 2560)
    y_range = np.linspace(0, 960, 1920)

    # 存储解
    solutions = []
    tolerance = 1e-3  # 容忍误差

    # 网格搜索
    for x in x_range:
        for y in y_range:
            error = equations_xto0(cameraModel, x, y, val)
            if error < tolerance:
                solutions.append((x, y))

    # 去除近似重复的解
    unique_solutions = []
    for sol in solutions:
        # 检查是否与已找到的解近似
        is_unique = True
        for usol in unique_solutions:
            if np.allclose(sol, usol, atol=1):
                is_unique = False
                break
        if is_unique:
            unique_solutions.append(sol)
    x = []
    y = []
    # print("找到的解:")
    for sol in unique_solutions:
        # print(f"x = {sol[0]:.4f}, y = {sol[1]:.4f}")
        x.append(sol[0])
        y.append(sol[1])
    return x, y

def equations_xto0(cameraModel, x, y, val):
    Xw, Yw = calc_distance(cameraModel, x, y)
    return (Xw - val)**2   # 误差平方和

def calc_zeros_xandyto0(cameraModel, xval, yval):
    # 定义搜索范围和步长
    x_range = np.linspace(0, 1280, 5000)
    y_range = np.linspace(0, 960, 5000)

    # 存储解
    solutions = []
    tolerance = 1e-1  # 容忍误差

    # 网格搜索
    for x in x_range:
        for y in y_range:
            error = equations_xandyto0(cameraModel, x, y, xval, yval)
            if error < tolerance:
                solutions.append((x, y))

    # 去除近似重复的解
    unique_solutions = []
    for sol in solutions:
        # 检查是否与已找到的解近似
        is_unique = True
        for usol in unique_solutions:
            if np.allclose(sol, usol, atol=1):
                is_unique = False
                break
        if is_unique:
            unique_solutions.append(sol)
    x = []
    y = []
    # print("找到的解:")
    for sol in unique_solutions:
        # print(f"x = {sol[0]:.4f}, y = {sol[1]:.4f}")
        x.append(sol[0])
        y.append(sol[1])
    return x, y

def equations_xandyto0(cameraModel, x, y, xval, yval):
    Xw, Yw = calc_distance(cameraModel, x, y)
    return (Xw - xval)**2 + (Yw - yval)**2  # 误差平方和
# model = cameramodel.CameraModel("updated_config.json")
# print(calc_distance(714,370,model))
# print(calc_distance2(638,-665,model))