class MotionController:
    # Command mapping for each axis
    AXIS_COMMANDS = {
        'X': {'pos': 'E', 'neg': 'W', 'stop': 'e'},
        'Y': {'pos': 'S', 'neg': 'N', 'stop': 'n'},
        'Z': {'pos': 'U', 'neg': 'D', 'stop': 'u'}
    }
    
    # Speed values matching Arduino speedArr
    SPEED_VALUES = [0, 1, 2, 5, 10, 30, 50, 70]
    
    def __init__(self, arduino, on_command_sent=None):
        """
        Args:
            arduino: Arduino instance for sending commands
            on_command_sent: Optional callback(cmd) when command is sent
        """
        self.arduino = arduino
        self.on_command_sent = on_command_sent
        
        # Axis state
        self.axis_direction = {'X': 1, 'Y': 1, 'Z': 1}
        self.axis_moving = {'X': False, 'Y': False, 'Z': False}
        
        # Speed state
        self.current_speed_index = 5
    
    def send_command(self, cmd):
        """Send command to Arduino and notify callback"""
        if self.arduino:
            self.arduino.send_command(cmd)
        if self.on_command_sent:
            self.on_command_sent(cmd)
    
    # === Axis Movement ===
    
    def get_direction(self, axis):
        """Get current direction for axis (1 = positive, -1 = negative)"""
        return self.axis_direction[axis]
    
    def toggle_direction(self, axis):
        """Toggle direction and return new direction"""
        self.axis_direction[axis] *= -1
        return self.axis_direction[axis]
    
    def is_moving(self, axis):
        """Check if axis is currently moving"""
        return self.axis_moving[axis]
    
    def start_movement(self, axis):
        """Start continuous movement on an axis"""
        commands = self.AXIS_COMMANDS[axis]
        cmd = commands['pos'] if self.axis_direction[axis] == 1 else commands['neg']
        self.send_command(cmd)
        self.axis_moving[axis] = True
    
    def stop_axis(self, axis):
        """Stop movement on a specific axis"""
        commands = self.AXIS_COMMANDS[axis]
        self.send_command(commands['stop'])
        self.axis_moving[axis] = False
    
    def stop_all(self):
        """Emergency stop all axes"""
        self.send_command('!')
        for axis in self.axis_moving:
            self.axis_moving[axis] = False
    
    def move_z_up(self):
        """Start Z axis moving up"""
        self.send_command('U')
    
    def move_z_down(self):
        """Start Z axis moving down"""
        self.send_command('D')
    
    def stop_z(self):
        """Stop Z axis"""
        self.send_command('u')
    
    # === Speed Control ===
    
    def get_speed_value(self, index):
        """Get actual speed value for index"""
        if 0 <= index < len(self.SPEED_VALUES):
            return self.SPEED_VALUES[index]
        return 0
    
    def set_speed(self, index):
        """Set speed by index (0-6)"""
        self.current_speed_index = index
        if index == 0:
            self.send_command("0")
        elif index <= 6:
            self.send_command(str(index))
    
    def set_speed_preset(self, preset):
        """Set speed by preset name: 'slow', 'medium', 'fast'"""
        presets = {'slow': 'L', 'medium': 'M', 'fast': 'F'}
        if preset in presets:
            self.send_command(presets[preset])
    
    # === Status Commands ===
    
    def request_status(self):
        """Request status from Arduino"""
        self.send_command('?')
    
    def zero_all(self):
        """Zero all axis positions"""
        self.send_command('Z')
    
    def go_origin(self):
        """Move to origin position"""
        self.send_command('G')
    
    def toggle_mode(self):
        """Toggle between serial/joystick mode"""
        self.send_command('T')