# face_follow_main.py — MPU (Linux) ฉบับย่อสำหรับเรียน ไม่มี Web UI
# วางเป็น python/main.py คู่กับ robot_base_servo.ino (หุ่นฐานเซอร์โวแบบกล่อง D9/D10)
# หรือ face_follow_sketch.ino ของต้นฉบับ (D3/D6) ไม่ต้องแก้ไฟล์นี้ เพราะใช้ set_wheel_pwm เหมือนกัน
# app.yaml ต้องมี brick arduino:video_object_detection (model: face-detection)
# ฉบับเต็มที่มี Web UI ปรับค่าได้ ใช้ python/main.py ของโปรเจกต์ face-following-robot-uno-q

import math
import threading
import time
from arduino.app_utils import App, Bridge
from arduino.app_bricks.video_objectdetection import VideoObjectDetection

# ---------- ค่าที่ปรับได้ ----------
TARGET_CLASS = "face"        # ต้องตรงกับ label ของโมเดลทุกตัวอักษร
CONFIDENCE = 0.5
FRAME_WIDTH = 640.0          # ตรวจจาก log sample_detections
STEER_GAIN = 0.15
MAX_TURN_SPEED = 0.25        # ~ +/-125 us จากจุดหยุด
STEER_CURVE = 0.4
CENTER_DEADBAND = 0.05
STEER_SIGN = 1.0             # ใช้ -1.0 ถ้าหุ่นหันหนีใบหน้า
TRACKING_TIMEOUT = 0.5       # วินาทีที่ยังเลี้ยวต่อเมื่อใบหน้าหายชั่วคราว
NO_DETECTION_TIMEOUT = 1.5   # watchdog: ไม่มีผลตรวจจับนานเท่านี้ให้หยุด
MIN_BRIDGE_INTERVAL = 0.05   # ส่งคำสั่งไม่เกิน 20 ครั้ง/วินาที

STOP_US = 1500

detector = VideoObjectDetection(confidence=CONFIDENCE, debounce_sec=0.0)
last_cmd = (STOP_US, STOP_US)
last_bridge_ts = 0.0
last_callback_ts = 0.0
last_target_cx = None
last_target_ts = 0.0


def clamp(v, lo, hi):
    return max(lo, min(hi, v))


def speed_to_pulse(speed):
    return int(clamp(STOP_US + speed * 500, 1000, 2000))


def best_target_cx(detections):
    """คืนตำแหน่งกลางกรอบ (0-1) ของกรอบที่ใหญ่ที่สุด หรือ None"""
    boxes = detections.get(TARGET_CLASS, []) if isinstance(detections, dict) else []
    if isinstance(boxes, dict):
        boxes = [boxes]
    best, best_area = None, -1
    for det in boxes:
        x0, y0, x1, y1 = det["bounding_box_xyxy"]
        area = max(0, x1 - x0) * max(0, y1 - y0)
        if area > best_area:
            best, best_area = (x0 + x1) / 2 / FRAME_WIDTH, area
    return best


def steer(target_cx):
    """ตัวควบคุมแบบสัดส่วน: ตำแหน่งใบหน้า -> (left_us, right_us)"""
    heading_error = (target_cx - 0.5) * 2.0
    if abs(heading_error) < CENTER_DEADBAND:
        heading_error = 0.0
    shaped = math.copysign(abs(heading_error) ** STEER_CURVE, heading_error)
    turn = clamp(STEER_SIGN * STEER_GAIN * shaped, -MAX_TURN_SPEED, MAX_TURN_SPEED)
    return speed_to_pulse(turn), speed_to_pulse(-turn)


def send(left_us, right_us, urgent=False):
    """ส่งคำสั่งเฉพาะเมื่อเปลี่ยน และจำกัดความถี่ (ยกเว้นคำสั่งเร่งด่วน)"""
    global last_cmd, last_bridge_ts
    now = time.monotonic()
    if (left_us, right_us) == last_cmd and not urgent:
        return
    if not urgent and now - last_bridge_ts < MIN_BRIDGE_INTERVAL:
        return
    last_cmd, last_bridge_ts = (left_us, right_us), now
    Bridge.notify("set_wheel_pwm", left_us, right_us)
    print(f"wheel left={left_us} right={right_us}")


def on_detections(detections):
    global last_callback_ts, last_target_cx, last_target_ts
    last_callback_ts = time.monotonic()
    cx = best_target_cx(detections)
    if cx is not None:
        last_target_cx, last_target_ts = cx, last_callback_ts
    elif last_target_cx is not None and last_callback_ts - last_target_ts < TRACKING_TIMEOUT:
        cx = last_target_cx                      # coasting
    if cx is None:
        send(STOP_US, STOP_US)                   # lost target: หยุด
    else:
        send(*steer(cx))


def watchdog():
    while True:
        time.sleep(0.25)
        if time.monotonic() - last_callback_ts > NO_DETECTION_TIMEOUT:
            send(STOP_US, STOP_US, urgent=True)


threading.Thread(target=watchdog, daemon=True).start()
detector.on_detect_all(on_detections)
App.run()