import sensor, image, time

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)

clock = time.clock()

def find_blobs_in_rois(img):            #感兴趣区找线
    roi_blobs_result = {}
    for roi_direct in ROIS.keys():
        roi_blobs_result[roi_direct] = {
            "cx": -1,
            "cy": -1,
            "blob_flag": False
        }
    for roi_direct, roi in ROIS.items():
        blobs = img.find_blobs(Color_threshold, roi=roi, merge=True, pixels_area=40)
        #print(roi_direct, end="\t")
        #print(blobs)  >>>[{"x":155, "y":18, "w":4, "h":14, "pixels":73, "cx":157, "cy":24, "rotation":1.550840, "code":1, "count":1}, {"x":155, "y":85, "w":4, "h":16, "pixels":85, "cx":157, "cy":93, "rotation":1.570796, "code":1, "count":1}]
        #pyb.delay(500)
        #print("****************")
        if len(blobs) == 0:
            continue
        if len(blobs) >= 1:
            largest_blob = max(blobs, key=lambda b: b.pixels())
            for abc in blobs:
                if abc == largest_blob:
                    blobs.remove(abc)
                    #print(blobs)
                    break
            #print(len(blobs))
            #print(largest_blob)
            #pyb.delay(500);
            x, y, width, height = largest_blob[:4]
            if not (width >= areas[0] and width <= areas[1] and height >= areas[2] and height <= areas[3]):
                continue
            roi_blobs_result[roi_direct]['cx'] = largest_blob.cx()
            roi_blobs_result[roi_direct]['cy'] = largest_blob.cy()
            roi_blobs_result[roi_direct]['blob_flag'] = True
            if is_debug and (not roi_direct=='middle' or is_middle):
                img.draw_rectangle((x, y, width, height), color=(255))
            if len(blobs) >= 1:
                largest_blob1 = max(blobs, key=lambda b: b.pixels())
                x, y, width, height = largest_blob1[:4]
                if not (width >= areas[0] and width <= areas[1] and height >= areas[2] and height <= areas[3]):
                    continue
                if(roi_direct == "left" or roi_direct == "right"):
                    if(abs(largest_blob1.cy() - largest_blob.cy()) < 35):
                        roi_blobs_result[roi_direct]["cx"] = int((largest_blob1.cx() + largest_blob.cx()) / 2)
                        roi_blobs_result[roi_direct]["cy"] = int((largest_blob1.cy() + largest_blob.cy()) / 2)
                        roi_blobs_result[roi_direct]['blob_flag'] = True
                        if is_debug and (not roi_direct=='middle' or is_middle):
                            if assist:
                                img.draw_cross(int(roi_blobs_result[roi_direct]["cx"]), int(roi_blobs_result[roi_direct]["cy"]),
                                           size=5, color=(255))
                            img.draw_rectangle((x, y, width, height), color=(255))
                if(roi_direct == "up" or roi_direct == "down" or roi_direct == 'middle'):
                    if(abs(largest_blob1.cx() - largest_blob.cx()) < 35):
                        roi_blobs_result[roi_direct]["cx"] = int((largest_blob1.cx() + largest_blob.cx()) / 2)
                        roi_blobs_result[roi_direct]["cy"] = int((largest_blob1.cy() + largest_blob.cy()) / 2)
                        roi_blobs_result[roi_direct]['blob_flag'] = True
                        if is_debug and (not roi_direct=='middle' or assist):
                            if assist:
                                img.draw_cross(int(roi_blobs_result[roi_direct]["cx"]), int(roi_blobs_result[roi_direct]["cy"]),
                                           size=5, color=(255))
                            img.draw_rectangle((x, y, width, height), color=(255))
    #print(roi_blobs_result)  >>>{'right': {'blob_flag': True, 'cx': 157, 'cy': 61}, 'middle': {'blob_flag': False, 'cx': -1, 'cy': -1}, 'down': {'blob_flag': False, 'cx': -1, 'cy': -1}, 'up': {'blob_flag': True, 'cx': 78, 'cy': 2}, 'left': {'blob_flag': True, 'cx': 2, 'cy': 62}}
    return roi_blobs_result
