MonitorTracking.py

import cv2
import numpy as np
import os
import mediapipe as mp
import time
import math
from scipy.spatial.transform import Rotation as Rscipy
from collections import deque
import pyautogui
import threading
import keyboard
import ctypes
import csv
import json

# Screen and mouse control setup (from old script)
MONITOR_WIDTH, MONITOR_HEIGHT = pyautogui.size()
CENTER_X = MONITOR_WIDTH // 2
CENTER_Y = MONITOR_HEIGHT // 2
mouse_control_enabled = False
filter_length = 10
gaze_length = 350

# === Stabilization parameters (Phase 1) ===
SMOOTH_TAU_BASE = 0.06
SMOOTH_TAU_MIN  = 0.028
SMOOTH_TAU_MAX  = 0.16
MAX_SPEED_BASE_PX_S = 3200.0

GAZE_FILTER_FREQ = 120.0
GAZE_MIN_CUTOFF  = 1.0
GAZE_BETA        = 0.65
GAZE_D_CUTOFF    = 1.0

GAZE_SPEED_ALPHA = 0.25
GAZE_SPEED_SAT   = 8.0

DEADZONE_PX           = 0.25
DEADZONE_EXTRA_STILL  = 2.2
SPEED_GAIN_MIN        = 1.2
SPEED_GAIN_MAX        = 3.5

IVT_FIX_NORM        = 0.03
IVT_SACCADE_NORM    = 999.0
IVT_FIX_DWELL_SEC   = 0.12
SACCADE_FREEZE_SEC  = 0.0
FIXATION_RADIUS_BASE = 6.0
FIX_HOLD_MIN_S       = 0.40
FIX_RADIUS_GAIN_PLANE = 0.25
FIX_RADIUS_GAIN_DPI   = 0.15

# === Velocity-mode control (intent→velocity→cursor) ===
VELOCITY_MODE_DEFAULT = False
BIO_DEADZONE_PX       = 3.0       # deadzone sinh học (px)
VEL_GAIN_BASE_X       = 2800.0    # px/s cho |dx_norm|=1 (đã hạ để giảm loạn)
VEL_GAIN_BASE_Y       = 2800.0    # px/s cho |dy_norm|=1 (đã hạ để giảm loạn)
INTENT_EMA_ALPHA      = 0.22      # EMA chậm hơn → ổn định hơn
BASELINE_TAU_S        = 0.8       # baseline cập nhật chậm (giây)
AB_INTENT_DEADZONE    = 0.08      # deadzone trên (a,b) cho ý định (tỷ lệ 0..1)
VEL_SHAPE_GAMMA       = 1.6       # shaping |x|^gamma để bóp nhỏ nhiễu gần 0
BASELINE_PCT_DEADZONE = 0.008     # 0.8% theo kích thước khung, cho baseline delta
VEL_MAX_PX_S          = 1800.0    # trần tốc độ
VEL_MAX_ACC_PX_S2     = 9000.0    # trần gia tốc
VEL_EMA_ALPHA         = 0.28      # mượt vận tốc đầu ra

# --- Head-motion compensation (Phase 2.1) ---
HEAD_ROT_SAT_DEG_S       = 45.0
HEAD_TRANS_SAT_WORLD_S   = 160.0
HEAD_MOTION_ALPHA        = 0.30
SACCADE_BOOST_DEG_S      = 25.0
SACCADE_BOOST_DURATION_S = 0.10

# --- Orbit camera state for the debug view ---
orbit_yaw   = -151.0          # radians, left/right
orbit_pitch = 00.0          # radians, up/down
orbit_radius = 1500.0       # distance from head center
orbit_fov_deg = 50.0       # horizontal FOV for projection

# --- Debug-view world freeze (pivot fixed after center calibration) ---
debug_world_frozen = False
orbit_pivot_frozen = None  # world-space point the debug camera orbits (monitor center at calib)

# Stored gaze markers on the monitor plane (as (a,b) in plane coords)
# a = 0..1 across width (p0->p1), b = 0..1 down height (p0->p3)
gaze_markers = []

# --- 3D monitor plane state (world space) ---
monitor_corners = None   # list of 4 world points (p0..p3)
monitor_center_w = None  # world center of the plane
monitor_normal_w = None  # world normal
units_per_cm = None      # world units per centimeter (computed at calibration)

# Shared mouse target position
mouse_target = [CENTER_X, CENTER_Y]
mouse_lock = threading.Lock()

# --- Camera orientation control (rotate captured frame) ---
camera_orientation_index = 0  # 0=default, 1=90° CW, 2=180°, 3=270° CW
CAMERA_ORIENTATION_LABELS = [
    "0° (landscape)",
    "90° CW (portrait)",
    "180°",
    "270° CW"
]

# Calibration offsets for screen mapping
calibration_offset_yaw = 0
calibration_offset_pitch = 0

# --- Simple monitor-edge calibration state ---
# 0 = waiting for center, 1 = waiting for left edge, 2 = done
calib_step = 0

# Buffers to store recent gaze data for smoothing
combined_gaze_directions = deque(maxlen=filter_length)

# reference matrices to fix coordinate flipping issue
# These help keep the axes consistent from frame to frame by stabilizing eigenvector directions
R_ref_nose = [None]
R_ref_forehead = [None]
calibration_nose_scale = None

# Initialize MediaPipe FaceMesh
mp_face_mesh = mp.solutions.face_mesh
face_mesh = mp_face_mesh.FaceMesh(
    static_image_mode=False,
    max_num_faces=1,
    refine_landmarks=True,
    min_detection_confidence=0.5,
    min_tracking_confidence=0.5
)

# === Open webcam ===
cap = cv2.VideoCapture(3)
w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))

# === Nose-only landmark indices (for stable up/down eye sphere tracking) ===
# These landmarks are near the nose and are less affected by lateral head movement
nose_indices = [4, 45, 275, 220, 440, 1, 5, 51, 281, 44, 274, 241, 
                461, 125, 354, 218, 438, 195, 167, 393, 165, 391,
                3, 248]

# ===== NEW: File writing for screen position =====
screen_position_file = "C:/Users/white/Desktop/EyeTracker-main/gaze_vector.txt"

def write_screen_position(x, y):
    """Write screen position to file, overwriting the same line"""
    with open(screen_position_file, 'w') as f:
        f.write(f"{x},{y}\n")

def _rot_x(a):
    ca, sa = math.cos(a), math.sin(a)
    return np.array([[1, 0, 0],
                     [0, ca, -sa],
                     [0, sa,  ca]], dtype=float)

def _rot_y(a):
    ca, sa = math.cos(a), math.sin(a)
    return np.array([[ ca, 0, sa],
                     [  0, 1,  0],
                     [-sa, 0, ca]], dtype=float)

def _normalize(v):
    v = np.asarray(v, dtype=float)
    n = np.linalg.norm(v)
    return v / n if n > 1e-9 else v

def apply_camera_orientation(frame):
    """Rotate incoming camera frame theo hướng người dùng chọn."""
    if camera_orientation_index == 1:
        return cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE)
    if camera_orientation_index == 2:
        return cv2.rotate(frame, cv2.ROTATE_180)
    if camera_orientation_index == 3:
        return cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE)
    return frame

class OneEuroFilter:
    def __init__(self, freq, min_cutoff=1.0, beta=0.0, d_cutoff=1.0):
        self.freq = freq
        self.min_cutoff = min_cutoff
        self.beta = beta
        self.d_cutoff = d_cutoff
        self.x_prev = None
        self.dx_prev = 0.0
        self.t_prev = None

    @staticmethod
    def _alpha(cutoff, dt):
        tau = 1.0 / (2 * math.pi * cutoff)
        return 1.0 / (1.0 + tau / dt)

    def filter(self, x, timestamp=None):
        x = float(x)
        if self.x_prev is None:
            self.x_prev = x
            self.dx_prev = 0.0
            self.t_prev = timestamp
            return x

        if timestamp is not None:
            dt = max(1e-6, (timestamp - self.t_prev) if self.t_prev is not None else (1.0 / self.freq))
            self.t_prev = timestamp
        else:
            dt = 1.0 / self.freq

        dx = (x - self.x_prev) / dt
        a_d = self._alpha(self.d_cutoff, dt)
        dx_hat = a_d * dx + (1.0 - a_d) * self.dx_prev
        cutoff = self.min_cutoff + self.beta * abs(dx_hat)
        a = self._alpha(max(cutoff, 1e-6), dt)
        x_hat = a * x + (1.0 - a) * self.x_prev

        self.x_prev = x_hat
        self.dx_prev = dx_hat
        return x_hat

# Filters and stabilization state
gaze_filter_x = OneEuroFilter(GAZE_FILTER_FREQ, GAZE_MIN_CUTOFF, GAZE_BETA, GAZE_D_CUTOFF)
gaze_filter_y = OneEuroFilter(GAZE_FILTER_FREQ, GAZE_MIN_CUTOFF, GAZE_BETA, GAZE_D_CUTOFF)

smoothed_mouse_pos   = [CENTER_X, CENTER_Y]
last_move_time       = time.perf_counter()
prev_step_len        = 0.0
current_smooth_tau   = SMOOTH_TAU_BASE
current_max_speed    = MAX_SPEED_BASE_PX_S
current_deadzone_px  = DEADZONE_PX
current_speed_gain   = 1.0

gaze_speed_ema       = 0.0
last_filtered_target = None
last_filter_timestamp = None
saccade_freeze_until = 0.0
fix_low_start_time   = None
fixation_anchor      = None
fixation_anchor_time = None
fix_anchor_hold_until = 0.0
stabilization_enabled = True
stabilization_warmup_until = 0.0
fixation_enabled = False
plane_mapping_enabled = True
plane_invert_x = False
plane_invert_y = False

# --- DPI per-axis scale (user adjustable) ---
dpi_scale_x = 1.6
dpi_scale_y = 2.0

plane_gain_x = 5.27
plane_gain_y = 7.51
PLANE_GAMMA_X = 1.25
PLANE_GAMMA_Y = 1.30

# --- Debug print for plane params (a,b) ---
ab_debug_enabled = False
ab_debug_last_print = 0.0

# --- Axis-dominance gating to reduce cross-axis drift at edges ---
ab_last_pair = None  # (a,b) from previous frame
AB_H_DOM_RATIO = 2.0  # horizontal dominates when |da| > R * |db|
AB_V_DOM_RATIO = 2.0  # vertical dominates when |db| > R * |da|
AB_MAX_B_STEP = 0.010  # max change of b per frame when horizontal dominates
AB_MAX_A_STEP = 0.030  # max change of a per frame when vertical dominates
ab_anchor_b = None
ab_anchor_on = False
ab_anchor_last = 0.0
AB_ANCHOR_EDGE_A = 0.85
AB_ANCHOR_TIMEOUT_S = 0.30
AB_ANCHOR_CENTER_DEV = 0.20
AB_H_MIN_DA = 0.015
AB_V_MIN_DB = 0.015
AB_H_DOT_RATIO = 1.25
AB_H_MIN_DOT = 0.10
ab_last_stable_b = None

# Mild temporal smoothing for (a,b) to giảm jitter nhưng giữ độ chính xác
ab_hist_a = deque(maxlen=5)
ab_hist_b = deque(maxlen=5)
ab_ema_a = None
ab_ema_b = None
AB_EMA_ALPHA_BASE = 0.28   # chuyển động chậm → mượt hơn
AB_EMA_ALPHA_FAST = 0.65   # chuyển động nhanh → bám sát hơn
AB_SPEED_THRESH   = 0.06   # ngưỡng tốc độ (đơn vị a/b per frame)

# === Online training (guided target) ===
training_enabled = False
training_window_name = "Gaze Trainer"
training_window_created = False
training_targets = []
training_index = 0
training_state = 0
training_state_until = 0.0
training_point_duration_s = 1.2
training_pause_s = 0.4
training_collect_eye = []
training_collect_head = []
training_collect_phi = []
saved_mouse_control = False
last_gpm_ab = None
last_raw_yaw_pitch = None
training_records = []
training_csv_path = os.path.join(os.path.dirname(__file__), "training_data.csv")

# === RLS models for adaptive DPI (speed & tau) ===
class RLSModel:
    def __init__(self, n_features, lam=0.995, delta=1000.0):
        self.lam = lam
        self.w = np.zeros((n_features, 1), dtype=float)
        self.P = (1.0 / delta) * np.eye(n_features, dtype=float)
        self.n = n_features

    def update(self, phi, y):
        # phi shape (n,), y scalar
        phi = np.asarray(phi, dtype=float).reshape((self.n, 1))
        y = float(y)
        # RLS update
        P = self.P
        w = self.w
        lam = self.lam
        Pi_phi = P @ phi
        gain_denom = lam + float(phi.T @ Pi_phi)
        K = Pi_phi / gain_denom
        err = y - float(phi.T @ w)
        w_new = w + K * err
        P_new = (P - (K @ phi.T @ P)) / lam
        self.w = w_new
        self.P = P_new

    def predict(self, phi):
        phi = np.asarray(phi, dtype=float).reshape((self.n, 1))
        return float(phi.T @ self.w)

def rls_save(path, model_speed, model_tau):
    try:
        data = {
            'speed': {'w': model_speed.w.flatten().tolist(), 'P': model_speed.P.tolist(), 'lam': model_speed.lam},
            'tau':   {'w': model_tau.w.flatten().tolist(),   'P': model_tau.P.tolist(),   'lam': model_tau.lam},
        }
        with open(path, 'w', encoding='utf-8') as f:
            json.dump(data, f)
    except Exception:
        pass

