import numpy as np


def generate_trajectory(start, end, steps=100, space="cartesian"):
    """
    Generates a linear trajectory from start to end over `steps`.
    Supported spaces: 'cartesian', 'joint'
    """
    if not isinstance(start, dict) or not isinstance(end, dict):
        raise ValueError("Start and end must be dictionaries.")

    keys = sorted(start.keys())
    if keys != sorted(end.keys()):
        raise ValueError("Start and end must have matching keys.")

    traj = []
    for i in range(steps):
        point = {}
        for key in keys:
            point[key] = float(np.linspace(start[key], end[key], steps)[i])
        traj.append(point)

    return traj


def execute_trajectory(api, trajectory, space="cartesian", rate=0.012):
    """
    Sends the trajectory to the robot using the RSIPI API.
    - space: 'cartesian' or 'joint'
    - rate: time between points in seconds (e.g., 0.012 for 12 ms)
    """
    import time

    for point in trajectory:
        if space == "cartesian":
            api.update_cartesian(**point)
        elif space == "joint":
            api.update_joints(**point)
        else:
            raise ValueError("Space must be 'cartesian' or 'joint'")
        time.sleep(rate)