import sensor, image, time, tf
from machine import UART
import seekfree
from seekfree import Timer

sensor.reset()                      # 复位并初始化传感器
sensor.set_pixformat(sensor.RGB565) # 设置像素格式为RGB565（或灰度）
sensor.set_framesize(sensor.QVGA)   # 设置帧大小为QVGA（320x240）
sensor.skip_frames(time=2000)       # 等待设置生效
clock = time.clock()                # 创建一个时钟对象来跟踪FPS
uart = UART(2, baudrate=115200)     # 初始化UART通信


# 目标检测模型
face_detect = '/sd/yolo3_iou_smartcar_final_with_post_processing.tflite'
net = tf.load(face_detect)
# 深度学习模型
net_path = "zimu_shuzi.tflite"#数字+字母
labels = [line.rstrip() for line in open("/sd/3.txt")]
net_eiq = tf.load(net_path, load_to_fb=True)
# 创建字符串映射到数字的字典
string_to_number = {label: index + 1 for index, label in enumerate(labels)}


# 深度学习模型
net_path1 = "mobilenet_v2-2024-03-08-99.6(1).tflite"#照片
labels1 = [line.rstrip() for line in open("/sd/labels.txt")]
net_eiq1 = tf.load(net_path1, load_to_fb=True)
# 创建字符串映射到数字的字典
string_to_number1 = {label: index + 1 for index, label in enumerate(labels1)}


# 定时器回调函数
def timer_callback(timer):
    global num
    num += 1

# 用于存储最近10次的 error_x 和 error_y
error_x_list = []
error_y_list = []

# 让eiq识别更准
previous_value = None
consecutive_count = 0


