# -*- coding:utf-8 -*-
# @Author len
# @Create 2023/12/13 15:36

# -*- coding:utf-8 -*-
# @Author len
# @Create 2023/11/30 11:08

import sensor, lcd, time
from machine import Timer, PWM, UART, Timer

lcd.init(freq=15000000)  # 初始化LCD
sensor.reset()  # 复位和初始化摄像头
sensor.set_vflip(1)  # 将摄像头设置成后置方式（所见即所得）
sensor.set_pixformat(sensor.RGB565)  # 设置像素格式为彩色 RGB565
sensor.set_framesize(sensor.QVGA)  # 设置帧大小为 QVGA (320x240)
# sensor.set_windowing((100, 0, 160, 240))
# sensor.set_windowing((0, 0, 320, 120))

sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.set_auto_gain(0, 0)
sensor.skip_frames(time=200)  # 等待设置生效


def Servo(angle):
    '''
    说明：舵机控制函数
    功能：180度舵机：angle:-90至90 表示相应的角度
         360连续旋转度舵机：angle:-90至90 旋转方向和速度值。
        【duty】占空比值：0-100
    '''
    # PWM通过定时器配置，接到IO17引脚
    tim_pwm = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
    S1 = PWM(tim_pwm, freq=50, duty=0, pin=17)
    S1.duty((angle + 90) / 180 * 10 + 2.5)


Servo(24)

# 阈值和颜色标签
# thresholds = [(35, 100, 6, 127, 0, 127, "红色"),  # 红色阈值
#               (25, 100, -6, 127, 5, 43, "黄色"),  # 黄色阈值
#               (52, 100, -128, -5, 5, 127, "绿色")]  # 绿色阈值

thresholds = [(51, 62, 33, 101, 14, 127, "红色"),  # 红色阈值
              (39, 98, -17, 21, 17, 57, "黄色"),  # 黄色阈值
              (44, 90, -61, -13, -3, 30, "绿色")]  # 绿色阈值
count = 0
while True:
    img = sensor.snapshot()
    max_blob = None
    max_blob_size = 0
    max_blob_color = ""
    max_blob_color_index = 0
    print(img.width(), img.height())

    img = img.crop((160, 0, 160, 240))

    image_save = "/sd/img/example{}.jpg".format(count)
    img.save(image_save)
    print("Image saved:", image_save)
    count += 1

    # 找圆
    circles = img.find_circles(threshold=3500, x_margin=10, y_margin=10, r_margin=10,
                               r_min=2, r_max=100, r_step=2)

    for index, threshold in enumerate(thresholds):
        # 查找每种颜色的色块
        blobs = img.find_blobs([threshold[:-1]], pixels_threshold=200, area_threshold=200, merge=True)
        if blobs:
            # 找到最大的色块
            blob = max(blobs, key=lambda b: b.pixels())
            if blob.pixels() > max_blob_size:
                max_blob = blob
                max_blob_size = blob.pixels()
                max_blob_color = threshold[-1]  # 颜色标签
                max_blob_color_index = index
    print(circles)
    # 画圆
    for c in circles:
        area = (c.x() - c.r(), c.y() - c.r(), 2 * c.r(), 2 * c.r())
        img.draw_rectangle(area, color=(255, 255, 255))
    # 绘制最大色块的矩形和中心十字，并输出颜色
    if max_blob:
        img.draw_rectangle(max_blob[0:4])
        img.draw_cross(max_blob[5], max_blob[6])
        print("最大色块的颜色是:", max_blob_color, max_blob_color_index)

    lcd.display(img)