function control = inlane_policy(world_state, id, goal_state, P)
% Policy to drive to a given goal state in the vehicle's lane ahead
% 
% The policy handles intersections with stop signs and
% yield signs; and following of other vehicles.
%
% The strategy is to first find relevant symbols on the road ahead:
%
% 1) vehicles in our lean
% 2) stop sign or yield sign ahead (if we need to yield)
% 3) pedestrians (currently unsupported)
% 4) ...
%
% These symbols generate subgoals with associated controls to reach them.
%
% We then compute control curves (acceleration and steering angle) to our main goal
% as well as each subgoal and take the control signal with the lowest acceleration 
% (most conservative) as the output control signal for this policy.
% The final step is to combine this acceleration with a steering angle control
% that ensures the vehicle stays at a lateral offset given by the goal_state

subgoal_states = generate_subgoals(world_state, id, goal_state, P);

state = world_state.car_states(id);

% ensure goal state has the same road and lane info as us
goal_state.road_id = state.road_id;

% if not particular ey is desired then use current lane ey
if isnan(goal_state.ey) || isnan(goal_state.lane_id)
  goal_state.lane_id = state.lane_id;
  goal_state = set_to_center(goal_state, P);
end

% default control is to the given final goal
control = acc_controller(state, goal_state, P);

% compute the most conservative control with respect to subgoals
for i=1:length(subgoal_states)
  acc_control = acc_controller(state, subgoal_states(i), P);
  if control.acc > acc_control.acc
    control.acc = acc_control.acc;
  end
end
  
% finally, add steering control to stay close to lane center

steering_control = velocity_lateral_controller(state, goal_state, P);
control.steering_angle = steering_control.steering_angle;
