import cv2
import sys
import os
from collections import deque
import numpy as np

# Add project root to path
current_dir = os.path.dirname(os.path.abspath(__file__))
parent_dir = os.path.dirname(current_dir)
sys.path.insert(0, parent_dir)

from src.face_detector.manager import FaceDetector
from src.evm.evm_manager import process_video_evm_vital_signs
from src.config import TARGET_ROI_SIZE

def measure_vital_signs(video_path, buffer_size=200):
    """Extract heart rate and respiratory rate from video."""
    cap = cv2.VideoCapture(video_path)
    if not cap.isOpened():
        print(f"Error: Could not open {video_path}")
        return
    
    # Initialize face detector
    face_detector = FaceDetector(model_type='mediapipe')
    frame_buffer = []
    hr_history = deque(maxlen=10)
    
    print("Processing video...")
    
    try:
        while True:
            ret, frame = cap.read()
            if not ret:
                break
            
            # Detect face and extract ROI
            roi = face_detector.detect_face(frame)
            
            if roi:
                x, y, w, h = roi
                roi_frame = frame[y:y+h, x:x+w]
                roi_frame = cv2.resize(roi_frame, TARGET_ROI_SIZE)
                frame_buffer.append(roi_frame)
                
                # Process when buffer is full
                if len(frame_buffer) >= buffer_size:
                    result = process_video_evm_vital_signs(frame_buffer)
                    
                    hr = result['heart_rate']
                    rr = result['respiratory_rate']
                    
                    if hr is not None:
                        hr_history.append(hr)
                        filtered_hr = np.median(list(hr_history))
                        print(f"Heart Rate: {filtered_hr:.1f} BPM")
                    
                    if rr is not None:
                        print(f"Respiratory Rate: {rr:.1f} BPM")
                    
                    frame_buffer.clear()
    
    finally:
        cap.release()
        face_detector.close()
        print("Processing complete!")

if __name__ == "__main__":
    # Replace with your video file path
    measure_vital_signs("path/to/your/video.mp4")

face_detector = FaceDetector(model_type='mediapipe')
# Fast and accurate - best for Raspberry Pi
# ~30-40 FPS on RPi 4