function control = lane_change_policy(world_state, id, lane_id, P)
% lane-shift macro-action for a given vehicle id
% the policy moves the vehicle laterally toward the desired new lane
%
% s: current decision state, 
% id: vehicle id
% shift: shift in lane id (-1 or +1 are valid)
% P: problem params

state = world_state.car_states(id);
road = P.roads(state.road_id);

assert(lane_id > 0 && lane_id <= length(road.lanes));

% dey is the desired lateral ey offset
dey = road.lanes(lane_id).ey - state.ey;

desired_state = state;
desired_state.yaw = 0;
desired_state.ey = state.ey + dey;


% We can either do this in two ways:
%
% 1) Planned maneuver to the new lane (using a spline)
%dx = state.v;
%desired_state.x = state.x + dx;
%desired_state.t = inf; %state.t + sqrt(dx*dx + dey*dey)/state.v;
%control = spline_controller(state, desired_state, P);

% 2) Reactive velocity-lateral motion
control = velocity_lateral_controller(state, desired_state, P);
control = bound_control(control, P.cars(id));

if control.steering_angle >= 0
  control.mode = P.kLeft;
else
  control.mode = P.kRight;
end
