import numpy as np

# Kinematics matrix - modify based on your robot type
J = ...

def controller(t, X_I, dX_I, target_pos):
    """
    Real robot controller function template.
    
    This function is called by runner.py at each control timestep.
    
    Args:
        t (float): Current time in seconds since control started
        X_I (numpy.ndarray): Current robot state [x, y, theta] as 3x1 column vector
                            - x: position in x-direction (meters)
                            - y: position in y-direction (meters) 
                            - theta: orientation angle (radians)
        dX_I (numpy.ndarray): Current robot velocities [dx, dy, dtheta] as 3x1 column vector
                             - dx: velocity in x-direction (m/s)
                             - dy: velocity in y-direction (m/s)
                             - dtheta: angular velocity (rad/s)
        target_pos (numpy.ndarray): Target position [x, y, theta] as 3x1 column vector
                                   - x: target x-position (meters)
                                   - y: target y-position (meters)
                                   - theta: target orientation (radians)
    
    Returns:
        U (numpy.ndarray): Control output - wheel speeds in rad/s
                          Shape depends on robot type:
                          - 2x1 for differential/tank drive
                          - 3x1 for 3-wheel omnidirectional  
                          - 4x1 for mecanum/4-wheel omnidirectional
    """
    
    dX_R_des = ...
    
    # Convert desired robot velocities to wheel speeds using kinematics
    U = J @ dX_R_des
    
    # Velocity limits 
    max_wheel_speed = 5.236  # rad/s = 50 rpm.
    U = np.clip(U, -max_wheel_speed, max_wheel_speed)
    
    return U

# Optional: Add any helper functions you might need
def normalize_angle(angle):
    """Normalize angle to [-pi, pi] range"""
    while angle > np.pi:
        angle -= 2*np.pi
    while angle < -np.pi:
        angle += 2*np.pi
    return angle

def distance_to_point(x, y, target_x, target_y):
    """Calculate Euclidean distance to a target point"""
    return np.sqrt((target_x - x)**2 + (target_y - y)**2)

def angle_to_point(x, y, target_x, target_y):
    """Calculate angle to a target point"""
    return np.arctan2(target_y - y, target_x - x)