#!/usr/bin/env python3

# =============================================================================
# Animatronic Servo Animation Controller

# Controls 7 MG90S servos via Pololu Mini Maestro 12ch using Logitech Extreme 3D Pro joystick throttle
# Records, combines, and exports animations to Maestro sequence scripts

# To start, open Command Prompt in the folder containing this file and enter "py servo_animator.py".
# =============================================================================
# Release Version: 2025.1.1
# 2025.1.0 - Initial Release
# 2025.1.1 - added code snippets for simultaneous playback.

# Initial code generated by Claude Sonnet 4 on 2025-08-31
# New code snippets for 2025.1.1 features generated 2025-09-19
# =============================================================================

import pygame
import serial
import time
import json
import os
from typing import List, Dict, Optional, Tuple
from dataclasses import dataclass, asdict
from datetime import datetime

# =============================================================================
# CONFIGURATION VARIABLES - Easy to modify later
# =============================================================================

# Hardware configuration
MAESTRO_DEVICE = "COM4"  # Change this to your Maestro's serial port (Linux: /dev/ttyACM0, Mac: /dev/cu.usbmodem)
MAESTRO_BAUDRATE = 9600  # Standard Maestro baud rate
SERVO_CHANNELS = [0, 2, 3, 4, 5, 6, 7]  # Maestro channels for your 7 servos (0-indexed)

# Timing configuration
COUNTDOWN_TIME = 10.0  # Seconds to count down before recording starts
RECORDING_SAMPLE_RATE = 20  # Hz - how often to record servo positions
PLAYBACK_SPEED = 1.0  # Multiplier for playback speed (1.0 = normal, 0.5 = half speed)

# Servo limits (in microseconds) - Adjust these for your robot's mechanical limits 
DEFAULT_SERVO_LIMITS = {
    "min_position": 600,   # Minimum servo position in microseconds
    "max_position": 2400   # Maximum servo position in microseconds
}

# Background playback configuration
BACKGROUND_PLAYBACK_ENABLED = True  # Set to False to disable background playback feature
BACKGROUND_PLAYBACK_DEFAULT = []    # Default servos for background playback (empty list = none)

# HARD CODED Servo limits (in microseconds) - Customize for your robot's mechanical limits. Number is Servo Channel, make sure it matches SERVO_CHALLENS in line 26 above.
SERVO_LIMITS = {
    0: {"min_position": 608, "max_position": 2496},   # Servo 1 (Channel 0) - Head pan
    2: {"min_position": 608, "max_position": 2496},   # Servo 2 (Channel 1) - Head tilt  
    3: {"min_position": 1792, "max_position": 2352},   # Servo 3 (Channel 2) - Left arm
    4: {"min_position": 1344, "max_position": 2496},   # Servo 4 (Channel 3) - Right arm
    5: {"min_position": 1808, "max_position": 2496},   # Servo 5 (Channel 4) - Body twist
    6: {"min_position": 1552, "max_position": 2256},   # Servo 6 (Channel 5) - Left hand
    7: {"min_position": 960, "max_position": 1952},   # Servo 7 (Channel 6) - Right hand
}



# Joystick configuration
JOYSTICK_DEADZONE = 0.05  # Ignore small joystick movements below this threshold
THROTTLE_AXIS = 3  # Logitech Extreme 3D Pro throttle axis (axis 4, but it indexes from 0, so it's 3)
RECORD_BUTTON = 0  # Button to start/stop recording (trigger button)
PLAY_BUTTON = 1  # Button to play back animation (thumb button)
SERVO_SELECT_BUTTONS = [4, 5, 6, 7, 8, 9, 10]  # Buttons to select servos 1-7

# File configuration
ANIMATIONS_FOLDER = "animations"  # Folder to store animation files
PROJECT_FILE_EXTENSION = ".json"  # File extension for project files
MAESTRO_SCRIPT_EXTENSION = ".txt"  # File extension for Maestro sequence scripts

# =============================================================================
# DATA STRUCTURES
# =============================================================================

@dataclass
class TimestampedPosition:
    """Stores a servo position at a specific time"""
    timestamp: float    # Time in seconds from start of recording
    position: int      # Servo position in microseconds (600-2400 typically)

@dataclass
class ServoLimits:
    """Stores the movement limits for a servo"""
    min_position: int  # Minimum allowed position in microseconds
    max_position: int  # Maximum allowed position in microseconds

@dataclass
class AnimationTrack:
    """Stores all recorded data for one servo"""
    servo_channel: int                    # Which servo channel this track controls
    positions: List[TimestampedPosition]  # All recorded positions with timestamps
    limits: ServoLimits                   # Movement limits for this servo

@dataclass
class AnimationProject:
    """Stores a complete animation project with multiple servo tracks"""
    name: str                             # Project name
    created_date: str                     # When project was created
    duration: float                       # Total duration in seconds
    tracks: Dict[int, AnimationTrack]     # Dictionary mapping servo channel to track data

# =============================================================================
# MAESTRO COMMUNICATION CLASS
# =============================================================================

class MaestroController:
    """Handles all communication with the Pololu Mini Maestro servo controller"""
    
    def __init__(self, device: str, baudrate: int = 9600):
        """
        Initialize connection to Maestro
        Args:
            device: Serial port name (e.g., "COM3" or "/dev/ttyACM0")
            baudrate: Communication speed (9600 is standard for Maestro)
        """
        self.device = device
        self.baudrate = baudrate
        self.serial_connection = None
        self.is_connected = False
        
    def connect(self) -> bool:
        """
        Establish serial connection to Maestro
        Returns:
            True if connection successful, False otherwise
        """
        try:
            # Open serial connection
            self.serial_connection = serial.Serial(self.device, self.baudrate, timeout=1)
            self.is_connected = True
            print(f"✓ Connected to Maestro on {self.device}")
            return True
        except Exception as e:
            print(f"✗ Failed to connect to Maestro: {e}")
            self.is_connected = False
            return False
    
    def disconnect(self):
        """Close the serial connection to Maestro"""
        if self.serial_connection:
            self.serial_connection.close()
            self.is_connected = False
            print("✓ Disconnected from Maestro")
    
    def set_servo_position(self, channel: int, position_us: int):
        """
        Set a servo to a specific position
        Args:
            channel: Servo channel number (0-11 for Mini Maestro 12ch)
            position_us: Position in microseconds (typically 600-2400)
        """
        if not self.is_connected:
            print("✗ Maestro not connected")
            return
        
        try:
            # Convert microseconds to quarter-microseconds (Maestro's native unit)
            target = position_us * 4
            
            # Send Maestro "Set Target" command
            # Command format: 0x84, channel, target_low_byte, target_high_byte
            command = bytes([
                0x84,                    # Set Target command
                channel,                 # Servo channel
                target & 0x7F,          # Target low 7 bits
                (target >> 7) & 0x7F    # Target high 7 bits
            ])
            
            self.serial_connection.write(command)
            
        except Exception as e:
            print(f"✗ Error setting servo position: {e}")

# =============================================================================
# JOYSTICK INPUT CLASS
# =============================================================================

