rover_brain.py

import time
import random
import threading
import argparse
import os
from logger import logger
from wirepod_bridge import WirePodBridge

# ── Gemma 4 brain swap ─────────────────────────────────────────────────────────
# Set GEMMA4=1 env var (or --gemma4 flag in main.py) to use Gemma 4 instead of LLaVA
_USE_GEMMA4 = os.getenv("GEMMA4", "0") == "1"
if _USE_GEMMA4:
    from gemma4_brain import Gemma4Brain, patch_all_models as _patch
    _patch()  # Upgrades mission_controller + learning_engine transparently
    AIBrain = Gemma4Brain  # Drop-in swap
    logger.log("GEMMA4", "🧠 Gemma 4 brain ACTIVE — LLaVA + Llama3.1 replaced")
else:
    from ai_brain import AIBrain

from mission_controller import MissionController
from voice_handler import VoiceHandler
from memory_bank import MemoryBank
from learning_engine import LearningEngine
from behavior_arbiter import BehaviorArbiter
from spatial_memory import SpatialMemory
from pet_profile import PetProfile
from boot_greeting import BootGreeting
from audio_listener import AudioListener

# States
MANUAL_CONTROL = "MANUAL_CONTROL"
PATROL = "PATROL"
AI_ROAMING = "AI_ROAMING"
REACTING = "REACTING"
RETURNING_HOME = "RETURNING_HOME"
LOW_BATTERY = "LOW_BATTERY"
CHARGING = "CHARGING"        # Sitting on charger, monitoring battery
MISSION = "MISSION"
OFFLINE_SENTRY = "OFFLINE_SENTRY"  # No Mac/Ollama — onboard-only behaviors

# Safety constants
PROXIMITY_DANGER_MM = 80     # Emergency stop if object this close
PROXIMITY_CAUTION_MM = 150   # Slow down / be cautious
AI_ANALYSIS_INTERVAL = 30    # Seconds between AI vision calls
REACTION_COOLDOWN = 25       # Seconds between verbal reactions

# Auto-wake thresholds
FULL_BATTERY_V = 4.05        # Battery considered full
RESUME_BATTERY_V = 3.90      # 80% — resume pending mission
IDLE_WAKE_SECONDS = 300      # 5 minutes idle on charger → wake up and explore

# Motor stall detection
STALL_SPEED_THRESHOLD = 5.0  # mm/s — if below this while driving, we're stalled
STALL_TICKS_REQUIRED = 2     # consecutive stall ticks before reacting

# Offline sentry
OLLAMA_HEALTH_INTERVAL = 60  # Seconds between Ollama health checks


