import math

import numpy as np

import model

model = model.Model()
u0 = model.u0
v0 = model.v0
f = model.f
H = model.H
dx = model.dx
dy = model.dy
alpha = model.alpha
beta = model.beta
q = model.q
x_change = model.x
y_change = model.y
"""
计算图像中一点到车辆坐标的距离
参数：点在图像中的坐标x、y，相机的外部偏转角参数alpha、beta
返回值：点在车辆坐标系下的二维坐标
"""
def calc_distance(x, y, alpha, beta):
    # print(f"q={q}")
    Xp = (x - u0) * dx
    Yp = -(y - v0) * dy
    Xp1 = (x - u0) * dx
    Yp1 = -(y - v0) * dy
    Xp = Xp1 * math.cos(q) + Yp1 * math.sin(q)
    Yp = -Xp1 * math.sin(q) + Yp1 * math.cos(q)
    # print(f"Yp1 = {Yp1},Yp = {Yp}")
    # print(f"Xp1 = {Xp1},Xp = {Xp}")
    sq = math.sqrt(f * f + Yp * Yp)
    eta = math.atan(Yp / f)
    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
    seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
    MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))
    OM = H / math.tan(seta + eta)
    epsilon = math.atan((H * Xp / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))) / OM)
    d = math.sqrt(MP ** 2 + OM ** 2)
    Xw = d * math.cos(gamma + epsilon) - 0*model.x
    Yw = -d * math.sin(gamma + epsilon) - 0*model.y
    return Xw, Yw

"""
计算车辆坐标系下某点在图像中的位置
参数：点在车辆坐标系下的坐标Xw、Yw，相机的外部偏转角参数alpha、beta
返回值：点在图像中的二维坐标
"""
def calc_distance2(Xw, Yw, alpha, beta):
    d = math.sqrt((Xw - model.x) ** 2 + (Yw + model.y) ** 2)
    seta = math.atan(1 / math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
    # 计算组合角度
    angle = math.atan2(-Yw, Xw)  # 使用atan2处理所有象限
    # 解出epsilon
    epsilon = angle - gamma
    # 将结果调整到[-pi, pi]范围内
    epsilon = (epsilon + math.pi) % (2 * math.pi) - math.pi
    OM = math.fabs(d * math.cos(epsilon))
    MP = math.fabs(d * math.sin(epsilon))
    eta = math.atan(H / OM) - seta
    Yp1 = math.tan(eta) * f
    Xp1 = MP * math.sqrt(f ** 2 + Yp1 ** 2) * math.sin(seta + eta)/H
    if epsilon > 0:
        Xp1 = Xp1
    else:
        Xp1 = -Xp1
    Xp = Xp1 * math.cos(-q) + Yp1 * math.sin(-q)
    Yp = -Xp1 * math.sin(-q) + Yp1 * math.cos(-q)
    x = Xp / dx +u0
    y = -Yp / dy +v0
    return int(x), int(y)

# print(f"result: {calc_distance2(1291,-562 , alpha, beta)}")

"""
计算图像中一点距离地面的高度
参数：地面端点在图像中的坐标x_bot、y_bot，垂线上端点在图像中的坐标x_top、y_top，相机的外部偏转角参数alpha、beta
返回值：点距离地面的高度
"""
def calc_height(x_bot,y_bot,x_top,y_top,alpha,beta):
    Xp = (x_bot - u0) * dx
    Yp = -(y_bot - v0) * dy
    Xp1 = (x_bot - u0) * dx
    Yp1 = -(y_bot - v0) * dy
    Xp = Xp1 * math.cos(q) + Yp1 * math.sin(q)
    Yp = -Xp1 * math.sin(q) + Yp1 * math.cos(q)
    sq = math.sqrt(f * f + Yp * Yp)
    eta = math.atan(Yp / f)
    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
    seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
    MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))
    OM = H/math.tan(seta + eta)
    Yp1 = -(y_top - v0) * dy
    Xp1 = (x_top- u0) * dx
    Yp1 = -(y_top - v0) * dy
    Xp = Xp1 * math.cos(q) + Yp1 * math.sin(q)
    Yp = -Xp1 * math.sin(q) + Yp1 * math.cos(q)
    sq1 = math.sqrt(f * f + Yp * Yp)
    eta1 = math.atan(Yp / f)
    # print(f"Yp1: {Yp1},eta1: {eta1}")
    Zw = H - OM*math.tan(seta + eta1)
    return Zw

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

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

    # 网格搜索
    for x in x_range:
        for y in y_range:
            error = equations_yto0(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(x, y, val):
    Xp = (x - u0) * dx
    Yp = -(y - v0) * dy
    Xp1 = (x - u0) * dx
    Yp1 = -(y - v0) * dy
    Xp = Xp1 * math.cos(q) + Yp1 * math.sin(q)
    Yp = -Xp1 * math.sin(q) + Yp1 * math.cos(q)
    sq = math.sqrt(f * f + Yp * Yp)
    eta = math.atan(Yp / f)
    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
    seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
    MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))
    OM = H / math.tan(seta + eta)
    epsilon = math.atan((H * Xp / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))) / OM)
    d = math.sqrt(MP ** 2 + OM ** 2)
    Xw = d * math.cos(gamma + epsilon)
    Yw = -d * math.sin(gamma + epsilon)
    return (Yw - val)**2   # 误差平方和

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

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

    # 网格搜索
    for x in x_range:
        for y in y_range:
            error = equations_xto0(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(x, y, val):
    Xp = (x - u0) * dx
    Yp = -(y - v0) * dy
    Xp1 = (x - u0) * dx
    Yp1 = -(y - v0) * dy
    Xp = Xp1 * math.cos(q) + Yp1 * math.sin(q)
    Yp = -Xp1 * math.sin(q) + Yp1 * math.cos(q)
    sq = math.sqrt(f * f + Yp * Yp)
    eta = math.atan(Yp / f)
    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
    seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
    MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))
    OM = H / math.tan(seta + eta)
    epsilon = math.atan((H * Xp / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))) / OM)
    d = math.sqrt(MP ** 2 + OM ** 2)
    Xw = d * math.cos(gamma + epsilon)
    Yw = -d * math.sin(gamma + epsilon)
    return (Xw-val)**2   # 误差平方和