import sensor, image, time, lcd, car as M

sensor.reset()
sensor.set_pixformat(sensor.RGB565) # grayscale is faster
#sensor.set_pixformat(sensor.GRAYSCALE) # grayscale is faster
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
clock = time.clock()
lcd.init(type=1, freq=15000000, color=lcd.BLACK)
lcd.mirror(True)
Speed = 90
mode = 1    # 1 for turn, 2 for run
delay = 0.000
nomove = False
while(True):
    clock.tick()
    img = sensor.snapshot()
    lines = img.find_lines(roi=(0,120,320,240),threshold = 1000, theta_margin = 50, rho_margin = 50)
    lindex = -1
    ltheta = 90
    x2 = 0
    y2 = 0
    '''
    for l in lines:
        img.draw_line(l.line(), color = (255, 0, 0), thickness=5)
    lcd.display(img)   
    continue 
    '''
    for i in range(0, len(lines)):
        th = lines[i].theta()
        if th >= 0 and th < ltheta: 
            #locate right-lower point, result (x2, y2)
            x2 = lines[i].x2()
            y2 = lines[i].y2()
            if lines[i].y2() < lines[i].y1():
                x2 = lines[i].x1()
                y2 = lines[i].y1()
            if x2 < 160: #only line at right half is valid
                lindex = i
                ltheta = th
    if lindex != -1:    #target line found
        img.draw_line(lines[lindex].line(), color = (255, 0, 0), thickness=5)
        if lines[lindex].theta() > 45: 
            if y2 < 239:  #approaching circle edge
                #forward
                print('forward')
                time.sleep(delay)
                img.draw_arrow(160,120,160,0,color=(0,255,0),thickness=5)
                if not nomove : M.forward(Speed)
                
            else:  #circle edge
                #left turn
                print('left')
                img.draw_arrow(160,120,320,120,color=(0,255,0),thickness=5)
                if mode == 1 :
                    if not nomove : M.left_turn(Speed)
                else:
                    if not nomove : M.left_run(Speed)
                time.sleep(delay)
        else:
            if x2 <= 20 and y2 <= 239 and y2 >= 215:
                #forward
                print('forward')
                img.draw_arrow(160,120,160,0,color=(0,255,0),thickness=5)
                if not nomove : M.forward(Speed)
                time.sleep(delay)
            elif x2 == 0 and y2 < 215:
                #right turn
                print('right')
                img.draw_arrow(160,120,0,120,color=(0,255,0),thickness=5)
                if mode == 1 :
                    if not nomove : M.right_turn(Speed)
                else:
                    if not nomove : M.right_run(Speed) 
                time.sleep(delay)    
            elif x2 > 20 and y2 == 239:
                #left turn
                print('left')
                img.draw_arrow(160,120,320,120,color=(0,255,0),thickness=5)
                if mode == 1 :
                    if not nomove : M.left_turn(Speed)
                else:
                    if not nomove : M.left_run(Speed)
                time.sleep(delay)    
    #========================
    else: #right line not exist, check left
        lindex = -1
        ltheta = 90
        x2 = 0
        y2 = 0
        for i in range(0, len(lines)):
            th = lines[i].theta()
            if th > ltheta: 
                #locate left-lower point, result (x2, y2)
                x2 = lines[i].x2()
                y2 = lines[i].y2()
                if lines[i].y2() < lines[i].y1():
                    x2 = lines[i].x1()
                    y2 = lines[i].y1()
                if x2 >= 160: #only line at left half is valid
                    lindex = i
                    ltheta = th
        if lindex != -1:    #target line found
            img.draw_line(lines[lindex].line(), color = (255, 0, 0), thickness=5)
            angle = lines[lindex].theta()
            if  (angle > 130 and x2 > 300) or y2 < 190 : #forward
                print('forward')
                img.draw_arrow(160,120,160,0,color=(255,0,0),thickness=5)
                if not nomove : M.forward(Speed)
            else:  #touching circle edge
                #right turn
                print('right')
                img.draw_arrow(160,120,0,120,color=(255,0,0),thickness=5)
                if mode == 1 :
                    if not nomove : M.right_turn(Speed)
                else:
                    if not nomove : M.right_run(Speed)
    #========================            
    img.draw_string(100,20,str(clock.fps()))
    lcd.display(img)