"""Physics-clock command envelope; perception remains asynchronous.

Limit wheel-surface acceleration without delaying a safety reduction. Scaling
both sides together preserves the planner's requested curvature. Stops bypass
the ramp, as required by the already qualified collision/braking envelope.
This is a simulation actuator, not a VESC controller or a motor calibration.
"""

CONTROL_PROFILE = {
    "clock": "physics-pre-step",
    "rate_hz": 60,
    "wheel_surface_acceleration_mps2": 0.2,
    "safety_reductions": "immediate",
    "authority": "simulation-only",
}


class DriveEnvelope:
    def __init__(self, track_width=0.9, acceleration=0.2):
        self.half_track = track_width / 2
        self.acceleration = acceleration
        self.wheels = (0.0, 0.0)

    def step(self, velocity, yaw_rate, dt, *, stop=False):
        requested = (velocity - yaw_rate * self.half_track, velocity + yaw_rate * self.half_track)
        if stop or max(map(abs, requested)) < 1e-8:
            self.wheels = (0.0, 0.0)
            return 0.0, 0.0
        scale = 1.0
        for old, new in zip(self.wheels, requested, strict=True):
            # Reversal starts from zero; never retain motion in the old direction.
            prior = abs(old) if old * new >= 0 else 0.0
            if abs(new) > prior:
                scale = min(scale, (prior + self.acceleration * max(0.0, dt)) / abs(new))
        self.wheels = tuple(value * scale for value in requested)
        return velocity * scale, yaw_rate * scale