def find_blobs_in_rois(img):            #感兴趣区找线
    roi_blobs_result = {}
    for roi_direct in ROIS.keys():
        roi_blobs_result[roi_direct] = {
            "cx": -1,
            "cy": -1,
            "blob_flag": False
        }
    for roi_direct, roi in ROIS.items():
        blobs = img.find_blobs(Color_threshold, roi=roi, merge=True, pixels_area=40)
        #print(roi_direct, end="\t")
        #print(blobs)  >>>[{"x":155, "y":18, "w":4, "h":14, "pixels":73, "cx":157, "cy":24, "rotation":1.550840, "code":1, "count":1}, {"x":155, "y":85, "w":4, "h":16, "pixels":85, "cx":157, "cy":93, "rotation":1.570796, "code":1, "count":1}]
        #pyb.delay(500)
        #print("****************")
        if len(blobs) == 0:
            continue
        if len(blobs) >= 1:
            largest_blob = max(blobs, key=lambda b: b.pixels())
            for abc in blobs:
                if abc == largest_blob:
                    blobs.remove(abc)
                    #print(blobs)
                    break
            #print(len(blobs))
            #print(largest_blob)
            #pyb.delay(500);
            x, y, width, height = largest_blob[:4]
            if not (width >= areas[0] and width <= areas[1] and height >= areas[2] and height <= areas[3]):
                continue
            roi_blobs_result[roi_direct]['cx'] = largest_blob.cx()
            roi_blobs_result[roi_direct]['cy'] = largest_blob.cy()
            roi_blobs_result[roi_direct]['blob_flag'] = True
            if is_debug and (not roi_direct=='middle' or is_middle):
                img.draw_rectangle((x, y, width, height), color=(255))
            if len(blobs) >= 1:
                largest_blob1 = max(blobs, key=lambda b: b.pixels())
                x, y, width, height = largest_blob1[:4]
                if not (width >= areas[0] and width <= areas[1] and height >= areas[2] and height <= areas[3]):
                    continue
                if(roi_direct == "left" or roi_direct == "right"):
                    if(abs(largest_blob1.cy() - largest_blob.cy()) < 35):
                        roi_blobs_result[roi_direct]["cx"] = int((largest_blob1.cx() + largest_blob.cx()) / 2)
                        roi_blobs_result[roi_direct]["cy"] = int((largest_blob1.cy() + largest_blob.cy()) / 2)
                        roi_blobs_result[roi_direct]['blob_flag'] = True
                        if is_debug and (not roi_direct=='middle' or is_middle):
                            if assist:
                                img.draw_cross(int(roi_blobs_result[roi_direct]["cx"]), int(roi_blobs_result[roi_direct]["cy"]),
                                           size=5, color=(255))
                            img.draw_rectangle((x, y, width, height), color=(255))
                if(roi_direct == "up" or roi_direct == "down" or roi_direct == 'middle'):
                    if(abs(largest_blob1.cx() - largest_blob.cx()) < 35):
                        roi_blobs_result[roi_direct]["cx"] = int((largest_blob1.cx() + largest_blob.cx()) / 2)
                        roi_blobs_result[roi_direct]["cy"] = int((largest_blob1.cy() + largest_blob.cy()) / 2)
                        roi_blobs_result[roi_direct]['blob_flag'] = True
                        if is_debug and (not roi_direct=='middle' or assist):
                            if assist:
                                img.draw_cross(int(roi_blobs_result[roi_direct]["cx"]), int(roi_blobs_result[roi_direct]["cy"]),
                                           size=5, color=(255))
                            img.draw_rectangle((x, y, width, height), color=(255))
    #print(roi_blobs_result)  >>>{'right': {'blob_flag': True, 'cx': 157, 'cy': 61}, 'middle': {'blob_flag': False, 'cx': -1, 'cy': -1}, 'down': {'blob_flag': False, 'cx': -1, 'cy': -1}, 'up': {'blob_flag': True, 'cx': 78, 'cy': 2}, 'left': {'blob_flag': True, 'cx': 2, 'cy': 62}}
    return roi_blobs_result
