import math
import model
import calc_way
from scipy import stats

model = model.Model()
alpha = model.alpha
beta = model.beta
q = model.q
"""
计算地面点的竖直垂线在图像中的斜率
参数：点在图像中的坐标x、y，相机的外部偏转角参数alpha、beta
返回值：斜率k
"""
def get_k(alpha,beta,x,y):
    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)))
    Xw,Yw = calc_way.calc_distance(x,y,alpha,beta)
    u = Xw * math.cos(gamma) - Yw * math.sin(gamma)
    v = Xw * math.sin(gamma) + Yw * math.cos(gamma)
    tanlamda = -v/u/math.sqrt(1+(math.tan(math.pi/2-seta))**2)
    lamda = math.atan(tanlamda)
    k = math.tan(math.pi/2-lamda)
    return k

def get_k2(alpha,beta,Xw,Yw):
    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)))
    u = Xw * math.cos(gamma) - Yw * math.sin(gamma)
    v = Xw * math.sin(gamma) + Yw * math.cos(gamma)
    tanlamda = -v / u / math.sqrt(1 + (math.tan(math.pi / 2 - seta)) ** 2)
    lamda = math.atan(tanlamda)
    k = math.tan(math.pi / 2 - lamda)
    return k

"""
计算地面点的竖直垂线在图像中的截距
参数：点在图像中的坐标x、y，斜率k
返回值：截距b
"""
def get_b(x,y,k):
    b = y - x * k
    return b


"""
将检测到的路沿上侧数据点拟合为一条直线
参数：上侧数据点坐标x_top,y_top
返回值：拟合出直线的斜率slope、截距intercept、r方值
"""
def linear_regression(x, y):
    slope, intercept, r_value, p_value, std_err = stats.linregress(x, y)
    return slope, intercept, abs(r_value)

"""
计算图像中两条直线的交点
参数：   line1: 第一条直线的系数 (A1, B1, C1)
        line2: 第二条直线的系数 (A2, B2, C2)
返回值：交点在图像中的二维坐标x、y
"""
def find_intersection(line1, line2):
    A1, B1, C1 = line1
    A2, B2, C2 = line2

    # 计算行列式
    denominator = A1 * B2 - A2 * B1

    if denominator == 0:
        # 直线平行或重合
        return None
    else:
        x = (B1 * C2 - B2 * C1) / denominator
        y = (A2 * C1 - A1 * C2) / denominator
        return (x, y)