import cv2
import numpy as np

def detect_lines_in_depth_image(depth_image, roi,min_depth):
    """
    Detect lines in a depth image within a specified Region of Interest (ROI).

    :param depth_image: The depth image in grayscale16 format.
    :param roi: The Region of Interest (ROI) to limit the detection region within the depth image.
    :return: The detected lines with coordinates relative to the original depth image.
    """
    lines_list = []
    # Convert the depth image to a format suitable for line detection using OpenCV
    # Scale the depth image from uint16 to uint8
    # Extract the ROI from the depth image
    x, y, w, h = roi
    roi_image = depth_image[y:y+h, x:x+w]

    min_dist = np.min(roi_image)
    max_dist = np.max(roi_image)

    print(f"ROI shape：{roi_image.shape}")
    print(f"ROI深度范围：{min_dist} - {max_dist}")
    # norm_img = cv2.normalize(roi_image, None, 0,255, cv2.NORM_MINMAX, cv2.CV_8U)
    
    min_dist = min_depth
    # max_dist = 1800
    norm_img = (roi_image-min_dist)/(max_dist-min_dist)
    norm_img = norm_img * 255
    norm_img = np.clip(norm_img,0,255)
    norm_img = norm_img.astype(np.uint8)
    
#    cv2.imwrite('dbg_norm.png',norm_img)


    edges = cv2.Canny(norm_img,15,30,apertureSize=3)
#    cv2.imwrite('dbg_edges.png',edges)

    # Use OpenCV's line detection algorithm (e.g., HoughLines or HoughLinesP) to detect lines within the specified ROI
    lines = cv2.HoughLinesP(edges, 1, np.pi/180, 20, minLineLength=10, maxLineGap=100)

    norm_depth_img = cv2.normalize(depth_image, None, 0,255, cv2.NORM_MINMAX, cv2.CV_8U)
    colored_img = cv2.cvtColor(norm_depth_img,cv2.COLOR_GRAY2BGR)
    # Adjust the line coordinates to be relative to the original depth image
    if lines is not None:
        for points in lines:
            # Extracted points nested in the list
            x1,y1,x2,y2=points[0]
            x1=x1+x
            y1=y1+y
            x2=x2+x
            y2=y2+y
            # Draw the lines joing the points
            # On the original image
            cv2.line(colored_img,(x1,y1),(x2,y2),(0,255,0),2)
            # Maintain a simples lookup list for points
            lines_list.append([(x1,y1),(x2,y2)])
            
    # Save the result image
    # cv2.imwrite('dbg_detectedLines.png',colored_img)
    # Return the detected lines with adjusted coordinates
    return lines_list

def calculate_line_angle(line):
    """
    计算线条与水平线（x轴）之间的角度。

    :param line: 线条的两个端点坐标，格式为 [(x1, y1), (x2, y2)]
    :return: 线条与水平线之间的角度（以度为单位，范围为0-180度）
    """
    # 提取两个点的坐标
    (x1, y1), (x2, y2) = line
    
    # 计算斜率，处理垂直线的情况
    if x2 - x1 == 0:
        return 90.0  # 垂直线
    
    # 计算斜率
    slope = (y2 - y1) / (x2 - x1)
    
    # 计算角度（弧度）并转换为度
    angle = np.arctan(slope) * 180 / np.pi
    
    # 确保角度为正（0-180度范围内）
    if angle < 0:
        angle += 180
    #转换成与相机中线之间的角度
     
    return angle




def calculate_distance_point_to_line(point, line):
    """
    计算一个点到一条线的距离。

    :param point: 点的坐标，格式为 (x, y)
    :param line: 线的两个端点坐标，格式为 [(x1, y1), (x2, y2)]
    :return: 点到线的距离
    """
    (x0, y0) = point
    (x1, y1), (x2, y2) = line
    numerator = abs((y2 - y1) * x0 - (x2 - x1) * y0 + x2 * y1 - y2 * x1)
    denominator = np.sqrt((y2 - y1) ** 2 + (x2 - x1) ** 2)
    return (numerator / denominator)*2.5+5

if __name__ == '__main__':
    img = cv2.imread('dbg_233900303_z.png',cv2.IMREAD_UNCHANGED)
    line_list = detect_lines_in_depth_image(img,(320,240,640,480),1100)
    print(line_list)
    for line in line_list:
        angle = calculate_line_angle(line)
        dist = calculate_distance_point_to_line((320,240),line)
        print(f'angle={angle} dist={dist}')