def rls_load(path, n_features):
    try:
        with open(path, 'r', encoding='utf-8') as f:
            data = json.load(f)
        ms = RLSModel(n_features, lam=float(data['speed'].get('lam', 0.995)))
        mt = RLSModel(n_features, lam=float(data['tau'].get('lam', 0.995)))
        ms.w = np.asarray(data['speed']['w'], dtype=float).reshape((n_features, 1))
        ms.P = np.asarray(data['speed']['P'], dtype=float)
        mt.w = np.asarray(data['tau']['w'], dtype=float).reshape((n_features, 1))
        mt.P = np.asarray(data['tau']['P'], dtype=float)
        return ms, mt
    except Exception:
        return RLSModel(n_features), RLSModel(n_features)

rls_feature_dim = 8
rls_model_path = os.path.join(os.path.dirname(__file__), 'dpi_model.json')
rls_speed_model, rls_tau_model = rls_load(rls_model_path, rls_feature_dim)
rls_training_enabled = True

def compute_context_features(fx=None, fy=None):
    # distance to target normalized
    try:
        sx, sy = smoothed_mouse_pos
    except Exception:
        sx, sy = CENTER_X, CENTER_Y
    if fx is None or fy is None:
        fx, fy = sx, sy
    dist = math.hypot(float(fx) - float(sx), float(fy) - float(sy))
    diag = max(1.0, math.hypot(MONITOR_WIDTH, MONITOR_HEIGHT))
    m_norm = float(np.clip(dist / (0.35 * diag), 0.0, 1.0))
    total = float(max(1e-6, head_motion_ema + eye_motion_ema))
    eye_ratio = float(eye_motion_ema / total)
    head_ratio = float(head_motion_ema / total)
    pm_flag = 1.0 if plane_mapping_enabled else 0.0
    cx, cy = CENTER_X, CENTER_Y
    dc = math.hypot(float(fx) - cx, float(fy) - cy) / (0.71 * diag)
    phi = np.array([
        1.0,
        m_norm,
        eye_ratio,
        head_ratio,
        float(head_motion_ema),
        float(eye_motion_ema),
        pm_flag,
        float(np.clip(dc, 0.0, 1.0)),
    ], dtype=float)
    return phi

# --- Dynamic motion/adaptation state ---
current_smooth_tau = SMOOTH_TAU_BASE
current_max_speed  = MAX_SPEED_BASE_PX_S
prev_head_R = None
prev_head_center = None
prev_gaze_dir = None
head_motion_ema = 0.0
eye_motion_ema = 0.0
last_motion_sample_time = None
saccade_boost_until = 0.0

# === Velocity-mode state ===
velocity_mode_enabled = VELOCITY_MODE_DEFAULT
baseline_px = None  # [x, y] in pixels, slow-updated
last_velocity_time = None
intent_ema_x = 0.0
intent_ema_y = 0.0
last_raw_screen_xy = None
intent_still_since = None

def update_cursor_target(screen_x, screen_y):
    global last_filtered_target, last_filter_timestamp, gaze_speed_ema
    global current_deadzone_px, current_speed_gain
    global saccade_freeze_until, fix_low_start_time, fixation_anchor
    global fixation_anchor_time, fix_anchor_hold_until

    raw = np.array([float(screen_x), float(screen_y)], dtype=float)
    now = time.perf_counter()

    if not stabilization_enabled:
        # Bỏ qua lọc: trả về raw trực tiếp (clamped)
        raw[0] = np.clip(raw[0], 0.0, MONITOR_WIDTH - 1)
        raw[1] = np.clip(raw[1], 0.0, MONITOR_HEIGHT - 1)
        last_filtered_target = raw.copy()
        last_filter_timestamp = now
        return raw

    # Warmup: bỏ saccade/fixation/deadzone trong ~0.7s sau calibrate để tránh kẹt
    if now < stabilization_warmup_until:
        filtered = np.array([
            gaze_filter_x.filter(raw[0], now),
            gaze_filter_y.filter(raw[1], now)
        ], dtype=float)
        filtered[0] = np.clip(filtered[0], 0.0, MONITOR_WIDTH - 1)
        filtered[1] = np.clip(filtered[1], 0.0, MONITOR_HEIGHT - 1)
        last_filtered_target = filtered
        last_filter_timestamp = now
        return filtered.copy()

    if last_filtered_target is None:
        filtered = np.array([
            gaze_filter_x.filter(raw[0], now),
            gaze_filter_y.filter(raw[1], now)
        ], dtype=float)
        filtered[0] = np.clip(filtered[0], 0.0, MONITOR_WIDTH - 1)
        filtered[1] = np.clip(filtered[1], 0.0, MONITOR_HEIGHT - 1)
        last_filtered_target = filtered
        last_filter_timestamp = now
        current_speed_gain = SPEED_GAIN_MIN
        current_deadzone_px = DEADZONE_PX + DEADZONE_EXTRA_STILL
        return filtered.copy()

    dt = max(1e-3, now - last_filter_timestamp)
    delta = raw - last_filtered_target
    delta_norm = np.array([
        delta[0] / max(1.0, MONITOR_WIDTH),
        delta[1] / max(1.0, MONITOR_HEIGHT)
    ])
    speed_norm = np.linalg.norm(delta_norm) / dt
    capped_speed = min(speed_norm, GAZE_SPEED_SAT)
    gaze_speed_ema = ((1.0 - GAZE_SPEED_ALPHA) * gaze_speed_ema + GAZE_SPEED_ALPHA * capped_speed)

    # If user moved far from fixation anchor, drop anchor (escape condition)
    if fixation_enabled and fixation_anchor is not None:
        try:
            dpi_factor_tmp = float(max(0.1, math.sqrt(max(0.1, dpi_scale_x) * max(0.1, dpi_scale_y))))
            plane_gain_mean = float((plane_gain_x + plane_gain_y) * 0.5)
            fix_r_dyn = float(FIXATION_RADIUS_BASE * (1.0 + FIX_RADIUS_GAIN_PLANE * (plane_gain_mean - 1.0)) * (1.0 + FIX_RADIUS_GAIN_DPI * (dpi_factor_tmp - 1.0)))
        except Exception:
            fix_r_dyn = float(FIXATION_RADIUS_BASE)
        if float(np.linalg.norm(raw - fixation_anchor)) > (fix_r_dyn * 1.15):
            fixation_anchor = None
            fix_low_start_time = None

    # I-VT saccade detection → freeze
    if speed_norm > IVT_SACCADE_NORM:
        saccade_freeze_until = now + SACCADE_FREEZE_SEC
        fix_low_start_time = None
        fixation_anchor = None

    if now < saccade_freeze_until and last_filtered_target is not None:
        raw = last_filtered_target.copy()
    else:
        # Mandatory hold while within hold window
        if fixation_enabled and fixation_anchor is not None and now < fix_anchor_hold_until:
            raw = fixation_anchor.copy()
        # Fixation dwell → stick to anchor (only when enabled)
        elif fixation_enabled and speed_norm < IVT_FIX_NORM:
            if fix_low_start_time is None:
                fix_low_start_time = now
                fixation_anchor = last_filtered_target.copy() if last_filtered_target is not None else None
                fixation_anchor_time = now
            elif (now - fix_low_start_time) >= IVT_FIX_DWELL_SEC and fixation_anchor is not None:
                dist_anchor = float(np.linalg.norm(raw - fixation_anchor))
                try:
                    dpi_factor_tmp = float(max(0.1, math.sqrt(max(0.1, dpi_scale_x) * max(0.1, dpi_scale_y))))
                    plane_gain_mean = float((plane_gain_x + plane_gain_y) * 0.5)
                    fix_r = float(FIXATION_RADIUS_BASE * (1.0 + FIX_RADIUS_GAIN_PLANE * (plane_gain_mean - 1.0)) * (1.0 + FIX_RADIUS_GAIN_DPI * (dpi_factor_tmp - 1.0)))
                except Exception:
                    fix_r = float(FIXATION_RADIUS_BASE)
                if dist_anchor <= fix_r:
                    raw = fixation_anchor.copy()
                    fix_anchor_hold_until = max(fix_anchor_hold_until, now + FIX_HOLD_MIN_S)
        else:
            fix_low_start_time = None
            fixation_anchor = None

    # Dynamic deadzone & speed gain from gaze speed
    gaze_ratio = float(np.clip(gaze_speed_ema / max(GAZE_SPEED_SAT, 1e-6), 0.0, 1.0))
    current_speed_gain = float(SPEED_GAIN_MIN + (SPEED_GAIN_MAX - SPEED_GAIN_MIN) * gaze_ratio)
    current_deadzone_px = float(DEADZONE_PX + DEADZONE_EXTRA_STILL * (1.0 - gaze_ratio))

    # Low-motion freeze: khi rất yên, giữ nguyên mục tiêu để cắt trôi chậm
    if gaze_ratio <= 0.08 and np.linalg.norm(delta) <= (current_deadzone_px * 0.8):
        raw = last_filtered_target.copy()
    if np.linalg.norm(delta) <= current_deadzone_px:
        # cho phép dịch chuyển nhỏ thay vì khoá cứng để tránh cảm giác "không di chuyển"
        soft_k = float(np.clip(0.15 + 0.65 * gaze_ratio, 0.10, 0.80))
        raw = last_filtered_target.copy() + delta * soft_k

    # Thích nghi tham số OneEuro theo tốc độ nhìn: chậm → cắt thấp (mượt), nhanh → cắt cao (nhạy)
    try:
        dyn_min_cut = float(np.clip(0.55 + 0.65 * gaze_ratio, 0.40, 1.40))
        dyn_beta    = float(np.clip(0.35 + 0.45 * gaze_ratio, 0.25, 0.80))
        gaze_filter_x.min_cutoff = dyn_min_cut
        gaze_filter_y.min_cutoff = dyn_min_cut
        gaze_filter_x.beta = dyn_beta
        gaze_filter_y.beta = dyn_beta
    except Exception:
        pass

    filtered = np.array([
        gaze_filter_x.filter(raw[0], now),
        gaze_filter_y.filter(raw[1], now)
    ], dtype=float)

    filtered[0] = np.clip(filtered[0], 0.0, MONITOR_WIDTH - 1)
    filtered[1] = np.clip(filtered[1], 0.0, MONITOR_HEIGHT - 1)

    last_filtered_target = filtered
    last_filter_timestamp = now
    return filtered.copy()
def apply_camera_orientation(frame):
    """Rotate incoming camera frame theo hướng người dùng chọn."""
    if camera_orientation_index == 1:
        return cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE)
    if camera_orientation_index == 2:
        return cv2.rotate(frame, cv2.ROTATE_180)
    if camera_orientation_index == 3:
        return cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE)
    return frame

def _focal_px(width, fov_deg):
    # horizontal pinhole focal length
    return 0.5 * width / math.tan(math.radians(fov_deg) * 0.5)


def create_monitor_plane(head_center, R_final, face_landmarks, w, h, 
                         forward_hint=None, gaze_origin=None, gaze_dir=None):
    """
    Build a 60cm x 40cm plane 50cm in front of the face, in world units.
    Monitor is oriented horizontally like a real monitor (top edge parallel to global X-axis).
    """
    # 1) Estimate scale from chin<->forehead distance
    try:
        lm_chin = face_landmarks[152]
        lm_fore = face_landmarks[10]
        chin_w = np.array([lm_chin.x * w,  lm_chin.y * h,  lm_chin.z * w], dtype=float)
        fore_w = np.array([lm_fore.x * w,  lm_fore.y * h,  lm_fore.z * w], dtype=float)
        face_h_units = np.linalg.norm(fore_w - chin_w)
        upc = face_h_units / 15.0  # units per cm
    except Exception:
        upc = 5.0
    
    # 2) Monitor geometry in world units
    dist_cm = 50.0

    mon_w_cm, mon_h_cm = 60.0, 40.0
    half_w = (mon_w_cm * 0.5) * upc
    half_h = (mon_h_cm * 0.5) * upc

    # Head forward vector
    head_forward = -R_final[:, 2]
    if forward_hint is not None:
        head_forward = forward_hint / np.linalg.norm(forward_hint)

    # --- NEW: use gaze ray intersection ---
    if gaze_origin is not None and gaze_dir is not None:
        gaze_dir = gaze_dir / np.linalg.norm(gaze_dir)

        # Place the monitor so its center is exactly at some point on the gaze ray
        # For simplicity: choose intersection at 50 cm from head_center along head_forward
        plane_point = head_center + head_forward * (50.0 * upc)
        plane_normal = head_forward

        denom = np.dot(plane_normal, gaze_dir)
        if abs(denom) > 1e-6:
            t = np.dot(plane_normal, plane_point - gaze_origin) / denom
            center_w = gaze_origin + t * gaze_dir
        else:
            # fallback: use fixed distance
            center_w = head_center + head_forward * (50.0 * upc)
    else:
        # fallback: original placement
        center_w = head_center + head_forward * (50.0 * upc)

    # Compute right/up using head orientation
    world_up = np.array([0, -1, 0], dtype=float)
    head_right = np.cross(world_up, head_forward)
    head_right /= np.linalg.norm(head_right)
    head_up = np.cross(head_forward, head_right)
    head_up /= np.linalg.norm(head_up)

    # Corners
    p0 = center_w - head_right * half_w - head_up * half_h
    p1 = center_w + head_right * half_w - head_up * half_h
    p2 = center_w + head_right * half_w + head_up * half_h
    p3 = center_w - head_right * half_w + head_up * half_h

    normal_w = head_forward / (np.linalg.norm(head_forward) + 1e-9)
    return [p0, p1, p2, p3], center_w, normal_w, upc




