sys.path.append('/flash/apps')

import hkylen

import unit

from m5stack import *

from m5ui import *

from time import sleep

#===== Declarations =====#

servo_0 = unit.get(unit.SERVO, unit.PORTC)

#===== User-Defined Functions =====#

def displayResult(faceID):

    if (faceID == 1):

        lbl.setColor(lcd.RED)

        rgb.setColorAll(lcd.RED)

        lbl.setText('FACE ID: 1')

        servo_0_90_0()

    elif (faceID == 2):

        lbl.setColor(lcd.GREEN)

        rgb.setColorAll(lcd.GREEN)

        lbl.setText('FACE ID: 2')

        servo_0_90_0()

    elif (faceID == 3):

        lbl.setColor(lcd.BLUE)

        rgb.setColorAll(lcd.BLUE)

        lbl.setText('FACE ID: 3')

        servo_0_90_0()

    elif (faceID == 4):

        lbl.setColor(lcd.YELLOW)

        rgb.setColorAll(lcd.YELLOW)

        lbl.setText('FACE ID: 4')  

        servo_0_90_0()

    else:

        lbl.setColor(lcd.WHITE)

        rgb.setColorAll(lcd.BLACK)

        lbl.setText('No Face ID')

def servo_0_90_0():

    servo_0.write_angle(90)

    sleep (2)

    servo_0.write_angle(0)

#===== Main =====#

lcd.clear(lcd.BLACK)

lcd.font(lcd.FONT_DejaVu24)

servo_0.write_angle(0)

hkylen.setAlgorithm(hkylen.FACE_RECOGNITION)

while not (hkylen.isReady()):

    lcd.print('Huskylens Not Ready!', 10, 0, lcd.CYAN)

lcd.clear(lcd.BLACK)

lcd.print('Face Security Demo', 10, 0, lcd.CYAN)

lbl = M5TextBox(15, 90, '', lcd.FONT_DejaVu56, lcd.WHITE)

displayResult(0)

while True:

    hkylen.update()

    if hkylen.blockIsReady(1):

        displayResult(1)

    elif hkylen.blockIsReady(2):

        displayResult(2)

    elif hkylen.blockIsReady(3):

        displayResult(3)

    elif hkylen.blockIsReady(4):

        displayResult(4)

    else:   

        displayResult(0)      

    sleep(2)