# 中值计算函数
def median(data):
    data.sort()
    n = len(data)
    if n % 2 == 0:
        return (data[n // 2 - 1] + data[n // 2]) / 2
    else:
        return data[n // 2]

x, error_x, error_y, error_x_sign, error_y_sign = 0, 0, 0, 0, 0  # 初始化变量
min_x_obj = [0, 0]            #目标检测找最小值
min_x_center = [0, 0]         #目标检测找最小值中点左边
#data = b''
num = 200                     #计时器，现在没用到
fc_num = 0                    #只是一个标志位，防跳变的，这里是限定时间的
fc = 0                        #只是一个标志位，防跳变的
flag = 0
n = 0
pic_x = 127
pic_y = 172
flag_exchange = 0             #处理uart的状态标志位
roi=[0,0,320,240]             #敏感区
roi_x = 0                     #敏感区四个值
roi_y = 0
roi_width = 320
roi_height = 240
open_close = 0
eiq_mode_left_flag = 5

has_valid_rect_flag = 0

def process_rects(rects):
    global flag, error_x_list, error_ys_list, median_error_x, median_error_y, error_x_sign, error_y_sign,error_x,error_y# 确保可以修改全局变量
    global target_x,target_y,rect_top_right_x,rect_top_right_y,rect_bottom_right_x,rect_bottom_right_y,rect_bottom_left_x,rect_bottom_left_y,rect_top_left_x,rect_top_left_y,yigezhi####
    global roi,flag_exchange
    global roi_x,roi_y,roi_width,roi_height
    global fc,fc_num,num
    for rect in rects:
        w_h_rate = rect.w() / rect.h() if rect.w() > rect.h() else rect.h() / rect.w()
        rect_center_x = rect.x() + rect.w() // 2
        rect_center_y = rect.y() + rect.h() // 2

        rect_top_right_x = rect.x() + rect.w()   #矩形右上点坐标
        rect_top_right_y = rect.y()              #矩形右上点坐标

        rect_bottom_right_x = rect.x() + rect.w()#矩形右下点坐标
        rect_bottom_right_y = rect.y() + rect.h()#矩形右下点坐标

        rect_bottom_left_x = rect.x()            #矩形左下点坐标
        rect_bottom_left_y = rect.y() + rect.h() #矩形左下点坐标

        rect_top_left_x = rect.x()               #矩形左上点坐标
        rect_top_left_y = rect.y()               #矩形左上点坐标
        #print(rect_center_x)
        #print(rect_center_y)
        #print("宽高比: ", w_h_rate)
        #print("矩形面积: ", rect.w() * rect.h())

        yigezhi_x = 40#(rect_bottom_right_x - rect_bottom_left_x)/3
        yigezhi_y = 40#(rect_bottom_left_y - rect_top_left_y)/3

        target_x = 160
        target_y = 120
        #照片位于左上角
        if (rect_center_x<target_x) and (rect_center_y<target_y):
            print(1)
            roi_x = rect_top_left_x - yigezhi_x
            roi_y = rect_top_left_y - yigezhi_y
            roi_width = target_x+30-rect_top_left_x + 2*yigezhi_x
            roi_height = target_y+30-rect_top_left_y + 2*yigezhi_y
        #照片位于右上角
        elif (rect_center_x>target_x) and (rect_center_y<target_y):
            print(2)
            roi_x = target_x-30 - yigezhi_x
            roi_y = rect_top_right_y - yigezhi_y
            roi_width = rect_top_right_x-(target_x-30) + 2*yigezhi_x
            roi_height = target_y+30-rect_top_right_y + 2*yigezhi_y
        #照片位于左下角
        elif (rect_center_x<target_x) and (rect_center_y>target_y):
            print(3)
            roi_x = rect_bottom_left_x - yigezhi_x
            roi_y = target_y-30 - yigezhi_y
            roi_width = target_x+30-rect_bottom_left_x + 2*yigezhi_x
            roi_height = rect_bottom_left_y-(target_y-30) + 2*yigezhi_y
        #照片位于右下角
        elif (rect_center_x>target_x) and (rect_center_y>target_y):
            print(4)
            roi_x = target_x-30 - yigezhi_x
            roi_y = target_y-30 - yigezhi_y
            roi_width = rect_bottom_right_x-(target_x-30) + 2*yigezhi_x
            roi_height = rect_bottom_right_y-(target_y-30) + 2*yigezhi_y
        #照片和目标点重合
        if ((abs(rect_center_x-target_x))<15) and ((abs(rect_center_y-target_y))<15):
            print(5)
            roi_x = max(target_x - 60, 0)
            roi_y = max(target_y - 60, 0)
            roi_width = min(120, 320 - roi_x)
            roi_height = min(120, 240 - roi_y)

        if roi_x + roi_width > 320:
            roi_width = 320 - roi_x
        if roi_y + roi_height > 240:
            roi_height = 240 - roi_y



        if roi_x < 0:
            roi_x = 0
        if roi_y < 0:
            roi_y = 0
        if roi_x > 320:
            roi_x = 320
        if roi_y > 240:
            roi_y = 240

        #if (rect_center_y)

        if (roi_width*roi_height)<24964:
            if (roi_y>82):
                roi_y = 82
                roi_width = 158
                roi_height = 158
            else:
                roi_width = 158
                roi_height = 158

        # 更新敏感区的坐标
        roi = [int(roi_x), int(roi_y), int(roi_width), int(roi_height)]

        img.draw_rectangle(roi, color=(0, 0, 0))
        if 1.5 > w_h_rate and 2000< rect.w() * rect.h() < 10000:  # 筛选符合要求的矩形
            error_x = rect_center_x - pic_x
            error_y = rect_center_y - pic_y
            img.draw_rectangle(rect.rect(), color=(0, 255, 0))
            img.draw_cross(rect_center_x, rect_center_y, color=(0, 255, 0))
            error_x_sign = 1 if error_x < 0 else 2
            error_y_sign = 1 if error_y < 0 else 2
            #print("error_x:",error_x)
            #print("error_y:",error_y)
            #print("fclalalalalalala",fc)
            if eiq_mode_left_flag == 5 or eiq_mode_left_flag == 6:
                if ((abs(error_x) >= 7) or (abs(error_y) >= 7 ) and flag_exchange == 1):
                    flag = 4
                if (abs(error_x) < 7) and (abs(error_y) < 7) and fc <= 30 and flag_exchange == 1:
                   fc = fc + 1                   #计算出现个数，防止跳变误判
                   fc_num = num
                   #flag = 5
                   #flag_exchange = 4
                if fc > 5 :
                   flag = 5                     #当大于10时候确定为目标检测成功
                   flag_exchange = 4
            if eiq_mode_left_flag == 7 :
                if ((abs(error_x) >= 20) or (abs(error_y) >= 20 ) and flag_exchange == 1):
                    flag = 4
                if (abs(error_x) < 20) and (abs(error_y) < 20) and fc <= 30 and flag_exchange == 1:
                   fc = fc + 1                   #计算出现个数，防止跳变误判
                   fc_num = num
                   flag = 5
                   flag_exchange = 4
                #if fc > 5 :
                #   flag = 5                     #当大于10时候确定为目标检测成功
                #   flag_exchange = 4


        if num - fc_num > 50:                 #清零(可有可无，影响不大)
           fc = 0
# 初始化 sorted_list 为一个空列表
sorted_list = []

# 创建定时器对象
seekfree.init()         # 初始化 seekfree 模块
tim = Timer(1, 100)     # 创建一个定时器：通道1，100ms间隔
tim.callback(timer_callback)  # 为定时器设置回调函数

while True:
    clock.tick()
    img = sensor.snapshot()
    # 定时器中断处理
    timer_callback(tim)

    # 从UART读取数据
    uart_data = uart.read()

    if uart_data:
        digit = chr(uart_data[0])  # 获取第一个字节的字符表示
        uart_data = bytes([ord(digit)])  # 将字符转换为对应的字节格式，如 b'\x06'
    # 存储检测到的对象
    detected_objects = []
    if eiq_mode_left_flag == 5 or  eiq_mode_left_flag == 6 :#捡照片
       pic_x = 138
       pic_y = 157
    if eiq_mode_left_flag == 7 :#放照片
       pic_x = 97
       pic_y = 112

    if n == 0:  # n==0时进行目标检测
        # 使用模型进行目标检测
        for obj in tf.detect(net, img):
            x1, y1, x2, y2, label, scores = obj
            if scores > 0.7:
                w = x2 - x1
                h = y2 - y1
                # 计算图像中边界框的坐标
                x1 = int((x1 - 0.1) * img.width())
                y1 = int(y1 * img.height())
                w = int(w * img.width())
                h = int(h * img.height())
                detected_objects.append((x1, y1, x1 + w, y1 + h))
                # 绘制边界框
                img.draw_rectangle((x1, y1, w, h), thickness=2)

        # 更新目标检测标志
        target_detected = 1 if detected_objects else 0

        # 找到最小 x 坐标的对象
        if detected_objects:
            min_x_obj = min(detected_objects, key=lambda obj: obj[0])
            min_x_center = ((min_x_obj[0] + min_x_obj[2]) // 2, (min_x_obj[1] + min_x_obj[3]) // 2)
            print("最小 x 坐标对象的中心坐标:", min_x_center)

        # 构建数据并通过UART发送
        if target_detected == 1:
            if eiq_mode_left_flag == 5:
                if min_x_center[0] > 160:
                   if min_x_center[1] > 90:  # 进行min_y_center的检查
                          flag = 1  # 减速
                elif min_x_center[0] <= 160:
                   if min_x_center[1] > 90:  # 进行min_y_center的检查
                      flag = 2  # 停车
                      fc += 1  # 计算出现次数，防止跳变误判
                      fc_num = num
                      if fc > 10:  # 当大于10时确定为目标检测成功
                         flag = 3  # 惯性导航，到照片附近
                         n = 1  # 进入下一个流程
                         fc = 0


            elif eiq_mode_left_flag == 6 :
                #if min_x_center[0] > 160:
                #   flag = 1  # 减速
                #if min_x_center[0] <= 160:
                   flag = 2  # 停车
                   fc += 1  # 计算出现次数，防止跳变误判
                   fc_num = num
                   if fc > 10:  # 当大于10时确定为目标检测成功
                      flag = 3  # 惯性导航，到照片附近
                      n = 1  # 进入下一个流程
                      fc = 0

            elif eiq_mode_left_flag == 7:
                #if min_x_center[0] > 160:
                #   flag = 1  # 减速
                #if min_x_center[0] <= 160:
                   flag = 2  # 停车
                   fc += 1  # 计算出现次数，防止跳变误判
                   fc_num = num
                   if fc > 10:  # 当大于10时确定为目标检测成功
                      flag = 3  # 惯性导航，到照片附近
                      n = 1  # 进入下一个流程
                      fc = 0
        elif target_detected == 0:
             flag = 0  # 正常行驶

        if num - fc_num > 50:  # 清零（可有可无，影响不大）
            fc = 0


    if uart_data == b'\x01':#矩形识别靠近
        flag_exchange = 1
        print("uart_data:",1)

    if uart_data == b'\x02':
        flag_exchange = 2
        print("uart_data:",2)

    if uart_data == b'\x03':
        flag_exchange = 3
        print("uart_data:",3)

    if uart_data == b'\x04':
        flag_exchange = 4
        print("uart_data:",4)

    if uart_data == b'\x05':
        #flag_exchange = 5
        eiq_mode_left_flag = 5
        print("uart_data:",5)

    if uart_data == b'\x06':
        #flag_exchange = 6
        eiq_mode_left_flag = 6
        print("uart_data:",6)

    if uart_data == b'\x07':
        #flag_exchange = 7
        eiq_mode_left_flag = 7
        print("uart_data:",7)

    if uart_data == b'\x08':#开启摄像头
        n = 0
        open_close = 0
        flag_exchange = 0
        flag = 0
        print("uart_data:",8)
    if uart_data == b'\x09':#关闭摄像头
        n = 666
        flag = 666
        open_close = 1
        flag_exchange = 0
        print("uart_data:",9)

    if n == 1:
        if flag_exchange == 1:
            rects = img.find_rects(threshold=6000,min_area=3000,max_area=7000,roi=roi)
            if rects:
                process_rects(rects)
            else:
                roi_x=abs(roi_x-10)
                roi_y=abs(roi_y-10)
                roi_width=min(roi_width+20,320)
                roi_height=min(roi_height+20,240)
                roi=[roi_x,roi_y,roi_width,roi_height]
                img.draw_rectangle(roi, color=(0, 0, 0))
        if flag_exchange == 2:
            if has_valid_rect_flag == 0:
                fc_num = num
                has_valid_rect_flag = 1

            rects = img.find_rects(threshold=6000,min_area=3000,max_area=7000,roi=roi)
            if rects:
                process_rects(rects)
            else:
                roi_x=abs(roi_x-10)
                roi_y=abs(roi_y-10)
                roi_width=min(roi_width+20,320)
                roi_height=min(roi_height+20,240)
                roi=[roi_x,roi_y,roi_width,roi_height]
                img.draw_rectangle(roi, color=(0, 0, 0))

            has_valid_rect = False

            if flag != 8:  # 检查 flag 是否已经是 8，如果是，则跳过处理逻辑
                if rects:
                    for rect in rects:
                        w_h_rate = rect.w() / rect.h() if rect.w() > rect.h() else rect.h() / rect.w()
                        if 1.5 > w_h_rate and 2000 < rect.w() * rect.h() < 10000:  # 筛选符合要求的矩形
                            has_valid_rect = True
                            rect_center_x = rect.x() + rect.w() // 2
                            rect_center_y = rect.y() + rect.h() // 2
                            error_x = rect_center_x - pic_x
                            error_y = rect_center_y - pic_y
                            error_x_sign = 1 if error_x < 0 else 2
                            error_y_sign = 1 if error_y < 0 else 2
                            break
                if has_valid_rect:
                    process_rects(rects)
                    print("发现符合要求的矩形，再次执行处理逻辑")
                    fc_num = num
                    flag = 7
                    flag_exchange = 666
                    has_valid_rect_flag = 0
                else:
                    if num - fc_num > 50:  # 超过一定时间未找到有效矩形
                        flag = 8
                        flag_exchange = 666
                        has_valid_rect_flag = 0
                        print("超过一定时间未找到有效矩形，设置flag为8")
                    else:
                        print("没有符合要求的矩形，继续等待")

    if n == 1 and flag_exchange == 4:  # 深度学习
        #rects = img.find_rects(threshold=5000,min_aspect_ratio=0.5, max_aspect_ratio=1.5)
        #if rects:
        #    for rect in rects:
        #        w_h_rate = rect.w() / rect.h() if rect.w() > rect.h() else rect.h() / rect.w()
        #        print(w_h_rate)
        #        print(rect.w() * rect.h())
        #        if (1.5 > w_h_rate) and (2000 < rect.w() * rect.h() < 10000):  # 筛选符合要求的矩形
        #            img.draw_rectangle(rect.rect(), color=(255, 0, 0))  # 绘制矩形外框
        #
        #            # 提取矩形框内的图像
        #            img1 = img.copy(1,1,roi=rect.rect())
        #            img2 = img.copy(roi=rect.rect())
                 for obj in tf.detect(net, img):
                    x1, y1, x2, y2, label, scores = obj

                    if scores > 0.70:
                        print(obj)
                        w = x2 - x1
                        h = y2 - y1
                        x1 = int((x1 - 0.1) * img.width())
                        y1 = int(y1 * img.height())
                        w = int(w * img.width())
                        h = int(h * img.height())
                        if x1 > 160:
                            x1 -= 25  # 向左移动10个像素

                        # 确保 x1 不小于 0
                        if x1 < 0:
                            x1 = 0
                        img.draw_rectangle((x1, y1, w, h), thickness=2)

                        # 复制矩形区域到img2
                        roi = (x1, y1, w, h)
                        img1 = img.copy(1,1,roi=roi)
                        img2 = img.copy(roi=roi)
                    if eiq_mode_left_flag == 7:
                        # 在矩形框内进行目标分类
                        for obj in tf.classify(net_eiq, img1, min_scale=1.0, scale_mul=0.5, x_overlap=0.0, y_overlap=0.0):
                            print("**********\nTop 1 检测在 [x=%d,y=%d,w=%d,h=%d]" % obj.rect())

                            # 将分类结果和置信度进行排序
                            sorted_list = sorted(zip(labels, obj.output()), key=lambda x: x[1], reverse=True)

                            # 打印置信度最高的结果
                            if sorted_list and len(sorted_list) > 0:
                                print("%s = %f" % (sorted_list[0][0], sorted_list[0][1]))

                                current_value = string_to_number[sorted_list[0][0]]
                                print(current_value)

                                # 检查是否连续5次相同
                                if previous_value == current_value:
                                    consecutive_count += 1
                                else:
                                    consecutive_count = 1
                                    previous_value = current_value

                                if consecutive_count >= 5:
                                    flag = 6
                                    flag_exchange = 666
                                    consecutive_count = 0
                            else:
                                print("未检测到对象")

                    if eiq_mode_left_flag == 5 or eiq_mode_left_flag == 6:
                        for obj in tf.classify(net_eiq1, img2, min_scale=1.0, scale_mul=0.5, x_overlap=0.0, y_overlap=0.0):
                            print("**********\nTop 1 检测在 [x=%d,y=%d,w=%d,h=%d]" % obj.rect())
                            # 将分类结果和置信度进行排序
                            sorted_list = sorted(zip(labels1, obj.output()), key=lambda x: x[1], reverse=True)

                            # 打印置信度最高的结果
                            if sorted_list and len(sorted_list) > 0:
                                print("%s = %f" % (sorted_list[0][0], sorted_list[0][1]))

                                current_value = string_to_number1[sorted_list[0][0]]
                                print(current_value)

                                # 检查是否连续5次相同
                                if previous_value == current_value:
                                    consecutive_count += 1
                                else:
                                    consecutive_count = 1
                                    previous_value = current_value

                                if consecutive_count >= 5:
                                    flag = 6
                                    flag_exchange = 666
                                    consecutive_count = 0
                            else:
                                print("未检测到对象")

    if flag_exchange == 3:
        flag = 0
        n = 0
        flag_exchange = 0
        error_x_list.clear()
        error_y_list.clear()
    if flag ==4:#矩形识别平移
        img_data = bytearray([0x2C, 0x12,
                              flag,
                              int(abs(error_x)),
                              int(abs(error_y)),
                              error_x_sign,#有个问题sign没有中值滤波
                              error_y_sign,
                              0,
                              eiq_mode_left_flag,
                              open_close, 0x5B])
        for byte in img_data:
            uart.write(bytearray([byte]))
            time.sleep(0.01)  # 每个字节之间稍作延迟（可根据需要调整）
        print(error_x)
        print(error_y)
        print([int(byte) for byte in img_data])

    if flag == 6:#深度学习eiq
        if eiq_mode_left_flag == 7:
            img_data = bytearray([0x2C, 0x12,
                                  6,
                                  12,
                                  0,
                                  0,#有个问题sign没有中值滤波
                                  0,
                                  string_to_number[sorted_list[0][0]],
                                  eiq_mode_left_flag,
                                  open_close, 0x5B])
            for byte in img_data:
                uart.write(bytearray([byte]))
                time.sleep(0.01)  # 每个字节之间稍作延迟（可根据需要调整）
            print([int(byte) for byte in img_data])
        if eiq_mode_left_flag == 5 or eiq_mode_left_flag == 6 :
            img_data = bytearray([0x2C, 0x12,
                                  6,
                                  12,
                                  0,
                                  0,#有个问题sign没有中值滤波
                                  0,
                                  string_to_number1[sorted_list[0][0]],
                                  eiq_mode_left_flag,
                                  open_close, 0x5B])
            for byte in img_data:
                uart.write(bytearray([byte]))
                time.sleep(0.01)  # 每个字节之间稍作延迟（可根据需要调整）
            print([int(byte) for byte in img_data])

    if flag == 666:
        img_data = bytearray([0x2C, 0x12,
                              0,
                              0,
                              0,
                              0,#有个问题sign没有中值滤波
                              0,
                              0,
                              eiq_mode_left_flag,
                              open_close, 0x5B])
        for byte in img_data:
            uart.write(bytearray([byte]))
            time.sleep(0.01)  # 每个字节之间稍作延迟（可根据需要调整）
        print([int(byte) for byte in img_data])
    if flag == 0 or flag == 1 or flag == 2 or flag == 3 or flag == 5 or flag == 7 or flag == 8:
        img_data = bytearray([0x2C, 0x12,
                              flag,
                              int(abs(error_x)),
                              int(abs(error_y)),
                              error_x_sign,
                              error_y_sign,
                              0,
                              eiq_mode_left_flag,
                              open_close, 0x5B])
        for byte in img_data:
            uart.write(bytearray([byte]))
            time.sleep(0.01)  # 每个字节之间稍作延迟（可根据需要调整）
        print([int(byte) for byte in img_data])
    print(n)
    print(flag)
    print(flag_exchange)
    print("eiq_mode_left_flag:",eiq_mode_left_flag)
    print(clock.fps())
    if uart.read():
        print(uart.read())