class JoystickController:
    """Handles input from Logitech Extreme 3D Pro joystick"""
    
    def __init__(self):
        """Initialize pygame and joystick"""
        pygame.init()
        pygame.joystick.init()
        
        self.joystick = None
        self.is_connected = False
        
        # Button press tracking (to detect button press events, not just held buttons)
        self.previous_button_states = {}
        
    def connect(self) -> bool:
        """
        Find and connect to the first available joystick
        Returns:
            True if joystick found and connected, False otherwise
        """
        if pygame.joystick.get_count() == 0:
            print("✗ No joystick found")
            return False
        
        try:
            # Connect to first joystick
            self.joystick = pygame.joystick.Joystick(0)
            self.joystick.init()
            self.is_connected = True
            
            # Initialize button state tracking
            self.previous_button_states = {i: False for i in range(self.joystick.get_numbuttons())}
            
            print(f"✓ Connected to joystick: {self.joystick.get_name()}")
            return True
            
        except Exception as e:
            print(f"✗ Failed to connect to joystick: {e}")
            return False
    
    def update(self):
        """Update joystick state - call this every frame"""
        if self.is_connected:
            pygame.event.pump()  # Update pygame event system
    
    def get_throttle_position(self) -> float:
        """
        Get throttle position from joystick
        Returns:
            Throttle position as float between -1.0 and 1.0
        """
        if not self.is_connected:
            return 0.0
        
        # Get raw throttle axis value
        raw_value = self.joystick.get_axis(THROTTLE_AXIS)
        
        # Apply deadzone to ignore small movements
        if abs(raw_value) < JOYSTICK_DEADZONE:
            return 0.0
        
        return raw_value
    
    def is_button_pressed(self, button_number: int) -> bool:
        """
        Check if a button was just pressed (not held down)
        Args:
            button_number: Button index to check
        Returns:
            True if button was just pressed this frame, False otherwise
        """
        if not self.is_connected or button_number >= self.joystick.get_numbuttons():
            return False
        
        # Get current button state
        current_state = self.joystick.get_button(button_number)
        previous_state = self.previous_button_states.get(button_number, False)
        
        # Update button state tracking
        self.previous_button_states[button_number] = current_state
        
        # Button was "pressed" if it's currently down but wasn't down last frame
        return current_state and not previous_state

# =============================================================================
# ANIMATION RECORDING AND PLAYBACK
# =============================================================================

