import sensor
import image
import time
import math
import lcd

threshold_index = 0

thresholds = [(72, 40, -29, 60, -62, -2)]

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=2000)
sensor.set_auto_gain(False) # 关闭增益
sensor.set_auto_whitebal(False) # 关闭白平衡
clock = time.clock()
lcd.init()

roi = (0, 0, 320, 240)

blue_threshold = (72, 40, -29, 60, -62, -2)

detected_rectangles = []  # 添加一个保存矩形的列表

# 定义矩形重叠函数
def rectangle_overlap(rect1, rect2):
    x_overlap = max(0, min(rect1[0] + rect1[2], rect2[0] + rect2[2]) - max(rect1[0], rect2[0]))
    y_overlap = max(0, min(rect1[1] + rect1[3], rect2[1] + rect2[3]) - max(rect1[1], rect2[1]))
    overlap_area = x_overlap * y_overlap
    rect1_area = rect1[2] * rect1[3]
    rect2_area = rect2[2] * rect2[3]
    return overlap_area > 0 and overlap_area / min(rect1_area, rect2_area) > 0.5

while True:
    clock.tick()
    img = sensor.snapshot()
    time.sleep(2)
    img = img.crop(roi)
    blobs = img.find_blobs([blue_threshold])
    for blob in img.find_blobs([thresholds[threshold_index]], pixels_threshold=50, area_threshold=50, invert=True, merge=False):
        if blob.elongation() > 0.5:
            img.draw_edges(blob.min_corners(), color=(255, 0, 0))
            img.draw_line(blob.major_axis_line(), color=(0, 255, 0))
            img.draw_line(blob.minor_axis_line(), color=(0, 0, 255))
        img.draw_rectangle(blob.rect())
        current_rectangle = blob.rect()  # 记录当前矩形
        is_overlap = False
        for rectangle in detected_rectangles:
            if rectangle_overlap(current_rectangle, rectangle):
                is_overlap = True
                break
        if not is_overlap:
            detected_rectangles.append(current_rectangle)  # 将矩形添加到列表中
        img.draw_cross(blob.cx(), blob.cy())
        img.draw_keypoints([(blob.cx(), blob.cy(), int(math.degrees(blob.rotation())))], size=20)
    print("target count:", len(detected_rectangles))  # 统计矩形个数
    print(clock.fps())

    lcd.display(img)