# v3.1 - same as v3 but with sliders for exposure / wb / focus
# python Modified_Blur_V3_1.py --camera 1

import cv2
import numpy as np
import argparse
import time
from datetime import datetime


WARMUP_SECONDS = 10
INPUT_BLUR = 5

BGR_THRESHOLD = 25
HSV_THRESHOLD = 35
BGR_STRONG_THRESHOLD = 50

MORPH_OPEN_SIZE = 5
MORPH_OPEN_ITERS = 2
MORPH_CLOSE_SIZE = 13
MORPH_CLOSE_ITERS = 2
MORPH_DILATE_SIZE = 5
MORPH_DILATE_ITERS = 1

USE_MOSAIC = True
MOSAIC_BLOCK_SIZE = 12
MOSAIC_MIN_BLOCK = 4
MOSAIC_MAX_BLOCK = 30

GAUSSIAN_STRENGTH = 99

BLUR_PAD = 8
BLUR_FEATHER_SIZE = 9
BLUR_FEATHER_SIGMA = 3

EXTREMITY_CONNECT_SIZE = 7
EXTREMITY_CONNECT_ITERS = 1

IGNORE_TOP_FRACTION = 0.05

MIN_CONTOUR_AREA = 2000
MIN_HEIGHT = 50
MIN_WIDTH = 20
MIN_ASPECT_RATIO = 0.3
MAX_ASPECT_RATIO = 10.0
MERGE_DISTANCE = 60

SETUP_WINDOW = "Modified Blur V3.1 - Camera Setup"
MAX_CAMERA_INDEX = 10

EXP_OFFSET = 13
EXP_MAX = 26
WB_MIN_HUNDREDS = 20
WB_MAX_HUNDREDS = 65
FOCUS_MAX = 250


def open_camera(idx, width, height):
    cap = cv2.VideoCapture(idx)
    if not cap.isOpened():
        cap.release()
        return None
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
    ok, _ = cap.read()
    if not ok:
        cap.release()
        return None
    return cap


def try_disable_auto_exposure(cap):
    for manual_val in (0.25, 0.0, 1.0):
        try:
            cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, manual_val)
            actual = cap.get(cv2.CAP_PROP_AUTO_EXPOSURE)
            if abs(actual - manual_val) < 0.15:
                return True
        except cv2.error:
            continue
    return False


def try_disable_auto_wb(cap):
    try:
        cap.set(cv2.CAP_PROP_AUTO_WB, 0)
        return cap.get(cv2.CAP_PROP_AUTO_WB) <= 0.5
    except cv2.error:
        return False


def try_disable_autofocus(cap):
    try:
        cap.set(cv2.CAP_PROP_AUTOFOCUS, 0)
        return cap.get(cv2.CAP_PROP_AUTOFOCUS) <= 0.5
    except cv2.error:
        return False


def disable_all_auto(cap):
    return {
        "ae": try_disable_auto_exposure(cap),
        "awb": try_disable_auto_wb(cap),
        "af":  try_disable_autofocus(cap),
    }


def safe_get(cap, prop, fallback):
    try:
        v = cap.get(prop)
        return v if v not in (-1, 0) else fallback
    except cv2.error:
        return fallback


def initial_slider_values(cap):
    exp = safe_get(cap, cv2.CAP_PROP_EXPOSURE, -6)
    wb = safe_get(cap, cv2.CAP_PROP_WB_TEMPERATURE, 4500)
    focus = safe_get(cap, cv2.CAP_PROP_FOCUS, 0)

    exp_pos = max(0, min(EXP_MAX, int(round(exp)) + EXP_OFFSET))
    wb_pos = max(WB_MIN_HUNDREDS,
                 min(WB_MAX_HUNDREDS, int(round(wb / 100))))
    focus_pos = max(0, min(FOCUS_MAX, int(round(focus))))
    return exp_pos, wb_pos, focus_pos


