import cv2
from picamera2 import Picamera2

# --- Camera setup ---
picam2 = Picamera2()
# Lower resolution = faster on Pi 3B+
config = picam2.create_preview_configuration(main={"size": (640, 360), "format": "RGB888"})
picam2.configure(config)
picam2.start()

# --- People detector (HOG) ---
hog = cv2.HOGDescriptor()
hog.setSVMDetector(cv2.HOGDescriptor_getDefaultPeopleDetector())

while True:
    frame = picam2.capture_array()  # RGB
    frame_bgr = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)

    # Detect people
    boxes, weights = hog.detectMultiScale(
        frame_bgr,
        winStride=(8, 8),
        padding=(8, 8),
        scale=1.05
    )

    # Draw detections
    for (x, y, w, h), conf in zip(boxes, weights):
        if conf < 0.4:
            continue
        cv2.rectangle(frame_bgr, (x, y), (x + w, y + h), (0, 255, 0), 2)
        cv2.putText(frame_bgr, f"person {conf:.2f}", (x, max(0, y - 6)),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)

    cv2.imshow("Body / Person Detector (press q)", frame_bgr)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cv2.destroyAllWindows()
picam2.stop()