# Detect and match features between images
orb = cv2.ORB_create(5000)
kp1, des1 = orb.detectAndCompute(gray1, None)
kp2, des2 = orb.detectAndCompute(gray2, None)

matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = sorted(matcher.match(des1, des2), key=lambda x: x.distance)

# Compute homography for alignment
src_pts = np.float32([kp1[m.queryIdx].pt for m in matches[:50]])
dst_pts = np.float32([kp2[m.trainIdx].pt for m in matches[:50]])
H, _ = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC)

# Warp image 1 to align with image 2
img1_aligned = cv2.warpPerspective(img1, H, (w, h))

# Standard methods
pixel_diff = cv2.absdiff(gray1_aligned, gray2).astype(float) / 255.0
ssim_score, ssim_map = ssim(gray1_aligned, gray2, full=True)
ssim_diff = 1 - ssim_map

# Edge detection for texture analysis (tire wear)
edges1 = cv2.Canny(gray1_aligned, 50, 150)
edges2 = cv2.Canny(gray2, 50, 150)
edge_diff = cv2.absdiff(edges1, edges2).astype(float) / 255.0

# Edge density for coarse texture changes
kernel = np.ones((15, 15), np.float32) / 225
edge_density1 = cv2.filter2D(edges1.astype(float) / 255.0, -1, kernel)
edge_density2 = cv2.filter2D(edges2.astype(float) / 255.0, -1, kernel)
density_diff = np.abs(edge_density1 - edge_density2)

# Weighted fusion
difference_map = (0.2 * pixel_diff + 
                 0.2 * ssim_diff + 
                 0.3 * edge_diff + 
                 0.3 * density_diff)

mask = np.zeros(img.shape[:2], np.uint8)
bgd_model = np.zeros((1, 65), np.float64)
fgd_model = np.zeros((1, 65), np.float64)

# Define rectangle around subject (center 80%)
h, w = img.shape[:2]
rect = (int(w*0.1), int(h*0.1), int(w*0.8), int(h*0.8))

# Run GrabCut
cv2.grabCut(img, mask, rect, bgd_model, fgd_model, 5, cv2.GC_INIT_WITH_RECT)
mask2 = np.where((mask == 2) | (mask == 0), 0, 1).astype('uint8')
result = img * mask2[:, :, np.newaxis]

def is_text_region(change):
    """Detect text-like regions to filter out"""
    area = change['area']
    aspect_ratio = change['aspect_ratio']
    
    # Text has high aspect ratio
    is_elongated = aspect_ratio > 2.5 or aspect_ratio < 0.4
    # Text is small-medium size
    is_small_medium = 100 < area < 5000
    
    return is_elongated and is_small_medium

CONFIG = {
    'use_roi': False,              # Focus on specific region
    'roi_coords': None,            # (x, y, w, h) or None for manual
    'remove_background': True,     # GrabCut background removal
    'use_edge_detection': True,    # Detect texture changes
    'filter_text_regions': True,   # Ignore text labels
    'sensitivity': 0.01,           # Threshold (0.01-0.2)
    'gen_image': False,            # Use test images vs GUI selection
}

