import cv2
import math
from utils.pixel_convert import pixel2robot

def draw_lines_on_image(image):
    # 存储线段的起点和终点坐标
    lines = []
    start_point = None

    # 鼠标回调函数，用于捕获鼠标事件
    def draw_line(event, x, y, flags, param):
        nonlocal start_point
        if event == cv2.EVENT_LBUTTONDOWN:
            if start_point is None:
                start_point = (x, y)
            else:
                end_point = (x, y)
                lines.append((start_point, end_point))
                cv2.line(image, start_point, end_point, (0, 255, 0), 2)
                start_point = None

    # 创建一个窗口
    cv2.namedWindow('Image')
    cv2.setMouseCallback('Image', draw_line)

    while True:
        cv2.imshow('Image', image)
        key = cv2.waitKey(1) & 0xFF
        if key == 27:  # 按下 'ESC' 键退出
            break

    cv2.destroyAllWindows()
    return lines

def calculate_midpoint_and_angle_2d(start_point, end_point):
    # 计算中点坐标
    mid_point = (
        (start_point[0] + end_point[0]) / 2,
        (start_point[1] + end_point[1]) / 2
    )

    # 计算线段的角度（以度为单位）
    delta_x = end_point[0] - start_point[0]
    delta_y = end_point[1] - start_point[1]
    angle = math.degrees(math.atan2(delta_y, delta_x))
    # 将角度值换算到0-180度范围内
    angle = (angle + 180) % 180

    return mid_point, angle

def cap_and_mark(cap):
    if not cap.isOpened():
        print("无法打开摄像头。")
        return

    ret, frame = cap.read()
    if not ret:
        print("无法获取图像帧。")
        return
    else:
        lines = draw_lines_on_image(frame)
        robo_lines = []

        for line in lines:
            start_point, end_point = line
            start_point = pixel2robot(start_point[0], start_point[1])
            end_point = pixel2robot(end_point[0], end_point[1])
            # turn array to tuple
            start_point = tuple(start_point)
            end_point = tuple(end_point)

            mid_point, angle = calculate_midpoint_and_angle_2d(start_point, end_point)
            robo_lines.append((mid_point, angle))
        return robo_lines
    

# 测试函数
if __name__ == "__main__":
    # image_path = '1723457093.2643137.jpg'
    # image = cv2.imread(image_path)
    # if image is None:
    #     print("图像加载失败！")
    # else:
    #     lines = draw_lines_on_image(image)
    #     print("绘制的线段坐标:", lines)
    cap = cv2.VideoCapture(1)
    cap.set(6,cv2.VideoWriter.fourcc('M','J','P','G'))

    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
    lines = cap_and_mark(cap)
    print(lines)