def _hit(rect, p):
    x, y, w, h = rect
    return x <= p[0] < x + w and y <= p[1] < y + h


def _draw_button(img, rect, text, *, enabled=True, accent=False):
    x, y, w, h = rect
    if accent and enabled:
        color = (0, 160, 0)
        text_color = (255, 255, 255)
    elif enabled:
        color = (90, 90, 90)
        text_color = (255, 255, 255)
    else:
        color = (50, 50, 50)
        text_color = (110, 110, 110)
    cv2.rectangle(img, (x, y), (x + w, y + h), color, -1)
    cv2.rectangle(img, (x, y), (x + w, y + h), (0, 0, 0), 1)
    (tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.55, 1)
    tx = x + (w - tw) // 2
    ty = y + (h + th) // 2
    cv2.putText(img, text, (tx, ty), cv2.FONT_HERSHEY_SIMPLEX, 0.55,
                text_color, 1, cv2.LINE_AA)


def run_setup_phase(initial_cam_idx, width, height):
    state = {
        "cam_idx": max(0, initial_cam_idx),
        "cap": None,
        "click": None,
        "support": {"ae": False, "awb": False, "af": False},
        "exp": 0, "wb": 4500, "focus": 0,
        "switching_camera": False,  # suppress trackbar callbacks during open
        "msg": "",
        "msg_until": 0.0,
    }

    def flash(text, seconds=2.0):
        state["msg"] = text
        state["msg_until"] = time.time() + seconds
        print(text)

    def open_at(idx):
        cap = open_camera(idx, width, height)
        if cap is None:
            return None
        state["support"] = disable_all_auto(cap)
        return cap

    state["cap"] = open_at(state["cam_idx"])
    if state["cap"] is None:
        for i in range(MAX_CAMERA_INDEX + 1):
            cap = open_at(i)
            if cap is not None:
                state["cap"] = cap
                state["cam_idx"] = i
                break
        if state["cap"] is None:
            print("ERROR: No working camera found.")
            return None

    state["exp"], wb_h, state["focus"] = initial_slider_values(state["cap"])
    state["wb"] = wb_h * 100

    cv2.namedWindow(SETUP_WINDOW)

    def on_cam(pos):
        if state["switching_camera"]:
            return
        if pos == state["cam_idx"]:
            return
        state["switching_camera"] = True
        new_cap = open_at(pos)
        if new_cap is not None:
            state["cap"].release()
            state["cap"] = new_cap
            state["cam_idx"] = pos
            ep, wp, fp = initial_slider_values(new_cap)
            state["exp"] = ep - EXP_OFFSET
            state["wb"] = wp * 100
            state["focus"] = fp
            cv2.setTrackbarPos("Exposure", SETUP_WINDOW, ep)
            cv2.setTrackbarPos("WB/100K", SETUP_WINDOW, wp)
            cv2.setTrackbarPos("Focus", SETUP_WINDOW, fp)
            flash(f"Switched to camera {pos}")
        else:
            cv2.setTrackbarPos("Camera", SETUP_WINDOW, state["cam_idx"])
            flash(f"Camera {pos} not available")
        state["switching_camera"] = False

    def on_exp(pos):
        if state["switching_camera"]:
            return
        val = pos - EXP_OFFSET
        state["exp"] = val
        try:
            state["cap"].set(cv2.CAP_PROP_EXPOSURE, float(val))
        except cv2.error:
            pass

    def on_wb(pos):
        if state["switching_camera"]:
            return
        val = pos * 100
        state["wb"] = val
        try:
            state["cap"].set(cv2.CAP_PROP_WB_TEMPERATURE, float(val))
        except cv2.error:
            pass

    def on_focus(pos):
        if state["switching_camera"]:
            return
        state["focus"] = pos
        try:
            state["cap"].set(cv2.CAP_PROP_FOCUS, float(pos))
        except cv2.error:
            pass

    cv2.createTrackbar("Camera",   SETUP_WINDOW, state["cam_idx"],
                       MAX_CAMERA_INDEX, on_cam)
    cv2.createTrackbar("Exposure", SETUP_WINDOW, state["exp"] + EXP_OFFSET,
                       EXP_MAX, on_exp)
    cv2.createTrackbar("WB/100K",  SETUP_WINDOW, state["wb"] // 100,
                       WB_MAX_HUNDREDS, on_wb)
    cv2.setTrackbarMin("WB/100K", SETUP_WINDOW, WB_MIN_HUNDREDS)
    cv2.createTrackbar("Focus",    SETUP_WINDOW, state["focus"],
                       FOCUS_MAX, on_focus)

    on_exp(state["exp"] + EXP_OFFSET)
    on_wb(state["wb"] // 100)
    on_focus(state["focus"])

    def on_mouse(event, mx, my, flags, _):
        if event == cv2.EVENT_LBUTTONDOWN:
            state["click"] = (mx, my)

    cv2.setMouseCallback(SETUP_WINDOW, on_mouse)

    while True:
        ret, frame = state["cap"].read()
        if not ret or frame is None:
            frame = np.zeros((height, width, 3), dtype=np.uint8)
            cv2.putText(frame, f"No frames from camera {state['cam_idx']}",
                        (20, height // 2), cv2.FONT_HERSHEY_SIMPLEX,
                        0.7, (0, 0, 255), 2)

        h, w = frame.shape[:2]
        panel_h = 110
        display = np.zeros((h + panel_h, w, 3), dtype=np.uint8)
        display[:h, :w] = frame

        cv2.rectangle(display, (0, h), (w, h + panel_h), (35, 35, 35), -1)
        cv2.line(display, (0, h), (w, h), (80, 80, 80), 1)

        sup = state["support"]
        ok_color = lambda b: (0, 200, 0) if b else (160, 160, 160)
        cv2.putText(display, "Auto disabled:", (10, h + 22),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (220, 220, 220), 1, cv2.LINE_AA)
        cv2.putText(display, f"AE {'OK' if sup['ae']  else 'n/a'}", (130, h + 22),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, ok_color(sup["ae"]), 1, cv2.LINE_AA)
        cv2.putText(display, f"AWB {'OK' if sup['awb'] else 'n/a'}", (210, h + 22),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, ok_color(sup["awb"]), 1, cv2.LINE_AA)
        cv2.putText(display, f"AF {'OK' if sup['af']  else 'n/a'}", (310, h + 22),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, ok_color(sup["af"]), 1, cv2.LINE_AA)

        cv2.putText(display,
                    f"Cam {state['cam_idx']}  exp={state['exp']}  "
                    f"WB={state['wb']}K  focus={state['focus']}",
                    (10, h + 45), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
                    (200, 200, 200), 1, cv2.LINE_AA)

        cont_btn = (w // 2 - 110, h + panel_h - 40, 220, 32)
        _draw_button(display, cont_btn,
                     "CONTINUE  ->  Background Learning",
                     enabled=True, accent=True)

        if state["msg"] and time.time() < state["msg_until"]:
            cv2.putText(display, state["msg"], (10, h + 70),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 200, 255), 1, cv2.LINE_AA)

        cv2.imshow(SETUP_WINDOW, display)

        if state["click"] is not None:
            cx, cy = state["click"]
            state["click"] = None
            if _hit(cont_btn, (cx, cy)):
                cv2.destroyWindow(SETUP_WINDOW)
                return state["cap"], state["cam_idx"]

        key = cv2.waitKey(30) & 0xFF
        if key == ord('q'):
            state["cap"].release()
            cv2.destroyWindow(SETUP_WINDOW)
            return None
        elif key == ord(' '):
            cv2.destroyWindow(SETUP_WINDOW)
            return state["cap"], state["cam_idx"]


def parse_args():
    parser = argparse.ArgumentParser(description="Modified Blur V3.1 - Sliders")
    parser.add_argument("--camera", type=int, default=1)
    parser.add_argument("--output", type=str, default=None)
    parser.add_argument("--width", type=int, default=640)
    parser.add_argument("--height", type=int, default=480)
    return parser.parse_args()


def compute_foreground_mask(frame, background, bgr_thresh, hsv_thresh):
    bgr_diff = cv2.absdiff(frame, background)
    bgr_max = np.max(bgr_diff, axis=2)

    frame_hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    bg_hsv = cv2.cvtColor(background, cv2.COLOR_BGR2HSV)
    hsv_diff = cv2.absdiff(frame_hsv, bg_hsv)
    h_diff = np.minimum(hsv_diff[:, :, 0], 180 - hsv_diff[:, :, 0])
    s_diff = hsv_diff[:, :, 1]
    v_diff = hsv_diff[:, :, 2]
    hsv_max = np.maximum(np.maximum(h_diff, s_diff), v_diff)

    _, bgr_mask = cv2.threshold(bgr_max, bgr_thresh, 255, cv2.THRESH_BINARY)
    _, hsv_mask = cv2.threshold(hsv_max, hsv_thresh, 255, cv2.THRESH_BINARY)
    _, bgr_strong = cv2.threshold(bgr_max, BGR_STRONG_THRESHOLD, 255,
                                   cv2.THRESH_BINARY)

    both_agree = cv2.bitwise_and(bgr_mask, hsv_mask)
    combined = cv2.bitwise_or(both_agree, bgr_strong)

    return combined


def clean_mask(raw_mask):
    k_open = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,
                                        (MORPH_OPEN_SIZE, MORPH_OPEN_SIZE))
    mask = cv2.morphologyEx(raw_mask, cv2.MORPH_OPEN, k_open,
                            iterations=MORPH_OPEN_ITERS)

    k_connect = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,
                                           (EXTREMITY_CONNECT_SIZE,
                                            EXTREMITY_CONNECT_SIZE))
    mask = cv2.dilate(mask, k_connect, iterations=EXTREMITY_CONNECT_ITERS)

    k_close = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,
                                         (MORPH_CLOSE_SIZE, MORPH_CLOSE_SIZE))
    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, k_close,
                            iterations=MORPH_CLOSE_ITERS)

    k_erode = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
    mask = cv2.erode(mask, k_erode, iterations=1)

    k_dilate = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,
                                          (MORPH_DILATE_SIZE, MORPH_DILATE_SIZE))
    mask = cv2.dilate(mask, k_dilate, iterations=MORPH_DILATE_ITERS)

    return mask


