import time
import sensor
import image
from image import SEARCH_EX

# from image import SEARCH_DS

# 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("/o.battery1.pgm")
template2 = image.Image("/o.battery2.pgm")
template3 = image.Image("/o.battery3.pgm")
template4 = image.Image("/o.battery4.pgm")
template5 = image.Image("/o.battery5.pgm")
template6 = image.Image("/o.battery6.pgm")

clock = time.clock()

# Run template matching
while True:
    clock.tick()
    img = sensor.snapshot()

    # 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.
    r1 = img.find_template(template1, 0.70, step=4, search=SEARCH_EX)  # , roi=(10, 0, 60, 60))
    if r1:
        img.draw_rectangle(r1)
    r2 = img.find_template(template2, 0.70, step=4, search=SEARCH_EX)  # , roi=(10, 0, 60, 60))
    if r2:
        img.draw_rectangle(r2)
    r3 = img.find_template(template3, 0.70, step=4, search=SEARCH_EX)  # , roi=(10, 0, 60, 60))
    if r3:
        img.draw_rectangle(r3)
    r4= img.find_template(template4, 0.70, step=4, search=SEARCH_EX)  # , roi=(10, 0, 60, 60))
    if r4:
        img.draw_rectangle(r4)

    r5 = img.find_template(template5, 0.70, step=4, search=SEARCH_EX)  # , roi=(10, 0, 60, 60))
    if r5:
        img.draw_rectangle(r5)

    r6 = img.find_template(template6, 0.70, step=4, search=SEARCH_EX)  # , roi=(10, 0, 60, 60))
    if r6:
        img.draw_rectangle(r6)
    print(clock.fps())