import sensor, image, time
import car
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
block_box_color = [(0, 29, -52, 55, -114, 49),
                  (30, 100, -64, -8, -32, 32)]
def expand_roi(roi):
    extra = 5
    win_size = (640, 240)
    (x, y, width, height) = roi
    new_roi = [x-extra, y-extra, width+2*extra, height+2*extra]
    if new_roi[0] < 0:
        new_roi[0] = 0
    if new_roi[1] < 0:
        new_roi[1] = 0
    if new_roi[2] > win_size[0]:
        new_roi[2] = win_size[0]
    if new_roi[3] > win_size[1]:
        new_roi[3] = win_size[1]
    return tuple(new_roi)
i = 0
clock = time.clock()
while(True):
    clock.tick()
    img = sensor.snapshot()
    img.replace(img,vflip=True, hmirror=True, transpose=False)
    img.draw_line(0,200,320,200,(0,0,255),thinkness = 1)
    find_block = img.find_blobs([block_box_color], area_threshold=10000)
    car.run(25,25)
    if find_block:
        for blob in find_block:
            is_line = False
            max_line = None
            max_length = -1
            is_block = 0
            new_roi = expand_roi(blob.rect())
            img.draw_rectangle(new_roi)
            img.draw_cross(blob[5], blob[6])
            ltx = new_roi[0]
            lty = new_roi[1]
            lbx = new_roi[0]
            lby = new_roi[1] + new_roi[3]
            rtx = new_roi[0] + new_roi[2]
            rty = new_roi[1]
            rbx = new_roi[0] + new_roi[2]
            rby = new_roi[1] + new_roi[3]
            print("ltx=",ltx,"lty=",lty)
            print("lbx=",lbx,"lby=",lby)
            print("rtx=",rtx,"rty=",rty)
            print("rbx=",rbx,"rby=",rby)
            print("\n")
            if lby > 200 :
                car.turn_left(120)
                print("turn_left")