def merge_nearby_boxes(boxes, merge_dist):
    if len(boxes) <= 1:
        return boxes

    merged = True
    while merged:
        merged = False
        new_boxes = []
        used = [False] * len(boxes)

        for i in range(len(boxes)):
            if used[i]:
                continue
            x1, y1, w1, h1 = boxes[i][:4]
            ex1, ey1 = x1 - merge_dist, y1 - merge_dist
            ex2, ey2 = x1 + w1 + merge_dist, y1 + h1 + merge_dist

            for j in range(i + 1, len(boxes)):
                if used[j]:
                    continue
                x2, y2, w2, h2 = boxes[j][:4]
                if (ex1 < x2 + w2 and ex2 > x2 and
                    ey1 < y2 + h2 and ey2 > y2):
                    nx, ny = min(x1, x2), min(y1, y2)
                    nx2 = max(x1 + w1, x2 + w2)
                    ny2 = max(y1 + h1, y2 + h2)
                    x1, y1 = nx, ny
                    w1, h1 = nx2 - nx, ny2 - ny
                    ex1, ey1 = x1 - merge_dist, y1 - merge_dist
                    ex2, ey2 = x1 + w1 + merge_dist, y1 + h1 + merge_dist
                    used[j] = True
                    merged = True

            new_boxes.append((x1, y1, w1, h1, w1 * h1))
            used[i] = True
        boxes = new_boxes

    return boxes


