import time
import math
from picamera2 import Picamera2

TARGET_CLASSES = {15: "Gato", 16: "Perro"}
SPEED_THRESHOLD = 0.30

def calculate_centroid(box):
    if isinstance(box, dict):
        x = box.get("x", 0.0) + (box.get("width", 0.0) / 2.0)
        y = box.get("y", 0.0) + (box.get("height", 0.0) / 2.0)
    elif isinstance(box, (list, tuple)) and len(box) >= 4:
        x = (box[0] + box[2]) / 2.0
        y = (box[1] + box[3]) / 2.0
    else:
        x, y = 0.5, 0.5
    return x, y

def main():
    picam2 = Picamera2()
    
    post_process_file = "/usr/share/rpi-camera-assets/hailo_yolov8_inference.json"

    # Se inyecta la etapa Hailo directamente en la configuración de vídeo
    config = picam2.create_video_configuration(
        main={"size": (1280, 720), "format": "RGB888"},
        controls={"FrameRate": 30},
        post_process_file=post_process_file
    )
    
    picam2.configure(config)
    picam2.start()

    print("[INFO] Pipeline Picamera2 + Hailo-8L activo.")

    last_centroid = None
    last_time = time.time()

    try:
        while True:
            metadata = picam2.capture_metadata()
            current_time = time.time()
            dt = current_time - last_time

            detections = metadata.get("hailo_detections", [])
            
            pet_found = False
            for det in detections:
                class_id = det.get("class_id")
                confidence = det.get("confidence", 0.0)

                if class_id in TARGET_CLASSES and confidence > 0.45:
                    pet_found = True
                    label = TARGET_CLASSES[class_id]
                    box = det.get("box", [0, 0, 0, 0])
                    
                    centroid = calculate_centroid(box)

                    if last_centroid and dt > 0:
                        dx = centroid[0] - last_centroid[0]
                        dy = centroid[1] - last_centroid[1]
                        dist = math.hypot(dx, dy)
                        speed = dist / dt

                        if speed > SPEED_THRESHOLD:
                            print(f"⚠️ [ALERTA] ¡{label} corriendo! Vel: {speed:.2f} norm/s (Conf: {confidence:.2f})")
                        else:
                            print(f"[OK] {label} detectado (calmado). Vel: {speed:.2f} norm/s")

                    last_centroid = centroid
                    break

            if not pet_found:
                last_centroid = None

            last_time = current_time
            time.sleep(0.03)

    except KeyboardInterrupt:
        print("\n[INFO] Deteniendo monitor...")
    finally:
        picam2.stop()

if __name__ == "__main__":
    main()