function control = acc_policy(world_state, id, desired_state, P)
% A policy which uses an acc controller and a lateral controller to 
% reach a desired car state ahead. 
% The longitudinal controller uses the variables (x,v) in desired state and
% reaches them optimally, while the lateral controller tracks the desired state ey offset 

% our car state
state = world_state.car_states(id);

% invoke the lateral controller
control = velocity_lateral_controller(state, desired_state, P);

if ~isinf(desired_state.x)
  % invoke the acc controller
  longitudinal_control = acc_controller(state, desired_state, P);
  % overwrite lateral
  control.acc = longitudinal_control.acc;
end

% add actional regularization
if isfield(world_state, 'car_controls') && P.acc_prior > eps
  control.acc = (1 - P.acc_prior)*control.acc + P.acc_prior*world_state.car_controls(id).acc;
end

% set mode for informational/plotting purposes
if abs(control.acc) < eps
  control.mode = P.kCoast;
else
  if control.acc < 0
    control.mode = P.kSlower;
  else
    control.mode = P.kFaster;
  end
end
