import sensor, time, image, pyb
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.B128X128)
sensor.set_windowing((92,112))
sensor.skip_frames(10)
sensor.skip_frames(time = 5000)
NUM_SUBJECTS = 2
NUM_SUBJECTS_IMGS = 3
img = sensor.snapshot()
d0 = img.find_lbp((0, 0, img.width(), img.height()))
img1 = None
pmin = 999999
num=0
def min(pmin, a, s):
    global num
    if a<pmin:
        pmin=a
        num=s
    return pmin
import sensor, time, image
from pyb import UART
sensor.reset()
sensor.set_contrast(1)
sensor.set_gainceiling(16)
sensor.set_framesize(sensor.HQVGA)
sensor.set_pixformat(sensor.GRAYSCALE)
face_cascade = image.HaarCascade("frontalface", stages=25)
uart = UART(3, 115200, timeout_char=1000)
clock = time.clock()
num = 0
while (True):
    clock.tick()
    img = sensor.snapshot()
    objects = img.find_features(face_cascade, threshold=0.75, scale_factor=1.25)
    for r in objects:
        if(r):
            img.draw_rectangle(r,color = (0,255,0))
            cx = r[0]+r[2]/2
            cy = r[1]+r[3]/2
            print('x:%d, y:%d, w:%d, h:%d' % (r[0],r[1],r[2],r[3]) )
            print('cx = %d, cy = %d' % (cx,cy))
            img = sensor.snapshot()
            d0 = img.find_lbp((0, 0, img.width(), img.height()))
            img1 = None
            pmin = 999999
            num=0
            for s in range(1, NUM_SUBJECTS+1):
                dist = 0
                for i in range(1, NUM_SUBJECTS_IMGS+1):
                    img1 = image.Image("singtown/s%d/%d.pgm"%(s, i))
                    d1 = img1.find_lbp((0, 0, img1.width(), img1.height()))
                    dist += image.match_descriptor(d0, d1)
                print("Average dist for subject %d: %d"%(s, dist/NUM_SUBJECTS_IMGS))
                pmin = min(pmin, dist/NUM_SUBJECTS_IMGS, s)
            print(pmin)
            print(num)
            uart.write('X%dY%dN%dOK\r\n' % (cx,cy,num))
            break
    else:
            uart.write('X-1Y-1N-1OK\r\n')