import sensor, image, time
import ustruct
import display
from pyb import UART,LED

#****************************阈值调整*******************************
area=(45, 2, 200, 198)
red=(88, 100, -16, 22, -128, 127)  #only red

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_auto_whitebal(True)
sensor.set_auto_gain(False)
sensor.skip_frames(time = 2000)
sensor.set_vflip(True)
sensor.set_hmirror(True)
lcd = display.SPIDisplay()
#****************************曝光时间*******************************
#sensor.set_auto_exposure(False,4500)

clock = time.clock()
#*****************************line_threshold**********************
line_threshold=2500
rect_threshold=10000

uart = UART(3,115200)
uart.init(115200, bits=8, parity=None, stop=1)

def calculate_intersection(line1, line2):
    '''
    计算两条线的交点
    '''
    a1 = line1.y2() - line1.y1()
    b1 = line1.x1() - line1.x2()
    c1 = line1.x2()*line1.y1() - line1.x1()*line1.y2()

    a2 = line2.y2() - line2.y1()
    b2 = line2.x1() - line2.x2()
    c2 = line2.x2() * line2.y1() - line2.x1()*line2.y2()

    if (a1 * b2 - a2 * b1) != 0 and (a2 * b1 - a1 * b2) != 0:
        cross_x = int((b1*c2-b2*c1)/(a1*b2-a2*b1))
        cross_y = int((c1*a2-c2*a1)/(a1*b2-a2*b1))
        return (cross_x, cross_y)
    return None

def sending_data(rx,ry,cx,c):
    global uart;
    data = ustruct.pack("<bhhhhhhhhb",
                        0xFF,
                        int(rx),
                        int(ry),
                        int(cx),
                        int(cy),
                        int(cw),
                        int(ch),
                        int(xx),
                        int(xy),
                        int(xw),
                        int(xh),
                        0xFE)
    uart.write(data);
cx=0
cy=0
rx=0
ry=0
a=[0,0,0,0,0,0,0,0,0,0]

while(True):
   # lcd.write(sensor.snapshot())  # 拍照并显示图像。
    clock.tick()
    img = sensor.snapshot().lens_corr(strength = 1.6, zoom = 1.0)

    red_point= img.find_blobs([red])
    if red_point:
        r= red_point[0]
        img.draw_rectangle(r[0:4],color=(0,0,0)) # rect
        rx=r[5]
        ry=r[6]
        #print(rx,ry)

#******************************************************************lines*****************
    lines = img.find_lines(roi=area, threshold=line_threshold)
    i=0
    if len(lines)==4 :
        theta0=lines[0].theta()
        theta1=lines[1].theta()
        theta2=lines[2].theta()
        theta3=lines[3].theta()
        for line in lines:
            img.draw_line(line.line(),color=[255,0,0])
        if abs(theta0-theta1)>45:
            a[i]=calculate_intersection(lines[0],lines[1])[0]
            i=i+1
            a[i]=calculate_intersection(lines[0],lines[1])[1]
            i=i+1
        if abs(theta0-theta2)>45:
            a[i]=calculate_intersection(lines[0],lines[2])[0]
            i=i+1
            a[i]=calculate_intersection(lines[0],lines[2])[1]
            i=i+1
        if abs(theta0-theta3)>45:
            a[i]=calculate_intersection(lines[0],lines[3])[0]
            i=i+1
            a[i]=calculate_intersection(lines[0],lines[3])[1]
            i=i+1
        if abs(theta1-theta2)>45:
            a[i]=calculate_intersection(lines[2],lines[1])[0]
            i=i+1
            a[i]=calculate_intersection(lines[2],lines[1])[1]
            i=i+1
        if abs(theta1-theta3)>45:
            a[i]=calculate_intersection(lines[1],lines[3])[0]
            i=i+1
            a[i]=calculate_intersection(lines[1],lines[3])[1]
            i=i+1
        if abs(theta3-theta2)>45:
            a[i]=calculate_intersection(lines[2],lines[3])[0]
            i=i+1
            a[i]=calculate_intersection(lines[2],lines[3])[1]
            i=i+1
        print(a)
        FH = bytearray([0xFF,rx,ry,a[0],a[1],a[2],a[3],a[4],a[5],a[6],a[7],0xFE])
        uart.write(FH)
        #print(rx,ry)

    print(clock.fps())