class CarDetector:
    def __init__(self):
        self.bg_subtractor = cv2.createBackgroundSubtractorMOG2(
            history=500, 
            varThreshold=16, 
            detectShadows=True
        )
    
    def detect_by_motion(self, frame):
        # Apply background subtraction
        fg_mask = self.bg_subtractor.apply(frame)
        
        # Morphological cleanup
        kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
        fg_mask = cv2.morphologyEx(fg_mask, cv2.MORPH_CLOSE, kernel)
        fg_mask = cv2.morphologyEx(fg_mask, cv2.MORPH_OPEN, kernel)
        
        # Find contours and filter by area & aspect ratio
        contours, _ = cv2.findContours(fg_mask, cv2.RETR_EXTERNAL, 
                                       cv2.CHAIN_APPROX_SIMPLE)
        
        detections = []
        for contour in contours:
            area = cv2.contourArea(contour)
            if CONFIG['min_car_area'] < area < CONFIG['max_car_area']:
                x, y, w, h = cv2.boundingRect(contour)
                aspect_ratio = w / h
                if 0.8 < aspect_ratio < 4.0:  # Cars are wider than tall
                    center = (x + w//2, y + h//2)
                    detections.append((center, (x, y, w, h)))
        
        return detections

class CarTrack:
    def get_velocity(self):
        """Estimate velocity from recent positions"""
        if len(self.positions) < 2:
            return (0, 0)
        
        recent = list(self.positions)[-5:]
        dx = recent[-1][0] - recent[0][0]
        dy = recent[-1][1] - recent[0][1]
        return (dx / len(recent), dy / len(recent))

class RaceTracker:
    def update_tracks(self, detections):
        """Match new detections to existing tracks"""
        for track_id, track in self.tracks.items():
            last_pos = track.get_current_position()
            
            # Find nearest detection
            best_match = None
            best_distance = float('inf')
            
            for i, (position, bbox) in enumerate(detections):
                distance = np.linalg.norm(np.array(position) - 
                                         np.array(last_pos))
                if distance < best_distance and distance < 100:
                    best_distance = distance
                    best_match = i
            
            if best_match is not None:
                track.update(detections[best_match][0], 
                           detections[best_match][1], 
                           self.frame_count)

def _check_overtake(self, track1, track2):
    """Check if track1 overtook track2"""
    # Get position history
    pos1_old = list(track1.positions)[0]
    pos1_new = list(track1.positions)[-1]
    pos2_old = list(track2.positions)[0]
    pos2_new = list(track2.positions)[-1]
    
    # Check position swap
    was_behind = pos1_old[0] < pos2_old[0]
    now_ahead = pos1_new[0] > pos2_new[0]
    
    # Verify lateral movement
    x1_change = pos1_new[0] - pos1_old[0]
    x2_change = pos2_new[0] - pos2_old[0]
    lateral_movement = abs(x1_change - x2_change)
    
    if was_behind and now_ahead and \
       lateral_movement > CONFIG['lateral_threshold']:
        # Record overtake event
        overtake = OvertakeEvent(
            frame=self.frame_count,
            timestamp=self.frame_count / 30.0,
            overtaking_car=track1.name,
            overtaken_car=track2.name,
            confidence=min(track1.confidence, track2.confidence) / 100.0
        )
        self.overtakes.append(overtake)

CONFIG = {
    # Detection
    'min_car_area': 500,           # Minimum pixels
    'max_car_area': 50000,         # Maximum pixels
    'detection_roi': None,         # Focus on track area
    
    # Tracking
    'max_track_age': 30,           # Frames before loss
    'min_track_confidence': 5,     # Frames to confirm
    
    # Overtake detection
    'overtake_cooldown': 60,       # Prevent duplicates
    'lateral_threshold': 30,       # Horizontal movement
    
    # Visualization
    'show_trails': True,           # Motion trails
    'trail_length': 30,            # Trail points
    'show_speed_estimate': True,   # Velocity display
    
    # Output
    'output_video': True,          # Save annotated video
    'save_telemetry': True,        # JSON export
    'generate_timeline': True,     # Overtake list
}

def preprocess_frame(frame):
    """Apply CLAHE to normalize lighting across onboard footage"""
    processed = frame.copy()
    
    # Denoise
    if CONFIG['denoise_strength'] > 0:
        processed = cv2.fastNlMeansDenoisingColored(
            processed, None, 
            CONFIG['denoise_strength'], 
            CONFIG['denoise_strength'], 7, 21
        )
    
    # CLAHE on LAB color space
    if CONFIG['apply_clahe']:
        lab = cv2.cvtColor(processed, cv2.COLOR_BGR2LAB)
        l, a, b = cv2.split(lab)
        
        clahe = cv2.createCLAHE(
            clipLimit=CONFIG['clahe_clip_limit'],
            tileGridSize=CONFIG['clahe_grid_size']
        )
        l = clahe.apply(l)
        
        processed = cv2.merge([l, a, b])
        processed = cv2.cvtColor(processed, cv2.COLOR_LAB2BGR)
    
    return processed

def refine_mask(mask):
    """Clean up motion mask with morphological operations"""
    refined = mask.copy()
    
    # Define kernels
    kernel_open = cv2.getStructuringElement(
        cv2.MORPH_ELLIPSE, CONFIG['open_kernel_size']
    )
    kernel_close = cv2.getStructuringElement(
        cv2.MORPH_ELLIPSE, CONFIG['close_kernel_size']
    )
    kernel_dilate = cv2.getStructuringElement(
        cv2.MORPH_ELLIPSE, CONFIG['dilate_kernel_size']
    )
    
    # Remove small noise (opening)
    for _ in range(CONFIG['morphology_iterations']):
        refined = cv2.morphologyEx(refined, cv2.MORPH_OPEN, kernel_open)
    
    # Fill holes (closing)
    for _ in range(CONFIG['morphology_iterations']):
        refined = cv2.morphologyEx(refined, cv2.MORPH_CLOSE, kernel_close)
    
    # Expand slightly (dilation)
    refined = cv2.dilate(refined, kernel_dilate, iterations=1)
    
    # Filter by area
    contours, _ = cv2.findContours(refined, cv2.RETR_EXTERNAL, 
                                   cv2.CHAIN_APPROX_SIMPLE)
    filtered_mask = np.zeros_like(refined)
    
    for contour in contours:
        area = cv2.contourArea(contour)
        if CONFIG['min_motion_area'] < area < CONFIG['max_motion_area']:
            cv2.drawContours(filtered_mask, [contour], -1, 255, -1)
    
    return filtered_mask, contours

def apply_temporal_smoothing(mask, mask_history):
    """Average recent masks to smooth temporal jitter"""
    mask_history.append(mask.astype(float) / 255.0)
    
    # Average across window
    avg_mask = np.mean(mask_history, axis=0)
    
    # Threshold back to binary
    smoothed = (avg_mask > 0.3).astype(np.uint8) * 255
    
    return smoothed

def detect_roi_auto(cap, bg_subtractor):
    """Sample 50 frames to find consistent driver motion area"""
    ret, first_frame = cap.read()
    if not ret:
        return None
    
    motion_accumulator = np.zeros(first_frame.shape[:2], dtype=np.float32)
    
    # Accumulate motion over 50 frames
    motion_accumulator += bg_subtractor.apply(first_frame).astype(float) / 255.0
    
    for _ in range(49):
        ret, sample_frame = cap.read()
        if not ret:
            break
        fg_mask = bg_subtractor.apply(sample_frame)
        motion_accumulator += fg_mask.astype(float) / 255.0
    
    # Find bounding box of accumulated motion
    motion_map = (motion_accumulator > 10).astype(np.uint8) * 255
    contours, _ = cv2.findContours(motion_map, cv2.RETR_EXTERNAL, 
                                   cv2.CHAIN_APPROX_SIMPLE)
    
    if contours:
        largest_contour = max(contours, key=cv2.contourArea)
        x, y, w, h = cv2.boundingRect(largest_contour)
        
        # Add padding
        pad_x = int(w * CONFIG['roi_padding'])
        pad_y = int(h * CONFIG['roi_padding'])
        
        return (max(0, x - pad_x), max(0, y - pad_y),
                w + 2*pad_x, h + 2*pad_y)
    
    return None

CONFIG = {
    # Background subtraction
    'bg_history': 500,              # Learning frames
    'bg_var_threshold': 25,         # Sensitivity
    'detect_shadows': False,        # Ignore shadows
    'learning_rate': 0.001,         # Adaptation speed
    
    # Preprocessing
    'denoise_strength': 5,          # Noise reduction
    'apply_clahe': True,            # Contrast enhancement
    'clahe_clip_limit': 2.0,
    'clahe_grid_size': (8, 8),
    
    # Mask refinement
    'morphology_iterations': 2,     # Cleanup passes
    'open_kernel_size': (3, 3),     # Noise removal
    'close_kernel_size': (9, 9),    # Hole filling
    'dilate_kernel_size': (5, 5),   # Mask expansion
    
    # Motion filtering
    'min_motion_area': 200,         # Minimum pixels
    'max_motion_area': 50000,       # Maximum pixels
    'temporal_smoothing': True,     # Temporal filter
    'smooth_window': 5,             # Frame window
    
    # ROI
    'use_roi': True,                # Enable ROI
    'roi_coords': None,             # Auto or (x,y,w,h)
    'roi_padding': 0.1,             # 10% padding
    
    # Visualization (4 modes)
    'output_mode': 'overlay',       # mask/overlay/side_by_side/heatmap
    'mask_color': (0, 255, 0),      # Green motion
    'overlay_alpha': 0.6,           # Transparency
    'show_contours': True,          # Boundaries
    'show_trails': True,            # Motion history
    'trail_length': 15,             # Trail frames
    
    # Output
    'show_preview': False,          # Live window (needs GUI OpenCV)
    'save_debug_frames': False,     # Individual frames
}

# High Quality: Best results, slower
CONFIG['denoise_strength'] = 7
CONFIG['morphology_iterations'] = 3
CONFIG['temporal_smoothing'] = True
CONFIG['smooth_window'] = 7

# Balanced: Good quality, moderate speed (default)
# Uses base CONFIG values

# Fast Preview: Lower quality, faster
CONFIG['denoise_strength'] = 3
CONFIG['morphology_iterations'] = 1
CONFIG['temporal_smoothing'] = False
CONFIG['learning_rate'] = 0.005

# ============================================================================
# Example: Compare Two F1 Car Images
# ============================================================================

# 1. Set configuration in Cell 2
CONFIG = {
    'use_roi': False,              # Set True to focus on specific area
    'remove_background': True,     # Remove background clutter
    'use_edge_detection': True,    # Detect texture changes (tire wear)
    'filter_text_regions': True,   # Ignore year labels
    'sensitivity': 0.01,           # Lower = more sensitive
    'gen_image': False,            # Use GUI to select images
}

# 2. Run cells in order (Cells 1-10)
# 3. View results: heatmaps, bounding boxes, change metrics

# For automated comparison:
# Modify Cell 3 to load specific images
img1 = cv2.imread('path/to/car_before.jpg')
img2 = cv2.imread('path/to/car_after.jpg')

CONFIG['use_roi'] = False
CONFIG['remove_background'] = True
CONFIG['use_edge_detection'] = False
CONFIG['filter_text_regions'] = True
CONFIG['sensitivity'] = 0.015

CONFIG['use_roi'] = True  # Select tire area in Cell 4
CONFIG['remove_background'] = True
CONFIG['use_edge_detection'] = True  # Detect texture changes
CONFIG['sensitivity'] = 0.01

CONFIG['use_roi'] = True  # Select common area
CONFIG['remove_background'] = True
CONFIG['use_edge_detection'] = False
CONFIG['sensitivity'] = 0.02

# ============================================================================
# Example: Track Race Cars and Detect Overtakes
# ============================================================================

# 1. Configure in Cell 2
CONFIG = {
    'video_path': None,            # None = GUI file dialog
    'use_webcam': False,           # True for webcam testing
    
    # Detection (adjust based on video resolution)
    'min_car_area': 500,           # Smaller for distant shots
    'max_car_area': 50000,         # Larger for close-ups
    'detection_roi': None,         # Optional: (x, y, w, h)
    
    # Tracking
    'max_track_age': 30,           # Keep tracks for 30 frames
    'min_track_confidence': 5,     # Require 5 frames to confirm
    
    # Overtake detection
    'overtake_cooldown': 60,       # 2 seconds @ 30fps
    'lateral_threshold': 30,       # Pixels of movement
    
    # Visualization
    'show_trails': True,           # Motion trails
    'show_speed_estimate': True,   # Velocity display
    'output_video': True,          # Save result
    'output_path': 'f1_race_analysis.mp4',
    
    # Analysis
    'save_telemetry': True,        # Export JSON data
    'telemetry_path': 'race_telemetry.json',
}

# 2. Run Cells 1-6 to process video
# 3. View results in Cell 7 (reports) and Cell 8 (plots)

CONFIG['min_car_area'] = 1000
CONFIG['max_car_area'] = 20000
CONFIG['lateral_threshold'] = 50
CONFIG['max_track_age'] = 20

CONFIG['min_car_area'] = 500
CONFIG['max_car_area'] = 30000
CONFIG['lateral_threshold'] = 30
CONFIG['max_track_age'] = 40

CONFIG['min_car_area'] = 2000
CONFIG['max_car_area'] = 50000
CONFIG['detection_roi'] = (100, 200, 1000, 400)  # Focus on pit lane

# ============================================================================
# Example: Track Driver Hand Movement
# ============================================================================

# 1. Choose quality preset in Cell 6
preset = "2"  # Balanced (default)
# preset = "1"  # High Quality (slower, best results)
# preset = "3"  # Fast Preview (faster, lower quality)

# 2. Choose visualization mode
viz_mode = "1"  # Overlay (green motion mask)
# viz_mode = "2"  # Heatmap (thermal-style)
# viz_mode = "3"  # Side-by-Side (comparison)
# viz_mode = "4"  # Mask Only (black & white)

# 3. Process video
INPUT_VIDEO = 'path/to/onboard_footage.mp4'
OUTPUT_VIDEO = 'onboard_motion_tracked.mp4'

success = process_driver_onboard(INPUT_VIDEO, OUTPUT_VIDEO)

# ============================================================================
# Fine-Tuning CONFIG for Specific Scenarios
# ============================================================================

# Scenario 1: High-Speed Cockpit Footage (Good Lighting)
CONFIG = {
    'bg_history': 300,              # Shorter history
    'bg_var_threshold': 30,         # Less sensitive
    'learning_rate': 0.005,         # Faster adaptation
    'denoise_strength': 3,          # Minimal denoising
    'apply_clahe': False,           # Good lighting already
    'temporal_smoothing': True,
    'smooth_window': 3,             # Less smoothing
    'use_roi': True,                # Focus on driver area
}

# Scenario 2: Night Race / Low Light
CONFIG = {
    'bg_history': 600,              # Longer learning
    'bg_var_threshold': 20,         # More sensitive
    'learning_rate': 0.001,         # Slow adaptation
    'denoise_strength': 7,          # Heavy denoising
    'apply_clahe': True,            # Enhance contrast
    'clahe_clip_limit': 3.0,        # Strong enhancement
    'temporal_smoothing': True,
    'smooth_window': 7,             # Heavy smoothing
}

# Scenario 3: Static Onboard (Training/Sim)
CONFIG = {
    'bg_history': 200,              # Very short
    'bg_var_threshold': 35,         # Less sensitive
    'learning_rate': 0.01,          # Very fast
    'temporal_smoothing': False,    # Not needed
    'use_roi': False,               # Whole frame
}

# ============================================================================
# Process Multiple Videos
# ============================================================================

import os
from glob import glob

input_folder = 'onboard_videos/'
output_folder = 'processed_videos/'

# Get all MP4 files
video_files = glob(os.path.join(input_folder, '*.mp4'))

for video_path in video_files:
    filename = os.path.basename(video_path)
    output_path = os.path.join(output_folder, f'tracked_{filename}')
    
    print(f"\n{'='*70}")
    print(f"Processing: {filename}")
    print('='*70)
    
    success = process_driver_onboard(video_path, output_path)
    
    if success:
        print(f"✅ Completed: {output_path}")
    else:
        print(f"❌ Failed: {filename}")

print("\n🎉 Batch processing complete!")

# Graceful GUI handling
if CONFIG['show_preview']:
    try:
        cv2.imshow('Preview', frame)
        key = cv2.waitKey(1) & 0xFF
    except cv2.error:
        CONFIG['show_preview'] = False
        print("⚠️ Preview disabled (OpenCV GUI not available)")

roi = detect_roi_auto(cap.get(0), bg_subtractor)  # ❌ Passes frame number (0.0)

# Fixed function signature
def detect_roi_auto(cap, bg_subtractor):
    """Accept VideoCapture object, read frames internally"""
    ret, first_frame = cap.read()  # ✅ Read actual frame
    if not ret:
        return None
    
    motion_accumulator = np.zeros(first_frame.shape[:2], dtype=np.float32)
    
    # Process 50 frames to find consistent motion area
    for _ in range(50):
        ret, sample_frame = cap.read()
        if not ret:
            break
        fg_mask = bg_subtractor.apply(sample_frame)
        motion_accumulator += fg_mask.astype(float) / 255.0
    
    # Find bounding box...

roi = detect_roi_auto(cap, bg_subtractor)  # ✅ Pass VideoCapture object

def apply_temporal_smoothing(mask, mask_history):
    """Average recent masks to smooth jitter"""
    mask_history.append(mask.astype(float) / 255.0)
    
    # Average across 5-7 frame window
    avg_mask = np.mean(mask_history, axis=0)
    
    # Threshold back to binary
    smoothed = (avg_mask > 0.3).astype(np.uint8) * 255
    
    return smoothed

# Match detections to existing tracks
for track in self.tracks.values():
    last_pos = track.get_current_position()
    
    # Find nearest detection within threshold
    for i, (position, bbox) in enumerate(detections):
        distance = np.linalg.norm(np.array(position) - np.array(last_pos))
        
        if distance < 100:  # Max matching distance
            track.update(position, bbox, frame_num)
            track.confidence += 1

def is_text_region(change):
    """Detect text-like regions by shape"""
    area = change['area']
    aspect_ratio = change['aspect_ratio']
    
    # Text characteristics: elongated, small-medium size
    is_elongated = aspect_ratio > 2.5 or aspect_ratio < 0.4
    is_small_medium = 100 < area < 5000
    
    return is_elongated and is_small_medium

# Filter out text regions
structural_changes = [c for c in changes if not is_text_region(c)]

fourcc = cv2.VideoWriter_fourcc(*'mp4v')
out = cv2.VideoWriter(output_path, fourcc, fps, (width, height))

# Efficient memory usage
mask_history = deque(maxlen=CONFIG['smooth_window'])  # Auto-drops old frames
motion_trails = deque(maxlen=CONFIG['trail_length'])   # Limited history

# Simple preprocessing with huge impact
lab = cv2.cvtColor(frame, cv2.COLOR_BGR2LAB)
l, a, b = cv2.split(lab)
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
l = clahe.apply(l)
# Result: Uniform lighting across entire video

# Single frame → Flickering, noisy mask
mask = bg_subtractor.apply(frame)

# 5-7 frame average → Smooth, professional result
mask_history.append(mask)
smoothed_mask = np.mean(mask_history, axis=0)

cv2.imshow('Preview', frame)  # ❌ Crashes on headless systems

if CONFIG['show_preview']:
    try:
        cv2.imshow('Preview', frame)
    except cv2.error:
        CONFIG['show_preview'] = False
        print("⚠️ Preview disabled (GUI not available)")

# User selects "Balanced" → 22 parameters auto-configured
# User selects "High Quality" → Different optimized values
# Power users can still override any parameter

def detect_roi(first_arg, bg_subtractor):
    # What is first_arg? Frame? VideoCapture? Frame number?
    ...

def detect_roi_auto(cap: cv2.VideoCapture, bg_subtractor) -> Optional[Tuple[int, int, int, int]]:
    """
    Auto-detect driver region by analyzing motion in first 50 frames.
    
    Args:
        cap: VideoCapture object (will read frames internally)
        bg_subtractor: Initialized MOG2 background subtractor
    
    Returns:
        (x, y, w, h) tuple or None if detection fails
    """