# v3.2 - settle 3s with auto on, then read values and lock them as manual
# python Modified_Blur_V3_2.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.2 - Camera Setup"
MAX_CAMERA_INDEX = 10
SETTLE_SECONDS = 3.0


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_enable_auto_exposure(cap):
    for auto_val in (0.75, 3.0, 1.0):
        try:
            cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, auto_val)
            actual = cap.get(cv2.CAP_PROP_AUTO_EXPOSURE)
            if abs(actual - auto_val) < 0.15:
                return True
        except cv2.error:
            continue
    return False


def try_enable_auto_wb(cap):
    try:
        cap.set(cv2.CAP_PROP_AUTO_WB, 1)
        return cap.get(cv2.CAP_PROP_AUTO_WB) > 0.5
    except cv2.error:
        return False


def try_enable_autofocus(cap):
    try:
        cap.set(cv2.CAP_PROP_AUTOFOCUS, 1)
        return cap.get(cv2.CAP_PROP_AUTOFOCUS) > 0.5
    except cv2.error:
        return False


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 enable_all_auto(cap):
    return {
        "ae":  try_enable_auto_exposure(cap),
        "awb": try_enable_auto_wb(cap),
        "af":  try_enable_autofocus(cap),
    }


def safe_get(cap, prop):
    try:
        return cap.get(prop)
    except cv2.error:
        return -1.0


def lock_current_values(cap):
    captured = {
        "exposure": safe_get(cap, cv2.CAP_PROP_EXPOSURE),
        "wb":       safe_get(cap, cv2.CAP_PROP_WB_TEMPERATURE),
        "focus":    safe_get(cap, cv2.CAP_PROP_FOCUS),
        "gain":     safe_get(cap, cv2.CAP_PROP_GAIN),
    }

    disabled = {
        "ae":  try_disable_auto_exposure(cap),
        "awb": try_disable_auto_wb(cap),
        "af":  try_disable_autofocus(cap),
    }

    set_status = {}
    for name, prop_id in [
        ("exposure", cv2.CAP_PROP_EXPOSURE),
        ("wb",       cv2.CAP_PROP_WB_TEMPERATURE),
        ("focus",    cv2.CAP_PROP_FOCUS),
        ("gain",     cv2.CAP_PROP_GAIN),
    ]:
        val = captured[name]
        if val == -1 or val is None:
            set_status[name] = "n/a"
            continue
        try:
            cap.set(prop_id, float(val))
            readback = cap.get(prop_id)
            if abs(readback - val) < max(1.0, abs(val) * 0.1):
                set_status[name] = "locked"
            else:
                set_status[name] = "drift"
        except cv2.error:
            set_status[name] = "fail"

    return {"captured": captured, "disabled": disabled, "set_status": set_status}


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 _fmt_value(v):
    if v == -1 or v is None:
        return "n/a"
    if abs(v) >= 100:
        return f"{v:.0f}"
    return f"{v:.2f}"


def begin_settle(cap):
    enable_all_auto(cap)
    return time.time() + SETTLE_SECONDS


