import pyb
import sensor
import image
import time

led = pyb.LED(3)
maestro_uart_ch = 1   # UART channel connected to Maestro board
baud_rate = 9600  # Baud rate of Mini Maestro servo controller

sensor.reset()                          # Configure camera
sensor.set_contrast(3)                  # Configure camera
sensor.set_gainceiling(16)              # Configure camera
sensor.set_framesize(sensor.QVGA)       # Configure camera
sensor.set_pixformat(sensor.GRAYSCALE)  # Configure camera

uart = pyb.UART(maestro_uart_ch, baud_rate)  # Pour a bowl of serial
clock = time.clock()  # Start clock

face_cascade = image.HaarCascade("frontalface", stages=25)  # Create cascade for finding faces

while(True):  # Superloop
    in0 = pyb.Pin('PG7',pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
    in0.value(1)
    clock.tick()

    img = sensor.snapshot()   # Take photo
    objects = img.find_features(face_cascade, threshold=0.75, scale_factor=1.25)   # Find faces in image

    largest_face_size = 0   # Find largest face in image
    largest_face_bb = None
    for r in objects:
        face_size = r[2] * r[3]    # Find largest bounding box
        if (face_size > largest_face_size):
            largest_face_size = face_size
            largest_face_bb = r
        img.draw_rectangle(r)   # Draw bounding boxes around all faces

    if largest_face_bb is not None:
        led.on()
        timer = pyb.Timer(4, freq=1000)



        in1 = pyb.Pin('PA9', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)  # turn on motors
        in2 = pyb.Pin('PA10', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
        in1.value(1)
        in2.value(0)

        pyb.delay(2000)

    else:
        timer = pyb.Timer(4, freq=1000)

        in1 = pyb.Pin('PA9', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)   # turn off motors
        in2 = pyb.Pin('PA10', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
        in1.value(0)
        in2.value(0)
        led.off()