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


def vs_measurement(txt_name, position=784, config_path: str = None):
    # 初始化相机模型
    cameraModel = cameramodel.CameraModel(config_path)

    # 加载数据
    state, x_bot, y_bot, slope_bot, intercept_bot, x_top, y_top, slope_top, intercept_top = get_data.get_data(cameraModel, txt_name)
    if state == 0:
        return 0, None, None

    # 计算底部路沿
    Xw_bot = []
    Yw_bot = []
    for i in range(len(x_bot)):
        Xw, Yw = calc_way.calc_distance(cameraModel, x_bot[i], y_bot[i])
        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)

    # 确认目标点位置
    Yw_pos = slope_Xw * position + intercept_Xw
    x_pos, y_pos = calc_way.calc_distance2(cameraModel, position, Yw_pos)
    k_pos = calc_slope_line.get_k(cameraModel, x_pos, y_pos, calc_way.calc_distance)
    b_pos = calc_slope_line.get_b(x_pos, y_pos, k_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(cameraModel, x_pos, y_pos, x_pos_top, y_pos_top)

    # 位置修正
    distance_pos = (-Yw_pos + cameraModel.position_offset_y) * math.cos(angle)
    return 1, Zw_pos, distance_pos, int(x_pos), cameraModel.camera_height - int(y_pos)

# folder_path = r"C:\Users\Administrator\Desktop\BYD\7.16\0716"
# txt_files = glob.glob(os.path.join(folder_path, '*.txt'))
# for file_path in txt_files:
#     with open(file_path, 'r', encoding='utf-8') as file:
#         print(file_path)
#         result = vs_measurement(file_path, position=784, config_path="updated_config.json")
#         print(result)

# result = vs_measurement(r"C:\Users\Administrator\Desktop\BYD\0718\new233.1718914966782.txt", position=200)
# print(result)