def run_setup_phase(initial_cam_idx, width, height):
    cam_idx = max(0, initial_cam_idx)
    cap = open_camera(cam_idx, width, height)
    if cap is None:
        for i in range(MAX_CAMERA_INDEX + 1):
            cap = open_camera(i, width, height)
            if cap is not None:
                cam_idx = i
                break
        if cap is None:
            print("ERROR: No working camera found.")
            return None

    settle_until = begin_settle(cap)

    state = {
        "phase": "settling",   # "settling" | "locked"
        "lock_info": None,
        "click": None,
        "msg": "",
        "msg_until": 0.0,
    }

    cv2.namedWindow(SETUP_WINDOW)

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

    cv2.setMouseCallback(SETUP_WINDOW, on_mouse)

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

    def switch_to(new_idx):
        nonlocal cap, cam_idx, settle_until
        new_cap = open_camera(new_idx, width, height)
        if new_cap is None:
            flash(f"Camera {new_idx} not available")
            return
        cap.release()
        cap = new_cap
        cam_idx = new_idx
        settle_until = begin_settle(cap)
        state["phase"] = "settling"
        state["lock_info"] = None
        flash(f"Switched to camera {cam_idx} - re-settling")

    def relock():
        nonlocal settle_until
        settle_until = begin_settle(cap)
        state["phase"] = "settling"
        state["lock_info"] = None
        flash("Re-settling…")

    while True:
        ret, frame = 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 {cam_idx}",
                        (20, height // 2), cv2.FONT_HERSHEY_SIMPLEX,
                        0.7, (0, 0, 255), 2)

        if state["phase"] == "settling" and time.time() >= settle_until:
            state["lock_info"] = lock_current_values(cap)
            state["phase"] = "locked"
            cap_d = state["lock_info"]["captured"]
            print(f"LOCKED: exp={_fmt_value(cap_d['exposure'])} "
                  f"WB={_fmt_value(cap_d['wb'])} "
                  f"focus={_fmt_value(cap_d['focus'])} "
                  f"gain={_fmt_value(cap_d['gain'])}")

        h, w = frame.shape[:2]
        panel_h = 200
        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)

        row_y = h + 12
        cam_left_btn = (10, row_y, 36, 28)
        cam_right_btn = (50, row_y, 36, 28)
        _draw_button(display, cam_left_btn, "<")
        _draw_button(display, cam_right_btn, ">")
        cv2.putText(display, f"Camera index: {cam_idx}",
                    (100, row_y + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.55,
                    (255, 255, 255), 1, cv2.LINE_AA)

        if state["phase"] == "settling":
            remaining = max(0.0, settle_until - time.time())
            cv2.putText(display,
                        f"SETTLING (auto on)... {remaining:.1f}s - keep frame empty",
                        (10, h + 70), cv2.FONT_HERSHEY_SIMPLEX, 0.55,
                        (0, 200, 255), 1, cv2.LINE_AA)
            bar_x = 10
            bar_y = h + 80
            bar_w = w - 20
            bar_h = 8
            cv2.rectangle(display, (bar_x, bar_y),
                          (bar_x + bar_w, bar_y + bar_h), (60, 60, 60), -1)
            progress = 1.0 - (remaining / SETTLE_SECONDS)
            cv2.rectangle(display, (bar_x, bar_y),
                          (bar_x + int(bar_w * progress), bar_y + bar_h),
                          (0, 200, 255), -1)
        else:
            info = state["lock_info"]
            cap_d = info["captured"]
            ss = info["set_status"]
            line1 = (f"LOCKED  exp={_fmt_value(cap_d['exposure'])} ({ss['exposure']})  "
                     f"WB={_fmt_value(cap_d['wb'])} ({ss['wb']})")
            line2 = (f"        focus={_fmt_value(cap_d['focus'])} ({ss['focus']})  "
                     f"gain={_fmt_value(cap_d['gain'])} ({ss['gain']})")
            cv2.putText(display, line1, (10, h + 70),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5,
                        (0, 220, 0), 1, cv2.LINE_AA)
            cv2.putText(display, line2, (10, h + 92),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5,
                        (0, 220, 0), 1, cv2.LINE_AA)
            d = info["disabled"]
            ok_color = lambda b: (0, 200, 0) if b else (160, 160, 160)
            cv2.putText(display, "Auto disabled:", (10, h + 118),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.45, (200, 200, 200), 1)
            cv2.putText(display, f"AE {'OK' if d['ae']  else 'n/a'}",
                        (130, h + 118), cv2.FONT_HERSHEY_SIMPLEX, 0.45,
                        ok_color(d["ae"]), 1)
            cv2.putText(display, f"AWB {'OK' if d['awb'] else 'n/a'}",
                        (200, h + 118), cv2.FONT_HERSHEY_SIMPLEX, 0.45,
                        ok_color(d["awb"]), 1)
            cv2.putText(display, f"AF {'OK' if d['af']  else 'n/a'}",
                        (290, h + 118), cv2.FONT_HERSHEY_SIMPLEX, 0.45,
                        ok_color(d["af"]), 1)

        relock_btn = (10, h + panel_h - 45, 130, 32)
        cont_btn = (w - 240, h + panel_h - 45, 230, 32)
        _draw_button(display, relock_btn, "Re-lock",
                     enabled=state["phase"] == "locked")
        _draw_button(display, cont_btn, "CONTINUE  ->  Background Learning",
                     enabled=state["phase"] == "locked",
                     accent=state["phase"] == "locked")

        if state["msg"] and time.time() < state["msg_until"]:
            cv2.putText(display, state["msg"], (150, h + panel_h - 55),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.45,
                        (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(cam_left_btn, (cx, cy)):
                if cam_idx > 0:
                    switch_to(cam_idx - 1)
            elif _hit(cam_right_btn, (cx, cy)):
                if cam_idx < MAX_CAMERA_INDEX:
                    switch_to(cam_idx + 1)
            elif _hit(relock_btn, (cx, cy)) and state["phase"] == "locked":
                relock()
            elif _hit(cont_btn, (cx, cy)) and state["phase"] == "locked":
                cv2.destroyWindow(SETUP_WINDOW)
                return cap, cam_idx

        key = cv2.waitKey(30) & 0xFF
        if key == ord('q'):
            cap.release()
            cv2.destroyWindow(SETUP_WINDOW)
            return None
        elif key == ord('['):
            if cam_idx > 0:
                switch_to(cam_idx - 1)
        elif key == ord(']'):
            if cam_idx < MAX_CAMERA_INDEX:
                switch_to(cam_idx + 1)
        elif key == ord('r') and state["phase"] == "locked":
            relock()
        elif key == ord(' ') and state["phase"] == "locked":
            cv2.destroyWindow(SETUP_WINDOW)
            return cap, cam_idx


def parse_args():
    parser = argparse.ArgumentParser(description="Modified Blur V3.2 - Auto-then-Lock")
    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.2", 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.2", 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()
