import math
import model
import calc_way
from scipy import stats

model = model.Model()
alpha = model.alpha
beta = model.beta

"""
计算地面点的竖直垂线在图像中的斜率
参数：点在图像中的坐标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

"""
计算地面点的竖直垂线在图像中的截距
参数：点在图像中的坐标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, r_value**2