def update_orbit_from_keys():
    """Keyboard orbit controls that PRINT every frame while a key is held."""
    global orbit_yaw, orbit_pitch, orbit_radius
    yaw_step   = math.radians(1.5)
    pitch_step = math.radians(1.5)
    zoom_step  = 12.0

    changed = False

    # Rotate
    if keyboard.is_pressed('j'):  # yaw left
        orbit_yaw -= yaw_step; changed = True
    if keyboard.is_pressed('l'):  # yaw right
        orbit_yaw += yaw_step; changed = True
    if keyboard.is_pressed('i'):  # pitch up
        orbit_pitch += pitch_step; changed = True
    if keyboard.is_pressed('k'):  # pitch down
        orbit_pitch -= pitch_step; changed = True

    # Zoom
    if keyboard.is_pressed('['):  # zoom out
        orbit_radius += zoom_step; changed = True
    if keyboard.is_pressed(']'):  # zoom in
        orbit_radius = max(80.0, orbit_radius - zoom_step); changed = True

    # Reset (prints every frame while held)
    if keyboard.is_pressed('r'):
        orbit_yaw = 0.0
        orbit_pitch = 0.0
        orbit_radius = 600.0
        changed = True

    # Clamp pitch & radius
    orbit_pitch = max(math.radians(-89), min(math.radians(89), orbit_pitch))
    orbit_radius = max(80.0, orbit_radius)

    if changed:
        print(f"[Orbit Debug] yaw={math.degrees(orbit_yaw):.2f}°, "
              f"pitch={math.degrees(orbit_pitch):.2f}°, "
              f"radius={orbit_radius:.2f}, "
              f"fov={orbit_fov_deg:.1f}°")




def compute_scale(points_3d):
    # Use average pairwise distance for robustness
    n = len(points_3d)
    total = 0
    count = 0
    for i in range(n):
        for j in range(i + 1, n):
            dist = np.linalg.norm(points_3d[i] - points_3d[j])
            total += dist
            count += 1
    return total / count if count > 0 else 1.0

def draw_gaze(frame, eye_center, iris_center, eye_radius, color, gaze_length):
    # Gaze vector
    gaze_direction = iris_center - eye_center
    gaze_direction /= np.linalg.norm(gaze_direction)
    gaze_endpoint = eye_center + gaze_direction * gaze_length

    cv2.line(frame, tuple(int(v) for v in eye_center[:2]), tuple(int(v) for v in gaze_endpoint[:2]), color, 2)

    # Segment points
    iris_offset = eye_center + gaze_direction * (1.2 * eye_radius)

    # ---- PART 1: back segment (behind iris) ----
    cv2.line(
        frame,
        (int(eye_center[0]), int(eye_center[1])),
        (int(iris_offset[0]), int(iris_offset[1])),
        color,
        1
    )

    # ---- IRIS (occludes part of the ray) ----
    up_dir = np.array([0, -1, 0])
    right_dir = np.cross(gaze_direction, up_dir)
    if np.linalg.norm(right_dir) < 1e-6:
        right_dir = np.array([1, 0, 0])
    up_dir = np.cross(right_dir, gaze_direction)
    up_dir /= np.linalg.norm(up_dir)
    right_dir /= np.linalg.norm(right_dir)
    ellipse_axes = (
        int((eye_radius / 3) * np.linalg.norm(right_dir[:2])),
        int((eye_radius / 3) * np.linalg.norm(up_dir[:2]))
    )
    angle = math.degrees(math.atan2(gaze_direction[1], gaze_direction[0]))

    # ---- PART 2: front segment (on top of iris) ----
    cv2.line(
        frame,
        (int(iris_offset[0]), int(iris_offset[1])),
        (int(gaze_endpoint[0]), int(gaze_endpoint[1])),
        color,
        1
    )

def draw_wireframe_cube(frame, center, R, size=80):
    # Given a center and rotation matrix, draw a cube aligned to that orientation
    right = R[:, 0]
    up = -R[:, 1]
    forward = -R[:, 2]

    hw, hh, hd = size * 1, size * 1, size * 1

    def corner(x_sign, y_sign, z_sign):
        return (center +
                x_sign * hw * right +
                y_sign * hh * up +
                z_sign * hd * forward)

    # 8 corners of the cube
    corners = [corner(x, y, z) for x in [-1, 1] for y in [1, -1] for z in [-1, 1]]
    projected = [(int(pt[0]), int(pt[1])) for pt in corners]

    # Edges connecting the corners
    edges = [
        (0, 1), (1, 3), (3, 2), (2, 0),
        (4, 5), (5, 7), (7, 6), (6, 4),
        (0, 4), (1, 5), (2, 6), (3, 7)
    ]
    for i, j in edges:
        cv2.line(frame, projected[i], projected[j], (255, 128, 0), 2)

def compute_and_draw_coordinate_box(frame, face_landmarks, indices, ref_matrix_container, color=(0, 255, 0), size=80):
    # Extract 3D positions of selected landmarks
    points_3d = np.array([
        [face_landmarks[i].x * w, face_landmarks[i].y * h, face_landmarks[i].z * w]
        for i in indices
    ])

    # Compute the average position as the center of this substructure
    center = np.mean(points_3d, axis=0)

    # Draw the raw 2D landmark points
    for i in indices:
        x, y = int(face_landmarks[i].x * w), int(face_landmarks[i].y * h)
        cv2.circle(frame, (x, y), 3, color, -1)

    # PCA-based orientation: Compute eigenvectors of the covariance matrix
    centered = points_3d - center
    cov = np.cov(centered.T)
    eigvals, eigvecs = np.linalg.eigh(cov)
    eigvecs = eigvecs[:, np.argsort(-eigvals)]  # Sort by descending eigenvalue (major axes)

    # Ensure the orientation matrix is right-handed
    if np.linalg.det(eigvecs) < 0:
        eigvecs[:, 2] *= -1

    # Convert to Euler angles and re-construct rotation matrix (optional but clarifies the transform)
    r = Rscipy.from_matrix(eigvecs)
    roll, pitch, yaw = r.as_euler('zyx', degrees=False)
    yaw *= 1
    roll *= 1
    R_final = Rscipy.from_euler('zyx', [roll, pitch, yaw]).as_matrix()

    # === Stabilize rotation with reference matrix to avoid flipping during eigenvector sign change ===
    if ref_matrix_container[0] is None:
        ref_matrix_container[0] = R_final.copy()
    else:
        R_ref = ref_matrix_container[0]
        for i in range(3):
            if np.dot(R_final[:, i], R_ref[:, i]) < 0:
                R_final[:, i] *= -1

    # Draw cube and orientation axes on the image
    draw_wireframe_cube(frame, center, R_final, size)

    # Draw X (green), Y (blue), Z (red) axes
    axis_length = size * 1.2
    axis_dirs = [R_final[:, 0], -R_final[:, 1], -R_final[:, 2]]
    axis_colors = [(0, 255, 0), (0, 0, 255), (255, 0, 0)]

    for i in range(3):
        end_pt = center + axis_dirs[i] * axis_length
        cv2.line(frame, (int(center[0]), int(center[1])), (int(end_pt[0]), int(end_pt[1])), axis_colors[i], 2)

    return center, R_final, points_3d

def convert_gaze_to_screen_coordinates(combined_gaze_direction, calibration_offset_yaw, calibration_offset_pitch):
    """
    Convert 3D gaze direction vector to 2D screen coordinates
    This function is adapted from the old script's vector-to-screen mapping logic
    """
    # Reference forward direction (camera looking straight ahead)
    reference_forward = np.array([0, 0, -1])  # Z-axis into the screen

    # Normalize the gaze direction
    avg_direction = combined_gaze_direction / np.linalg.norm(combined_gaze_direction)

    # Horizontal (yaw) angle from reference (project onto XZ plane)
    xz_proj = np.array([avg_direction[0], 0, avg_direction[2]])
    xz_proj /= np.linalg.norm(xz_proj)
    yaw_rad = math.acos(np.clip(np.dot(reference_forward, xz_proj), -1.0, 1.0))
    if avg_direction[0] < 0:
        yaw_rad = -yaw_rad  # left is negative

    # Vertical (pitch) angle from reference (project onto YZ plane)
    yz_proj = np.array([0, avg_direction[1], avg_direction[2]])
    yz_proj /= np.linalg.norm(yz_proj)
    pitch_rad = math.acos(np.clip(np.dot(reference_forward, yz_proj), -1.0, 1.0))
    # down (y>0) -> positive pitch, up (y<0) -> negative pitch
    if avg_direction[1] > 0:
        pitch_rad = +pitch_rad
    else:
        pitch_rad = -pitch_rad

    # Convert to degrees and re-center around 0
    yaw_deg = np.degrees(yaw_rad)
    pitch_deg = np.degrees(pitch_rad)


    #yaw is now converted to -90 (looking directly left) to +90 (looking directly right), wrt camera
    #pitch is now converted to +90 (looking straight up) and -90 (looking straight down), wrt camera
    
    raw_yaw_deg = yaw_deg
    raw_pitch_deg = pitch_deg

    
    # Specify degrees at which screen border will be reached
    yawDegrees = 15.0  # x degrees left or right
    pitchDegrees = 10.0  # x degrees up or down

    # Apply calibration offsets
    yaw_deg += calibration_offset_yaw
    pitch_deg += calibration_offset_pitch

    # Map to full screen resolution
    screen_x = int(((yaw_deg + yawDegrees) / (2 * yawDegrees)) * MONITOR_WIDTH)
    # pitch_deg > 0 khi nhìn xuống => y phải tăng
    screen_y = int(((pitch_deg + pitchDegrees) / (2 * pitchDegrees)) * MONITOR_HEIGHT)

    # Clamp screen position to monitor bounds
    screen_x = max(10, min(screen_x, MONITOR_WIDTH - 10))
    screen_y = max(10, min(screen_y, MONITOR_HEIGHT - 10))

    return screen_x, screen_y, raw_yaw_deg, raw_pitch_deg

