import sensor, image, time,math,pyb
from image import SEARCH_EX, SEARCH_DS
from pyb import UART
import json

red=pyb.LED(1)
green=pyb.LED(2)
blue=pyb.LED(3)
red_threshold =  (19, 55, 32, 70, -9, 58)#颜色阈值(minL, maxL, minA, maxA, minB, maxB)

sensor.reset()#重置感光元件和摄像机
sensor.set_pixformat(sensor.RGB565)#设置颜色格式为RGB565
sensor.set_framesize(sensor.QQVGA)#设置图像大小QVGA
sensor.skip_frames(time = 100)#跳过一些刚开始不稳定的时候再开始读取图像
sensor.set_auto_gain(False)#关闭白平衡
sensor.set_auto_whitebal(False)#关闭自动增益
#sensor.set_windowing((16,12,128,98))
clock = time.clock()

uart = UART(3, 115200)
flag=0
template_flag=0
uart_flag=0
start_flag=0
templates = ["/1.pgm","/2.pgm","/3.pgm","/4.pgm","/5.pgm","/6.pgm","/7.pgm","/8.pgm"]
lin1_flag=0
lin2_flag=80
r_flag=0
class Coordinate: #对应颜色相对图片左下角的坐标（图片左下角为原点）
    def __init__(self,xx,yy):
        self.xx=xx
        self.yy=98-yy
        pass
    def x(self):
        return self.xx
    def y(self):
        return self.yy
    pass

def Actual_size(f_x,f_y,f_z):
    output = "%d %d %d" % (f_x, f_y, f_z)
    return output


def find_max(max_img):
    max_size=0
    for blob in max_img:
        if blob.pixels() > max_size:
            max_blob=blob
            max_size = blob.pixels()
    return max_blob
#d=1
while(True):
    if uart.any():
        a=uart.readline().decode().strip()
        if(a=='a'):
            uart_flag=1
    #if d:
        #uart_flag=1
        #d=0
    else:
        while(uart_flag==1):
            flag=0
            img = sensor.snapshot()
            img = img.to_grayscale()
            for t in templates:
                template = image.Image(t)
                if flag==0:
                    r = img.find_template(template, 0.75,step=4, search=SEARCH_EX)
                elif flag==1:
                    r = img.find_template(template, 0.70,step=4, search=SEARCH_EX)
                else:
                    r = img.find_template(template, 0.70,step=4, search=SEARCH_EX)
                flag+=1
                blue.on()
                red.off()
                green.off()
                if r:
                    template_flag=flag-1
                    img.draw_rectangle(r)
                    output_str=Actual_size(80,0,template_flag+1)
                    #print(flag,template_flag)
                    print(output_str)
                    uart.write(output_str+'\r\n')
                    blue.off()
                    time.sleep_ms(3000)
                    uart_flag=0
                    start_flag=1
            if uart_flag==0:
                break
        if(start_flag==1):
            img = sensor.snapshot()
            lin1 = img.find_blobs([red_threshold],roi=[130,50,30,60],merge=True)
            img.draw_rectangle( (130,50,30,40))
            lin2 = img.find_blobs([red_threshold],roi=[0,90,160,20],merge=True)
            img = img.to_grayscale()
            if template_flag>1:
                r = img.find_template(image.Image(templates[template_flag]), 0.70,step=4, search=SEARCH_EX)
                if r:
                    if r[0]<80:
                        r_flag=50;
                    if r[0]>80:
                        r_flag=100;
                else:
                    r_flag=template_flag+1
            else:
                if lin1:
                    if template_flag==0:
                        r_flag=50
                    elif template_flag==1:
                        r_flag=100
                else:
                    r_flag=template_flag+1
            #检测是否偏离
            if lin2:
                j=find_max(lin2)
                img.draw_rectangle(j.rect())
                img.draw_cross(j.cx(), j.cy())
                lin2_flag=j.cx()
                red.off()
                green.on()
                blue.off()
            else:
                lin2_flag=80
            #检测是否到十字路口
            if lin1:
                i=find_max(lin1)
                img.draw_rectangle(i.rect())
                img.draw_cross(i.cx(), i.cy())
                lin1_flag=i.cy()
                output_str=Actual_size(lin2_flag,lin1_flag,r_flag)
            else:
                lin1_flag=0
                output_str=Actual_size(lin2_flag,lin1_flag,template_flag+1)
            print(output_str)
            #j = find_max(lin2)
            #img.draw_rectangle(j.rect())
            #img.draw_cross(j.cx(), j.cy())
            #output_str=Actual_size(j.cx(),j.cy())
            #print(output_str)
            time.sleep_ms(5)
            uart.write(output_str+'\r\n')
            #else :
                #red.on()
                #green.off()
                #blue.off()