function f = test_planner_lane_stops()
% 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.cars(1).feasible_actions = @nonturn_feasible_actions;
%P.cars(1).feasible_actions = @simple_actions;

P.cars(4).action_policy = @(world_state,P)rand_action_policy(world_state, 4, P.cars(4).goal_state, P);

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

P.time_final = 20;

fig1 = figure;

P.action_duration = 1;
P.planner_horizon = 5;

[trace, violations, 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_all)
  animate_callback(trace(i), P);
end

fig2 = figure;
plot_results(trace_all, P);

close(P.video)

figure
plot_stats(stats, P);


function action = rand_action_policy(world_state, id, goal_state, P)

% distribution over actions
ps = zeros(8,1);

ps(P.kInlane) = 0.4;

% uncomment to create stochasticity
%ps(P.kLane.Left) = 0.15;
%ps(P.kLane.Right) = 0.15;
%ps(P.kSpeed.Zero) = 0.15;
%ps(P.kSpeed.Nominal) = 0.15;

actions = feasible_actions(world_state, id, goal_state, P);

% sample an action
ps = ps([actions.id]);
i = discreternd(ps);
action = actions(i);


function actions = nonturn_feasible_actions(world_state, id, goal_state, P)
% for hero we will ignore iterating over left/right turns

actions = feasible_actions(world_state, id, goal_state, P);
actions = actions([actions.id] ~= P.kTurn.Right & [actions.id] ~= P.kTurn.Left);