def gaze_point_on_monitor(origin, direction):
    """Project gaze ray (origin, direction) to calibrated monitor plane and map to screen pixels.
    Returns (x, y, a, b) or None if invalid/outside.
    Ensure local axes are ordered TL->TR (u) and TL->BL (v) so b grows downward.
    """
    if (monitor_corners is None or monitor_center_w is None or monitor_normal_w is None):
        return None

    O = np.asarray(origin, dtype=float)
    D = _normalize(np.asarray(direction, dtype=float))
    C = np.asarray(monitor_center_w, dtype=float)
    N = _normalize(np.asarray(monitor_normal_w, dtype=float))

    denom = float(np.dot(N, D))
    if abs(denom) < 1e-6:
        return None
    t = float(np.dot(N, (C - O)) / denom)
    if t <= 0.0:
        return None

    P = O + t * D

    p0, p1, p2, p3 = [np.asarray(p, dtype=float) for p in monitor_corners]
    # Local axes from construction
    u = p1 - p0  # width vector
    v = p3 - p0  # height vector
    uu = float(np.dot(u, u))
    vv = float(np.dot(v, v))
    uv = float(np.dot(u, v))
    if uu < 1e-9 or vv < 1e-9:
        return None

    # Solve exact (a,b) in oblique basis via Gram matrix inverse
    wv = P - p0
    uw = float(np.dot(u, wv))
    vw = float(np.dot(v, wv))
    det = uu * vv - uv * uv
    if abs(det) < 1e-9:
        return None
    a = float((vv * uw - uv * vw) / det)
    b = float((uu * vw - uv * uw) / det)

    # Decouple axes at edges: clamp a first, then recompute b from residual
    a_c = float(np.clip(a, 0.0, 1.0))
    wv_res = wv - a_c * u
    b_c = float(np.clip(np.dot(v, wv_res) / max(vv, 1e-9), 0.0, 1.0))

    # Axis-dominance gating across frames to avoid vertical push during horizontal scans
    global ab_last_pair, ab_anchor_on, ab_anchor_b, ab_anchor_last, ab_last_stable_b, ab_hist_a, ab_hist_b, ab_ema_a, ab_ema_b
    try:
        if ab_last_pair is not None:
            prev_a, prev_b = ab_last_pair
            da = float(a_c - prev_a)
            db = float(b_c - prev_b)
            # Hard guard: nếu đang quét ngang (da đáng kể) mà b rớt vào biên, giữ b của frame trước
            if (0.05 <= prev_b <= 0.95) and (b_c <= 0.005 or b_c >= 0.995) and (abs(da) >= 0.02):
                b_c = prev_b
            # Dot-based axis dominance from gaze ray vs plane axes
            u_len = math.sqrt(max(uu, 1e-9))
            v_len = math.sqrt(max(vv, 1e-9))
            u_hat = u / u_len
            v_hat = v / v_len
            h_dot = float(abs(np.dot(u_hat, D)))
            v_dot = float(abs(np.dot(v_hat, D)))
            dot_horiz = (h_dot > AB_H_MIN_DOT) and (h_dot > AB_H_DOT_RATIO * (v_dot + 1e-6))
            # Ratio-based dominance from parameter deltas
            dom_h_ratio = (abs(da) > AB_H_MIN_DA) and (abs(da) > AB_H_DOM_RATIO * (abs(db) + 1e-6))
            dom_v_ratio = (abs(db) > AB_V_MIN_DB) and (abs(db) > AB_V_DOM_RATIO * (abs(da) + 1e-6))
            dom_h = bool(dot_horiz or dom_h_ratio)
            dom_v = bool((not dom_h) and dom_v_ratio)
            if dom_h:
                # Horizontal dominates: limit b change per frame
                b_c = float(np.clip(prev_b + np.clip(db, -AB_MAX_B_STEP, AB_MAX_B_STEP), 0.0, 1.0))
            elif dom_v:
                # Vertical dominates: limit a change per frame
                a_c = float(np.clip(prev_a + np.clip(da, -AB_MAX_A_STEP, AB_MAX_A_STEP), 0.0, 1.0))
            # Boundary lock: nếu a chạm biên trái/phải, giữ nguyên b để tránh kéo dọc khi quét ngang
            if a_c <= 0.01 or a_c >= 0.99:
                b_c = prev_b
            # Ngược lại: nếu b chạm biên trên/dưới, giữ nguyên a khi quét dọc
            if b_c <= 0.01 or b_c >= 0.99:
                a_c = prev_a

            # Anchor b trong lúc quét ngang mạnh hoặc khi ở gần mép trái/phải
            now_t = time.perf_counter()
            near_edge = (a_c >= AB_ANCHOR_EDGE_A) or (a_c <= (1.0 - AB_ANCHOR_EDGE_A))
            mid_outer = (abs(a_c - 0.5) >= AB_ANCHOR_CENTER_DEV)
            if (dom_h or near_edge or mid_outer) and not dom_v:
                if not ab_anchor_on or ab_anchor_b is None:
                    ab_anchor_on = True
                    ab_anchor_b = prev_b
                b_c = ab_anchor_b
                ab_anchor_last = now_t
            else:
                # Thoát anchor nếu hết quét ngang hoặc timeout
                if ab_anchor_on and (now_t - (ab_anchor_last or now_t)) > AB_ANCHOR_TIMEOUT_S:
                    ab_anchor_on = False
                    ab_anchor_b = None

            # Nếu quét ngang mạnh và b chạm biên, giữ b theo khung trước hoặc theo giá trị ổn định gần nhất
            if dom_h and (b_c <= 0.005 or b_c >= 0.995):
                if ab_last_stable_b is not None:
                    b_c = ab_last_stable_b
                else:
                    b_c = prev_b

            # Cập nhật b ổn định khi ở vùng trong (tránh lưu giá trị sát biên)
            if 0.05 <= b_c <= 0.95:
                ab_last_stable_b = b_c
    except Exception:
        pass

    # Median-3 + EMA smoothing for (a,b) to giảm jitter high-frequency
    try:
        # median of last 3 corrected samples
        ab_hist_a.append(float(np.clip(a_c, 0.0, 1.0)))
        ab_hist_b.append(float(np.clip(b_c, 0.0, 1.0)))
        if len(ab_hist_a) >= 3:
            a_med = float(np.median(list(ab_hist_a)[-3:]))
            b_med = float(np.median(list(ab_hist_b)[-3:]))
        else:
            a_med, b_med = a_c, b_c

        # speed-adaptive EMA
        prev_a, prev_b = ab_last_pair if ab_last_pair is not None else (a_med, b_med)
        speed_ab = float(math.hypot(a_med - prev_a, b_med - prev_b))
        alpha = AB_EMA_ALPHA_FAST if speed_ab > AB_SPEED_THRESH else AB_EMA_ALPHA_BASE
        if ab_ema_a is None or ab_ema_b is None:
            ab_ema_a, ab_ema_b = a_med, b_med
        else:
            ab_ema_a = float((1.0 - alpha) * ab_ema_a + alpha * a_med)
            ab_ema_b = float((1.0 - alpha) * ab_ema_b + alpha * b_med)
        a_c = float(np.clip(ab_ema_a, 0.0, 1.0))
        b_c = float(np.clip(ab_ema_b, 0.0, 1.0))
    except Exception:
        pass

    ab_last_pair = (a_c, b_c)

    # Stable base mapping (no auto-orientation here): x = a, y = 1 - b
    # Any axis inversion is applied globally after mapping (F10/F11)
    x_px = float(np.clip(a_c, 0.0, 1.0))
    y_px = float(np.clip(1.0 - b_c, 0.0, 1.0))
    x_px = float(np.clip(0.5 + plane_gain_x * (x_px - 0.5), 0.0, 1.0))
    y_px = float(np.clip(0.5 + plane_gain_y * (y_px - 0.5), 0.0, 1.0))
    # Symmetric gamma to giảm nhạy gần tâm nhưng vẫn chạm mép dễ với Plane Gain
    try:
        def _sym_gamma(z, g):
            if g <= 1e-6:
                return z
            if z <= 0.5:
                return 0.5 * ((2.0 * z) ** g)
            else:
                return 1.0 - 0.5 * ((2.0 * (1.0 - z)) ** g)
        x_px = float(np.clip(_sym_gamma(x_px, PLANE_GAMMA_X), 0.0, 1.0))
        y_px = float(np.clip(_sym_gamma(y_px, PLANE_GAMMA_Y), 0.0, 1.0))
    except Exception:
        pass
    x = int(x_px * (MONITOR_WIDTH - 1))
    y = int(y_px * (MONITOR_HEIGHT - 1))
    return x, y, a_c, b_c

def adaptive_dpi_update(fx, fy):
    """Điều chỉnh tốc độ và độ mượt chuột theo ngữ cảnh (magnitude, eye/head motion).
    - Tăng tốc khi khoảng cách tới đích lớn, khi mắt/đầu di chuyển nhanh.
    - Giảm tau khi cần phản hồi nhanh; tăng nhẹ khi đầu di chuyển để mượt hơn.
    """
    global current_smooth_tau, current_max_speed
    try:
        sx, sy = smoothed_mouse_pos
        dist = math.hypot(float(fx) - float(sx), float(fy) - float(sy))
    except Exception:
        dist = 0.0
    diag = math.hypot(MONITOR_WIDTH, MONITOR_HEIGHT)
    m_norm = float(np.clip(dist / max(1.0, 0.35 * diag), 0.0, 1.0))

    total = float(max(1e-6, head_motion_ema + eye_motion_ema))
    eye_ratio = float(eye_motion_ema / total)
    head_ratio = float(head_motion_ema / total)

    # Base từ logic thích ứng sẵn có
    base_tau = SMOOTH_TAU_MIN * eye_ratio + SMOOTH_TAU_BASE * (1.0 - eye_ratio) + 0.04 * head_ratio
    base_speed = MAX_SPEED_BASE_PX_S * (0.9 + 2.0 * eye_ratio)

    # Điều chỉnh theo khoảng cách và tỉ lệ head/eye
    speed_mul = 1.0 + 1.2 * m_norm + 0.35 * head_ratio
    tau_adj = (-0.05 * m_norm) + (0.02 * head_ratio)

    phi = compute_context_features(fx, fy)
    if rls_training_enabled and phi is not None:
        try:
            pred_s = float(np.clip(rls_speed_model.predict(phi), -1.5, 1.5))
            pred_t = float(np.clip(rls_tau_model.predict(phi),   -0.06, 0.06))
            speed_mul *= float(np.clip(math.exp(pred_s), 0.4, 4.0))
            tau_adj += pred_t
        except Exception:
            pass

    current_max_speed = float(np.clip(base_speed * speed_mul, 400.0, MAX_SPEED_BASE_PX_S * 3.5))
    current_smooth_tau = float(np.clip(base_tau + tau_adj, SMOOTH_TAU_MIN * 0.5, SMOOTH_TAU_MAX))

def training_init_targets():
    xs = [int(MONITOR_WIDTH * r) for r in (0.15, 0.5, 0.85)]
    ys = [int(MONITOR_HEIGHT * r) for r in (0.15, 0.5, 0.85)]
    targets = []
    for yy in ys:
        for xx in xs:
            targets.append((xx, yy))
    return targets