def find_persons(mask, frame_height):
    min_y = int(frame_height * IGNORE_TOP_FRACTION)
    contours, _ = cv2.findContours(
        mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    boxes = []
    for contour in contours:
        area = cv2.contourArea(contour)
        if area < MIN_CONTOUR_AREA:
            continue
        x, y, w, h = cv2.boundingRect(contour)
        if (y + h) < min_y:
            continue
        boxes.append((x, y, w, h, area))

    merged = merge_nearby_boxes(boxes, MERGE_DISTANCE)

    persons = []
    for (x, y, w, h, area) in merged:
        if h < MIN_HEIGHT or w < MIN_WIDTH:
            continue
        aspect = h / w
        if aspect < MIN_ASPECT_RATIO or aspect > MAX_ASPECT_RATIO:
            continue
        persons.append((x, y, w, h, area))

    return persons


def apply_mosaic(frame, mask, block_size):
    h, w = frame.shape[:2]
    if BLUR_PAD > 0:
        k_pad = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,
                                           (BLUR_PAD * 2 + 1, BLUR_PAD * 2 + 1))
        mask = cv2.dilate(mask, k_pad, iterations=1)

    small_w = max(1, w // block_size)
    small_h = max(1, h // block_size)
    small = cv2.resize(frame, (small_w, small_h), interpolation=cv2.INTER_AREA)
    pixelated = cv2.resize(small, (w, h), interpolation=cv2.INTER_NEAREST)

    mask_3ch = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR).astype(np.float32) / 255.0
    mask_3ch = cv2.GaussianBlur(mask_3ch,
                                 (BLUR_FEATHER_SIZE, BLUR_FEATHER_SIZE),
                                 BLUR_FEATHER_SIGMA)

    result = (pixelated.astype(np.float32) * mask_3ch +
              frame.astype(np.float32) * (1.0 - mask_3ch))
    return result.astype(np.uint8)


def apply_gaussian_blur(frame, mask):
    fully_blurred = cv2.GaussianBlur(frame, (GAUSSIAN_STRENGTH, GAUSSIAN_STRENGTH), 30)

    blur_mask = mask.copy()
    if BLUR_PAD > 0:
        k_pad = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,
                                           (BLUR_PAD * 2 + 1, BLUR_PAD * 2 + 1))
        blur_mask = cv2.dilate(blur_mask, k_pad, iterations=1)

    mask_3ch = cv2.cvtColor(blur_mask, cv2.COLOR_GRAY2BGR).astype(np.float32) / 255.0
    mask_3ch = cv2.GaussianBlur(mask_3ch,
                                 (BLUR_FEATHER_SIZE, BLUR_FEATHER_SIZE),
                                 BLUR_FEATHER_SIGMA)

    result = (fully_blurred.astype(np.float32) * mask_3ch +
              frame.astype(np.float32) * (1.0 - mask_3ch))
    return result.astype(np.uint8)


