# Template Matching Example - Normalized Cross Correlation (NCC)
#
# This example shows off how to use the NCC feature of your OpenMV Cam to match
# image patches to parts of an image... expect for extremely controlled enviorments
# NCC is not all to useful.
#
# WARNING: NCC supports needs to be reworked! As of right now this feature needs
# a lot of work to be made into somethin useful. This script will reamin to show
# that the functionality exists, but, in its current state is inadequate.

import time, sensor, image,ustruct
from pyb import UART,LED
from image import SEARCH_EX, SEARCH_DS
#从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)


LED(1).on()
LED(2).on()
LED(3).on()
# Load template.
# Template should be a small (eg. 32x32 pixels) grayscale image.
template1 = image.Image("/1.pgm")
template2 = image.Image("/2.pgm")
templates3 = ["/3.pgm","/3_1.pgm","/3_2.pgm","/3_3.pgm","/3_4.pgm","/3_5.pgm","/3_6.pgm","/3_7.pgm","/3_8.pgm"]
templates4 = ["/4.pgm","/4_1.pgm","/4_2.pgm","/4_3.pgm","/4_4.pgm","/4_5.pgm","/4_6.pgm","/4_7.pgm","/4_8.pgm"]
templates5 = ["/5.pgm","/5_1.pgm","/5_2.pgm","/5_3.pgm","/5_4.pgm","/5_5.pgm","/5_6.pgm","/5_7.pgm","/5_8.pgm"]
templates6 = ["/6.pgm","/6_1.pgm","/6_2.pgm","/6_3.pgm","/6_4.pgm","/6_5.pgm","/6_6.pgm","/6_7.pgm","/6_8.pgm"]
templates7 = ["/7.pgm","/7_1.pgm","/7_2.pgm","/7_3.pgm","/7_4.pgm","/7_5.pgm","/7_6.pgm","/7_7.pgm","/7_8.pgm"]
templates8 = ["/8.pgm","/8_1.pgm","/8_2.pgm","/8_3.pgm","/8_4.pgm","/8_5.pgm","/8_6.pgm","/8_7.pgm","/8_8.pgm"]
#加载模板图片

clock = time.clock()
uart = UART(3,115200)   #定义串口3变量
uart.init(115200, bits=8, parity=None, stop=1) # init with given parameters

def outuart(x,num):
    global uart
    #frame=[0x2C,18,cx%0xff,int(cx/0xff),cy%0xff,int(cy/0xff),0x5B];
    #data = bytearray(frame)
    data = ustruct.pack("<bbhhhhb",      #格式为俩个字符俩个短整型(2字节)
                   0x2C,                      #帧头1
                   0x12,                      #帧头2
                   int(x), # up sample by 4   #数据1
                   int(num), # up sample by 4    #数据2
                   int(0), # up sample by 4    #数据1
                   int(0), # up sample by 4    #数据2
                   0x5B)
    for x in range(5):
        uart.write(data)#必须要传入一个字节数组
        time.sleep_ms(1)
        print(num)

# Run template matching
while (True):
    clock.tick()
    img = sensor.snapshot()
    num=0
    # find_template(template, threshold, [roi, step, search])
    # ROI: The region of interest tuple (x, y, w, h).
    # Step: The loop step used (y+=step, x+=step) use a bigger step to make it faster.
    # Search is either image.SEARCH_EX for exhaustive search or image.SEARCH_DS for diamond search
    #
    # Note1: ROI has to be smaller than the image and bigger than the template.
    # Note2: In diamond search, step and ROI are both ignored.
    r = img.find_template(template1, 0.70, step=5, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
    #find_template(template, threshold, [roi, step, search]),threshold中
    #的0.7是相似度阈值,roi是进行匹配的区域（左上顶点为（10，0），长80宽60的矩形），
    #注意roi的大小要比模板图片大，比frambuffer小。
    #把匹配到的图像标记出来
    if r:
        print(r)
        #img.draw_rectangle(r)
        print('1')
        num=1
        outuart(0,num)
        for x in range(5):
            LED(1).on()
            LED(2).off()
            LED(3).off()
            time.sleep_ms(100)
            LED(1).on()
            LED(2).on()
            LED(3).on()
            time.sleep_ms(100)

    r2_0 = img.find_template(template2, 0.70, step=5, search=SEARCH_EX)
    if r2_0:
        print(r2_0)
        #img.draw_rectangle(r1_3)
        print('2')
        num=2
        outuart(0,num)
        for x in range(5):
            LED(1).on()
            LED(2).off()
            LED(3).off()
            time.sleep_ms(100)
            LED(1).on()
            LED(2).on()
            LED(3).on()
            time.sleep_ms(100)

    r3_0 = img.find_template(image.Image(templates3[0]), 0.70, step=5, search=SEARCH_EX)
    if r3_0:
        print(r3_0)
        #img.draw_rectangle(r1_1)
        print('3')
        num=3
        outuart(0,num)
        for x in range(5):
            LED(1).on()
            LED(2).off()
            LED(3).off()
            time.sleep_ms(100)
            LED(1).on()
            LED(2).on()
            LED(3).on()
            time.sleep_ms(100)