from common.numpy_fast import clip

def rate_limit(new_value, last_value, dw_step, up_step):
  return clip(new_value, last_value + dw_step, last_value + up_step)

def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, steer_override):
  # simple integral controller that learns how much steering offset to put to have the car going straight
  min_offset = -1.  # deg
  max_offset =  1.  # deg
  alpha = 1./36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
  min_learn_speed = 1.

  # learn less at low speed or when turning
  alpha_v = alpha*(max(v_ego - min_learn_speed, 0.))/(1. + 0.5*abs(y_des))

  # only learn if lateral control is active and if driver is not overriding:
  if lateral_control and not steer_override:
    angle_offset += d_poly[3] * alpha_v
    angle_offset = clip(angle_offset, min_offset, max_offset)

  return angle_offset