def main():
    args = parse_args()

    setup_result = run_setup_phase(args.camera, args.width, args.height)
    if setup_result is None:
        print("Setup cancelled.")
        return
    cap, cam_idx = setup_result

    actual_w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
    actual_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
    fps = cap.get(cv2.CAP_PROP_FPS) or 30.0
    print(f"Camera {cam_idx}: {actual_w}x{actual_h} @ {fps:.0f} FPS")

    output_file = args.output or f"mosaic_{datetime.now().strftime('%Y%m%d_%H%M%S')}.avi"
    fourcc = cv2.VideoWriter_fourcc(*'XVID')
    writer = cv2.VideoWriter(output_file, fourcc, fps, (actual_w, actual_h))
    print(f"Recording to: {output_file}")

    background = None
    warmup_frames = []
    start_time = time.time()
    paused = False
    bgr_thresh = BGR_THRESHOLD
    hsv_thresh = HSV_THRESHOLD
    use_mosaic = USE_MOSAIC
    block_size = MOSAIC_BLOCK_SIZE
    view_mode = 2
    key = 0

    print(f"\n{'='*55}")
    print(f"  LEARNING BACKGROUND for {WARMUP_SECONDS} seconds")
    print(f"  >> Keep ALL people OUT of the frame! <<")
    print(f"{'='*55}\n")

    while True:
        if paused:
            key = cv2.waitKey(30) & 0xFF
        else:
            ret, frame_raw = cap.read()
            if not ret:
                break

            frame = cv2.GaussianBlur(frame_raw, (INPUT_BLUR, INPUT_BLUR), 0)

            elapsed = time.time() - start_time
            warmup_remaining = max(0, WARMUP_SECONDS - elapsed)
            is_warming_up = warmup_remaining > 0

            if is_warming_up:
                warmup_frames.append(frame.astype(np.float64))

                display = frame_raw.copy()
                progress = 1.0 - (warmup_remaining / WARMUP_SECONDS)
                bar_w = int(actual_w * 0.6)
                bar_x = int(actual_w * 0.2)
                bar_y = actual_h - 50
                cv2.rectangle(display, (bar_x, bar_y),
                              (bar_x + bar_w, bar_y + 25), (60, 60, 60), -1)
                cv2.rectangle(display, (bar_x, bar_y),
                              (bar_x + int(bar_w * progress), bar_y + 25),
                              (0, 255, 255), -1)
                cv2.putText(display,
                            f"LEARNING BACKGROUND... {warmup_remaining:.1f}s",
                            (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8,
                            (0, 255, 255), 2)
                cv2.putText(display,
                            f"Frames: {len(warmup_frames)}  |  "
                            f"Cam {cam_idx}  |  Res: {actual_w}x{actual_h}",
                            (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
                            (200, 200, 200), 1)
                writer.write(frame_raw)
                cv2.imshow("Modified Blur V3.1", display)

            elif background is None:
                print(f"Averaging {len(warmup_frames)} frames...")
                bg_sum = np.zeros_like(warmup_frames[0])
                for f in warmup_frames:
                    bg_sum += f
                background = np.uint8(bg_sum / len(warmup_frames))
                warmup_frames = []
                print("Background FROZEN. Walk into frame now!\n")

            else:
                raw_mask = compute_foreground_mask(frame, background,
                                                   bgr_thresh, hsv_thresh)
                clean = clean_mask(raw_mask)
                persons = find_persons(clean, actual_h)

                person_mask = np.zeros_like(clean)
                for (x, y, w, h, _) in persons:
                    person_mask[y:y+h, x:x+w] = clean[y:y+h, x:x+w]

                if use_mosaic:
                    anonymized = apply_mosaic(frame_raw, person_mask, block_size)
                else:
                    anonymized = apply_gaussian_blur(frame_raw, person_mask)

                if view_mode == 1:
                    display = frame_raw.copy()
                    green = np.zeros_like(display)
                    green[:, :, 1] = person_mask
                    display = cv2.addWeighted(display, 0.7, green, 0.5, 0)
                    for (x, y, w, h, area) in persons:
                        cv2.rectangle(display, (x, y), (x+w, y+h),
                                      (0, 0, 255), 2)
                        cv2.putText(display, f"Person {w}x{h}",
                                    (x, y-10), cv2.FONT_HERSHEY_SIMPLEX,
                                    0.5, (0, 0, 255), 2)
                    n = len(persons)
                    color = (0, 255, 0) if n > 0 else (100, 100, 255)
                    cv2.putText(display, f"Detections: {n}", (10, 30),
                                cv2.FONT_HERSHEY_SIMPLEX, 0.8, color, 2)

                elif view_mode == 2:
                    display = anonymized
                    writer.write(display)
                    mode_str = f"MOSAIC (block={block_size})" if use_mosaic else "GAUSSIAN"
                    if persons:
                        cv2.putText(display,
                                    f"Anonymizing {len(persons)} person(s) [{mode_str}]",
                                    (10, 30), cv2.FONT_HERSHEY_SIMPLEX,
                                    0.6, (0, 255, 0), 2)

                elif view_mode == 3:
                    h2, w2 = actual_h // 2, actual_w // 2
                    p1 = cv2.cvtColor(clean, cv2.COLOR_GRAY2BGR)
                    cv2.putText(p1, "Detection Mask", (5, 25),
                                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 2)
                    p2 = cv2.cvtColor(person_mask, cv2.COLOR_GRAY2BGR)
                    cv2.putText(p2, "Person Mask", (5, 25),
                                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 2)
                    for (x, y, w, h, _) in persons:
                        cv2.rectangle(p2, (x, y), (x+w, y+h), (0, 0, 255), 2)
                    p3_mosaic = apply_mosaic(frame_raw, person_mask, block_size)
                    cv2.putText(p3_mosaic, f"Mosaic (block={block_size})",
                                (5, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
                                (0, 255, 255), 2)
                    p4_gauss = apply_gaussian_blur(frame_raw, person_mask)
                    cv2.putText(p4_gauss, "Gaussian", (5, 25),
                                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 2)
                    top = np.hstack([cv2.resize(p1, (w2, h2)),
                                     cv2.resize(p2, (w2, h2))])
                    bot = np.hstack([cv2.resize(p3_mosaic, (w2, h2)),
                                     cv2.resize(p4_gauss, (w2, h2))])
                    display = np.vstack([top, bot])
                    n = len(persons)
                    color = (0, 255, 0) if n > 0 else (100, 100, 255)
                    cv2.putText(display, f"Detections: {n}", (10, 30),
                                cv2.FONT_HERSHEY_SIMPLEX, 0.8, color, 2)

                mode_str = f"MOSAIC({block_size})" if use_mosaic else "GAUSSIAN"
                info = (f"BGR={bgr_thresh} HSV={hsv_thresh} "
                        f"{mode_str} cam{cam_idx} {actual_w}x{actual_h} | "
                        f"[m]mode [n/N]size [b]reset [q]quit")
                cv2.putText(display, info, (10, display.shape[0] - 10),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.35, (200, 200, 200), 1)

                cv2.imshow("Modified Blur V3.1", display)

            key = cv2.waitKey(1) & 0xFF

        if key == ord('q'):
            break
        elif key == ord('b'):
            background = None
            warmup_frames = []
            start_time = time.time()
            print("Background reset!")
        elif key == ord('p'):
            paused = not paused
        elif key == ord('+') or key == ord('='):
            bgr_thresh = max(5, bgr_thresh - 3)
            print(f"BGR: {bgr_thresh}")
        elif key == ord('-'):
            bgr_thresh = min(80, bgr_thresh + 3)
            print(f"BGR: {bgr_thresh}")
        elif key == ord('['):
            hsv_thresh = max(5, hsv_thresh - 3)
            print(f"HSV: {hsv_thresh}")
        elif key == ord(']'):
            hsv_thresh = min(80, hsv_thresh + 3)
            print(f"HSV: {hsv_thresh}")
        elif key == ord('m'):
            use_mosaic = not use_mosaic
            print(f"Mode: {'MOSAIC' if use_mosaic else 'GAUSSIAN'}")
        elif key == ord('n'):
            block_size = max(MOSAIC_MIN_BLOCK, block_size - 2)
            print(f"Mosaic block size: {block_size}")
        elif key == ord('N'):
            block_size = min(MOSAIC_MAX_BLOCK, block_size + 2)
            print(f"Mosaic block size: {block_size}")
        elif key == ord('1'):
            view_mode = 1
        elif key == ord('2'):
            view_mode = 2
        elif key == ord('3'):
            view_mode = 3

    cap.release()
    writer.release()
    cv2.destroyAllWindows()
    print(f"Done! Video saved to: {output_file}")


if __name__ == "__main__":
    main()