class AnimationRecorder:
    """Handles recording, playback, and management of servo animations"""
    
    def __init__(self, maestro: MaestroController, joystick: JoystickController):
        """
        Initialize the animation recorder
        Args:
            maestro: Connected MaestroController instance
            joystick: Connected JoystickController instance
        """
        self.maestro = maestro
        self.joystick = joystick
        
        # Recording state
        self.is_recording = False
        self.recording_start_time = 0.0
        self.current_recording = []  # List of TimestampedPosition objects
        
        # Playback state
        self.is_playing = False
        self.playback_start_time = 0.0
        self.playback_animation = None
        
        # Current project
        self.current_project = AnimationProject(
            name="Untitled Animation",
            created_date=datetime.now().isoformat(),
            duration=0.0,
            tracks={}
        )
        
        # Servo configuration
        self.active_servo_channel = 0  # Which servo we're currently controlling
        self.servo_limits = {channel: ServoLimits(**DEFAULT_SERVO_LIMITS) for channel in SERVO_CHANNELS}
        
        # Background playback state
        self.background_servos = BACKGROUND_PLAYBACK_DEFAULT.copy()  # List of servo channels for background playback
        self.background_playback_active = False
        self.background_start_time = 0.0

        # Ensure animations folder exists
        if not os.path.exists(ANIMATIONS_FOLDER):
            os.makedirs(ANIMATIONS_FOLDER)
            print(f"✓ Created animations folder: {ANIMATIONS_FOLDER}")
    
    def set_servo_limits(self, channel: int, min_pos: int, max_pos: int):
        """
        Set movement limits for a specific servo
        Args:
            channel: Servo channel to set limits for
            min_pos: Minimum position in microseconds
            max_pos: Maximum position in microseconds
        """
        if channel in SERVO_CHANNELS:
            self.servo_limits[channel] = ServoLimits(min_pos, max_pos)
            print(f"✓ Set servo {channel + 1} limits: {min_pos}-{max_pos} μs")
    
    def constrain_position(self, channel: int, position: int) -> int:
        """
        Ensure a servo position is within the set limits
        Args:
            channel: Servo channel
            position: Desired position in microseconds
        Returns:
            Position constrained to servo limits
        """
        limits = self.servo_limits.get(channel, ServoLimits(**DEFAULT_SERVO_LIMITS))
        return max(limits.min_position, min(limits.max_position, position))
    
    def throttle_to_servo_position(self, throttle_value: float, channel: int) -> int:
        """
        Convert joystick throttle position to servo microseconds
        Args:
            throttle_value: Throttle position (-1.0 to 1.0)
            channel: Servo channel (for limit lookup)
        Returns:
            Servo position in microseconds
        """
        limits = self.servo_limits.get(channel, ServoLimits(**DEFAULT_SERVO_LIMITS))
        
        # Map throttle range (-1.0 to 1.0) to servo range (min to max)
        # Note: -1.0 throttle = max position, 1.0 throttle = min position (typical throttle behavior)
        normalized = (-throttle_value + 1.0) / 2.0  # Convert -1:1 to 0:1, inverted
        position = int(limits.min_position + normalized * (limits.max_position - limits.min_position))
        
        return self.constrain_position(channel, position)
    
    def countdown_before_recording(self):
        """Display a countdown timer before starting recording"""
        print(f"\n🎬 Starting recording countdown for Servo {self.active_servo_channel + 1}...")
        
        for i in range(int(COUNTDOWN_TIME), 0, -1):
            print(f"⏰ Recording starts in {i}...")
            time.sleep(1.0)
        
        print("🔴 RECORDING! Move the throttle to animate the servo.")
    
    def start_recording(self):
        """Begin recording servo positions"""
        if self.is_recording:
            print("✗ Already recording")
            return
        
        # Show countdown
        self.countdown_before_recording()
        
        # Initialize recording
        self.is_recording = True
        self.recording_start_time = time.time()
        self.current_recording = []

        # Start background playback if servos are enabled
        if self.background_servos:
            # Remove active servo from background playback to avoid conflicts
            active_background_servos = [ch for ch in self.background_servos if ch != self.active_servo_channel]
            if len(active_background_servos) != len(self.background_servos):
                print(f"⚠️  Disabled background playback for Servo {self.active_servo_channel + 1} (currently recording)")
            
            if active_background_servos:
                self.background_playback_active = True
                self.background_start_time = self.recording_start_time
                print(f"🔄 Background playback started for servos: {[ch+1 for ch in active_background_servos]}")
        
        print(f"✓ Recording servo {self.active_servo_channel + 1} (Press record button again to stop)")
    
    def stop_recording(self):
        """Stop recording and save the recorded data"""
        if not self.is_recording:
            print("✗ Not currently recording")
            return
        
        self.is_recording = False
        # Stop background playback
        self.background_playback_active = False
        
        if not self.current_recording:
            print("✗ No data recorded")
            return
        
        # Calculate duration
        duration = self.current_recording[-1].timestamp
        
        # Create animation track
        track = AnimationTrack(
            servo_channel=self.active_servo_channel,
            positions=self.current_recording.copy(),
            limits=self.servo_limits[self.active_servo_channel]
        )
        
        # Add track to current project
        self.current_project.tracks[self.active_servo_channel] = track
        self.current_project.duration = max(self.current_project.duration, duration)
        
        print(f"✓ Recording stopped. Captured {len(self.current_recording)} positions over {duration:.2f} seconds")
        print(f"✓ Added to project as Servo {self.active_servo_channel + 1} track")
        
        # Clear current recording
        self.current_recording = []
    
    def update_recording(self):
        """Update recording with current servo position - call this every frame during recording"""
        if not self.is_recording:
            return
        
        # Get current throttle position
        throttle = self.joystick.get_throttle_position()
        servo_position = self.throttle_to_servo_position(throttle, self.active_servo_channel)
        
        # Send position to servo
        self.maestro.set_servo_position(self.active_servo_channel, servo_position)
        
        # Record position with timestamp
        current_time = time.time() - self.recording_start_time
        self.current_recording.append(TimestampedPosition(current_time, servo_position))
        
        # Update background servo playback
        self.update_background_playback()

        # Show live feedback
        if len(self.current_recording) % (RECORDING_SAMPLE_RATE // 2) == 0:  # Print every half second
            print(f"📍 Time: {current_time:.1f}s, Position: {servo_position}μs")
    
    # def start_playback(self):
    #     """Start playing back the complete animation project"""
    #     if not self.current_project.tracks:
    #         print("✗ No animation tracks to play")
    #         return
        
    #     if self.is_playing:
    #         print("✗ Already playing animation")
    #         return
        
    #     self.is_playing = True
    #     self.playback_start_time = time.time()
        
    #     print(f"▶️  Playing animation with {len(self.current_project.tracks)} tracks...")
    #     print(f"   Duration: {self.current_project.duration:.2f} seconds")

    def start_playback(self, loop: bool = False): # AI EDIT - New Playback for looped playback.
        """Start playing back the complete animation project"""
        if not self.current_project.tracks:
            print("✗ No animation tracks to play")
            return
        
        if self.is_playing:
            print("✗ Already playing animation")
            return
        
        self.is_playing = True
        self.playback_start_time = time.time()
        self.loop_playback = loop  # Add this line
        
        loop_text = " (LOOPING)" if loop else ""
        print(f"▶️  Playing animation{loop_text} with {len(self.current_project.tracks)} tracks...")
        print(f"   Duration: {self.current_project.duration:.2f} seconds")

    
    # def update_playback(self):
    #     """Update playback - call this every frame during playback"""
    #     if not self.is_playing:
    #         return
        
    #     # Calculate current playback time
    #     current_time = (time.time() - self.playback_start_time) * PLAYBACK_SPEED
        
    #     # Check if playback is complete
    #     if current_time >= self.current_project.duration:
    #         self.stop_playback()
    #         return
        
    #     # Update each servo track
    #     for channel, track in self.current_project.tracks.items():
    #         position = self.get_position_at_time(track, current_time)
    #         if position is not None:
    #             self.maestro.set_servo_position(channel, position)

    def update_playback(self):      # AI EDIT - New Playback for looped playback.
        """Update playback - call this every frame during playback"""
        if not self.is_playing:
            return
        
        # Calculate current playback time
        elapsed_time = (time.time() - self.playback_start_time) * PLAYBACK_SPEED
        
        # Handle looping
        if hasattr(self, 'loop_playback') and self.loop_playback:
            current_time = elapsed_time % self.current_project.duration  # Loop back to start
        else:
            current_time = elapsed_time
            # Check if playback is complete (non-looping)
            if current_time >= self.current_project.duration:
                self.stop_playback()
                return
        
        # Update each servo track (rest stays the same)
        for channel, track in self.current_project.tracks.items():
            position = self.get_position_at_time(track, current_time)
            if position is not None:
                self.maestro.set_servo_position(channel, position)
        
    def get_position_at_time(self, track: AnimationTrack, time_target: float) -> Optional[int]:
        """
        Get the servo position at a specific time in a track using linear interpolation
        Args:
            track: AnimationTrack to get position from
            time_target: Time to get position for
        Returns:
            Interpolated position in microseconds, or None if no data
        """
        if not track.positions:
            return None
        
        # Handle edge cases
        if time_target <= track.positions[0].timestamp:
            return track.positions[0].position
        if time_target >= track.positions[-1].timestamp:
            return track.positions[-1].position
        
        # Find the two positions to interpolate between
        for i in range(len(track.positions) - 1):
            pos1 = track.positions[i]
            pos2 = track.positions[i + 1]
            
            if pos1.timestamp <= time_target <= pos2.timestamp:
                # Linear interpolation
                time_ratio = (time_target - pos1.timestamp) / (pos2.timestamp - pos1.timestamp)
                interpolated_position = pos1.position + (pos2.position - pos1.position) * time_ratio
                return int(interpolated_position)
        
        return None
    
    def stop_playback(self):
        """Stop animation playback"""
        if self.is_playing:
            self.is_playing = False
            print("⏹️  Playback stopped")
    
    def save_project(self, filename: str):
        """
        Save the current animation project to a file
        Args:
            filename: Name of file to save (without extension)
        """
        filepath = os.path.join(ANIMATIONS_FOLDER, filename + PROJECT_FILE_EXTENSION)
        
        try:
            # Convert project to dictionary for JSON serialization
            project_dict = asdict(self.current_project)
            
            with open(filepath, 'w') as f:
                json.dump(project_dict, f, indent=2)
            
            print(f"✓ Project saved to {filepath}")
            
        except Exception as e:
            print(f"✗ Error saving project: {e}")
    
    def load_project(self, filename: str) -> bool:
        """
        Load an animation project from a file
        Args:
            filename: Name of file to load (without extension)
        Returns:
            True if loaded successfully, False otherwise
        """
        filepath = os.path.join(ANIMATIONS_FOLDER, filename + PROJECT_FILE_EXTENSION)
        
        if not os.path.exists(filepath):
            print(f"✗ File not found: {filepath}")
            return False
        
        try:
            with open(filepath, 'r') as f:
                project_dict = json.load(f)
            
            # Reconstruct project from dictionary
            tracks = {}
            for channel_str, track_dict in project_dict['tracks'].items():
                channel = int(channel_str)
                
                # Reconstruct positions
                positions = [
                    TimestampedPosition(**pos_dict) 
                    for pos_dict in track_dict['positions']
                ]
                
                # Reconstruct limits
                limits = ServoLimits(**track_dict['limits'])
                
                # Reconstruct track
                tracks[channel] = AnimationTrack(
                    servo_channel=track_dict['servo_channel'],
                    positions=positions,
                    limits=limits
                )
            
            # Reconstruct project
            self.current_project = AnimationProject(
                name=project_dict['name'],
                created_date=project_dict['created_date'],
                duration=project_dict['duration'],
                tracks=tracks
            )
            
            print(f"✓ Project loaded: {self.current_project.name}")
            print(f"   Tracks: {list(self.current_project.tracks.keys())}")
            print(f"   Duration: {self.current_project.duration:.2f}s")
            
            return True
            
        except Exception as e:
            print(f"✗ Error loading project: {e}")
            return False
    
    def combine_with_project(self, filename: str):
        """
        Load another project and combine it with the current project
        Args:
            filename: Name of project file to combine with current project
        """
        # Save current project temporarily
        temp_project = self.current_project
        
        # Load the project to combine
        if not self.load_project(filename):
            # Restore original project if load failed
            self.current_project = temp_project
            return
        
        loaded_project = self.current_project
        
        # Restore original project
        self.current_project = temp_project
        
        # Combine tracks
        for channel, track in loaded_project.tracks.items():
            if channel in self.current_project.tracks:
                print(f"⚠️  Warning: Overwriting existing track for servo {channel + 1}")
            
            self.current_project.tracks[channel] = track
            print(f"✓ Added track for servo {channel + 1} from {filename}")
        
        # Update duration
        self.current_project.duration = max(self.current_project.duration, loaded_project.duration)
        
        print(f"✓ Combined projects. New duration: {self.current_project.duration:.2f}s")
    
    # def export_maestro_script(self, filename: str):
    #     """
    #     Export the current project as a Maestro sequence script
    #     Args:
    #         filename: Name of script file to create (without extension)
    #     """
    #     if not self.current_project.tracks:
    #         print("✗ No tracks to export")
    #         return
        
    #     filepath = os.path.join(ANIMATIONS_FOLDER, filename + MAESTRO_SCRIPT_EXTENSION)
        
    #     try:
    #         with open(filepath, 'w') as f:
    #             # Write sequence header
    #             f.write(f"# Maestro Sequence Script: {self.current_project.name}\n")
    #             f.write(f"# Generated: {datetime.now().isoformat()}\n")
    #             f.write(f"# Duration: {self.current_project.duration:.2f} seconds\n")
    #             f.write(f"# Tracks: {list(self.current_project.tracks.keys())}\n\n")
                
    #             # Calculate frame duration in milliseconds
    #             # Using 50ms frames for smooth playback (20 FPS)
    #             frame_duration_ms = 50
    #             total_frames = int((self.current_project.duration * 1000) / frame_duration_ms) + 1
                
    #             f.write(f"# Frame duration: {frame_duration_ms}ms\n")
    #             f.write(f"# Total frames: {total_frames}\n\n")
                
    #             # Write sequence commands
    #             f.write("# Sequence start\n")
    #             f.write("begin\n\n")
                
    #             # Generate frames
    #             for frame in range(total_frames):
    #                 frame_time = (frame * frame_duration_ms) / 1000.0  # Convert to seconds
                    
    #                 f.write(f"# Frame {frame} (t={frame_time:.3f}s)\n")
                    
    #                 # Set position for each servo that has a track
    #                 for channel, track in self.current_project.tracks.items():
    #                     position = self.get_position_at_time(track, frame_time)
    #                     if position is not None:
    #                         # Convert microseconds to quarter-microseconds for Maestro
    #                         target = position * 4
    #                         f.write(f"{target} {channel} servo\n")
                    
    #                 # Add frame delay
    #                 f.write(f"{frame_duration_ms} delay\n\n")
                
    #             # Write sequence end
    #             f.write("# Sequence end\n")
    #             f.write("quit\n")
            
    #         print(f"✓ Maestro script exported to {filepath}")
    #         print(f"   Load this script into Maestro Control Center and save as a sequence")
            
    #     except Exception as e:
    #         print(f"✗ Error exporting Maestro script: {e}")

    def export_maestro_script(self, filename: str, use_keyframes: bool = True, keyframe_threshold: int = 50):
        """
        Export the current project as a Maestro sequence script
        Args:
            filename: Name of script file to create (without extension)
            use_keyframes: If True, use keyframe optimization
            keyframe_threshold: Minimum position change in μs to create keyframe
        """
        if not self.current_project.tracks:
            print("✗ No tracks to export")
            return
        
        filepath = os.path.join(ANIMATIONS_FOLDER, filename + MAESTRO_SCRIPT_EXTENSION)
        
        try:
            with open(filepath, 'w', encoding='utf-8') as f:
                # Write sequence header
                f.write(f"# Maestro Sequence Script: {self.current_project.name}\n")
                f.write(f"# Generated: {datetime.now().isoformat()}\n")
                f.write(f"# Duration: {self.current_project.duration:.2f} seconds\n")
                f.write(f"# Tracks: {list(self.current_project.tracks.keys())}\n")
                f.write(f"# Keyframe optimization: {'ON' if use_keyframes else 'OFF'}\n")
                if use_keyframes:
                    f.write(f"# Keyframe threshold: {keyframe_threshold} μs\n")
                f.write("\n")
                
                if use_keyframes:
                    # Extract keyframes for each track
                    print("🔍 Extracting keyframes...")
                    track_keyframes = {}
                    for channel, track in self.current_project.tracks.items():
                        track_keyframes[channel] = self.extract_keyframes(track, keyframe_threshold)
                    
                    # Create a timeline of all keyframes from all tracks
                    all_keyframes = []
                    for channel, keyframes in track_keyframes.items():
                        for kf in keyframes:
                            all_keyframes.append((kf.timestamp, channel, kf.position))
                    
                    # Sort by timestamp
                    all_keyframes.sort(key=lambda x: x[0])
                    
                    f.write("# Keyframe-based sequence\n")
                    f.write("begin\n\n")
                    
                    current_time = 0.0
                    servo_positions = {}  # Track current position of each servo
                    
                    # Process each keyframe
                    for i, (timestamp, channel, position) in enumerate(all_keyframes):
                        # Calculate delay from last keyframe
                        delay_ms = int((timestamp - current_time) * 1000)
                        
                        if delay_ms > 0:
                            f.write(f"{delay_ms} delay  # Wait {delay_ms}ms\n")
                        
                        # Calculate speed if we know the previous position
                        speed = 0  # Default unlimited speed
                        if channel in servo_positions:
                            prev_position = servo_positions[channel]
                            time_diff = timestamp - current_time if timestamp > current_time else 0.1
                            position_diff = position - prev_position
                            speed = self.calculate_servo_speed(position_diff, time_diff)
                        
                        # Set speed if it's limited
                        if speed > 0:
                            f.write(f"{speed} {channel} speed\n")
                        
                        # Set position
                        target = position * 4  # Convert to quarter-microseconds
                        f.write(f"{target} {channel} servo  # t={timestamp:.2f}s, pos={position}μs\n")
                        
                        # Update tracking
                        servo_positions[channel] = position
                        current_time = timestamp
                        f.write("\n")
                    
                else:
                    # Original frame-based approach
                    frame_duration_ms = 50
                    total_frames = int((self.current_project.duration * 1000) / frame_duration_ms) + 1
                    
                    f.write(f"# Frame-based sequence (50ms frames)\n")
                    f.write(f"# Total frames: {total_frames}\n")
                    f.write("begin\n\n")
                    
                    # Generate frames
                    for frame in range(total_frames):
                        frame_time = (frame * frame_duration_ms) / 1000.0
                        
                        f.write(f"# Frame {frame} (t={frame_time:.3f}s)\n")
                        
                        for channel, track in self.current_project.tracks.items():
                            position = self.get_position_at_time(track, frame_time)
                            if position is not None:
                                target = position * 4
                                f.write(f"{target} {channel} servo\n")
                        
                        f.write(f"{frame_duration_ms} delay\n\n")
                
                # Write sequence end
                f.write("# Sequence end\n")
                f.write("quit\n")
            
            print(f"✓ Maestro script exported to {filepath}")
            
            # Show file size
            file_size = os.path.getsize(filepath)
            print(f"   Script size: {file_size} bytes")
            if file_size > 8000:  # Mini Maestro limit is around 8KB
                print("   ⚠️  Warning: Script may be too large for Mini Maestro")
                print("   Try increasing keyframe_threshold or reducing animation length")
            
        except Exception as e:
            print(f"✗ Error exporting Maestro script: {e}")

    def extract_keyframes(self, track: AnimationTrack, threshold: int = 50) -> List[TimestampedPosition]:
        """
        Extract keyframes where position changes significantly
        Args:
            track: Animation track to process
            threshold: Minimum position change in microseconds to create a keyframe
        Returns:
            List of keyframe positions with timestamps
        """
        if not track.positions:
            return []
        
        keyframes = [track.positions[0]]  # Always include first frame
        
        for i in range(1, len(track.positions)):
            current_pos = track.positions[i]
            last_keyframe = keyframes[-1]
            
            # Add keyframe if position changed significantly OR if it's been too long since last keyframe
            position_change = abs(current_pos.position - last_keyframe.position)
            time_since_last = current_pos.timestamp - last_keyframe.timestamp
            
            # Create keyframe if:
            # 1. Position changed significantly, OR
            # 2. It's been more than 1 second since last keyframe (prevents long gaps)
            if position_change >= threshold or time_since_last >= 1.0:
                keyframes.append(current_pos)
        
        # Always include last frame
        if keyframes[-1] != track.positions[-1]:
            keyframes.append(track.positions[-1])
        
        print(f"    Servo {track.servo_channel + 1}: {len(track.positions)} recorded → {len(keyframes)} keyframes")
        
        return keyframes
    def calculate_servo_speed(self, position_change: int, time_change: float) -> int:
        """
        Calculate Maestro servo speed based on position and time change
        Args:
            position_change: Change in position (microseconds)
            time_change: Time for the change (seconds)
        Returns:
            Maestro speed value (0 = unlimited, 1-127 = limited speed)
        """
        if time_change <= 0:
            return 0  # Unlimited speed for instant moves
        
        # Convert to quarter-microseconds per 10ms (Maestro speed units)
        # Speed = (position_change_in_quarter_us) / (time_in_10ms_units)
        quarter_us_change = abs(position_change * 4)
        time_in_10ms = time_change * 100  # Convert seconds to 10ms units
        
        speed = quarter_us_change / time_in_10ms
        
        # Clamp to Maestro limits (1-127, 0 = unlimited)
        if speed > 127:
            return 0  # Use unlimited speed for very fast movements
        elif speed < 1:
            return 1  # Minimum speed
        else:
            return int(speed)
    def set_background_servos(self, servo_channels: List[int]):
        """
        Set which servos should play in background during recording
        Args:
            servo_channels: List of servo channel numbers for background playback
        """
        # Filter to only include servos that have existing tracks
        valid_servos = [ch for ch in servo_channels if ch in self.current_project.tracks]
        invalid_servos = [ch for ch in servo_channels if ch not in self.current_project.tracks]
        
        if invalid_servos:
            print(f"⚠️  Servos {[ch+1 for ch in invalid_servos]} have no animation data - skipping")
        
        self.background_servos = valid_servos
        print(f"✓ Background playback servos: {[ch+1 for ch in self.background_servos] if self.background_servos else 'None'}")

    def update_background_playback(self):
        """
        Update background servo playback during recording - call every frame during recording
        """
        if not self.background_playback_active or not self.background_servos:
            return
        
        # Calculate current playback time (same timing as main recording)
        current_time = time.time() - self.background_start_time
        
        # Stop background playback if we've exceeded the duration of any background track
        max_duration = 0
        for channel in self.background_servos:
            if channel in self.current_project.tracks:
                track_duration = self.current_project.tracks[channel].positions[-1].timestamp
                max_duration = max(max_duration, track_duration)
        
        if current_time >= max_duration:
            self.background_playback_active = False
            print("🔄 Background playback completed")
            return
        
        # Update each background servo
        for channel in self.background_servos:
            if channel in self.current_project.tracks:
                track = self.current_project.tracks[channel]
                position = self.get_position_at_time(track, current_time)
                if position is not None:
                    self.maestro.set_servo_position(channel, position)

# =============================================================================
# MAIN APPLICATION
# =============================================================================

class ServoAnimatorApp:
    """Main application class that coordinates all components"""
    
    def __init__(self):
        """Initialize the application"""
        print("🤖 Servo Animator MVP Starting...")
        print("=" * 50)
        
        # Initialize components
        self.maestro = MaestroController(MAESTRO_DEVICE, MAESTRO_BAUDRATE)
        self.joystick = JoystickController()
        self.recorder = None  # Will be initialized after connections
        
        self.running = False
    
    def setup_connections(self) -> bool:
        """
        Establish connections to hardware
        Returns:
            True if all connections successful, False otherwise
        """
        print("🔌 Connecting to hardware...")
        
        # Connect to Maestro
        if not self.maestro.connect():
            return False
        
        # Connect to joystick
        if not self.joystick.connect():
            self.maestro.disconnect()
            return False
        
        # Initialize recorder after successful connections
        self.recorder = AnimationRecorder(self.maestro, self.joystick)
        
        print("✓ All hardware connected successfully!")
        return True
    
    # def setup_servo_limits(self):         FIX - ORIGINAL VERSION
    #     """Interactive setup of servo movement limits"""
    #     print("\n⚙️  Servo Limits Configuration")
    #     print("=" * 30)
    #     print("Configure movement limits for each servo to prevent mechanical damage")
    #     print("Press Enter to use default values or type new limits")
    #     print(f"Default limits: {DEFAULT_SERVO_LIMITS['min_position']}-{DEFAULT_SERVO_LIMITS['max_position']} microseconds")
    
        # for channel in SERVO_CHANNELS:
        #     servo_num = channel + 1
        #     print(f"\n🔧 Servo {servo_num} (Channel {channel}):")
            
        #     try:
        #         # Get minimum position
        #         min_input = input(f"  Min position [{DEFAULT_SERVO_LIMITS['min_position']}]: ").strip()
        #         min_pos = int(min_input) if min_input else DEFAULT_SERVO_LIMITS['min_position']
                
        #         # Get maximum position
        #         max_input = input(f"  Max position [{DEFAULT_SERVO_LIMITS['max_position']}]: ").strip()
        #         max_pos = int(max_input) if max_input else DEFAULT_SERVO_LIMITS['max_position']
                
        #         # Validate and set limits
        #         if min_pos >= max_pos:
        #             print(f"  ⚠️  Invalid range, using defaults")
        #             min_pos, max_pos = DEFAULT_SERVO_LIMITS['min_position'], DEFAULT_SERVO_LIMITS['max_position']
                
        #         self.recorder.set_servo_limits(channel, min_pos, max_pos)
                
        #     except ValueError:
        #         print(f"  ⚠️  Invalid input, using defaults for servo {servo_num}")
        #         self.recorder.set_servo_limits(channel, 
        #                                        DEFAULT_SERVO_LIMITS['min_position'], 
        #                                        DEFAULT_SERVO_LIMITS['max_position'])

    def setup_servo_limits(self):
        """Interactive setup of servo movement limits"""
        print("\n⚙️  Servo Limits Configuration")
        print("=" * 30)
        
        # Show current hard-coded limits
        print("Current hard-coded limits:")
        for channel in SERVO_CHANNELS:
            limits = SERVO_LIMITS[channel]
            print(f"  Servo {channel + 1}: {limits['min_position']}-{limits['max_position']} μs")
        
        # Ask if user wants to change them
        response = input("\nUse these limits? (y/n): ").strip().lower()
        
        if response == 'y' or response == '':
            # Use hard-coded limits
            for channel in SERVO_CHANNELS:
                self.recorder.set_servo_limits(channel, 
                                             SERVO_LIMITS[channel]['min_position'],
                                             SERVO_LIMITS[channel]['max_position'])
            print("✓ Using hard-coded servo limits")
            return
        
        # Interactive setup (copy the original code from here down)
        print("Configure movement limits for each servo to prevent mechanical damage")
        print("Press Enter to use default values or type new limits")
        print(f"Default limits: {DEFAULT_SERVO_LIMITS['min_position']}-{DEFAULT_SERVO_LIMITS['max_position']} microseconds")
        
        for channel in SERVO_CHANNELS:
            servo_num = channel + 1
            print(f"\n🔧 Servo {servo_num} (Channel {channel}):")
            
            try:
                # Get minimum position
                min_input = input(f"  Min position [{DEFAULT_SERVO_LIMITS['min_position']}]: ").strip()
                min_pos = int(min_input) if min_input else DEFAULT_SERVO_LIMITS['min_position']
                
                # Get maximum position
                max_input = input(f"  Max position [{DEFAULT_SERVO_LIMITS['max_position']}]: ").strip()
                max_pos = int(max_input) if max_input else DEFAULT_SERVO_LIMITS['max_position']
                
                # Validate and set limits
                if min_pos >= max_pos:
                    print(f"  ⚠️  Invalid range, using defaults")
                    min_pos, max_pos = DEFAULT_SERVO_LIMITS['min_position'], DEFAULT_SERVO_LIMITS['max_position']
                
                self.recorder.set_servo_limits(channel, min_pos, max_pos)
                
            except ValueError:
                print(f"  ⚠️  Invalid input, using defaults for servo {servo_num}")
                self.recorder.set_servo_limits(channel, 
                                               DEFAULT_SERVO_LIMITS['min_position'], 
                                               DEFAULT_SERVO_LIMITS['max_position'])
    
    def select_active_servo(self) -> int:
        """
        Interactive servo selection
        Returns:
            Selected servo channel
        """
        print("\n🎯 Servo Selection")
        print("=" * 20)
        print("Which servo do you want to animate?")
        
        for i, channel in enumerate(SERVO_CHANNELS):
            print(f"  {i + 1}. Servo {channel + 1} (Channel {channel})")
        
        while True:
            try:
                choice = input(f"\nSelect servo (1-{len(SERVO_CHANNELS)}): ").strip()
                servo_index = int(choice) - 1
                
                if 0 <= servo_index < len(SERVO_CHANNELS):
                    selected_channel = SERVO_CHANNELS[servo_index]
                    print(f"✓ Selected Servo {selected_channel + 1} (Channel {selected_channel})")
                    return selected_channel
                else:
                    print(f"✗ Please enter a number between 1 and {len(SERVO_CHANNELS)}")
                    
            except ValueError:
                print("✗ Please enter a valid number")
    
    def show_controls_help(self):
        """Display control instructions"""
        print("\n🎮 Controls")
        print("=" * 15)
        print("Joystick Controls:")
        print(f"  • Throttle: Control servo position")
        print(f"  • Button {RECORD_BUTTON + 1}: Start/Stop recording")
        print(f"  • Button {PLAY_BUTTON + 1}: Play animation")
        print(f"  • Buttons 5-11: Quick servo selection")
        print("\nKeyboard Commands:")
        print("  • 'h' or 'help': Show this help")
        print("  • 's' or 'servo': Select different servo")
        print("  • 'save': Save current project")
        print("  • 'load': Load existing project")
        print("  • 'combine': Combine with another project")
        print("  • 'export': Export Maestro script")
        print("  • 'q' or 'quit': Exit program")
        print("  • 'test': Test servo movement")
        print("  • 'p' or 'playback': Select background playback servos")
    
    def handle_keyboard_commands(self):
        """Handle keyboard input for various commands"""
        try:
            # Non-blocking input check (this is a simplified approach)
            # In a production app, you'd use threading or async input
            command = input("Command (or 'h' for help): ").strip().lower()
            
            if command in ['h', 'help']:
                self.show_controls_help()
                
            elif command in ['s', 'servo']:
                self.recorder.active_servo_channel = self.select_active_servo()
                
            elif command == 'save':
                filename = input("Enter filename to save: ").strip()
                if filename:
                    self.recorder.save_project(filename)
                
            elif command == 'load':
                filename = input("Enter filename to load: ").strip()
                if filename:
                    self.recorder.load_project(filename)
                    
            elif command == 'combine':
                filename = input("Enter filename to combine with current project: ").strip()
                if filename:
                    self.recorder.combine_with_project(filename)
                    
            elif command == 'export':
                filename = input("Enter filename for Maestro script: ").strip()
                if filename:
                    self.recorder.export_maestro_script(filename)
                    
            elif command == 'test':
                self.test_servo_movement()
                
            elif command in ['q', 'quit']:
                self.running = False
                print("👋 Goodbye!")

            elif command in ['p', 'playback']:
                self.configure_background_servos()
                
            else:
                print("✗ Unknown command. Type 'h' for help.")
                
        except KeyboardInterrupt:
            # Handle Ctrl+C gracefully
            self.running = False
            print("\n👋 Goodbye!")
    
    def test_servo_movement(self):
        """Test servo movement with joystick"""
        print(f"\n🔧 Testing Servo {self.recorder.active_servo_channel + 1}")
        print("Move the throttle to test servo movement. Press any key to stop...")
        
        # Simple test loop
        start_time = time.time()
        while time.time() - start_time < 10:  # Test for 10 seconds max
            # Update joystick
            self.joystick.update()

            # # DEBUG: Add this debug code temporarily
            # print(f"Joystick has {self.joystick.joystick.get_numaxes()} axes")
            # for i in range(self.joystick.joystick.get_numaxes()):
            #     axis_value = self.joystick.joystick.get_axis(i)
            #     print(f"Axis {i}: {axis_value:.3f}")
            # print("Move your throttle and see which axis changes...")
            
            # Get throttle position and move servo
            throttle = self.joystick.get_throttle_position()
            servo_position = self.recorder.throttle_to_servo_position(throttle, self.recorder.active_servo_channel)
            self.maestro.set_servo_position(self.recorder.active_servo_channel, servo_position)
            
            # Show position
            print(f"\rThrottle: {throttle:6.3f} → Position: {servo_position:4d}μs", end="", flush=True)
            
            # Small delay
            time.sleep(0.05)  # 20 FPS
            
            # Check for any button press to exit
            if any(self.joystick.is_button_pressed(i) for i in range(self.joystick.joystick.get_numbuttons())):
                break
        
        print("\n✓ Test complete")
    
    def handle_joystick_input(self):
        """Handle joystick button presses and servo selection"""
        # Check for record button press
        if self.joystick.is_button_pressed(RECORD_BUTTON):
            if self.recorder.is_recording:
                self.recorder.stop_recording()
            else:
                self.recorder.start_recording()
        
        # # Check for play button press
        # if self.joystick.is_button_pressed(PLAY_BUTTON):
        #     if self.recorder.is_playing:
        #         self.recorder.stop_playback()
        #     else:
        #         self.recorder.start_playback()

        # Check for play button press - AI EDIT Added Loop Mode - hold down Button #3 and press Button #2
        if self.joystick.is_button_pressed(PLAY_BUTTON):
            if self.recorder.is_playing:
                self.recorder.stop_playback()
            else:
                # Hold another button for looping (e.g., button 3)
                loop_mode = self.joystick.joystick.get_button(2) if self.joystick.joystick.get_numbuttons() > 2 else False
                self.recorder.start_playback(loop=loop_mode)
                if loop_mode:
                    print("🔄 Loop mode activated - hold button 4 + play for looping")        

        # Check for command mode button (let's use button 3)
        if self.joystick.is_button_pressed(3):  # Button 4 on joystick
            print("\n⌨️  Command Mode - Enter your command:")
            self.handle_keyboard_commands()
            print("✓ Back to animation control")
        # # Check for menu button (let's use button 2)
        # if self.joystick.is_button_pressed(2):  # Button 3 on joystick
        #     print("\n📋 Returning to main menu...")
        #     self.running = False  # This exits the main_loop and returns to interactive_menu
        
        # Check for servo selection buttons (if we have enough buttons)
        for i, button_index in enumerate(SERVO_SELECT_BUTTONS):
            if i < len(SERVO_CHANNELS) and self.joystick.is_button_pressed(button_index):
                self.recorder.active_servo_channel = SERVO_CHANNELS[i]
                print(f"🎯 Switched to Servo {SERVO_CHANNELS[i] + 1}")
                break

        
    
    def main_loop(self):
        """Main application loop"""
        print(f"\n🎬 Animation Control Active")
        print("=" * 30)
        print(f"Active Servo: {self.recorder.active_servo_channel + 1}")
        print(f"Recording: {self.recorder.is_recording}")
        print(f"Playing: {self.recorder.is_playing}")
        self.show_controls_help()
        
        # Main loop
        clock_time = time.time()
        frame_duration = 1.0 / RECORDING_SAMPLE_RATE  # Target frame time
        
        while self.running:
            try:
                current_time = time.time()
                
                # Update joystick input
                self.joystick.update()
                
                # Handle joystick button presses
                self.handle_joystick_input()
                
                # Update recording if active
                if self.recorder.is_recording:
                    self.recorder.update_recording()
                
                # Update playback if active
                if self.recorder.is_playing:
                    self.recorder.update_playback()
                
                # Handle manual servo control when not recording or playing
                if not self.recorder.is_recording and not self.recorder.is_playing:
                    throttle = self.joystick.get_throttle_position()
                    if abs(throttle) > JOYSTICK_DEADZONE:  # Only move if throttle is moved
                        servo_position = self.recorder.throttle_to_servo_position(throttle, self.recorder.active_servo_channel)
                        self.maestro.set_servo_position(self.recorder.active_servo_channel, servo_position)
                
                # Maintain consistent frame rate
                elapsed = current_time - clock_time
                if elapsed < frame_duration:
                    time.sleep(frame_duration - elapsed)
                clock_time = time.time()
                
                # Check for keyboard input periodically (every second)
                if int(current_time) % 1 == 0 and current_time - int(current_time) < frame_duration:
                    # Only check for commands when not recording/playing to avoid interrupting
                    if not self.recorder.is_recording and not self.recorder.is_playing:
                        print(f"\n📊 Status - Servo: {self.recorder.active_servo_channel + 1}, "
                              f"Tracks: {len(self.recorder.current_project.tracks)}, "
                              f"Duration: {self.recorder.current_project.duration:.1f}s")
                        # Note: In a real implementation, you'd handle keyboard input asynchronously
                        # For this MVP, we'll rely primarily on joystick control
                
            except KeyboardInterrupt:
                # Handle Ctrl+C gracefully
                print("\n🛑 Received interrupt signal")
                self.running = False
    
    def interactive_menu(self):
        """Simple interactive menu for file operations"""
        while self.running:
            print(f"\n📋 Servo Animator Menu")
            print("=" * 25)
            print(f"Current Project: {self.recorder.current_project.name}")
            print(f"Tracks: {len(self.recorder.current_project.tracks)}")
            print(f"Duration: {self.recorder.current_project.duration:.2f}s")
            print(f"Active Servo: {self.recorder.active_servo_channel + 1}")
            print()
            print("Options:")
            print("1. Start Animation Control (use joystick)")
            print("2. Test Servo Movement")
            print("3. Change Active Servo")
            print("4. Save Project")
            print("5. Load Project") 
            print("6. Combine Projects")
            print("7. Export Maestro Script")
            print("8. Play Animation (Loop)")  # NEW Add this
            print("9. List Saved Projects")
            print("10. Quit")  # Update the quit number
            print("11. Configure Background Playback Servos")  # Background Servos
            
            try:
                choice = input("\nSelect option (1-9): ").strip()
                
                if choice == '1':
                    # Enter main control loop
                    self.main_loop()
                    
                elif choice == '2':
                    self.test_servo_movement()
                    
                elif choice == '3':
                    self.recorder.active_servo_channel = self.select_active_servo()
                    
                elif choice == '4':
                    filename = input("Enter filename to save: ").strip()
                    if filename:
                        self.recorder.save_project(filename)
                    
                elif choice == '5':
                    self.list_saved_projects()
                    filename = input("Enter filename to load: ").strip()
                    if filename:
                        self.recorder.load_project(filename)
                        
                elif choice == '6':
                    self.list_saved_projects()
                    filename = input("Enter filename to combine: ").strip()
                    if filename:
                        self.recorder.combine_with_project(filename)
                        
                # elif choice == '7':
                #     filename = input("Enter filename for Maestro script: ").strip()
                #     if filename:
                #         self.recorder.export_maestro_script(filename)

                elif choice == '7':
                    filename = input("Enter filename for Maestro script: ").strip()
                    if filename:
                        use_keyframes = input("Use keyframe optimization? (y/n) [y]: ").strip().lower()
                        use_keyframes = use_keyframes != 'n'  # Default to yes
                        
                        if use_keyframes:
                            threshold = input("Keyframe threshold in μs (50): ").strip()
                            threshold = int(threshold) if threshold.isdigit() else 50
                            self.recorder.export_maestro_script(filename, True, threshold)
                        else:
                            self.recorder.export_maestro_script(filename, False)
                elif choice == '8':
                    self.recorder.start_playback(loop=True)
                    print("Press Ctrl+C to stop looping playback")
                    try:
                        while self.recorder.is_playing:
                            self.recorder.update_playback()
                            time.sleep(0.05)  # 20 FPS
                    except KeyboardInterrupt:
                        self.recorder.stop_playback()
                        print("\n⏹️  Looping playback stopped")

                elif choice == '9':  # Update list projects
                    self.list_saved_projects()
                    input("Press Enter to continue...")

                elif choice == '10':  # Update quit
                    self.running = False
                    print("👋 Goodbye!")
                    break        
                
                elif choice == '11':
                    self.configure_background_servos()

                # elif choice == '8':
                #     self.list_saved_projects()
                #     input("Press Enter to continue...")
                    
                # elif choice == '9':
                #     self.running = False
                #     print("👋 Goodbye!")
                #     break
                    
                else:
                    print("✗ Invalid option. Please select 1-11.")
                    
            except KeyboardInterrupt:
                self.running = False
                print("\n👋 Goodbye!")
                break
    
    def list_saved_projects(self):
        """List all saved project files"""
        print(f"\n📁 Saved Projects in {ANIMATIONS_FOLDER}:")
        print("=" * 30)
        
        if not os.path.exists(ANIMATIONS_FOLDER):
            print("No animations folder found.")
            return
        
        project_files = [f for f in os.listdir(ANIMATIONS_FOLDER) if f.endswith(PROJECT_FILE_EXTENSION)]
        
        if not project_files:
            print("No saved projects found.")
            return
        
        for filename in sorted(project_files):
            # Remove extension for display
            project_name = filename[:-len(PROJECT_FILE_EXTENSION)]
            filepath = os.path.join(ANIMATIONS_FOLDER, filename)
            
            try:
                # Get file modification time
                mod_time = os.path.getmtime(filepath)
                mod_time_str = datetime.fromtimestamp(mod_time).strftime("%Y-%m-%d %H:%M")
                
                # Try to get project info
                with open(filepath, 'r') as f:
                    project_data = json.load(f)
                    duration = project_data.get('duration', 0)
                    track_count = len(project_data.get('tracks', {}))
                
                print(f"  📄 {project_name}")
                print(f"      Modified: {mod_time_str}")
                print(f"      Duration: {duration:.2f}s, Tracks: {track_count}")
                print()
                
            except Exception as e:
                print(f"  📄 {project_name} (error reading file: {e})")

    def configure_background_servos(self):
        """Configure which servos should play in background during recording"""
        print(f"\n🔄 Background Playback Configuration")
        print("=" * 35)
        
        # Show available servos with tracks
        available_servos = list(self.recorder.current_project.tracks.keys())
        if not available_servos:
            print("No servos have recorded animation data yet.")
            input("Press Enter to continue...")
            return
        
        print("Available servos with animation data:")
        for channel in sorted(available_servos):
            duration = self.recorder.current_project.tracks[channel].positions[-1].timestamp
            print(f"  Servo {channel + 1} (Channel {channel}) - Duration: {duration:.2f}s")
        
        current_bg = [ch+1 for ch in self.recorder.background_servos]
        print(f"\nCurrently enabled: {current_bg if current_bg else 'None'}")
        
        print(f"\nEnter servo numbers for background playback (e.g., '1,3,5' or 'none'):")
        user_input = input("Background servos: ").strip().lower()
        
        if user_input == 'none' or user_input == '':
            self.recorder.set_background_servos([])
        else:
            try:
                # Parse servo numbers (convert to 0-indexed channels)
                servo_numbers = [int(x.strip()) for x in user_input.split(',')]
                servo_channels = [num - 1 for num in servo_numbers]
                
                # Validate servo numbers are in valid range
                invalid_numbers = [num for num in servo_numbers if num < 1 or num > len(SERVO_CHANNELS)]
                if invalid_numbers:
                    print(f"❌ Invalid servo numbers: {invalid_numbers}")
                    print(f"Valid range: 1-{len(SERVO_CHANNELS)}")
                    return
                
                # Convert to actual channel numbers
                valid_channels = [SERVO_CHANNELS[num-1] for num in servo_numbers if 1 <= num <= len(SERVO_CHANNELS)]
                self.recorder.set_background_servos(valid_channels)
                
            except ValueError:
                print("❌ Invalid input. Use format like '1,3,5' or 'none'")

    def cleanup(self):
        """Clean up resources before exit"""
        print("\n🧹 Cleaning up...")
        
        # Stop any ongoing operations
        if self.recorder:
            if self.recorder.is_recording:
                self.recorder.stop_recording()
            if self.recorder.is_playing:
                self.recorder.stop_playback()
        
        # Disconnect hardware
        if self.maestro:
            self.maestro.disconnect()
        
        # Cleanup pygame
        pygame.quit()
        
        print("✓ Cleanup complete")
    
    def run(self):
        """Main application entry point"""
        try:
            # Setup hardware connections
            if not self.setup_connections():
                print("✗ Failed to connect to hardware. Exiting...")
                return
            
            # Setup servo limits
            self.setup_servo_limits()
            
            # Select initial active servo
            print(f"\n🎯 Initial servo selection:")
            self.recorder.active_servo_channel = self.select_active_servo()
            
            # Start application
            self.running = True
            print(f"\n🚀 Servo Animator MVP Ready!")
            print(f"   Hardware: ✓ Connected")
            print(f"   Servos: 7 configured")
            print(f"   Active Servo: {self.recorder.active_servo_channel + 1}")
            
            # Run interactive menu
            self.interactive_menu()
            
        except Exception as e:
            print(f"✗ Unexpected error: {e}")
            
        finally:
            # Always cleanup
            self.cleanup()

# =============================================================================
# PROGRAM ENTRY POINT
# =============================================================================

if __name__ == "__main__":
    """Main program entry point"""
    
    print("🤖 Animatronic Servo Animation Controller MVP")
    print("=" * 50)
    print("Controls 7 MG90S servos via Pololu Mini Maestro 12ch")
    print("Uses Logitech Extreme 3D Pro joystick for recording")
    print("Version: 1.0 MVP")
    print("=" * 50)
    
    # Check Python version (optional but helpful)
    import sys
    if sys.version_info < (3, 7):
        print("⚠️  Warning: This program requires Python 3.7 or newer")
    
    # Create and run application
    app = ServoAnimatorApp()
    app.run()
    
    print("\n🎬 Thank you for using Servo Animator!")

# =============================================================================
# INSTALLATION INSTRUCTIONS (COMMENTED)
# =============================================================================

"""
INSTALLATION REQUIREMENTS:

1. Install Python 3.7 or newer from python.org

2. Install required packages:
   pip install pygame pyserial

3. Hardware setup:
   - Connect Pololu Mini Maestro 12ch to computer via USB
   - Connect 7 MG90S servos to channels 0-6 on the Maestro
   - Connect Logitech Extreme 3D Pro joystick to computer
   - Power the servos appropriately (5V, adequate current)

4. Configuration:
   - Update MAESTRO_DEVICE variable with your serial port
   - Windows: Usually "COM3", "COM4", etc.
   - Linux: Usually "/dev/ttyACM0" or "/dev/ttyUSB0"  
   - Mac: Usually "/dev/cu.usbmodem*"

5. Usage:
   - Run: python servo_animator.py
   - Follow the interactive setup prompts
   - Use joystick throttle to control servos
   - Press joystick buttons to record/play animations
   - Export animations as Maestro sequence scripts

TROUBLESHOOTING:

- If Maestro connection fails: Check device name and permissions
- If joystick not detected: Ensure it's plugged in and recognized by OS
- If servos don't move: Check power supply and wiring
- If positions are inverted: Adjust throttle_to_servo_position() function

CUSTOMIZATION VARIABLES (at top of file):
- COUNTDOWN_TIME: How long to count down before recording
- RECORDING_SAMPLE_RATE: How often to record positions (Hz)
- DEFAULT_SERVO_LIMITS: Default min/max positions for servos
- JOYSTICK button mappings for different controllers
"""