"""Velocity range presets. V4 is time-based; v5/v6/v7 are stair-gated.

S10 v7 uses ``V7_STAGES`` as the 1.5/0.8/1.2 cap, but stage advancement comes
from ``stair_curriculum.next_stage`` (actual up-stair pass rate). Only v4
advances by iteration count alone.
"""
STAGES = (
  (0, (-.3,.8), (-.2,.2), (-.8,.8)),
  (1000, (-.5,1.), (-.4,.4), (-1.,1.)),
  (2500, (-.8,1.5), (-.6,.6), (-1.5,1.5)),
  (5000, (-1.5,1.8), (-.8,.8), (-2.,2.)),
  (7500, (-2.,2.), (-1.,1.), (-3.,3.)),
)
V7_STAGES = (
  (0, (-.3,.8), (-.2,.2), (-.5,.5)),
  (1000, (-.5,1.), (-.4,.4), (-.7,.7)),
  (2500, (-.8,1.2), (-.6,.6), (-.9,.9)),
  (5000, (-1.2,1.4), (-.7,.7), (-1.1,1.1)),
  (7500, (-1.5,1.5), (-.8,.8), (-1.2,1.2)),
)


def stages_for(revision):
  return V7_STAGES if revision >= 7 else STAGES


def set_command_stage(cfg, iteration, stage=None, stages=STAGES):
  index = stage if stage is not None else max(
    i for i, item in enumerate(stages) if max(0, iteration) >= item[0])
  _, x, y, yaw = stages[index]
  cfg.ranges.lin_vel_x, cfg.ranges.lin_vel_y, cfg.ranges.ang_vel_z = x, y, yaw
  return index