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
import glob
import JS
model = model.Model()
alpha = model.alpha
beta = model.beta
q=model.q

def vs_measurement(txt_name, position = 784, config_path: str = None):
    cameraModel = JS.CameraModel(config_path)
    # 加载数据
    state, x_bot, y_bot, slope_bot, intercept_bot, x_top, y_top, slope_top, intercept_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


def vs_measurement_pic(txt_name, img_path, output_folder):
    # #画图
    # fig, axes = plt.subplots(nrows=4, ncols=2, figsize=(10, 8))

    state, x_bot, y_bot, slope_bot, intercept_bot, x_top, y_top, slope_top, intercept_top = get_data.get_data(txt_name)
    if state == 0:
        return 0, None, None

    for i in range(len(x_bot)):
        k = calc_slope_line.get_k(alpha,beta,x_bot[i],y_bot[i])
        b = calc_slope_line.get_b(x_bot[i],y_bot[i],k)
        x = (intercept_top - b) / (k - slope_top)
        y = k * x + b
        Zw = calc_way.calc_height(x_bot[i],y_bot[i], x, y, alpha, beta)
        Xw, Yw = calc_way.calc_distance(x_bot[i],y_bot[i], alpha, beta)
        # 3. 读取图像
        image = cv2.imread(img_path)

        # 检查图像是否成功加载
        if image is None:
            print(f"错误: 无法读取图像 {img_path}")
            exit()
        # 计算直线上的两个点（x=0 和 x=图像宽度）
        # x1 = 0
        # y1 = 960 - int(slope_top * x1 + intercept_top)  # 计算 y1
        #
        # x2 = int(x_top[-1])  # 图像宽度
        # y2 = 960 - int(slope_top * x2 + intercept_top)  # 计算 y2
        #
        # # 绘制直线（BGR 颜色：(0, 255, 0) 绿色，线宽 2）
        # cv2.line(image, (x1, y1), (x2, y2), (0, 255, 0), 1)
        #
        # x1 = 0
        # y1 = 960 - int(slope_bot * x1 + intercept_bot)  # 计算 y1
        #
        # x2 = int(x_bot[-1])  # 图像宽度
        # y2 = 960 - int(slope_bot * x2 + intercept_bot)  # 计算 y2
        #
        # # 绘制直线（BGR 颜色：(0, 255, 0) 绿色，线宽 2）
        # cv2.line(image, (x1, y1), (x2, y2), (0, 255, 0), 1)

        cv2.line(image, (int(x_bot[i]), 960 - int(y_bot[i])), (int(x), 960 - int(y)), (0, 0, 255), 2)
        cv2.putText(image, f"Xw = {int(Xw)},Yw = {int(Yw)},Zw = {int(Zw)}", (640, 200), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
        cv2.putText(image, f"x = {x_bot[i]}, y = {y_bot[i]}", (int(x_bot[i]), 960 - int(y_bot[i])),
                    cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
        filename = f"{i}.jpg"
        output_path = os.path.join(output_folder, filename)
        cv2.imwrite(output_path, image)


# t = time.time()
# result = vs_measurement(r"C:\Users\Administrator\Desktop\BYD\0718\new428.75662880363143.txt", position=500)
# print(result)
# vs_measurement_pic(r"C:\Users\Administrator\Desktop\BYD\0718\new428.75662880363143.txt",r"C:\Users\Administrator\Desktop\BYD\0718\new428.75662880363143.jpg",r"C:\Users\Administrator\Desktop\BYD\0718\new428.75662880363143")
# print(f"time: {time.time() - t}
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)
        print(result)