from .safety_manager import SafetyManager
import time

def generate_trajectory(start, end, steps=100, space="cartesian", mode="absolute", include_resets=False):
    """
    Generates a trajectory from start to end across N steps.

    - Absolute mode (default): full poses, no resets
    - Relative mode: incremental steps, optional resets after each step
    """
    if mode not in ["relative", "absolute"]:
        raise ValueError("mode must be 'relative' or 'absolute'")
    if space not in ["cartesian", "joint"]:
        raise ValueError("space must be 'cartesian' or 'joint'")

    if mode == "absolute":
        include_resets = False  # Smart safeguard

    axes = start.keys()
    trajectory = []

    # Optional safety check hook — assumes SafetyManager has static validation methods
    safety_fn = SafetyManager.check_cartesian_limits if space == "cartesian" else SafetyManager.check_joint_limits
    global enforce_safety
    enforce_safety = hasattr(SafetyManager, "check_cartesian_limits")  # Enable only if those methods exist


    for i in range(1, steps + 1):
        point = {}
        for axis in axes:
            delta = end[axis] - start[axis]
            value = start[axis] + (delta * i / steps)

            point[axis] = delta / steps if mode == "relative" else value

        # Optional safety enforcement
        if enforce_safety and not safety_fn(point):
            raise ValueError(f"⚠️ Safety check failed at step {i}: {point}")

        trajectory.append(point)

        if mode == "relative" and include_resets:
            # Insert a zero-correction step to prevent drift
            trajectory.append({axis: 0.0 for axis in axes})

    return trajectory

def execute_trajectory(api, trajectory, space="cartesian", rate=0.004):
    """
    Sends a list of corrections to the RSI API at fixed intervals.

    Args:
        api: An RSI-compatible API object with update_cartesian / update_joints methods.
        trajectory (list[dict]): Movement steps generated by generate_trajectory().
        space (str): "cartesian" or "joint".
        rate (float): Time between steps in seconds (default = 4ms).
    """
    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)