# ------------------------------
# 人脸检测处理
# ------------------------------
from maix import image, app, time
from config import *

def process_faces(img, objs):
    
    max_face = None
    max_width = 0
    
    if objs:
        for obj in objs:
            if obj.w > max_width:
                max_width = obj.w
                max_face = obj
        
        # 绘制人脸框（仅跟踪模式开启时执行）
        for obj in objs:
            color = image.COLOR_GREEN if obj == max_face else image.COLOR_RED
            img.draw_rect(obj.x, obj.y, obj.w, obj.h, color=color, thickness=2)
    
    if max_face:
        center_x = int(max_face.x + max_face.w / 2)
        center_y = int(max_face.y + max_face.h / 2)
        img.draw_keypoints([center_x, center_y], image.COLOR_GREEN, size=3)
        return (center_x, center_y, int(max_face.w))
    else:
        return (-1, -1, -1)

# ------------------------------
# 人脸追踪
# ------------------------------
def calculate_deviation_face(center, image_center, serial_dev):

    center_x, center_y = center
    
    # 偏差计算
    deviation_x = center_x - image_center[0]
    deviation_y = center_y - image_center[1]
    
    # 输出像素偏差
    if abs(deviation_x) > 10 or abs(deviation_y) > 10 :
        cmd = f"DX:{deviation_x},DY:{deviation_y};"  # 图像尺寸: w320xh224
        serial_dev.write_str(cmd)
        print(cmd)
        return (cmd)
    else:
        return (None)

# ------------------------------
# 主循环（同步）
# ------------------------------
def trace_face_main(resources):

    detector = resources["detector"]
    cam = resources["cam"]
    disp = resources["disp"]
    serial_dev = resources["serial_dev"]
    img_width = resources["img_width"]
    img_height = resources["img_height"]
    image_center = resources["image_center"]
    
    while not app.need_exit():

        img = cam.read().flip(image.FlipDir.Y)
        objs = detector.detect(img, conf_th=0.4, iou_th=0.45)
        center_x, center_y, face_width = process_faces(img, objs)

        # 绘制基础状态
        img.draw_string(20, 20, f"Center: ({center_x}, {center_y})", image.COLOR_GREEN, 1.0)
        img.draw_string(20, 40, f"Max Width: {face_width}", image.COLOR_YELLOW, 1.0)
        
        # 人脸追踪（仅跟踪模式开启时执行）
        if  center_x != -1 and center_y != -1:
            cmd = calculate_deviation_face((center_x, center_y), image_center,  serial_dev)
            if cmd:
                img.draw_string(20, 60, f"Tracking: {cmd}", image.COLOR_BLUE, 1.0)
        elif  center_x == -1:
            cmd = f"DX:9999,DY:9999;"  # 未找到
            serial_dev.write_str(cmd)
            print(cmd)
            img.draw_string(20, 60, "No face detected", image.COLOR_RED, 1.0)
        else:
            img.draw_string(20, 60, "Track Mode: OFF", image.COLOR_ORANGE, 1.0)
        
        disp.show(img)
        time.sleep_ms(1)