def ensure_training_window():
    global training_window_created
    if not training_window_created:
        cv2.namedWindow(training_window_name, cv2.WINDOW_NORMAL)
        cv2.setWindowProperty(training_window_name, cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
        training_window_created = True

def training_show_dot(px, py):
    img = np.zeros((MONITOR_HEIGHT, MONITOR_WIDTH, 3), dtype=np.uint8)
    cv2.circle(img, (int(px), int(py)), 12, (0, 255, 255), -1)
    cv2.circle(img, (int(px), int(py)), 22, (0, 140, 255), 2)
    cv2.imshow(training_window_name, img)

def write_training_csv(path, records):
    try:
        with open(path, 'w', newline='', encoding='utf-8') as f:
            wtr = csv.writer(f)
            wtr.writerow(["index", "target_x", "target_y", "yaw", "pitch", "a", "b", "samples_eye", "samples_head"]) 
            for r in records:
                wtr.writerow([r.get('idx'), r.get('tx'), r.get('ty'), r.get('yaw'), r.get('pitch'), r.get('a'), r.get('b'), r.get('n_eye'), r.get('n_head')])
    except Exception:
        pass

def training_begin():
    global training_targets, training_index, training_state, training_state_until
    global training_collect_eye, training_collect_head, training_records
    global saved_mouse_control
    training_targets = training_init_targets()
    training_index = 0
    training_state = 1
    training_state_until = time.perf_counter() + training_point_duration_s
    training_collect_eye = []
    training_collect_head = []
    training_collect_phi = []
    training_records = []
    saved_mouse_control = bool(mouse_control_enabled)
    if saved_mouse_control:
        # disable during training
        globals()['mouse_control_enabled'] = False
    ensure_training_window()
    tx, ty = training_targets[training_index]
    training_show_dot(tx, ty)
    print("[Training] Started. Look at the dot.")

def training_end(save=True):
    global training_enabled, training_window_created
    global training_state, training_targets
    try:
        if save and training_records:
            write_training_csv(training_csv_path, training_records)
            print(f"[Training] Saved: {training_csv_path}")
    except Exception:
        pass
    if training_window_created:
        try:
            cv2.destroyWindow(training_window_name)
        except Exception:
            pass
        training_window_created = False
    if saved_mouse_control:
        globals()['mouse_control_enabled'] = True
    training_enabled = False
    training_state = 0
    training_targets = []
    print("[Training] Finished.")
    try:
        rls_save(rls_model_path, rls_speed_model, rls_tau_model)
        print(f"[RLS] Model saved: {rls_model_path}")
    except Exception:
        pass

def training_step(now):
    global training_state, training_state_until, training_index
    global training_collect_eye, training_collect_head, training_records
    if training_state == 0:
        return
    if training_state == 1:
        if last_raw_yaw_pitch is not None:
            training_collect_eye.append(last_raw_yaw_pitch)
        if last_gpm_ab is not None:
            training_collect_head.append(last_gpm_ab)
        # collect context features per frame
        try:
            if last_filtered_target is not None and isinstance(last_filtered_target, np.ndarray):
                fx, fy = float(last_filtered_target[0]), float(last_filtered_target[1])
            else:
                fx = fy = None
        except Exception:
            fx = fy = None
        try:
            phi = compute_context_features(fx, fy)
            training_collect_phi.append(phi)
        except Exception:
            pass
        tx, ty = training_targets[training_index]
        training_show_dot(tx, ty)
        if now >= training_state_until:
            yaw = pitch = a = b = None
            if training_collect_eye:
                ys = np.array(training_collect_eye, dtype=float)
                yaw = float(np.mean(ys[:, 0]))
                pitch = float(np.mean(ys[:, 1]))
            if training_collect_head:
                hs = np.array(training_collect_head, dtype=float)
                a = float(np.mean(hs[:, 0]))
                b = float(np.mean(hs[:, 1]))
            # Average features to one sample per target
            phi_avg = None
            try:
                if training_collect_phi:
                    phi_avg = np.mean(np.stack(training_collect_phi, axis=0), axis=0)
            except Exception:
                phi_avg = None

            # Heuristic labels for speed (log-mult) and tau adjustment
            if phi_avg is not None:
                try:
                    m_norm = float(phi_avg[1])
                    eye_ratio = float(phi_avg[2])
                    head_ratio = float(phi_avg[3])
                    y_s = math.log(max(0.4, min(4.0, 1.0 + 1.2 * m_norm + 0.35 * head_ratio)))
                    y_t = float(np.clip((-0.05 * m_norm) + (0.02 * head_ratio), -0.06, 0.06))
                    rls_speed_model.update(phi_avg, float(np.clip(y_s, -1.5, 1.5)))
                    rls_tau_model.update(phi_avg, float(np.clip(y_t, -0.06, 0.06)))
                except Exception:
                    pass

            rec = {
                'idx': training_index,
                'tx': int(tx),
                'ty': int(ty),
                'yaw': yaw,
                'pitch': pitch,
                'a': a,
                'b': b,
                'n_eye': len(training_collect_eye),
                'n_head': len(training_collect_head),
            }
            training_records.append(rec)
            training_collect_eye = []
            training_collect_head = []
            training_collect_phi = []
            training_state = 2
            training_state_until = now + training_pause_s
    elif training_state == 2:
        img = np.zeros((MONITOR_HEIGHT, MONITOR_WIDTH, 3), dtype=np.uint8)
        cv2.imshow(training_window_name, img)
        if now >= training_state_until:
            training_index += 1
            if training_index < len(training_targets):
                training_state = 1
                training_state_until = now + training_point_duration_s
                tx, ty = training_targets[training_index]
                training_show_dot(tx, ty)
            else:
                training_state = 3
    elif training_state == 3:
        training_end(save=True)

def render_debug_view_orbit(
    h, w,
    head_center3d=None,
    sphere_world_l=None, scaled_radius_l=None,
    sphere_world_r=None, scaled_radius_r=None,
    iris3d_l=None, iris3d_r=None,
    left_locked=False, right_locked=False,
    landmarks3d=None,
    combined_dir=None,
    gaze_len=430,
    monitor_corners=None,
    monitor_center=None,
    monitor_normal=None,
    gaze_markers=None,
):
    if head_center3d is None:
        return

    debug = np.zeros((h, w, 3), dtype=np.uint8)

    # --- Choose orbit pivot ---
    head_w = np.asarray(head_center3d, dtype=float)

    # NEW: if we've frozen the world, orbit around the frozen pivot (monitor center at calib)
    global debug_world_frozen, orbit_pivot_frozen
    if debug_world_frozen and orbit_pivot_frozen is not None:
        pivot_w = np.asarray(orbit_pivot_frozen, dtype=float)
    else:
        if monitor_center is not None:
            pivot_w = (head_w + np.asarray(monitor_center, dtype=float)) * 0.5
        else:
            pivot_w = head_w

    # --- Camera pose (orbit around pivot_w) ---
    f_px = _focal_px(w, orbit_fov_deg)
    cam_offset = _rot_y(orbit_yaw) @ (_rot_x(orbit_pitch) @ np.array([0.0, 0.0, orbit_radius]))
    cam_pos = pivot_w + cam_offset

    up_world = np.array([0.0, -1.0, 0.0])   # image-space up is -Y
    fwd = _normalize(pivot_w - cam_pos)     # look at pivot
    right = _normalize(np.cross(fwd, up_world))
    up = _normalize(np.cross(right, fwd))
    V = np.stack([right, up, fwd], axis=0)

    def project_point(P):
        Pw = np.asarray(P, dtype=float)
        Pc = V @ (Pw - cam_pos)
        if Pc[2] <= 1e-3:
            return None
        x = f_px * (Pc[0] / Pc[2]) + w * 0.5
        y = -f_px * (Pc[1] / Pc[2]) + h * 0.5
        if not (np.isfinite(x) and np.isfinite(y)):
            return None
        return (int(x), int(y)), Pc[2]

    # --- helper draws ---
    def draw_poly_3d(pts, color=(0, 200, 255), thickness=2):
        projs = [project_point(p) for p in pts]
        if any(p is None for p in projs): return
        p2 = [p[0] for p in projs]
        for a, b in zip(p2, p2[1:] + [p2[0]]):
            cv2.line(debug, a, b, color, thickness)

    def draw_cross_3d(P, size=12, color=(255, 0, 255), thickness=2):
        res = project_point(P)
        if res is None: return
        (x, y), _ = res
        cv2.line(debug, (x - size, y), (x + size, y), color, thickness)
        cv2.line(debug, (x, y - size), (x, y + size), color, thickness)

    def draw_arrow_3d(P0, P1, color=(0, 200, 255), thickness=3):
        a = project_point(P0); b = project_point(P1)
        if a is None or b is None: return
        p0, p1 = a[0], b[0]
        cv2.line(debug, p0, p1, color, thickness)
        v = np.array([p1[0]-p0[0], p1[1]-p0[1]], dtype=float)
        n = np.linalg.norm(v)
        if n > 1e-3:
            v /= n
            l = np.array([-v[1], v[0]])
            ah = 10
            a1 = (int(p1[0] - v[0]*ah + l[0]*ah*0.6), int(p1[1] - v[1]*ah + l[1]*ah*0.6))
            a2 = (int(p1[0] - v[0]*ah - l[0]*ah*0.6), int(p1[1] - v[1]*ah - l[1]*ah*0.6))
            cv2.line(debug, p1, a1, color, thickness)
            cv2.line(debug, p1, a2, color, thickness)

    # --- Landmarks ---
    if landmarks3d is not None:
        for P in landmarks3d:
            res = project_point(P)
            if res is not None:
                cv2.circle(debug, res[0], 0, (200, 200, 200), -1)

    # --- Head center ---
    draw_cross_3d(head_w, size=12, color=(255, 0, 255), thickness=2)
    hc2d = project_point(head_w)
    if hc2d is not None:
        cv2.putText(debug, "Head Center", (hc2d[0][0] + 12, hc2d[0][1] - 12),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 255), 1, cv2.LINE_AA)

    # --- Pivot visual (small cross + line to head and monitor) ---
    draw_cross_3d(pivot_w, size=8, color=(180, 120, 255), thickness=2)
    if monitor_center is not None:
        mc2d = project_point(monitor_center)
        pv2d = project_point(pivot_w)
        if mc2d is not None and pv2d is not None and hc2d is not None:
            cv2.line(debug, pv2d[0], hc2d[0], (160, 100, 255), 1)
            cv2.line(debug, pv2d[0], mc2d[0], (160, 100, 255), 1)

    # --- Eyes + per-eye gaze (unchanged from your version) ---
    left_dir = None
    right_dir = None

    if left_locked and sphere_world_l is not None:
        res = project_point(sphere_world_l)
        if res is not None:
            (cx, cy), z = res
            r_px = max(2, int((scaled_radius_l if scaled_radius_l else 6) * f_px / max(z, 1e-3)))
            cv2.circle(debug, (cx, cy), r_px, (255, 255, 25), 1)
            if iris3d_l is not None:
                left_dir = np.asarray(iris3d_l) - np.asarray(sphere_world_l)
                p1 = project_point(np.asarray(sphere_world_l) + _normalize(left_dir) * gaze_len)
                if p1 is not None:
                    cv2.line(debug, (cx, cy), p1[0], (155, 155, 25), 1)
    elif iris3d_l is not None:
        res = project_point(iris3d_l)
        if res is not None:
            cv2.circle(debug, res[0], 2, (255, 255, 25), 1)

    if right_locked and sphere_world_r is not None:
        res = project_point(sphere_world_r)
        if res is not None:
            (cx, cy), z = res
            r_px = max(2, int((scaled_radius_r if scaled_radius_r else 6) * f_px / max(z, 1e-3)))
            cv2.circle(debug, (cx, cy), r_px, (25, 255, 255), 1)
            if iris3d_r is not None:
                right_dir = np.asarray(iris3d_r) - np.asarray(sphere_world_r)
                p1 = project_point(np.asarray(sphere_world_r) + _normalize(right_dir) * gaze_len)
                if p1 is not None:
                    cv2.line(debug, (cx, cy), p1[0], (25, 155, 155), 1)
    elif iris3d_r is not None:
        res = project_point(iris3d_r)
        if res is not None:
            cv2.circle(debug, res[0], 2, (25, 255, 255), 1)

    if left_locked and right_locked and sphere_world_l is not None and sphere_world_r is not None:
        origin_mid = (np.asarray(sphere_world_l) + np.asarray(sphere_world_r)) / 2.0
        if combined_dir is None and (left_dir is not None or right_dir is not None):
            parts = []
            if left_dir is not None:  parts.append(_normalize(left_dir))
            if right_dir is not None: parts.append(_normalize(right_dir))
            if parts:
                combined_dir = _normalize(np.mean(parts, axis=0))
        if combined_dir is not None:
            p0 = project_point(origin_mid)
            p1 = project_point(origin_mid + _normalize(combined_dir) * (gaze_len * 1.2))
            if p0 is not None and p1 is not None:
                cv2.line(debug, p0[0], p1[0], (155, 200, 10), 2)

    # --- Monitor plane ---
    if monitor_corners is not None:
        def draw_poly(points, color, thickness):
            projs = [project_point(p) for p in points]
            if any(p is None for p in projs): return
            p2 = [p[0] for p in projs]
            for a, b in zip(p2, p2[1:] + [p2[0]]):
                cv2.line(debug, a, b, color, thickness)
        draw_poly(monitor_corners, (0, 200, 255), 2)
        draw_poly([monitor_corners[0], monitor_corners[2]], (0, 150, 210), 1)
        draw_poly([monitor_corners[1], monitor_corners[3]], (0, 150, 210), 1)
        if monitor_center is not None:
            draw_cross_3d(monitor_center, size=8, color=(0, 200, 255), thickness=2)
            if monitor_normal is not None:
                tip = np.asarray(monitor_center) + np.asarray(monitor_normal) * (20.0 * (units_per_cm or 1.0))
                draw_arrow_3d(monitor_center, tip, color=(0, 220, 255), thickness=2)

    # --- Stored gaze markers on the monitor plane (green circles) ---
    if (gaze_markers and monitor_corners is not None):
        p0, p1, p2, p3 = [np.asarray(p, dtype=float) for p in monitor_corners]
        u = p1 - p0  # width direction
        v = p3 - p0  # height direction
        width_world = float(np.linalg.norm(u))
        if width_world > 1e-9:
            u_hat = u / width_world
            r_world = 0.01 * width_world  # 2% of width
            for (a, b) in gaze_markers:
                Pm = p0 + a * u + b * v
                projP = project_point(Pm)
                projR = project_point(Pm + u_hat * r_world)
                if projP is not None and projR is not None:
                    center_px = projP[0]
                    r_px = int(max(1, np.linalg.norm(np.array(projR[0]) - np.array(center_px))))
                    cv2.circle(debug, center_px, r_px, (0, 255, 0), 1, lineType=cv2.LINE_AA)



    # --- Gaze hit on monitor plane (circle at intersection) ---
    if (monitor_corners is not None and monitor_center is not None and monitor_normal is not None
        and combined_dir is not None
        and sphere_world_l is not None and sphere_world_r is not None):

        # Ray: origin at midpoint between eyes; direction = combined gaze
        O = (np.asarray(sphere_world_l, dtype=float) + np.asarray(sphere_world_r, dtype=float)) * 0.5
        D = _normalize(np.asarray(combined_dir, dtype=float))

        # Plane: through monitor_center with normal = monitor_normal
        C = np.asarray(monitor_center, dtype=float)
        N = _normalize(np.asarray(monitor_normal, dtype=float))

        denom = float(np.dot(N, D))
        if abs(denom) > 1e-6:
            t = float(np.dot(N, (C - O)) / denom)
            if t > 0.0:
                P = O + t * D  # world-space intersection point

                # Inside-quad test using monitor's local axes (top-left p0, top-right p1, bottom-left p3)
                p0, p1, p2, p3 = [np.asarray(p, dtype=float) for p in monitor_corners]
                u = p1 - p0             # horizontal (width) vector
                v = p3 - p0             # vertical (height) vector
                wv = P  - p0

                u_len2 = float(np.dot(u, u))
                v_len2 = float(np.dot(v, v))
                if u_len2 > 1e-9 and v_len2 > 1e-9:
                    a = float(np.dot(wv, u) / u_len2)  # 0..1 across width
                    b = float(np.dot(wv, v) / v_len2)  # 0..1 across height

                    if 0.0 <= a <= 1.0 and 0.0 <= b <= 1.0:
                        # Project center to pixels
                        projP = project_point(P)
                        if projP is not None:
                            center_px = projP[0]

                            # Circle radius = 5% of monitor width (world), projected to pixels
                            width_world = math.sqrt(u_len2)
                            r_world = 0.05 * width_world
                            u_hat = u / max(width_world, 1e-9)

                            projR = project_point(P + u_hat * r_world)
                            if projR is not None:
                                r_px = int(max(1, np.linalg.norm(np.array(projR[0]) - np.array(center_px))))
                                cv2.circle(debug, center_px, r_px, (0, 255, 255), 2, lineType=cv2.LINE_AA)


    # --- Key command help text in lower-left ---
    help_text = [
        "C = calibrate screen center",
        "J = yaw left",
        "L = yaw right",
        "I = pitch up",
        "K = pitch down",
        "O = next camera rotate",
        "0-3 = set camera rotate",
        "[ = zoom out",
        "] = zoom in",
        "R = reset view",
        "X = add marker",
        "q = quit",
        "F7 = toggle mouse control",
        "F6 = toggle stabilization",
        "F5 = toggle fixation",
        "F9 = toggle plane mapping",
        "F10 = invert X (plane)",
        "F11 = invert Y (plane)",
        "V = toggle velocity mode"
    ]

    font        = cv2.FONT_HERSHEY_SIMPLEX
    font_scale  = 0.5
    thickness   = 1
    line_height = 18  # pixels between lines

    # Start a bit above the bottom-left corner
    y0 = h - (len(help_text) * line_height) - 10
    x0 = 10

    for i, text in enumerate(help_text):
        y = y0 + i * line_height
        cv2.putText(debug, text, (x0, y), font, font_scale, (200, 200, 200), thickness, cv2.LINE_AA)


    cv2.imshow("Head/Eye Debug", debug)



