import math
import numpy as np
import matplotlib.pyplot as plt
import calc_way
import get_data
import calc_slope_line
import cv2
import model
import os
import time

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

def vs_measurement(txt_name, position = 784):
    # 加载数据
    state, x_bot, y_bot, x_top, y_top = get_data.get_data(txt_name)
    if state == 0:
        return 0, None, None
    # 拟合上下沿
    slope_bot, intercept_bot, r2_bot = calc_slope_line.linear_regression(x_bot, y_bot)
    slope_top, intercept_top, r2_top = calc_slope_line.linear_regression(x_top, y_top)
    Xw_bot = []
    Yw_bot = []
    for i in range(len(x_bot)):
        Xw, Yw = calc_way.calc_distance(x_bot[i], y_bot[i], alpha, beta)
        Xw_bot.append(Xw)
        Yw_bot.append(Yw)
    slope_Xw, intercept_Xw, r2_Xw = calc_slope_line.linear_regression(Xw_bot, Yw_bot)
    # 计算路沿与车身夹角
    angle = math.atan(slope_Xw)
    # 位置修正
    position = position
    # 确认目标点位置并计算高度
    Yw_pos = slope_Xw * position + intercept_Xw
    x_pos, y_pos = calc_way.calc_distance2(position, Yw_pos, alpha, beta)
    print(x_pos, y_pos)
    k_pos = calc_slope_line.get_k(alpha, beta, x_pos, y_pos)
    b_pos = calc_slope_line.get_b(x_pos, y_pos, k_pos)
    print(k_pos, b_pos)
    x_pos_top, y_pos_top = calc_slope_line.find_intersection((k_pos, -1, b_pos), (slope_top, -1, intercept_top))
    Zw_pos = calc_way.calc_height(x_pos, y_pos, x_pos_top, y_pos_top, alpha, beta)
    distance_pos = -(Yw_pos - 53) * math.cos(angle)
    return 1, Zw_pos, distance_pos


result = vs_measurement(r"C:\Users\Administrator\Desktop\BYD\7.16\0716\new175.8595023857456.txt", position=900)
print(result)