function states = generate_subgoals(world_state, id, goal_state, P)
% Generate subgoals corresponding to symbols of interest on the road
% 
% It handles intersections with stop signs and
% yield signs; and following of other vehicles.
%
% Possible subgoals include:
%
% 1) vehicles in our lane
% 2) intersections (if a possible collision could happen)
% 3) crosswalks (currently not supported)
% 4) ...

states = [];

car = P.cars(id);
state = world_state.car_states(id);
road = P.roads(state.road_id);
lane = road.lanes(state.lane_id);

% relevant symbols
symbols = state.symbols;

% check cars ahead
if ~isempty(symbols)
  car_symbols = symbols(find([symbols.type] == P.kCar));
  if ~isempty(car_symbols)
    car_states = world_state.car_states([car_symbols.id]);
    car_states_ahead = car_states(find([car_states.x] > state.x & [car_states.lane_id] == state.lane_id));
    %    [x,ind] = sort([car_states_ahead.x], 1, 'ascend');
    %for i=1:length(ind)
    %  car_state = car_states_ahead(ind);      
    %  gap = car_state.ey 
    %end
    if ~isempty(car_states_ahead)
      [min_x, min_ind] = min([car_states_ahead.x]);
      
      % create a subgoal to follow the car
      follow_state = car_states_ahead(min_ind);
      follow_car = P.cars(follow_state.id);
      follow_state.x = follow_state.x - follow_car.length - P.follow_distance;
      follow_state = set_to_center(follow_state, P);
      
      states = [states, follow_state];
    end
  end
  
  % if car is driving toward a stop sign, execute a stopping motion
  stop_symbols = symbols(find([symbols.type] == P.kStop));
  if ~isempty(stop_symbols)
    % if we haven't already stopped at the stop-sign
    front_s = car_front_s(state, car);
    if state.v > 0 && front_s < road.intersection.s0 && isnan(state.time_stopped_at_stop)
      % generate a subgoal at stop sign
      stop_state = state;
      stop_state.x = road.intersection.s0 - P.stop_region_length/2 - car.length/2;
      stop_state.v = 0; 
      
      states = [states, stop_state];
    end
  end
end
  

% if car hasn't passed intersection and must yield
if state.x < road.intersection.sf && state.yield
  % obtain an acceleration profile to the intersection under nominal driving
  state_at_int = state;
  state_at_int.x = max(road.intersection.s0 - car.length/2, state_at_int.x);
  state_at_int.v = car.v_nominal;
  [states, controls] = acc_profile(state, state_at_int, P);
  
  % compute possible collision time with other car
  time_in = states(end).t - state.t;
  
  for car_id=1:length(P.cars)
    % skip self
    if car_id == id
      continue;
    end
    car_state = world_state.car_states(car_id);
    car_road = P.roads(car_state.road_id);
    
    front_s = car_front_s(car_state, P.cars(car_id));
    
    car_time_in = (car_road.intersection.s0 - front_s)/car_state.v;
    car_time_out = (car_road.intersection.sf - front_s)/car_state.v;
    
    % generate stop before intersection if car is about to enter intersection
    % within some time_margin of car being in and out
    if (time_in > car_time_in - P.yield_time_margin_in && ...
        time_in < car_time_out + P.yield_time_margin_out)

      state_at_int = state;
      state_at_int.x = road.intersection.s0 - car.length/2;
      state_at_int.v = 0;

      states = [states, state_at_int];
      break
    end
  end
end