def find_blobs_in_rois(img):            #感兴趣区找线
    roi_blobs_result = {}
    for roi_direct in ROIS.keys():
        roi_blobs_result[roi_direct] = {
            "cx": -1,
            "cy": -1,
            "blob_flag": False
        }
    for roi_direct, roi in ROIS.items():
        blobs = img.find_blobs(Color_threshold, roi=roi, merge=True, pixels_area=40)
        #print(roi_direct, end="\t")
        #print(blobs)  >>>[{"x":155, "y":18, "w":4, "h":14, "pixels":73, "cx":157, "cy":24, "rotation":1.550840, "code":1, "count":1}, {"x":155, "y":85, "w":4, "h":16, "pixels":85, "cx":157, "cy":93, "rotation":1.570796, "code":1, "count":1}]
        #pyb.delay(500)
        #print("****************")
        if len(blobs) == 0:
            continue
        if len(blobs) >= 1:
            largest_blob = max(blobs, key=lambda b: b.pixels())
            for abc in blobs:
                if abc == largest_blob:
                    blobs.remove(abc)
                    #print(blobs)
                    break
            #print(len(blobs))
            #print(largest_blob)
            #pyb.delay(500);
            x, y, width, height = largest_blob[:4]
            if not (width >= areas[0] and width <= areas[1] and height >= areas[2] and height <= areas[3]):
                continue
            roi_blobs_result[roi_direct]['cx'] = largest_blob.cx()
            roi_blobs_result[roi_direct]['cy'] = largest_blob.cy()
            roi_blobs_result[roi_direct]['blob_flag'] = True
            if is_debug and (not roi_direct=='middle' or is_middle):
                img.draw_rectangle((x, y, width, height), color=(255))
            if len(blobs) >= 1:
                largest_blob1 = max(blobs, key=lambda b: b.pixels())
                x, y, width, height = largest_blob1[:4]
                if not (width >= areas[0] and width <= areas[1] and height >= areas[2] and height <= areas[3]):
                    continue
                if(roi_direct == "left" or roi_direct == "right"):
                    if(abs(largest_blob1.cy() - largest_blob.cy()) < 35):
                        roi_blobs_result[roi_direct]["cx"] = int((largest_blob1.cx() + largest_blob.cx()) / 2)
                        roi_blobs_result[roi_direct]["cy"] = int((largest_blob1.cy() + largest_blob.cy()) / 2)
                        roi_blobs_result[roi_direct]['blob_flag'] = True
                        if is_debug and (not roi_direct=='middle' or is_middle):
                            if assist:
                                img.draw_cross(int(roi_blobs_result[roi_direct]["cx"]), int(roi_blobs_result[roi_direct]["cy"]),
                                           size=5, color=(255))
                            img.draw_rectangle((x, y, width, height), color=(255))
                if(roi_direct == "up" or roi_direct == "down" or roi_direct == 'middle'):
                    if(abs(largest_blob1.cx() - largest_blob.cx()) < 35):
                        roi_blobs_result[roi_direct]["cx"] = int((largest_blob1.cx() + largest_blob.cx()) / 2)
                        roi_blobs_result[roi_direct]["cy"] = int((largest_blob1.cy() + largest_blob.cy()) / 2)
                        roi_blobs_result[roi_direct]['blob_flag'] = True
                        if is_debug and (not roi_direct=='middle' or assist):
                            if assist:
                                img.draw_cross(int(roi_blobs_result[roi_direct]["cx"]), int(roi_blobs_result[roi_direct]["cy"]),
                                           size=5, color=(255))
                            img.draw_rectangle((x, y, width, height), color=(255))
    #print(roi_blobs_result)  >>>{'right': {'blob_flag': True, 'cx': 157, 'cy': 61}, 'middle': {'blob_flag': False, 'cx': -1, 'cy': -1}, 'down': {'blob_flag': False, 'cx': -1, 'cy': -1}, 'up': {'blob_flag': True, 'cx': 78, 'cy': 2}, 'left': {'blob_flag': True, 'cx': 2, 'cy': 62}}
    return roi_blobs_result
