import sensor, image, time
import json
from pyb import UART
import pyb
import math
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=2000)
sensor.set_auto_whitebal(True)
sensor.set_auto_gain(True)
clock = time.clock()
def get_biggest_blob(b):
    max_blob_nb = 0
    max_blob = 0
    for i in range(len(b)):
        area = b[i].pixels()
        if area > max_blob:
            max_blob = area
            max_blob_nb = i
    return max_blob_nb
def modified_data(data):
   data = int(data)
   str_data = ''
   if data < 10:
       str_data = str_data + '000' + str(data)
   elif data >= 10 and data < 100:
       str_data = str_data + '00' + str(data)
   elif data >=100 and data <1000:
       str_data = str_data + '0' + str(data)
   else:
       str_data = str_data + str(data)
   return str_data.encode('utf-8')
uart = UART(3, 115200)
red_threshold = [(31, 4, 6, 41, 34, -126),
                 (61, 2, 5, 44, 43, -126),
                 (81, 6, 15, 49, 90, -125)]
green_threshold = [(43, 8, -70, -20, 51, -59),
                    (61, 12, -62, -20, 60, 6),
                    (46, 11, -56, -9, 47, -121)]
color = 0
num_s = 0
num_x = 0
num_y = 0
num_l = 0
k = 2600
b=0
gui=0
you=0
def my():
    pj=1
while(True):
    t = 0
    clock.tick()
    img = sensor.snapshot()
    if uart.any():
        a = uart.readline().decode().strip()
        print(a)
        b = int(a)
    blobs1 = img.find_blobs(red_threshold,pixels_threshold=1100, area_threshold=1100)
    badblobs = img.find_blobs(green_threshold,pixels_threshold=1100, area_threshold=1100)
    if blobs1 or  badblobs :
        if blobs1 and  badblobs :
            jj = get_biggest_blob(blobs1)
            kk = get_biggest_blob(badblobs)
            if blobs1[jj].pixels() > badblobs[kk].pixels() :
                img.draw_rectangle(blobs1[jj].rect())
                img.draw_cross(blobs1[jj].cx(), blobs1[jj].cy())
                img.draw_rectangle(badblobs[kk].rect())
                Lm = (blobs1[jj][2]+blobs1[jj][3]) / 2
                leng = k / Lm
                num_s = num_s + 1
                x3=blobs1[jj].cx()
                y3=blobs1[jj].cy()
                l3 = leng
                num_x = num_x + x3
                num_y = num_y + y3
                num_l = num_l + l3
                w=5
                h=0
                z = modified_data(w)
                print(num_s)
                if num_s==1:
                    px=num_x/1
                    py=num_y/1
                    pl=num_l/1
                    x=modified_data(px)
                    y=modified_data(py)
                    l=modified_data(pl)
                    if b==111:
                        uart.write(z)
                        uart.write(x)
                        uart.write(y)
                        uart.write(l)
                        uart.write("\r\n")
                        z=modified_data(h)
                        print("success")
                    num_x = 0
                    num_y = 0
                    num_s = 0
                    num_l = 0
                    print('zhonglei', z,'cx:', x, 'cy:', y, 'length:', l)
                    you=1
                t = 1
                time.sleep(0.1)
        elif blobs1:
            ii = get_biggest_blob(blobs1)
            img.draw_rectangle(blobs1[ii].rect())
            img.draw_cross(blobs1[ii].cx(), blobs1[ii].cy())
            Lm = (blobs1[ii][2]+blobs1[ii][3]) / 2
            leng = k / Lm
            num_s = num_s + 1
            x3=blobs1[ii].cx()
            y3=blobs1[ii].cy()
            l3 = leng
            num_x = num_x + x3
            num_y = num_y + y3
            num_l = num_l + l3
            w=5
            h=0
            z = modified_data(w)
            print(num_s)
            if num_s==1:
                px=num_x/1
                py=num_y/1
                pl=num_l/1
                x=modified_data(px)
                y=modified_data(py)
                l=modified_data(pl)
                if b==111:
                    uart.write(z)
                    uart.write(x)
                    uart.write(y)
                    uart.write(l)
                    uart.write("\r\n")
                    z=modified_data(h)
                    print("success!!!")
                num_x = 0
                num_y = 0
                num_s = 0
                num_l = 0
                print('zhonglei', z,'cx:', x, 'cy:', y, 'length:', l)
                you=1
            t = 1
            time.sleep(0.1)
        elif  t == 0 and badblobs:
            ii = get_biggest_blob(badblobs)
            img.draw_rectangle(badblobs[ii].rect())

            Lm = (badblobs[ii][2]+badblobs[ii][3]) / 2
            leng = k / Lm
            num_s = num_s + 1
            x3=badblobs[ii].cx()
            y3=badblobs[ii].cy()
            l3 = leng
            num_x = num_x + x3
            num_y = num_y + y3
            num_l = num_l + l3
            w=2
            z = modified_data(w)
            print(num_s)
            if num_s==1:
                px=num_x/1
                py=num_y/1
                pl=num_l/1
                x=modified_data(px)
                y=modified_data(py)
                l=modified_data(pl)
                if b==111:
                    uart.write(z)
                    uart.write(x)
                    uart.write(y)
                    uart.write(l)
                    uart.write("\r\n")
                    w=0
                    z = modified_data(w)
                    print("lose")
                num_x = 0
                num_y = 0
                num_s = 0
                num_l = 0
                print('zhonglei', z,'cx:', x, 'cy:', y, 'length:', l)
                you=1
            b=0
            time.sleep(0.1)
    else:
        color=0
        x=modified_data(color)
        y=modified_data(color)
        l=modified_data(color)
        z = modified_data(color)
        if b==111:
            uart.write(z)
            uart.write(x)
            uart.write(y)
            uart.write(l)
            uart.write("\r\n")
            print("000000000")
        print("ao")