import pyb
from	pyb import Pin
RED_LED_PIN = 1
c=9
d=9
p_in1 = Pin('P7', Pin.IN, Pin.PULL_UP)
p_in2 = Pin('P8', Pin.IN, Pin.PULL_UP)
p_in3 = Pin('P9', Pin.IN, Pin.PULL_UP)
while(1):
    a=p_in1.value()
    b=p_in2.value()
    f=p_in3.value()
    if(a == 0):
        import sensor, time, image, pyb
        from pyb import UART
        uart = UART(3, 115200)
        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 = 3000)
        pyb.LED(RED_LED_PIN).on()
        sensor.skip_frames(10)
        sensor.skip_frames(time = 100)
        pyb.LED(RED_LED_PIN).off()
        NUM_SUBJECTS = c
        NUM_SUBJECTS_IMGS = 20
        img = sensor.snapshot()
        d0 = img.find_lbp((0, 0, img.width(), img.height()))
        img = None
        pmin = 999999
        num=0
        def min(pmin, a, s):
            global num
            if a<pmin:
                pmin=a
                num=s
            return pmin
        for s in range(1, NUM_SUBJECTS+1):
            dist = 0
            for i in range(2, NUM_SUBJECTS_IMGS+1):
                img = image.Image("singtown/s%d/%d.pgm"%(s, i))
                d1 = img.find_lbp((0, 0, img.width(), img.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)
            if(pmin>7200):
                num=91
        output_str="[%.0f]" %num
        print(num)
        uart.write(output_str+'\r\n')
    elif(b == 0):
            import sensor,	 image, pyb
            c=c-1
            d=d-1
            RED_LED_PIN = 1
            BLUE_LED_PIN = 3
            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 = 200)
            num = d
            n = 60
            while(n):
                pyb.LED(RED_LED_PIN).on()
                sensor.skip_frames(time = 200)
                pyb.LED(RED_LED_PIN).off()
                pyb.LED(BLUE_LED_PIN).on()
                print(c)
                print(d)
                print(n)
                sensor.snapshot().save("singtown/s%s/%s.pgm" % (num, n) )
                n -= 1
                pyb.LED(BLUE_LED_PIN).off()
                print("Done! Reset the camera to see the saved image.")
    if(f == 0):
        import sensor, image, time
        import math
        from pyb import UART
        uart = UART(3, 115200)
        red_threshold   = (0, 100, -128, 5, -128, -22)
        sensor.reset()
        sensor.set_pixformat(sensor.RGB565)
        sensor.set_framesize(sensor.QQVGA)
        sensor.skip_frames(10)
        sensor.set_auto_whitebal(False)
        clock = time.clock()
        K=2880
        i=0
        g=1
        while(g):
            i=i+1
            clock.tick()
            img2 = sensor.snapshot()
            blobs = img2.find_blobs([red_threshold])
            if len(blobs) == 0:
                length=92
                output_str="[%.0f]" % (length)
                print("angle:%.0f")
                print("distance:%.0f"%length)
                uart.write(output_str+'\r\n')
            elif len(blobs) == 1:
                b = blobs[0]
                img2.draw_rectangle(b[0:4])
                img2.draw_cross(b[5], b[6])
                Lm = (b[2]+b[3])/2
                length = K/Lm
            for blob in blobs:
               t=(blob.cx()-80)
               a=math.atan(t/66/1.732)*180/3.1415926
               b = blobs[0]
               Lm = (b[2]+b[3])/2
               length = 93
               output_str="[%.0f,%.0f]" % (length,a)
               print("angle:%.0f"%a)
               print("distance:%.0f"%length)
               uart.write(output_str+'\r\n')
               g=0