import cv2
import time
from picamera2 import Picamera2

# ---------- Camera (small = fast + low power) ----------
W, H = 320, 240
picam2 = Picamera2()
config = picam2.create_preview_configuration(main={"size": (W, H), "format": "RGB888"})
picam2.configure(config)
picam2.start()

# ---------- Motion detector (very cheap) ----------
bg = cv2.createBackgroundSubtractorMOG2(history=200, varThreshold=25, detectShadows=False)

# ---------- People detector (heavier, used sparingly) ----------
hog = cv2.HOGDescriptor()
hog.setSVMDetector(cv2.HOGDescriptor_getDefaultPeopleDetector())

last_boxes = []
last_conf = []
last_detect_time = 0.0

# Run HOG at most ~2 times/second when motion is present
DETECT_INTERVAL_SEC = 0.5

print("Low-power detector running. Press q to quit.")

while True:
    frame_rgb = picam2.capture_array()
    frame = cv2.cvtColor(frame_rgb, cv2.COLOR_RGB2BGR)

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    gray = cv2.GaussianBlur(gray, (5, 5), 0)

    # Motion mask
    fg = bg.apply(gray)
    fg = cv2.threshold(fg, 200, 255, cv2.THRESH_BINARY)[1]
    fg = cv2.erode(fg, None, iterations=1)
    fg = cv2.dilate(fg, None, iterations=2)

    motion_pixels = cv2.countNonZero(fg)
    motion = motion_pixels > (W * H * 0.02)  # ~2% of frame changed

    now = time.time()

    # Only run heavy person detection if there's motion AND enough time passed
    if motion and (now - last_detect_time) >= DETECT_INTERVAL_SEC:
        boxes, weights = hog.detectMultiScale(
            frame,
            winStride=(8, 8),
            padding=(8, 8),
            scale=1.10
        )
        # Filter weak detections
        filtered = []
        filtered_w = []
        for (x, y, w, h), conf in zip(boxes, weights):
            if conf >= 0.7:
                filtered.append((x, y, w, h))
                filtered_w.append(conf)
        last_boxes, last_conf = filtered, filtered_w
        last_detect_time = now

    # Draw motion status
    cv2.putText(frame, f"motion: {'YES' if motion else 'no'}", (8, 20),
                cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)

    # Draw last known person boxes (even between detections)
    for (x, y, w, h), conf in zip(last_boxes, last_conf):
        cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
        cv2.putText(frame, f"person {conf:.2f}", (x, max(0, y - 6)),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)

    cv2.imshow("Low-Power Body/Person Detector (q to quit)", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cv2.destroyAllWindows()
picam2.stop()