# Untitled - By: somnath - Sun Jan 22 2023
import sensor,image, lcd
import KPU as kpu
import time
from Maix import GPIO
from fpioa_manager import fm

fm.register(3, fm.fpioa.GPIO0, force=True) #Pin 13
fm.register(10, fm.fpioa.GPIO1, force=True) #Pin 12
led1 = GPIO(GPIO.GPIO0, GPIO.OUT)
led2 = GPIO(GPIO.GPIO1, GPIO.OUT)
led1.value(0)
led2.value(0)
# distance from camera to object(face) measured
# centimeter
P = 250

# width of face in the real world or Object Plane
# centimeter
Q = 24



lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
#sensor.run(1)
task = kpu.load(0x300000)
anchor = (1.889, 2.5245, 2.9465, 3.94056, 3.99987, 5.3658, 5.155437, 6.92275, 6.718375, 9.01025)
a = kpu.init_yolo2(task, 0.5, 0.3, 5, anchor)
while True:
  img = sensor.snapshot()
  code = kpu.run_yolo2(task, img)
  led1.value(0)
  led2.value(1)
  if code:
    for i in code:
      print(i)
      a = img.draw_rectangle(i.rect())
      a = lcd.display(img)
      face_width = i.w()
      face_dist = -7.142* face_width + 435.71
      lcd.draw_string(5,10, 'Dist:%d'% face_dist, lcd.GREEN, 4)
      if (face_dist<=250):
            led1.value(1)
            led2.value(0)

  else:
    a = lcd.display(img)
    led1.value(0)
    led2.value(1)


a = kpu.deinit(task)
fm.unregister(3)