def mouse_mover():
    """Mouse movement thread (smoothed, speed-limited)."""
    global smoothed_mouse_pos, last_move_time, current_smooth_tau, current_max_speed
    global current_deadzone_px, current_speed_gain
    while True:
        now = time.perf_counter()
        dt = max(1e-3, now - last_move_time)
        last_move_time = now
        if mouse_control_enabled:
            with mouse_lock:
                tx, ty = mouse_target
            sx, sy = smoothed_mouse_pos
            dx = tx - sx
            dy = ty - sy
            # Raw distance for deadzone logic
            raw_dist = math.hypot(dx, dy)
            # Per-axis DPI scaling affects movement speed directionally
            wdx = float(dx) * float(max(0.1, dpi_scale_x))
            wdy = float(dy) * float(max(0.1, dpi_scale_y))
            dist = math.hypot(wdx, wdy)
            active_deadzone = max(DEADZONE_PX, current_deadzone_px)
            # Soft-deadzone: vẫn cho bước nhỏ thay vì đứng im
            tau_base = max(1e-3, current_smooth_tau)
            # tăng ổn định khi DPI cao và khi Plane Gain lớn
            dpi_factor_stab = float(max(0.1, math.sqrt(max(0.1, dpi_scale_x) * max(0.1, dpi_scale_y))))
            plane_gain_mean = float((plane_gain_x + plane_gain_y) * 0.5)
            stability_mult = float(max(1.0, 1.0 + 0.25 * (dpi_factor_stab - 1.0) + 0.12 * (plane_gain_mean - 1.0)))
            tau = tau_base * stability_mult
            # damping thêm khi gần đích để giảm rung
            try:
                near_r = 15.0
                if raw_dist < near_r:
                    k_near = (near_r - raw_dist) / near_r
                    tau *= (1.0 + 0.6 * k_near)
            except Exception:
                pass
            alpha = 1 - math.exp(-dt / tau)
            # nếu trong deadzone, giảm alpha để mượt hơn
            eff_alpha = alpha if raw_dist > active_deadzone else (alpha * 0.50)
            step = dist * eff_alpha
            # tối thiểu một bước nhỏ khi dist vượt 0.25px
            if raw_dist > 0.25:
                step = max(step, 0.1)
            gain = max(0.2, float(current_speed_gain))
            # DPI speed factor: use geometric mean so tăng X/Y đều tăng tốc tổng
            dpi_factor = float(max(0.1, math.sqrt(max(0.1, dpi_scale_x) * max(0.1, dpi_scale_y))))
            max_step = current_max_speed * gain * dt * dpi_factor
            if step > max_step:
                step = max_step
            if step > 0 and dist > 1e-6:
                k = step / dist
                sx += wdx * k
                sy += wdy * k
                smoothed_mouse_pos = [sx, sy]
            try:
                if os.name == 'nt':
                    ret = ctypes.windll.user32.SetCursorPos(int(smoothed_mouse_pos[0]), int(smoothed_mouse_pos[1]))
                    if not ret:
                        pyautogui.moveTo(int(smoothed_mouse_pos[0]), int(smoothed_mouse_pos[1]))
                else:
                    pyautogui.moveTo(int(smoothed_mouse_pos[0]), int(smoothed_mouse_pos[1]))
            except Exception:
                try:
                    pyautogui.moveTo(int(smoothed_mouse_pos[0]), int(smoothed_mouse_pos[1]))
                except Exception:
                    pass
        time.sleep(0.005)

# Start mouse movement thread
threading.Thread(target=mouse_mover, daemon=True).start()

# Eye sphere tracking variables (from new script)
left_sphere_locked = False
left_sphere_local_offset = None
left_calibration_nose_scale = None

right_sphere_locked = False
right_sphere_local_offset = None
right_calibration_nose_scale = None

