function actions = feasible_actions(world_state, id, goal_state, P)
% Generate feasible actions for a given world_state
%
% We consider these macro-actions (actions)
%
% -------------------------------------------------
% Name        |  Function           |      Parameters
%--------------------------------------------------
% InLane      | drive to goal state | goal_state = next waypoint
% LaneChange  | lane-changing       | lane = {left_lane, right_lane}
% Acc         | velocity tracking   | goal_state with x=inf, v = {0, v_nominal}
% Turn        | turn into road      | {left, right}
% 

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



% 1) InLane Policy: we always provide the InLane policy
% don't car about ey
goal_state.ey = nan;
actions(1) = init_action(P.kInlane, @(world_state, P)inlane_policy(world_state, id, goal_state, P));
  
% 2) LangeChange(right)
% if there is room to move toward first lane
if state.lane_id > 1
  goal_state.lane_id = state.lane_id - 1;
  goal_state = set_to_center(goal_state, P);
  actions(end+1) = init_action(P.kLane.Right, @(world_state, P)inlane_policy(world_state, id, ...
                                                    goal_state, P), @(world_state, P)lane_change_termination(world_state, id, goal_state.lane_id, P));
  %  lane_id = state.lane_id - 1;
  %actions(end+1) = init_action(P.kLane.Right, @(world_state, P)lane_change_policy(world_state, id, lane_id, P), ...
  %                             @(world_state, P)lane_change_termination(world_state, id, lane_id, P));
end   

% 3) LangeChange(left)
% if there is room to move toward last lane
if state.lane_id < length(road.lanes)
  goal_state.lane_id = state.lane_id + 1;
  goal_state = set_to_center(goal_state, P);
  actions(end+1) = init_action(P.kLane.Left, @(world_state, P)inlane_policy(world_state, id, ...
                                                    goal_state, P), @(world_state, P)lane_change_termination(world_state, id, goal_state.lane_id, P));
  % lane_id = state.lane_id + 1;
  %  actions(end+1) = init_action(P.kLane.Left, @(world_state, P)lane_change_policy(world_state, id, lane_id, P), ...
  %                             @(world_state, P)lane_change_termination(world_state, id, lane_id, P));
end


% 4) AccPolicy(zero_velocity)
if state.v > P.v_tol
  % create a goal state with zero velocity
  desired_state = set_to_center(state, P);
  desired_state.v = 0;
  desired_state.x = inf;
  actions(end+1) = init_action(P.kSpeed.Zero, @(world_state, P)acc_policy(world_state, id, desired_state, P));
end

% 5) AccPolicy(nominal_velocity)
if state.v < P.cars(id).v_nominal - P.v_tol
  % create a goal state with desired nominal velocity
  desired_state = set_to_center(state, P);
  desired_state.v = P.cars(id).v_nominal;
  desired_state.x = inf;
  actions(end+1) = init_action(P.kSpeed.Nominal, @(world_state, P)acc_policy(world_state, id, desired_state, P));
end

return

% Turning to new roads (forking) when entering intersections
if P.enable_turns && state.in_intersection && ~state.turned_to_road_id && ~state.committed_turn
  road = P.roads(state.road_id);

  % 6) Turn(left)
  if ~isempty(road.left_connections)
    desired_state = road.left_connections(1).state;
    desired_state.v = P.cars(id).v_nominal;
    actions(end+1) = init_action(P.kTurn.Left, ...
                                 @(world_state, P)turn_policy(world_state, id, desired_state, P), ...
                                 @(world_state, P)turn_termination(world_state, id, desired_state, P));
  end
  
  % 7) Turn(right)
  if ~isempty(road.right_connections)
    desired_state = road.right_connections(1).state;
    desired_state.v = P.cars(id).v_nominal;
    actions(end+1) = init_action(P.kTurn.Right, ...
                                 @(world_state, P)turn_policy(world_state, id, desired_state, P), ...
                                 @(world_state, P)turn_termination(world_state, id, desired_state, P));    
  end
end
