function f = test_planner_simple_actions()
% setup a simple example stopping at a stop sign
% and yielding to another crossing vehicle

clear

P = setup_problem(4);

P.roads(1).intersection.sign = P.kStop;
P.roads(2).intersection.sign = P.kStop;

world_state.car_states(1) = init_state(1, 4, 2, -2);
world_state.car_states(1).road_id = 1;

world_state.car_states(2) = init_state(2, 16, 2, -2);
world_state.car_states(2).road_id = 1;
P.cars(2).v_nominal = 2;
P.cars(2).goal_state.v = 2;

world_state.car_states(3) = init_state(3, 0, 2, 2);
world_state.car_states(3).road_id = 1;
P.cars(3).v_nominal = 3;
P.cars(3).goal_state.v = .5;

world_state.car_states(4) = init_state(4, 0, 2, 2);
world_state.car_states(4).road_id = 2;

world_state.car_controls = [];

P.enable_turns = 0;

% choose different actions sets to compare performance
%
% P.cars(1).feasible_actions = @feasible_actions;
% P.cars(1).feasible_actions = @simple_velocity_lane_actions;
P.cars(1).feasible_actions = @simple_acceleration_lane_actions;

P.animate_motion = 1;
P.visualize_each_horizon = true;

fig1 = figure;

P.action_duration = 1;

P.number_samples = 1;
P.time_final = 20;
P.planner_horizon = 5;

[trace, violtions, stats] = execute_planner(world_state, P.planner_horizon, 1, P);

input('press any key to animate motion')

P.animate_motion = 1;
% do a complete simulation with the planner delays
for i=1:length(trace)
  animate_callback(trace(i), P);
end

fig2 = figure;
plot_results(trace, P);

close(P.video)

figure
plot_stats(stats, P);