while cap.isOpened():
    ret, frame = cap.read()
    if not ret:
        break

    frame = apply_camera_orientation(frame)
    h, w = frame.shape[:2]

    combined_dir = None  # will be filled once you compute a smoothed direction

    frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    results = face_mesh.process(frame_rgb)

    if results.multi_face_landmarks:
        face_landmarks = results.multi_face_landmarks[0].landmark

        # Index for left iris center point (from MediaPipe's iris model)
        left_iris_idx = 468
        right_iris_idx = 473
        left_iris = face_landmarks[left_iris_idx]
        right_iris = face_landmarks[right_iris_idx]

        # Compute and draw stabilized coordinate frame from nose region
        head_center, R_final, nose_points_3d = compute_and_draw_coordinate_box(
            frame,
            face_landmarks,
            nose_indices,
            R_ref_nose,
            color=(0, 255, 0),
            size=80
        )

        # TODO compute this radius using canthus during calibration
        base_radius = 20  # radius at calibration distance

        x_iris_l = int(left_iris.x * w)
        y_iris_l = int(left_iris.y * h)
        # === LEFT EYE visualization ===
        if not left_sphere_locked:
            cv2.circle(frame, (x_iris_l, y_iris_l), 10, (255, 25, 25), 2)
        else:
            current_nose_scale = compute_scale(nose_points_3d)
            scale_ratio = current_nose_scale / left_calibration_nose_scale if left_calibration_nose_scale else 1.0
            scaled_offset = left_sphere_local_offset * scale_ratio
            sphere_world_l = head_center + R_final @ scaled_offset
            x_sphere_l, y_sphere_l = int(sphere_world_l[0]), int(sphere_world_l[1])
            scaled_radius_l = int(base_radius * scale_ratio)
            cv2.circle(frame, (x_sphere_l, y_sphere_l), scaled_radius_l, (255, 255, 25), 2)

        x_iris_r = int(right_iris.x * w)
        y_iris_r = int(right_iris.y * h)
        # === RIGHT EYE visualization ===
        if not right_sphere_locked:
            cv2.circle(frame, (x_iris_r, y_iris_r), 10, (25, 255, 25), 2)
        else:
            current_nose_scale = compute_scale(nose_points_3d)
            scale_ratio_r = current_nose_scale / right_calibration_nose_scale if right_calibration_nose_scale else 1.0
            scaled_offset_r = right_sphere_local_offset * scale_ratio_r
            sphere_world_r = head_center + R_final @ scaled_offset_r
            x_sphere_r, y_sphere_r = int(sphere_world_r[0]), int(sphere_world_r[1])
            scaled_radius_r = int(base_radius * scale_ratio_r)
            cv2.circle(frame, (x_sphere_r, y_sphere_r), scaled_radius_r, (25, 255, 255), 2)

        iris_3d_left = np.array([left_iris.x * w, left_iris.y * h, left_iris.z * w])
        iris_3d_right = np.array([right_iris.x * w, right_iris.y * h, right_iris.z * w])
        
        if left_sphere_locked and right_sphere_locked:
            # ==== DRAW LEFT AND RIGHT GAZE ====
            draw_gaze(frame, sphere_world_l, iris_3d_left, scaled_radius_l, (55, 255, 0), 130)   
            draw_gaze(frame, sphere_world_r, iris_3d_right, scaled_radius_r, (55, 255, 0), 130)  

            # ==== COMPUTE COMBINED GAZE DIRECTION FOR SCREEN MAPPING ====
            # Calculate individual gaze directions
            left_gaze_dir = iris_3d_left - sphere_world_l
            left_gaze_dir /= np.linalg.norm(left_gaze_dir)
            
            right_gaze_dir = iris_3d_right - sphere_world_r
            right_gaze_dir /= np.linalg.norm(right_gaze_dir)
            
            # Combine gaze directions (average)
            raw_combined_direction = (left_gaze_dir + right_gaze_dir) / 2
            raw_combined_direction /= np.linalg.norm(raw_combined_direction)

            # Update direction buffer for smoothing
            combined_gaze_directions.append(raw_combined_direction)

            # Smoothed direction
            avg_combined_direction = np.mean(combined_gaze_directions, axis=0)
            avg_combined_direction /= np.linalg.norm(avg_combined_direction)

            combined_dir = avg_combined_direction

            # ==== Head/Eye motion metrics (EMA) for adaptive smoothing ====
            now_t = time.perf_counter()
            if last_motion_sample_time is None:
                last_motion_sample_time = now_t
            dt_motion = max(1e-3, now_t - last_motion_sample_time)
            last_motion_sample_time = now_t

            # Head rotation speed (deg/s) from relative rotation
            head_rot_speed_deg_s = 0.0
            if prev_head_R is not None and 'R_final' in locals() and R_final is not None:
                try:
                    R_rel = prev_head_R.T @ R_final
                    tr = float(np.trace(R_rel))
                    angle = math.degrees(math.acos(np.clip((tr - 1.0) * 0.5, -1.0, 1.0)))
                    head_rot_speed_deg_s = angle / dt_motion
                except Exception:
                    head_rot_speed_deg_s = 0.0
            if 'R_final' in locals() and R_final is not None:
                prev_head_R = R_final.copy()

            # Head translation speed (world units/s)
            head_trans_speed_w = 0.0
            if prev_head_center is not None and 'head_center' in locals() and head_center is not None:
                try:
                    head_trans_speed_w = float(np.linalg.norm(head_center - prev_head_center) / dt_motion)
                except Exception:
                    head_trans_speed_w = 0.0
            if 'head_center' in locals() and head_center is not None:
                prev_head_center = head_center.copy()

            # Eye motion speed (deg/s) via change in gaze direction
            eye_speed_deg_s = 0.0
            if prev_gaze_dir is not None and prev_gaze_dir is not None:
                try:
                    d = float(np.clip(np.dot(prev_gaze_dir, avg_combined_direction), -1.0, 1.0))
                    eye_angle = math.degrees(math.acos(d))
                    eye_speed_deg_s = eye_angle / dt_motion
                except Exception:
                    eye_speed_deg_s = 0.0
            prev_gaze_dir = avg_combined_direction.copy()

            # Normalize and EMA
            h_norm = max(
                min(head_rot_speed_deg_s / max(HEAD_ROT_SAT_DEG_S, 1e-6), 1.0),
                min(head_trans_speed_w / max(HEAD_TRANS_SAT_WORLD_S, 1e-6), 1.0)
            )
            e_norm = min(eye_speed_deg_s / 45.0, 1.0)
            head_motion_ema = HEAD_MOTION_ALPHA * h_norm + (1.0 - HEAD_MOTION_ALPHA) * head_motion_ema
            eye_motion_ema  = HEAD_MOTION_ALPHA * e_norm + (1.0 - HEAD_MOTION_ALPHA) * eye_motion_ema

            # Adapt smoothing and max speed
            total = head_motion_ema + eye_motion_ema + 1e-6
            eye_ratio = eye_motion_ema / total
            head_ratio = head_motion_ema / total
            # Khi mắt chiếm ưu thế → giảm tau, tăng speed; khi đầu chiếm ưu thế → tăng tau nhẹ
            target_tau = SMOOTH_TAU_MIN * eye_ratio + SMOOTH_TAU_BASE * (1.0 - eye_ratio) + 0.04 * head_ratio
            current_smooth_tau = float(np.clip(target_tau, SMOOTH_TAU_MIN, SMOOTH_TAU_MAX))
            current_max_speed = float(MAX_SPEED_BASE_PX_S * (0.9 + 2.0 * eye_ratio))

            # Short saccade boost: nếu eye_speed_deg_s lớn, tăng tốc và giảm tau tạm thời
            if eye_speed_deg_s >= SACCADE_BOOST_DEG_S:
                saccade_boost_until = now_t + SACCADE_BOOST_DURATION_S
            if now_t < saccade_boost_until:
                current_smooth_tau = min(current_smooth_tau, SMOOTH_TAU_MIN * 0.7)
                current_max_speed *= 2.0

            # ==== Map gaze to screen coords (prefer Plane Mapping if enabled) ====
            screen_x = screen_y = None
            # Update raw yaw/pitch for training collection
            try:
                _, _, raw_yaw_tmp, raw_pitch_tmp = convert_gaze_to_screen_coordinates(
                    avg_combined_direction,
                    calibration_offset_yaw,
                    calibration_offset_pitch
                )
                globals()['last_raw_yaw_pitch'] = (float(raw_yaw_tmp), float(raw_pitch_tmp))
            except Exception:
                globals()['last_raw_yaw_pitch'] = None
            if plane_mapping_enabled and (monitor_corners is not None and monitor_center_w is not None and monitor_normal_w is not None) and left_sphere_locked and right_sphere_locked:
                combined_origin = (sphere_world_l + sphere_world_r) / 2
                gpm = gaze_point_on_monitor(combined_origin, avg_combined_direction)
                if gpm is not None:
                    screen_x, screen_y = gpm[0], gpm[1]
                    # Apply manual plane inversion toggles globally after mapping
                    if plane_invert_x:
                        screen_x = (MONITOR_WIDTH - 1) - int(screen_x)
                    if plane_invert_y:
                        screen_y = (MONITOR_HEIGHT - 1) - int(screen_y)
                    try:
                        globals()['last_gpm_ab'] = (float(gpm[2]), float(gpm[3]))
                    except Exception:
                        globals()['last_gpm_ab'] = None
                    # Debug print a,b and final screen coords (throttled)
                    if ab_debug_enabled and globals()['last_gpm_ab'] is not None:
                        try:
                            tnow = time.perf_counter()
                            if tnow - globals()['ab_debug_last_print'] >= 0.2:
                                globals()['ab_debug_last_print'] = tnow
                                a_dbg, b_dbg = globals()['last_gpm_ab']
                                print(f"[Plane AB] a={a_dbg:.3f} b={b_dbg:.3f} -> screen=({int(screen_x)}, {int(screen_y)}) invX={plane_invert_x} invY={plane_invert_y}")
                        except Exception:
                            pass
                else:
                    globals()['last_gpm_ab'] = None
            else:
                globals()['last_gpm_ab'] = None

            if screen_x is None or screen_y is None:
                screen_x, screen_y, raw_yaw, raw_pitch = convert_gaze_to_screen_coordinates(
                    avg_combined_direction, 
                    calibration_offset_yaw, 
                    calibration_offset_pitch
                )
                # Apply inversion toggles for fallback mapping too
                if plane_invert_x:
                    screen_x = (MONITOR_WIDTH - 1) - int(screen_x)
                if plane_invert_y:
                    screen_y = (MONITOR_HEIGHT - 1) - int(screen_y)

            # ==== Phase 1 filtering & target update OR Velocity mode ====
            if velocity_mode_enabled:
                now_v = time.perf_counter()
                last_raw_screen_xy = (float(screen_x), float(screen_y))
                if last_velocity_time is None:
                    last_velocity_time = now_v
                dt_v = max(1e-3, now_v - last_velocity_time)
                last_velocity_time = now_v

                use_ab = (globals().get('last_gpm_ab') is not None)
                if use_ab:
                    try:
                        a_val, b_val = globals()['last_gpm_ab']
                        ax = float(2.0 * (float(a_val) - 0.5))
                        ay = float(2.0 * ((1.0 - float(b_val)) - 0.5))
                        dzx = float(AB_INTENT_DEADZONE)
                        dzy = float(AB_INTENT_DEADZONE)
                        if abs(ax) < dzx:
                            ix = 0.0
                        else:
                            nx = float(np.clip((abs(ax) - dzx) / max(1e-6, 1.0 - dzx), 0.0, 1.0))
                            ix = math.copysign(nx ** VEL_SHAPE_GAMMA, ax)
                        if abs(ay) < dzy:
                            iy = 0.0
                        else:
                            ny = float(np.clip((abs(ay) - dzy) / max(1e-6, 1.0 - dzy), 0.0, 1.0))
                            iy = math.copysign(ny ** VEL_SHAPE_GAMMA, ay)
                        intent_ema_x = float((1.0 - INTENT_EMA_ALPHA) * intent_ema_x + INTENT_EMA_ALPHA * ix)
                        intent_ema_y = float((1.0 - INTENT_EMA_ALPHA) * intent_ema_y + INTENT_EMA_ALPHA * iy)
                    except Exception:
                        use_ab = False

                if not use_ab:
                    if baseline_px is None:
                        baseline_px = [float(screen_x), float(screen_y)]
                    alpha_b = float(np.clip(dt_v / max(1e-3, BASELINE_TAU_S), 0.0, 1.0))
                    baseline_px[0] = float((1.0 - alpha_b) * baseline_px[0] + alpha_b * float(screen_x))
                    baseline_px[1] = float((1.0 - alpha_b) * baseline_px[1] + alpha_b * float(screen_y))
                    dx = float(screen_x) - float(baseline_px[0])
                    dy = float(screen_y) - float(baseline_px[1])
                    thx = float(max(BIO_DEADZONE_PX, BASELINE_PCT_DEADZONE * MONITOR_WIDTH))
                    thy = float(max(BIO_DEADZONE_PX, BASELINE_PCT_DEADZONE * MONITOR_HEIGHT))
                    if abs(dx) < thx: dx = 0.0
                    if abs(dy) < thy: dy = 0.0
                    dx_norm = float(np.clip(dx / max(1.0, 0.5 * MONITOR_WIDTH), -1.0, 1.0))
                    dy_norm = float(np.clip(dy / max(1.0, 0.5 * MONITOR_HEIGHT), -1.0, 1.0))
                    intent_ema_x = float((1.0 - INTENT_EMA_ALPHA) * intent_ema_x + INTENT_EMA_ALPHA * dx_norm)
                    intent_ema_y = float((1.0 - INTENT_EMA_ALPHA) * intent_ema_y + INTENT_EMA_ALPHA * dy_norm)
                # áp dụng invert plane để velocity cùng hướng với mapping
                if plane_invert_x:
                    intent_ema_x = -intent_ema_x
                if plane_invert_y:
                    intent_ema_y = -intent_ema_y
                still = (abs(intent_ema_x) < 0.02 and abs(intent_ema_y) < 0.02)
                if still:
                    if intent_still_since is None:
                        intent_still_since = now_v
                else:
                    intent_still_since = None
                if intent_still_since is not None and (now_v - intent_still_since) >= 0.25:
                    vx = 0.0
                    vy = 0.0
                else:
                    vx = float(VEL_GAIN_BASE_X * intent_ema_x * max(0.1, dpi_scale_x))
                    vy = float(VEL_GAIN_BASE_Y * intent_ema_y * max(0.1, dpi_scale_y))
                # clamp acceleration & speed; smooth velocity
                try:
                    if 'vel_prev' not in globals() or globals()['vel_prev'] is None:
                        globals()['vel_prev'] = (0.0, 0.0)
                    vpx, vpy = globals()['vel_prev']
                    ax = (vx - vpx) / dt_v
                    ay = (vy - vpy) / dt_v
                    amax = float(max(1000.0, VEL_MAX_ACC_PX_S2))
                    if abs(ax) > amax:
                        vx = vpx + math.copysign(amax * dt_v, ax)
                    if abs(ay) > amax:
                        vy = vpy + math.copysign(amax * dt_v, ay)
                    vmag = math.hypot(vx, vy)
                    vmax = float(max(400.0, VEL_MAX_PX_S))
                    if vmag > vmax:
                        k = vmax / vmag
                        vx *= k; vy *= k
                    vx = float((1.0 - VEL_EMA_ALPHA) * vpx + VEL_EMA_ALPHA * vx)
                    vy = float((1.0 - VEL_EMA_ALPHA) * vpy + VEL_EMA_ALPHA * vy)
                    globals()['vel_prev'] = (vx, vy)
                except Exception:
                    pass
                with mouse_lock:
                    tx, ty = mouse_target
                tx = float(np.clip(tx + vx * dt_v, 0.0, MONITOR_WIDTH - 1))
                ty = float(np.clip(ty + vy * dt_v, 0.0, MONITOR_HEIGHT - 1))
                fx, fy = int(tx), int(ty)
                last_filtered_target = np.array([fx, fy], dtype=float)
                with mouse_lock:
                    mouse_target[0] = tx
                    mouse_target[1] = ty
            else:
                filtered_xy = update_cursor_target(screen_x, screen_y)
                fx, fy = int(filtered_xy[0]), int(filtered_xy[1])
                adaptive_dpi_update(fx, fy)

            # Update mouse target (filtered or velocity integrated)
            if mouse_control_enabled and not velocity_mode_enabled:
                with mouse_lock:
                    mouse_target[0] = fx
                    mouse_target[1] = fy

            # Write filtered position to file
            write_screen_position(fx, fy)

            # Draw combined gaze ray for visualization
            combined_origin = (sphere_world_l + sphere_world_r) / 2
            combined_target = combined_origin + avg_combined_direction * gaze_length
            cv2.line(
                frame,
                (int(combined_origin[0]), int(combined_origin[1])),
                (int(combined_target[0]), int(combined_target[1])),
                (255, 255, 10), 3
            )

            # Center multiple lines of text
            texts = [
                f"Screen: ({fx}, {fy})",
                #f"Mouse: {'ON' if mouse_control_enabled else 'OFF'}"
            ]

            font = cv2.FONT_HERSHEY_SIMPLEX
            font_scale = 0.7
            thickness = 2
            line_spacing = 30

            for i, text in enumerate(texts):
                (text_width, text_height), baseline = cv2.getTextSize(text, font, font_scale, thickness)
                center_x = (w - text_width) // 2
                #center_y = (h // 2) + (i - len(texts)//2) * line_spacing
                
                color = (0, 255, 0) if "Mouse: ON" not in text else (0, 255, 0) if mouse_control_enabled else (0, 0, 255)
                cv2.putText(frame, text, (center_x, 30), font, font_scale, color, thickness)

        # Draw all landmark points in white
        for idx, lm in enumerate(face_landmarks):
            x, y = int(lm.x * w), int(lm.y * h)
            cv2.circle(frame, (x, y), 0, (255, 255, 255), -1)

        # Smooth orbit controls each frame
        update_orbit_from_keys()

        # Build 3D landmarks in your existing scale (x*w, y*h, z*w)
        landmarks3d = None
        if results.multi_face_landmarks:
            lm = results.multi_face_landmarks[0].landmark
            landmarks3d = np.array([[p.x * w, p.y * h, p.z * w] for p in lm], dtype=float)

        render_debug_view_orbit(
            h, w,
            head_center3d=head_center if 'head_center' in locals() else None,
            sphere_world_l=sphere_world_l if left_sphere_locked and 'sphere_world_l' in locals() else None,
            scaled_radius_l=scaled_radius_l if left_sphere_locked and 'scaled_radius_l' in locals() else None,
            sphere_world_r=sphere_world_r if right_sphere_locked and 'sphere_world_r' in locals() else None,
            scaled_radius_r=scaled_radius_r if right_sphere_locked and 'scaled_radius_r' in locals() else None,
            iris3d_l=iris_3d_left if 'iris_3d_left' in locals() else None,
            iris3d_r=iris_3d_right if 'iris_3d_right' in locals() else None,
            left_locked=left_sphere_locked,
            right_locked=right_sphere_locked,
            landmarks3d=landmarks3d,
            combined_dir=avg_combined_direction if 'avg_combined_direction' in locals() else None,
            gaze_len=5230,
            monitor_corners=monitor_corners,
            monitor_center=monitor_center_w,
            monitor_normal=monitor_normal_w,
            gaze_markers=gaze_markers
        )


    cv2.imshow("Integrated Eye Tracking", frame)
    # Step training state machine each frame (render after main window so it stays on top)
    if training_enabled:
        training_step(time.perf_counter())

    # Handle keyboard input
    if keyboard.is_pressed('f7'):
        mouse_control_enabled = not mouse_control_enabled
        print(f"[Mouse Control] {'Enabled' if mouse_control_enabled else 'Disabled'}")
        time.sleep(0.3)  # debounce to prevent rapid toggling
    if keyboard.is_pressed('f4'):
        if not training_enabled:
            training_enabled = True
            training_begin()
        else:
            training_end(save=True)
        time.sleep(0.3)
    if keyboard.is_pressed('f6'):
        stabilization_enabled = not stabilization_enabled
        print(f"[Stabilization] {'Enabled' if stabilization_enabled else 'Disabled'}")
        time.sleep(0.3)
    if keyboard.is_pressed('f5'):
        fixation_enabled = not fixation_enabled
        print(f"[Fixation] {'Enabled' if fixation_enabled else 'Disabled'}")
        time.sleep(0.3)
    if keyboard.is_pressed('f1'):
        ab_debug_enabled = not ab_debug_enabled
        print(f"[Plane AB Debug] {'ON' if ab_debug_enabled else 'OFF'} (require Plane Mapping F9 ON)")
        time.sleep(0.3)
    # Per-axis DPI controls
    if keyboard.is_pressed('f2'):
        dpi_scale_x = float(np.clip(dpi_scale_x * 0.85, 0.1, 6.0))
        print(f"[DPI] X scale = {dpi_scale_x:.2f}")
        time.sleep(0.2)
    if keyboard.is_pressed('f3'):
        dpi_scale_x = float(np.clip(dpi_scale_x * 1.15, 0.1, 6.0))
        print(f"[DPI] X scale = {dpi_scale_x:.2f}")
        time.sleep(0.2)
    if keyboard.is_pressed('f8'):
        dpi_scale_y = float(np.clip(dpi_scale_y * 0.85, 0.1, 6.0))
        print(f"[DPI] Y scale = {dpi_scale_y:.2f}")
        time.sleep(0.2)
    if keyboard.is_pressed('f12'):
        dpi_scale_y = float(np.clip(dpi_scale_y * 1.15, 0.1, 6.0))
        print(f"[DPI] Y scale = {dpi_scale_y:.2f}")
        time.sleep(0.2)
    if keyboard.is_pressed('f9'):
        plane_mapping_enabled = not plane_mapping_enabled
        print(f"[Plane Mapping] {'Enabled' if plane_mapping_enabled else 'Disabled'}")
        time.sleep(0.3)
    if keyboard.is_pressed('f10'):
        plane_invert_x = not plane_invert_x
        print(f"[Plane Mapping] Invert X = {plane_invert_x}")
        time.sleep(0.3)
    if keyboard.is_pressed('f11'):
        plane_invert_y = not plane_invert_y
        print(f"[Plane Mapping] Invert Y = {plane_invert_y}")
        time.sleep(0.3)
    if keyboard.is_pressed('v'):
        velocity_mode_enabled = not velocity_mode_enabled
        baseline_px = None
        last_velocity_time = None
        intent_ema_x = 0.0
        intent_ema_y = 0.0
        intent_still_since = None
        print(f"[Velocity Mode] {'Enabled' if velocity_mode_enabled else 'Disabled'}")
        time.sleep(0.25)

    key = cv2.waitKey(1) & 0xFF
    if key == ord('q'):
        break
    elif key == ord('m'):
        mouse_control_enabled = not mouse_control_enabled
        print(f"[Mouse Control] {'Enabled' if mouse_control_enabled else 'Disabled'}")
    elif key == ord('f') and (cv2.getWindowProperty('Integrated Eye Tracking', 0) >= 0):
        pass
    elif key == ord('F'):
        pass
    elif key == ord('6') and (cv2.getWindowProperty('Integrated Eye Tracking', 0) >= 0):
        pass
    elif key == ord('`'):
        pass
    elif key == ord('h'):
        pass
    elif key == ord('g'):
        pass
    elif key == ord('t'):
        pass
    elif key == ord('y'):
        pass
    elif key == ord('u'):
        pass
    elif key == ord('o'):
        camera_orientation_index = (camera_orientation_index + 1) % len(CAMERA_ORIENTATION_LABELS)
        print(f"[Camera] Orientation set to {CAMERA_ORIENTATION_LABELS[camera_orientation_index]}")
    elif key in (ord('0'), ord('1'), ord('2'), ord('3')):
        idx = key - ord('0')
        if 0 <= idx < len(CAMERA_ORIENTATION_LABELS):
            camera_orientation_index = idx
            print(f"[Camera] Orientation set to {CAMERA_ORIENTATION_LABELS[camera_orientation_index]}")
    elif key == ord('f') and keyboard.is_pressed('shift'):
        pass
    elif key == ord('k'):
        pass
    elif key == ord('='):
        plane_gain_x = float(np.clip(plane_gain_x * 1.10, 0.8, 8.0))
        plane_gain_y = float(np.clip(plane_gain_y * 1.10, 0.8, 8.0))
        print(f"[Plane Gain] X={plane_gain_x:.2f}, Y={plane_gain_y:.2f}")
    elif key == ord('-'):
        plane_gain_x = float(np.clip(plane_gain_x * 0.90, 0.8, 8.0))
        plane_gain_y = float(np.clip(plane_gain_y * 0.90, 0.8, 8.0))
        print(f"[Plane Gain] X={plane_gain_x:.2f}, Y={plane_gain_y:.2f}")
    elif key == ord(']'):
        plane_gain_x = float(np.clip(plane_gain_x * 1.10, 0.8, 8.0))
        print(f"[Plane Gain X] {plane_gain_x:.2f}")
    elif key == ord('['):
        plane_gain_x = float(np.clip(plane_gain_x * 0.90, 0.8, 8.0))
        print(f"[Plane Gain X] {plane_gain_x:.2f}")
    elif key == ord('\''):
        plane_gain_y = float(np.clip(plane_gain_y * 1.10, 0.8, 8.0))
        print(f"[Plane Gain Y] {plane_gain_y:.2f}")
    elif key == ord(';'):
        plane_gain_y = float(np.clip(plane_gain_y * 0.90, 0.8, 8.0))
        print(f"[Plane Gain Y] {plane_gain_y:.2f}")
    elif key == ord('p'):
        pass
    elif key == ord('f'):
        pass
    elif key == ord('6'):
        pass
    elif key == ord('^'):
        pass
    elif key == ord('F'):
        pass
    elif key == ord('f'):
        pass
    elif key == ord('F'):
        pass
    elif key == ord('F'):
        pass
    elif key == ord('f'):
        pass
    elif key == ord('F'):
        pass
    elif key == ord('f'):
        pass
    elif key == ord('F'):
        pass
    elif key == ord('!'):
        pass
    elif key == ord('^'):
        pass
    elif key == ord('~'):
        pass
    elif key == ord('6'):
        pass
    elif key == ord('f'):
        pass
    elif key == ord('o'):
        camera_orientation_index = (camera_orientation_index + 1) % len(CAMERA_ORIENTATION_LABELS)
        print(f"[Camera] Orientation set to {CAMERA_ORIENTATION_LABELS[camera_orientation_index]}")
    elif key in (ord('0'), ord('1'), ord('2'), ord('3')):
        idx = key - ord('0')
        if 0 <= idx < len(CAMERA_ORIENTATION_LABELS):
            camera_orientation_index = idx
            print(f"[Camera] Orientation set to {CAMERA_ORIENTATION_LABELS[camera_orientation_index]}")
    elif key == ord('f') and keyboard.is_pressed('6'):
        pass
    elif key == ord('f'):
        pass
    elif key == ord('6'):
        pass
    elif key == ord('F'):
        pass
    elif key == ord('`'):
        pass
    elif key == ord('~'):
        pass
    elif key == ord('h'):
        pass
    elif key == ord('g'):
        pass
    elif key == ord('t'):
        pass
    elif key == ord('y'):
        pass
    elif key == ord('u'):
        pass
    elif key == ord('c') and not (left_sphere_locked and right_sphere_locked):
        current_nose_scale = compute_scale(nose_points_3d)
        # Lock LEFT eye
        left_sphere_local_offset = R_final.T @ (iris_3d_left - head_center)
        camera_dir_world = np.array([0, 0, 1])
        camera_dir_local = R_final.T @ camera_dir_world
        left_sphere_local_offset += base_radius * camera_dir_local
        left_calibration_nose_scale = current_nose_scale
        left_sphere_locked = True

        # Lock RIGHT eye
        right_sphere_local_offset = R_final.T @ (iris_3d_right - head_center)
        right_sphere_local_offset += base_radius * camera_dir_local  # use same camera_dir_local
        right_calibration_nose_scale = current_nose_scale
        right_sphere_locked = True

        # === Create 3D monitor plane at calibration ===
        # Compute instantaneous sphere positions at calibration distance (scale=1)
        sphere_world_l_calib = head_center + R_final @ left_sphere_local_offset
        sphere_world_r_calib = head_center + R_final @ right_sphere_local_offset

        # Estimate a forward gaze direction from the two eyes
        left_dir  = iris_3d_left  - sphere_world_l_calib
        right_dir = iris_3d_right - sphere_world_r_calib
        # Normalize (guard zero)
        if np.linalg.norm(left_dir)  > 1e-9: left_dir  /= np.linalg.norm(left_dir)
        if np.linalg.norm(right_dir) > 1e-9: right_dir /= np.linalg.norm(right_dir)
        forward_hint = (left_dir + right_dir) * 0.5
        if np.linalg.norm(forward_hint) > 1e-9:
            forward_hint /= np.linalg.norm(forward_hint)
        else:
            forward_hint = None  # fallback to head frame

        gaze_origin = (sphere_world_l_calib + sphere_world_r_calib) / 2
        gaze_dir = forward_hint  # already normalized

        monitor_corners, monitor_center_w, monitor_normal_w, units_per_cm = create_monitor_plane(
            head_center, R_final, face_landmarks, w, h,
            forward_hint=forward_hint,
            gaze_origin=gaze_origin,
            gaze_dir=gaze_dir
        )

        # Freeze the debug world's orbit pivot at the calibrated monitor center
        #global debug_world_frozen, orbit_pivot_frozen
        debug_world_frozen = True
        orbit_pivot_frozen = monitor_center_w.copy()
        print("[Debug View] World pivot frozen at monitor center.")

        print(f"[Monitor] units_per_cm={units_per_cm:.3f}, center={monitor_center_w}, normal={monitor_normal_w}")


        print("[Both Spheres Locked] Eye sphere calibration complete.")
        # --- Auto screen center calibration (like pressing 's') ---
        try:
            left_gaze_dir = iris_3d_left - sphere_world_l_calib
            left_gaze_dir /= np.linalg.norm(left_gaze_dir)
            right_gaze_dir = iris_3d_right - sphere_world_r_calib
            right_gaze_dir /= np.linalg.norm(right_gaze_dir)
            current_combined_direction = (left_gaze_dir + right_gaze_dir) / 2
            current_combined_direction /= np.linalg.norm(current_combined_direction)
            _, _, raw_yaw_auto, raw_pitch_auto = convert_gaze_to_screen_coordinates(
                current_combined_direction, 0, 0
            )
            calibration_offset_yaw = 0 - raw_yaw_auto
            calibration_offset_pitch = 0 - raw_pitch_auto
            print(f"[Auto Screen Calibrated] Offset Yaw: {calibration_offset_yaw:.2f}, Offset Pitch: {calibration_offset_pitch:.2f}")
        except Exception as _:
            pass

        # --- Reset stabilization filters and seed with current gaze→monitor mapping ---
        try:

            # Re-init filters
            gaze_filter_x = OneEuroFilter(GAZE_FILTER_FREQ, GAZE_MIN_CUTOFF, GAZE_BETA, GAZE_D_CUTOFF)
            gaze_filter_y = OneEuroFilter(GAZE_FILTER_FREQ, GAZE_MIN_CUTOFF, GAZE_BETA, GAZE_D_CUTOFF)
            last_filtered_target = None
            last_filter_timestamp = None
            saccade_freeze_until = 0.0
            fix_low_start_time = None
            fixation_anchor = None

            # Seed position from calibrated plane
            seed_origin = (sphere_world_l_calib + sphere_world_r_calib) / 2
            seed_dir = gaze_dir if gaze_dir is not None else forward_hint
            if seed_dir is not None:
                gpm_seed = gaze_point_on_monitor(seed_origin, seed_dir)
            else:
                gpm_seed = None

            if gpm_seed is not None:
                sx, sy = int(gpm_seed[0]), int(gpm_seed[1])
            else:
                sx, sy = CENTER_X, CENTER_Y

            last_filtered_target = np.array([sx, sy], dtype=float)
            last_filter_timestamp = time.perf_counter()
            smoothed_mouse_pos = [float(sx), float(sy)]
            stabilization_warmup_until = time.perf_counter() + 0.7
            print(f"[Stabilization] Seed at ({sx}, {sy})")
        except Exception:
            pass
    elif key == ord('s') and left_sphere_locked and right_sphere_locked:
        # Screen calibration - user should look at center of screen when pressing 's'
        # Get current gaze direction
        left_gaze_dir = iris_3d_left - sphere_world_l
        left_gaze_dir /= np.linalg.norm(left_gaze_dir)
        right_gaze_dir = iris_3d_right - sphere_world_r
        right_gaze_dir /= np.linalg.norm(right_gaze_dir)
        current_combined_direction = (left_gaze_dir + right_gaze_dir) / 2
        current_combined_direction /= np.linalg.norm(current_combined_direction)
        
        # Calculate what the raw angles would be without calibration
        _, _, raw_yaw, raw_pitch = convert_gaze_to_screen_coordinates(
            current_combined_direction, 0, 0  # no calibration offset
        )
        
        # Set calibration offsets to center the gaze
        calibration_offset_yaw = 0 - raw_yaw
        calibration_offset_pitch = 0 - raw_pitch
        
        print(f"[Screen Calibrated] Offset Yaw: {calibration_offset_yaw:.2f}, Offset Pitch: {calibration_offset_pitch:.2f}")
    elif key == ord('f') and keyboard.is_pressed('6'):
        pass
    elif key == ord('x'):
        # Drop a marker at the current gaze∩monitor point
        if (monitor_corners is not None and monitor_center_w is not None and monitor_normal_w is not None
            and left_sphere_locked and right_sphere_locked):
            # Recompute current eye-sphere positions (scale-aware)
            current_nose_scale = compute_scale(nose_points_3d)
            scale_ratio_l = current_nose_scale / left_calibration_nose_scale if left_calibration_nose_scale else 1.0
            scale_ratio_r = current_nose_scale / right_calibration_nose_scale if right_calibration_nose_scale else 1.0
            sphere_world_l_now = head_center + R_final @ (left_sphere_local_offset * scale_ratio_l)
            sphere_world_r_now = head_center + R_final @ (right_sphere_local_offset * scale_ratio_r)

            # Combined gaze direction (use smoothed if available; otherwise instantaneous)
            if 'avg_combined_direction' in locals() and avg_combined_direction is not None:
                D = _normalize(np.asarray(avg_combined_direction, dtype=float))
            else:
                lg = iris_3d_left  - sphere_world_l_now
                rg = iris_3d_right - sphere_world_r_now
                if np.linalg.norm(lg) < 1e-9 or np.linalg.norm(rg) < 1e-9:
                    print("[Marker] Gaze direction invalid; try again.")
                    D = None
                else:
                    lg /= np.linalg.norm(lg)
                    rg /= np.linalg.norm(rg)
                    D = _normalize(lg + rg)

            if D is not None:
                O = (sphere_world_l_now + sphere_world_r_now) * 0.5
                C = np.asarray(monitor_center_w, dtype=float)
                N = _normalize(np.asarray(monitor_normal_w, dtype=float))
                denom = float(np.dot(N, D))
                if abs(denom) < 1e-6:
                    print("[Marker] Gaze ray parallel to monitor; no marker.")
                else:
                    t = float(np.dot(N, (C - O)) / denom)
                    if t <= 0.0:
                        print("[Marker] Intersection behind/at eye; no marker.")
                    else:
                        P = O + t * D  # world-space intersection
                        # Map P to monitor local (a,b), then store if inside the quad
                        p0, p1, p2, p3 = [np.asarray(p, dtype=float) for p in monitor_corners]
                        u = p1 - p0
                        v = p3 - p0
                        u_len2 = float(np.dot(u, u))
                        v_len2 = float(np.dot(v, v))
                        if u_len2 > 1e-9 and v_len2 > 1e-9:
                            wv = P - p0
                            a = float(np.dot(wv, u) / u_len2)
                            b = float(np.dot(wv, v) / v_len2)
                            if 0.0 <= a <= 1.0 and 0.0 <= b <= 1.0:
                                gaze_markers.append((a, b))
                                print(f"[Marker] Added at a={a:.3f}, b={b:.3f}")
                            else:
                                print("[Marker] Gaze not on monitor; no marker.")
                        else:
                            print("[Marker] Monitor dimensions degenerate; no marker.")
        else:
            print("[Marker] Monitor/gaze not ready; complete center calibration first.")


cap.release()

cv2.destroyAllWindows()