from collections import deque
import cv2 as cv
import numpy as np


def estimate_focus_measure(frame):
    gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
    laplacian = cv.Laplacian(gray, cv.CV_64F)
    return laplacian.var()


def update_threshold(blur_history):
    return np.percentile(blur_history, 50)


def select_most_focused(frame_history, threshold, select_n=5):
    sorted_frames = sorted(frame_history, key=lambda x: x[1], reverse=True)
    # Select least blurry frames from the last incomplete second
    selected = [frame for frame, fm in sorted_frames[:select_n] if fm > threshold]
    return selected


def filter_frames(cap):
    fps = int(cap.get(cv.CAP_PROP_FPS))
    frame_history = deque(maxlen=fps)
    blur_history = []
    most_focused_frames = []

    if cap.isOpened():
        ret, frame = cap.read()
        counter = 1
        second = 0

        while ret:
            fm = estimate_focus_measure(frame)
            frame_history.append([frame, fm])
            blur_history.append(fm)

            if counter >= fps:
                second += 1
                threshold = update_threshold(blur_history)
                if counter % fps == 0:
                    most_focused_frames += select_most_focused(frame_history, threshold)
                    frame_history.clear()
            ret, frame = cap.read()
            counter += 1

        if frame_history:
            threshold = update_threshold(blur_history)
            most_focused_frames += select_most_focused(frame_history, threshold)

    cap.release()
    return most_focused_frames