import time, sensor, image
from image import SEARCH_EX, SEARCH_DS
import ImageIdentify
import car
import road
#从imgae模块引入SEARCH_EX和SEARCH_DS。使用from import仅仅引入SEARCH_EX,
#SEARCH_DS两个需要的部分，而不把image模块全部引入。

# Reset sensor
sensor.reset()

# Set sensor settings
sensor.set_contrast(1)
sensor.set_gainceiling(16)
# Max resolution for template matching with SEARCH_EX is QQVGA
sensor.set_framesize(sensor.QQVGA)
# You can set windowing to reduce the search image.
#sensor.set_windowing(((640-80)//2, (480-60)//2, 80, 60))
sensor.set_pixformat(sensor.GRAYSCALE)

# Load template.
# Template should be a small (eg. 32x32 pixels) grayscale image.
template1 = image.Image("/1.pgm") #数字1的模板
template2 = image.Image("/2.pgm")
template3 = image.Image("/3.pgm")
template4 = image.Image("/4.pgm")
template5 = image.Image("/5.pgm")
template6 = image.Image("/6.pgm")
template7 = image.Image("/7.pgm")
template8 = image.Image("/8.pgm")
#加载模板图片

clock = time.clock()

target_digt = 5
cross = 1
direction = 0

while(1):   #返回转弯信息
#传入当前识别图片img，待匹配数字target_digt
    template = image.Image("/%s.pgm"%(target_digt))
    img = sensor.snapshot()
    if(target_digt == 1 and cross):  #1,2号门没有标识符，第一个十字路口转向
        car.run(20,50)
        direction = 1        #返航时的左右转弯相反，存放对应返航时的信息
    elif(target_digt == 2 and cross):
        car.run(50,20)
        direction = 2
    elif(target_digt > 2 and target_digt < 9):         #3-8号门的识别
        width = img.width()//2 #将图片分为左右区域
        heigh = img.height()
        left_roi = (0,0,width,heigh)
        right_roi = (width,0,width,heigh)

        #左右区域识别
        left_digt = img.find_template(template, 0.70, step=4, search=SEARCH_EX, roi=left_roi)
        right_digt = img.find_template(template, 0.70, step=4, search=SEARCH_EX, roi=right_roi)
        if(right_digt == target_digt):
            car.run(50,20)      #右匹配成功，右转
            direction = 2
            img.draw_rectangle(right_digt)
        elif(left_digt == target_digt):
            car.run(20,50)  #左匹配成功，左转（转弯角度、距离有待调整）
            direction = 1
            img.draw_rectangle(left_digt)
    else:
        direction = 0

    print(direction)