def find_blobs_in_rois(img):            #感兴趣区找线
    roi_blobs_result = {}
    for roi_direct in ROIS.keys():
        roi_blobs_result[roi_direct] = {
            "cx": -1,
            "cy": -1,
            "blob_flag": False
        }
    for roi_direct, roi in ROIS.items():
        blobs = img.find_blobs(Color_threshold, roi=roi, merge=True, pixels_area=40)
        #print(roi_direct, end="\t")
        #print(blobs)  >>>[{"x":155, "y":18, "w":4, "h":14, "pixels":73, "cx":157, "cy":24, "rotation":1.550840, "code":1, "count":1}, {"x":155, "y":85, "w":4, "h":16, "pixels":85, "cx":157, "cy":93, "rotation":1.570796, "code":1, "count":1}]
        #pyb.delay(500)
        #print("****************")
        if len(blobs) == 0:
            continue
        if len(blobs) >= 1:
            largest_blob = max(blobs, key=lambda b: b.pixels())
            for abc in blobs:
                if abc == largest_blob:
                    blobs.remove(abc)
                    #print(blobs)
                    break
            #print(len(blobs))
            #print(largest_blob)
            #pyb.delay(500);
            x, y, width, height = largest_blob[:4]
            if not (width >= areas[0] and width <= areas[1] and height >= areas[2] and height <= areas[3]):
                continue
            roi_blobs_result[roi_direct]['cx'] = largest_blob.cx()
            roi_blobs_result[roi_direct]['cy'] = largest_blob.cy()
            roi_blobs_result[roi_direct]['blob_flag'] = True
            if is_debug and (not roi_direct=='middle' or is_middle):
                img.draw_rectangle((x, y, width, height), color=(255))
            if len(blobs) >= 1:
                largest_blob1 = max(blobs, key=lambda b: b.pixels())
                x, y, width, height = largest_blob1[:4]
                if not (width >= areas[0] and width <= areas[1] and height >= areas[2] and height <= areas[3]):
                    continue
                if(roi_direct == "left" or roi_direct == "right"):
                    if(abs(largest_blob1.cy() - largest_blob.cy()) < 35):
                        roi_blobs_result[roi_direct]["cx"] = int((largest_blob1.cx() + largest_blob.cx()) / 2)
                        roi_blobs_result[roi_direct]["cy"] = int((largest_blob1.cy() + largest_blob.cy()) / 2)
                        roi_blobs_result[roi_direct]['blob_flag'] = True
                        if is_debug and (not roi_direct=='middle' or is_middle):
                            if assist:
                                img.draw_cross(int(roi_blobs_result[roi_direct]["cx"]), int(roi_blobs_result[roi_direct]["cy"]),
                                           size=5, color=(255))
                            img.draw_rectangle((x, y, width, height), color=(255))
                if(roi_direct == "up" or roi_direct == "down" or roi_direct == 'middle'):
                    if(abs(largest_blob1.cx() - largest_blob.cx()) < 35):
                        roi_blobs_result[roi_direct]["cx"] = int((largest_blob1.cx() + largest_blob.cx()) / 2)
                        roi_blobs_result[roi_direct]["cy"] = int((largest_blob1.cy() + largest_blob.cy()) / 2)
                        roi_blobs_result[roi_direct]['blob_flag'] = True
                        if is_debug and (not roi_direct=='middle' or assist):
                            if assist:
                                img.draw_cross(int(roi_blobs_result[roi_direct]["cx"]), int(roi_blobs_result[roi_direct]["cy"]),
                                           size=5, color=(255))
                            img.draw_rectangle((x, y, width, height), color=(255))
    #print(roi_blobs_result)  >>>{'right': {'blob_flag': True, 'cx': 157, 'cy': 61}, 'middle': {'blob_flag': False, 'cx': -1, 'cy': -1}, 'down': {'blob_flag': False, 'cx': -1, 'cy': -1}, 'up': {'blob_flag': True, 'cx': 78, 'cy': 2}, 'left': {'blob_flag': True, 'cx': 2, 'cy': 62}}
    return roi_blobs_result
