# decide_distance.py — ใบงานเสริม 4.3F ระดับ 2: รักษาระยะห่างด้วยขนาดกรอบ
# ตรรกะจำลอง ไม่มีคำสั่งควบคุมฮาร์ดแวร์ เขียนให้ผ่านกรณีทดสอบก่อน แล้วจึงย้ายไปใส่ main.py
# แนวคิด: กรอบใบหน้ากว้างน้อย = อยู่ไกล -> เดินหน้า, กว้างมาก = อยู่ใกล้ -> ถอย

NEAR = 0.35          # สัดส่วนความกว้างกรอบต่อความกว้างภาพ ที่ถือว่าใกล้เกินไป
FAR = 0.20           # ต่ำกว่านี้ถือว่าไกลเกินไป
DRIVE = 0.05         # ความเร็วเดินหน้า/ถอย (0.05 ~ 25 us)


def decide_drive(box_width_frac, found):
    """คืนค่า drive: + เดินหน้า, - ถอย, 0 อยู่กับที่"""
    if not found:
        return 0.0
    if box_width_frac < FAR:
        return DRIVE
    if box_width_frac > NEAR:
        return -DRIVE
    return 0.0


def wheel_pulses(drive, turn):
    """รวมการเดินหน้ากับการเลี้ยว (แบบเดียวกับ DRIVE_BIAS ใน main.py)"""
    left = round(1500 + (turn + drive) * 500)
    right = round(1500 + (-turn + drive) * 500)
    return left, right


test_cases = [
    (0.10, True, 0.05),     # ไกล -> เดินหน้า
    (0.28, True, 0.0),      # ระยะพอดี -> อยู่กับที่
    (0.50, True, -0.05),    # ใกล้ -> ถอย
    (0.00, False, 0.0),     # ไม่พบ -> หยุด
]
for width, found, expected in test_cases:
    actual = decide_drive(width, found)
    assert actual == expected, (width, found, actual)
    print(width, found, actual, wheel_pulses(actual, 0.0))

# ต่อยอด: ใน main.py คำนวณ box_width_frac = (x_max - x_min) / FRAME_WIDTH
# แล้วใช้ wheel_pulses(decide_drive(...), turn) แทน speed_to_pulse(turn)
# ทดสอบแบบยกล้อก่อนเสมอ และจำกัด DRIVE ไม่เกิน 0.08