class RoverBrain:
    """AI-powered autonomous behavior controller for Vector."""

    def __init__(self, bridge, auto_mode=False):
        self.bridge = bridge
        self.state = MANUAL_CONTROL
        self.running = True
        self.auto_mode = auto_mode
        self.ai_mode = False
        self.obstacle_detected = False
        self.lock = threading.Lock()

        # AI Brain
        self.memory_bank = MemoryBank()
        self.ai_brain = AIBrain(memory_bank=self.memory_bank)
        self.last_ai_analysis = None
        self._last_ai_time = 0
        self._camera_ready = False

        # Behavior Arbiter
        self.arbiter = BehaviorArbiter(bridge)

        # Spatial Memory
        self.spatial_memory = SpatialMemory()

        # Mission controller
        self.mission_controller = MissionController(bridge, self.ai_brain)

        # Learning engine
        self.learning_engine = LearningEngine(bridge, self.memory_bank, rover_brain=self)

        # Voice handler (with spatial memory for "find object" commands)
        self.voice_handler = VoiceHandler(bridge, rover_brain=self,
                                          spatial_memory=self.spatial_memory)

        # Battery thresholds
        self.low_battery_threshold = 3.6
        self.critical_battery_threshold = 3.5
        self._battery_voltage = 4.0

        # Auto-wake / charger management
        self._charger_idle_since = None   # When we started sitting idle on charger
        self._pending_mission = None      # Mission type to resume after charging
        self._was_on_mission = False       # Were we on a mission before low battery?
        self._pre_charge_state = None      # State before going to charger

        # Motor stall detection
        self._stall_counter = 0
        self._driving = False  # True when we've sent drive commands

        # Offline sentry
        self._last_health_check = 0
        self._pre_offline_state = None     # State to restore after reconnect

        # Pet profile + boot greeting
        self.pet_profile = PetProfile()
        self.boot_greeter = BootGreeting(bridge, self.pet_profile)

        logger.log("SYSTEM", "Rover Brain initialized")

    def run(self):
        logger.log("SYSTEM", "Rover Brain started")
        try:
            # Connect if not already connected
            if not self.bridge.connected:
                self.bridge.connect()

            # Register voice event handlers
            if self.bridge.robot:
                self.voice_handler.register_events(self.bridge.robot)
                logger.log("SYSTEM", "🎙️ Voice events registered")

                # Subscribe face events → arbiter
                self.bridge.subscribe_face_events(self.arbiter.on_face_detected)
                logger.log("SYSTEM", "👤 Face events → arbiter")
                
                # Set reconnect callback so events survive connection drops
                def _on_reconnect(new_robot):
                    self.voice_handler.register_events(new_robot)
                    self.bridge.subscribe_face_events(self.arbiter.on_face_detected)
                    logger.log("SYSTEM", "🎙️ Voice + face events re-registered after reconnect")
                self.bridge._on_reconnect = _on_reconnect

            # Start autonomous learning engine
            self.learning_engine.start()
            logger.log("SYSTEM", "🧠 Learning engine started")

            # Run boot greeting (hatching on first boot, personalized on return)
            try:
                self.boot_greeter.run_greeting()
            except Exception as e:
                logger.log("WARNING", f"Boot greeting failed: {e}")

            # Audio listener disabled — Wire-Pod handles voice via Vector's mic
            # Wire-Pod STT → /v1/chat/completions → OpenAI → TTS
            self.audio_listener = None

            if self.auto_mode:
                self.state = PATROL
                logger.log("AUTO", "Starting autonomous patrol mode")
            else:
                self.state = MANUAL_CONTROL
                logger.log("SYSTEM", "Waiting for manual control commands")
            
            # Check for saved mission to resume after charging
            if self.mission_controller.has_saved_mission():
                saved = self.mission_controller.load_saved_state()
                if saved:
                    self._was_on_mission = True
                    self._pending_mission = saved.get("mission_name")
                    self._pending_mission_type = saved.get("mission_name", "")
                    self._resume_saved = True  # Flag to use resume=True
                    logger.log("SYSTEM", f"📌 Found saved mission '{self._pending_mission}' — will resume after charging to 100%")
                    # If on charger, go to CHARGING state to wait for full battery
                    sensors = self.bridge.get_sensors()
                    if sensors.get('on_charger', False):
                        self.state = CHARGING
                        logger.log("CHARGER", "🔌 On charger — waiting for full battery to resume mission")

            while self.running:
                self._tick()
                time.sleep(1.5)

        except KeyboardInterrupt:
            print("\n🛑 Stopping...")
        except Exception as e:
            print(f"❌ Error: {e}")
            import traceback
            traceback.print_exc()
        finally:
            self.learning_engine.stop()
            self.bridge.stop()
            self.bridge.close()

    def _tick(self):
        with self.lock:
            current_state = self.state
            current_auto = self.auto_mode
            current_ai = self.ai_mode

        # Manual mode — just monitor battery
        if current_state == MANUAL_CONTROL:
            sensors = self.bridge.get_sensors()
            voltage = sensors.get('battery_voltage')
            if voltage:
                self._battery_voltage = voltage
            if voltage and voltage < self.critical_battery_threshold:
                logger.log("BATTERY", f"Battery critical in manual mode: {voltage:.2f}V")
            return

        # ----- Active modes below -----
        sensors = self.bridge.get_sensors()
        voltage = sensors.get('battery_voltage')
        if voltage:
            self._battery_voltage = voltage
        proximity_mm = sensors.get('proximity_mm', 9999)

        # ─── CHARGING STATE: auto-wake + mission resume ─────
        if current_state == CHARGING:
            self._charging_behavior(voltage)
            return

        # Battery management (applies to all active modes)
        if voltage and voltage < self.critical_battery_threshold:
            logger.log("BATTERY", f"Critical ({voltage:.2f}V)! Returning home.")
            self._save_pre_charge_state(current_state)
            with self.lock:
                self.state = RETURNING_HOME
                current_state = RETURNING_HOME
        elif voltage and voltage < self.low_battery_threshold and current_state in (PATROL, AI_ROAMING, MISSION):
            logger.log("BATTERY", f"Low ({voltage:.2f}V). Returning home.")
            self._save_pre_charge_state(current_state)
            with self.lock:
                self.state = RETURNING_HOME
                current_state = RETURNING_HOME

        # ─── Arbiter: safety checks (proximity, stall, cliff) ─────
        if current_state in (PATROL, AI_ROAMING, MISSION, OFFLINE_SENTRY):
            # Proximity check via arbiter
            if self.arbiter.check_proximity(proximity_mm, PROXIMITY_DANGER_MM, PROXIMITY_CAUTION_MM):
                logger.log("WARNING", f"Proximity danger! {proximity_mm}mm — arbiter handling")
                action = self.arbiter.arbitrate()
                self.arbiter.execute(action)
                self._stall_counter = 0
                return

            # Motor stall check via arbiter
            if self._driving:
                speeds = self.bridge.get_wheel_speeds()
                self._stall_counter = self.arbiter.check_stall(
                    speeds, self._driving,
                    STALL_SPEED_THRESHOLD, self._stall_counter, STALL_TICKS_REQUIRED
                )
                if self._stall_counter == 0 and self.arbiter._candidates:
                    action = self.arbiter.arbitrate()
                    self.arbiter.execute(action)
                    self._driving = False
                    logger.log("MOVE", "↪ Arbiter recovered from stall")
                    return

        # ─── Held detection — stop everything ───────────────
        if self.bridge.is_being_held():
            logger.log("INFO", "🤚 Being held! Stopping motors.")
            self.bridge.stop()
            self._driving = False
            return

        # ─── Periodic Ollama health check ────────────────────
        now = time.time()
        if current_state in (AI_ROAMING, PATROL) and (now - self._last_health_check) >= OLLAMA_HEALTH_INTERVAL:
            self._last_health_check = now
            if not self.bridge.check_ollama_health():
                logger.log("WARNING", "⚠️ Ollama unreachable — entering offline sentry mode")
                self._pre_offline_state = current_state
                with self.lock:
                    self.state = OFFLINE_SENTRY
                self.bridge.say_text("Lost connection to brain. Sentry mode.")
                time.sleep(2)
                return

        # State dispatch + eye colors
        if current_state == AI_ROAMING:
            self.bridge.set_eye_state('exploring')
            self._ai_roaming_behavior(proximity_mm)
        elif current_state == PATROL:
            self.bridge.set_eye_state('idle')
            self._patrol_behavior()
        elif current_state == RETURNING_HOME:
            self.bridge.set_eye_state('charging')
            self._return_to_charger()
        elif current_state == MISSION:
            self.bridge.set_eye_state('mission')
            self.mission_controller.step()
        elif current_state == OFFLINE_SENTRY:
            self._offline_sentry_behavior()
        elif current_state == LOW_BATTERY:
            self.bridge.stop()
            logger.log("BATTERY", "Idle on charger")

    # ─────────────────────────────────────────────
    # CHARGER / AUTO-WAKE / MISSION RESUME
    # ─────────────────────────────────────────────

    def _save_pre_charge_state(self, state):
        """Remember what we were doing before heading to charger."""
        if state == MISSION:
            self._was_on_mission = True
            if self.mission_controller.current_mission:
                mission = self.mission_controller.current_mission
                self._pending_mission = mission.name
                # Track mission type for battery threshold decisions
                self._pending_mission_type = getattr(mission, 'name', '')
                logger.log("MISSION", f"📌 Saving mission '{self._pending_mission}' to resume after charge")
        elif state == AI_ROAMING:
            self._pre_charge_state = AI_ROAMING
        else:
            self._pre_charge_state = None

    def _charging_behavior(self, voltage):
        """Smart charger behavior: wait for appropriate charge level, then resume.
        
        Space Mapping requires 100% charge (FULL_BATTERY_V).
        Other missions resume at 80% (RESUME_BATTERY_V).
        Voice interaction always works while charging.
        """
        self.bridge.set_eye_state('charging')

        # Track how long we've been idle on charger
        if self._charger_idle_since is None:
            self._charger_idle_since = time.time()
            logger.log("CHARGER", f"🔌 Charging... ({voltage:.2f}V)")

        idle_duration = time.time() - self._charger_idle_since

        # 1. Resume pending mission — threshold depends on mission type
        if self._was_on_mission and voltage:
            is_space_mapping = getattr(self, '_pending_mission_type', '') == 'Space Mapping'
            required_v = FULL_BATTERY_V if is_space_mapping else RESUME_BATTERY_V
            label = '100%' if is_space_mapping else '80%'
            
            if voltage >= required_v:
                logger.log("MISSION", f"🔋 Battery at {voltage:.2f}V (≥{label}) — resuming mission!")
                self._charger_idle_since = None
                self._was_on_mission = False
                self.bridge.say_text(f"Fully charged. Back to my mission.")
                time.sleep(2)
                self.bridge.drive_off_charger()
                time.sleep(3)
                # Drive forward for turning clearance
                self.bridge.drive_wheels(60, 60)
                time.sleep(2)
                self.bridge.stop()
                with self.lock:
                    self.state = MISSION
                    self.auto_mode = True
                
                # Use resume=True if we have saved state on disk
                use_resume = getattr(self, '_resume_saved', False)
                if use_resume:
                    self.mission_controller.start_mission(resume=True)
                    self._resume_saved = False
                elif self._pending_mission:
                    self.mission_controller.start_mission(self._pending_mission)
                else:
                    self.mission_controller.start_mission()
                self._pending_mission = None
                self._pending_mission_type = None
                return
            else:
                # Log charging progress periodically
                if int(idle_duration) % 60 == 0:
                    pct = max(0, min(100, int((voltage - 3.5) / (4.1 - 3.5) * 100)))
                    logger.log("CHARGER", f"🔋 Charging for {label} — {voltage:.2f}V (~{pct}%)")
                return

        # 2. Auto-scan if coverage gaps exist and battery is full
        from pathlib import Path
        if voltage and voltage >= FULL_BATTERY_V and idle_duration >= IDLE_WAKE_SECONDS:
            auto_flag = Path(__file__).parent / "auto_scan_pending.json"
            if auto_flag.exists():
                try:
                    import json as _json
                    flag_data = _json.loads(auto_flag.read_text())
                    if flag_data.get("pending"):
                        gaps = flag_data.get("gaps", [])
                        logger.log("AUTO", f"🔄 Auto-scan triggered! Coverage gaps: {gaps}")
                        auto_flag.unlink()  # Clear the flag
                        self._charger_idle_since = None
                        self.bridge.say_text("Charged up. Scanning the gaps I missed.")
                        time.sleep(2)
                        # Start scan sprint
                        if hasattr(self, 'room_scanner'):
                            # Disabled per user request
                            # self.room_scanner.start_sprint()
                            pass
                        return
                except Exception as e:
                    logger.log("WARNING", f"Auto-scan check failed: {e}")

        # 3. Auto-wake to explore when fully charged + idle for 5 minutes
        if voltage and voltage >= FULL_BATTERY_V and idle_duration >= IDLE_WAKE_SECONDS:
            logger.log("AUTO", f"⚡ Full battery ({voltage:.2f}V) + idle {idle_duration:.0f}s — time to explore!")
            self._charger_idle_since = None
            phrases = [
                "Fully charged! Time to explore.",
                "I'm bored. Let's go see things.",
                "Adventure time! Battery full.",
                "Can't sit still. Off I go!",
            ]
            self.bridge.say_text(random.choice(phrases))
            time.sleep(2)
            self.bridge.drive_off_charger()
            time.sleep(2)
            # Start AI roaming
            with self.lock:
                self.ai_mode = True
                self.auto_mode = True
                self.state = AI_ROAMING
                self.ai_brain.enabled = True
            return

        # 3. Log charging progress occasionally
        if int(idle_duration) % 60 == 0 and int(idle_duration) > 0 and voltage:
            logger.log("CHARGER", f"🔋 Charging: {voltage:.2f}V (idle {idle_duration:.0f}s)")

    # ─────────────────────────────────────────────
    # MOTOR STALL DETECTION
    # ─────────────────────────────────────────────

    def _check_motor_stall(self):
        """Detect if motors are commanded but wheels aren't moving (hit something)."""
        speeds = self.bridge.get_wheel_speeds()
        left_speed = abs(speeds['left'])
        right_speed = abs(speeds['right'])
        avg_speed = (left_speed + right_speed) / 2.0

        if avg_speed < STALL_SPEED_THRESHOLD:
            self._stall_counter += 1
            if self._stall_counter >= STALL_TICKS_REQUIRED:
                logger.log("WARNING", f"🔧 Motor stall detected! Wheels at {avg_speed:.1f}mm/s — hit something")
                self.bridge.set_eye_state('danger')
                self.bridge.stop()
                time.sleep(0.2)
                # Back up
                self.bridge.drive_wheels(-80, -80)
                time.sleep(1.0)
                self.bridge.stop()
                # Turn away randomly
                turn = random.choice([-1, 1])
                self.bridge.drive_wheels(turn * 60, turn * -60)
                time.sleep(random.uniform(0.8, 1.5))
                self.bridge.stop()
                self._stall_counter = 0
                self._driving = False
                logger.log("MOVE", "↪ Recovered from stall")
        else:
            self._stall_counter = 0

    # ─────────────────────────────────────────────
    # AI ROAMING — The main new behavior
    # ─────────────────────────────────────────────

    def _ai_roaming_behavior(self, proximity_mm):
        """AI-driven exploration using LLaVA vision + Llama 3 personality.
        
        Uses the BehaviorArbiter to merge AI decisions with onboard events.
        """

        # Initialize camera on first AI tick
        if not self._camera_ready:
            logger.log("SYSTEM", "🧠 Initializing AI roaming...")
            success = self.bridge.enable_camera_feed()
            if success:
                self._camera_ready = True
                logger.log("SUCCESS", "Camera ready for AI vision")
                self.bridge.say_text("AI mode activated! Let me look around.")
                time.sleep(2)
                logger.log("MOVE", "👀 Looking around to get bearings...")
                self.bridge.look_around()
            else:
                logger.log("ERROR", "Camera init failed — falling back to basic patrol")
                with self.lock:
                    self.state = PATROL
                return

        # Decide if we should run AI analysis this tick
        now = time.time()
        should_analyze = (now - self._last_ai_time) >= AI_ANALYSIS_INTERVAL

        if should_analyze:
            frame = self.bridge.get_camera_image()
            if frame:
                logger.log("VISION", "🧠 Thinking...")
                self.bridge.start_thinking_indicator()

                result = self.ai_brain.process_frame(frame)

                self.bridge.stop_thinking_indicator()

                self.last_ai_analysis = result
                self._last_ai_time = now

                # Submit AI decision to arbiter (handles both nav + reactions)
                self.arbiter.translate_ai_decision(result)

            else:
                # No frame — submit idle forward
                self.arbiter.submit_idle("no_frame_forward",
                                        motor_command=("forward", {"speed": 30, "duration": 1.0}))
        else:
            # Between AI analyses, continue last decision at reduced speed
            if self.last_ai_analysis:
                self.arbiter.translate_ai_decision(self.last_ai_analysis, slow=True)
            else:
                self.arbiter.submit_idle("drift_forward",
                                        motor_command=("forward", {"speed": 30, "duration": 0.5}))

        # Arbitrate: pick the highest-priority action this tick
        action = self.arbiter.arbitrate()
        if action:
            # If it's a speech-only onboard action (face reaction), handle state
            if action.speech and action.source == "onboard" and not action.motor_command:
                with self.lock:
                    self.state = REACTING
                self.bridge.stop()
                self.arbiter.execute(action)
                with self.lock:
                    self.state = AI_ROAMING
            else:
                if action.motor_command and action.motor_command[0] in ("forward", "backup_and_turn"):
                    self._driving = True
                self.arbiter.execute(action)
                self._driving = False

    def _execute_nav_decision(self, analysis, proximity_mm, slow=False):
        """Execute a navigation decision from AI analysis."""
        decision = analysis.get("nav_decision", "STOP")
        danger = analysis.get("danger_level", 1)
        speed = 40 if slow else 70

        # Safety override: reduce speed near obstacles
        if proximity_mm < PROXIMITY_CAUTION_MM:
            speed = max(20, speed // 2)
            logger.log("INFO", f"Proximity caution ({proximity_mm}mm) — reducing speed")

        # High danger — stop immediately
        if danger >= 3:
            logger.log("WARNING", f"AI danger level {danger} — STOP")
            self.bridge.stop()
            time.sleep(0.5)
            self.bridge.drive_wheels(-50, -50)
            time.sleep(0.8)
            self.bridge.stop()
            return

        if decision == "FORWARD":
            self._driving = True
            self.bridge.drive_wheels(speed, speed)
            time.sleep(1.2 if not slow else 0.6)
            self.bridge.stop()
            self._driving = False

        elif decision == "TURN_LEFT":
            logger.log("MOVE", "↰ Turning left")
            self.bridge.drive_wheels(-speed // 2, speed // 2)
            time.sleep(random.uniform(0.6, 1.2))
            self.bridge.stop()
            time.sleep(0.2)

        elif decision == "TURN_RIGHT":
            logger.log("MOVE", "↱ Turning right")
            self.bridge.drive_wheels(speed // 2, -speed // 2)
            time.sleep(random.uniform(0.6, 1.2))
            self.bridge.stop()
            time.sleep(0.2)

        elif decision == "BACKUP":
            logger.log("MOVE", "↩ Backing up")
            self.bridge.drive_wheels(-speed, -speed)
            time.sleep(0.8)
            self.bridge.stop()
            turn_dir = random.choice([-1, 1])
            self.bridge.drive_wheels(turn_dir * 40, turn_dir * -40)
            time.sleep(random.uniform(0.5, 1.0))
            self.bridge.stop()

        elif decision == "STOP":
            self.bridge.stop()
            self._driving = False
            time.sleep(0.5)

    # ─────────────────────────────────────────────
    # BASIC PATROL — Random wander (no AI)
    # ─────────────────────────────────────────────

    def _patrol_behavior(self):
        """Random wander with basic obstacle avoidance."""
        if self.obstacle_detected:
            logger.log("MOVE", "Obstacle detected! Backing up and turning...")
            self.bridge.drive_wheels(-50, -50)
            self._driving = True
            time.sleep(1.0)
            self.bridge.stop()

            turn_direction = random.choice([-1, 1])
            self.bridge.drive_wheels(turn_direction * -50, turn_direction * 50)
            time.sleep(1.5)
            self.bridge.stop()
            self._driving = False

            self.obstacle_detected = False
            return

        # Normal patrol: turn then drive
        turn_duration = random.uniform(0.5, 1.5)
        turn_direction = random.choice([-1, 1])

        self.bridge.drive_wheels(turn_direction * -30, turn_direction * 30)
        time.sleep(turn_duration)
        self.bridge.stop()
        time.sleep(0.5)

        drive_duration = random.uniform(1.0, 3.0)
        self.bridge.drive_wheels(50, 50)
        self._driving = True
        time.sleep(drive_duration)
        self.bridge.stop()
        self._driving = False

    # ─────────────────────────────────────────────
    # OFFLINE SENTRY — onboard-only when no Mac
    # ─────────────────────────────────────────────

    def _offline_sentry_behavior(self):
        """Onboard-only fallback when Ollama/WiFi is unreachable.
        
        Uses onboard face detection, basic proximity avoidance,
        and slow patrol. Periodically retries health check.
        """
        self.bridge.set_eye_state('danger')  # Red eyes = degraded

        # Periodically retry Ollama
        now = time.time()
        if (now - self._last_health_check) >= OLLAMA_HEALTH_INTERVAL:
            self._last_health_check = now
            if self.bridge.check_ollama_health():
                logger.log("SUCCESS", "🧠 Brain reconnected!")
                self.bridge.say_text("Brain reconnected! Back online.")
                time.sleep(2)
                restore = self._pre_offline_state or AI_ROAMING
                with self.lock:
                    self.state = restore
                self._pre_offline_state = None
                return

        # Slow onboard patrol: turn then drive, proximity-only avoidance
        sensors = self.bridge.get_sensors()
        proximity_mm = sensors.get('proximity_mm', 9999)

        if proximity_mm < PROXIMITY_CAUTION_MM:
            self.bridge.drive_wheels(-40, -40)
            time.sleep(0.8)
            self.bridge.stop()
            turn = random.choice([-1, 1])
            self.bridge.drive_wheels(turn * 30, turn * -30)
            time.sleep(random.uniform(0.6, 1.2))
            self.bridge.stop()
        else:
            # Slow wander
            self.bridge.drive_wheels(30, 30)
            time.sleep(random.uniform(1.0, 2.0))
            self.bridge.stop()
            # Occasional turn
            if random.random() < 0.3:
                turn = random.choice([-1, 1])
                self.bridge.drive_wheels(turn * 25, turn * -25)
                time.sleep(random.uniform(0.4, 0.8))
                self.bridge.stop()

        # Try onboard face detection
        try:
            self.bridge.find_faces()
        except Exception:
            pass

    # ─────────────────────────────────────────────
    # CHARGER / MODES
    # ─────────────────────────────────────────────

    def _return_to_charger(self):
        """Navigate back to charging station using Vector's built-in charger seek."""
        logger.log("CHARGER", "Driving to charger using built-in behavior...")

        try:
            if self.bridge.robot:
                self.bridge.set_eye_state('charging')
                self.bridge.say_text("Heading home to charge")
                time.sleep(2)
                self.bridge.robot.behavior.drive_on_charger()
                logger.log("SUCCESS", "Docked on charger!")
                with self.lock:
                    self.state = CHARGING
                self._charger_idle_since = None  # Reset for fresh idle tracking
            else:
                logger.log("ERROR", "No robot connection for charger return")
                with self.lock:
                    self.state = MANUAL_CONTROL
        except Exception as e:
            logger.log("ERROR", f"Charger docking failed: {e}")
            self.bridge.say_text("I cant find my charger")
            self.bridge.stop()
            with self.lock:
                self.state = MANUAL_CONTROL

    def set_auto_mode(self, enabled):
        """Enable/disable basic autonomous mode."""
        with self.lock:
            self.auto_mode = enabled
            if enabled:
                self.state = PATROL
                logger.log("AUTO", "Autonomous patrol mode enabled")
            else:
                self.ai_mode = False
                self.state = MANUAL_CONTROL
                self.bridge.stop()
                logger.log("SYSTEM", "Manual control mode")

    def set_ai_mode(self, enabled):
        """Enable/disable AI-powered roaming (LLaVA + Llama 3)."""
        with self.lock:
            self.ai_mode = enabled
            self.auto_mode = enabled  # AI mode implies auto
            if enabled:
                self.state = AI_ROAMING
                self.ai_brain.enabled = True
                self.bridge.set_eye_state('exploring')
                logger.log("AUTO", "🧠 AI roaming mode activated!")
            else:
                self.state = MANUAL_CONTROL
                self.ai_brain.enabled = False
                self._camera_ready = False
                self._driving = False
                self.bridge.stop()
                self.bridge.set_eye_state('idle')
                logger.log("SYSTEM", "AI mode deactivated")

    def set_mission_mode(self, enabled, mission_type=None):
        """Enable/disable mission-based behavior."""
        with self.lock:
            if enabled:
                self.auto_mode = True
                self.ai_mode = False
                self.state = MISSION
                # Enable camera for vision-based missions
                if not self._camera_ready:
                    success = self.bridge.enable_camera_feed()
                    if success:
                        self._camera_ready = True
                        logger.log("MISSION", "Camera enabled for missions")
                self.bridge.set_eye_state('mission')
                self.mission_controller.start_mission(mission_type)
                logger.log("MISSION", f"🎯 Mission mode activated: {mission_type or 'auto-cycle'}")
            else:
                self.state = MANUAL_CONTROL
                self.auto_mode = False
                self.mission_controller.stop()
                self.bridge.stop()
                self.bridge.set_eye_state('idle')
                logger.log("SYSTEM", "Mission mode deactivated")


if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--ip", required=True, help="Robot IP Address")
    parser.add_argument("--guid", required=True, help="Robot GUID")
    parser.add_argument("--cert", default="vector.pem", help="Path to robot certificate")
    parser.add_argument("--manual", action="store_true", help="Start in manual control mode")
    parser.add_argument("--ai", action="store_true", help="Start in AI roaming mode")
    args = parser.parse_args()

    bridge = WirePodBridge(args.ip, args.guid, args.cert)
    brain = RoverBrain(bridge, auto_mode=not args.manual)
    if args.ai:
        brain.set_ai_mode(True)
    elif args.mission:
        brain.set_mission_mode(True, args.mission)
    brain.run()