def find_blobs_in_rois(img):            #感兴趣区找线
    roi_blobs_result = {}
    for roi_direct in ROIS.keys():
        roi_blobs_result[roi_direct] = {
            "cx": -1,
            "cy": -1,
            "blob_flag": False
        }
    for roi_direct, roi in ROIS.items():
        blobs = img.find_blobs(Color_threshold, roi=roi, merge=True, pixels_area=40)
        #print(roi_direct, end="\t")
        #print(blobs)  >>>[{"x":155, "y":18, "w":4, "h":14, "pixels":73, "cx":157, "cy":24, "rotation":1.550840, "code":1, "count":1}, {"x":155, "y":85, "w":4, "h":16, "pixels":85, "cx":157, "cy":93, "rotation":1.570796, "code":1, "count":1}]
        #pyb.delay(500)
        #print("****************")
        if len(blobs) == 0:
            continue
        if len(blobs) >= 1:
            largest_blob = max(blobs, key=lambda b: b.pixels())
            for abc in blobs:
                if abc == largest_blob:
                    blobs.remove(abc)
                    #print(blobs)
                    break
            #print(len(blobs))
            #print(largest_blob)
            #pyb.delay(500);
            x, y, width, height = largest_blob[:4]
            if not (width >= areas[0] and width <= areas[1] and height >= areas[2] and height <= areas[3]):
                continue
            roi_blobs_result[roi_direct]['cx'] = largest_blob.cx()
            roi_blobs_result[roi_direct]['cy'] = largest_blob.cy()
            roi_blobs_result[roi_direct]['blob_flag'] = True
            if is_debug and (not roi_direct=='middle' or is_middle):
                img.draw_rectangle((x, y, width, height), color=(255))
            if len(blobs) >= 1:
                largest_blob1 = max(blobs, key=lambda b: b.pixels())
                x, y, width, height = largest_blob1[:4]
                if not (width >= areas[0] and width <= areas[1] and height >= areas[2] and height <= areas[3]):
                    continue
                if(roi_direct == "left" or roi_direct == "right"):
                    if(abs(largest_blob1.cy() - largest_blob.cy()) < 35):
                        roi_blobs_result[roi_direct]["cx"] = int((largest_blob1.cx() + largest_blob.cx()) / 2)
                        roi_blobs_result[roi_direct]["cy"] = int((largest_blob1.cy() + largest_blob.cy()) / 2)
                        roi_blobs_result[roi_direct]['blob_flag'] = True
                        if is_debug and (not roi_direct=='middle' or is_middle):
                            if assist:
                                img.draw_cross(int(roi_blobs_result[roi_direct]["cx"]), int(roi_blobs_result[roi_direct]["cy"]),
                                           size=5, color=(255))
                            img.draw_rectangle((x, y, width, height), color=(255))
                if(roi_direct == "up" or roi_direct == "down" or roi_direct == 'middle'):
                    if(abs(largest_blob1.cx() - largest_blob.cx()) < 35):
                        roi_blobs_result[roi_direct]["cx"] = int((largest_blob1.cx() + largest_blob.cx()) / 2)
                        roi_blobs_result[roi_direct]["cy"] = int((largest_blob1.cy() + largest_blob.cy()) / 2)
                        roi_blobs_result[roi_direct]['blob_flag'] = True
                        if is_debug and (not roi_direct=='middle' or assist):
                            if assist:
                                img.draw_cross(int(roi_blobs_result[roi_direct]["cx"]), int(roi_blobs_result[roi_direct]["cy"]),
                                           size=5, color=(255))
                            img.draw_rectangle((x, y, width, height), color=(255))
    #print(roi_blobs_result)  >>>{'right': {'blob_flag': True, 'cx': 157, 'cy': 61}, 'middle': {'blob_flag': False, 'cx': -1, 'cy': -1}, 'down': {'blob_flag': False, 'cx': -1, 'cy': -1}, 'up': {'blob_flag': True, 'cx': 78, 'cy': 2}, 'left': {'blob_flag': True, 'cx': 2, 'cy': 62}}
    return roi_blobs_result

while(True):
    clock.tick()
    img = sensor.snapshot()
    print(clock.fps())