function f = test_mdp_is_planner()
% setup a simple example with four vehicles on an intersection
% with two-lane one-way streets
% Hero is #1, Car#4 has stochastic behavior

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(2) = init_state(2, 16, 2, -2);
% P.cars(2).v_nominal = 2;
% P.cars(2).goal_state.v = 2;
P.cars(2).goal_state = set_to_center(P.cars(2).goal_state, P);

%P.cars(2).action_distribution = @(world_state,P)rand_action_distribution(world_state, 2, P.cars(2).goal_state, P);

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

world_state.car_states(4) = init_state(4, 0, 2, 2);
world_state.car_states(4).road_id = 2;
world_state.car_states(4).lane_id = 2;
P.cars(4).goal_state.road_id = 2;
P.cars(4).goal_state = set_to_center(P.cars(4).goal_state, P);

% for this scenario we do not need to consider left/right turn actions
P.enable_turns = 0;
P.cars(1).feasible_actions = @feasible_actions;
% P.cars(4).action_distribution = @(world_state,P)rand_action_distribution(world_state, 4, P.cars(4).goal_state, P);

P.enable_turns = 0;

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

fig1 = figure;

P.number_samples = 3;
P.time_final = 20;
P.planner_horizon = 6;

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

fig2 = figure;
plot_results(trace, P);

close(P.video)

figure
plot_stats(stats, P);


function distribution = rand_action_distribution(world_state, id, goal_state, P)

goal_state.lane_id = 1;
goal_state = set_to_center(goal_state, P);

distribution.samples(1).action = init_action(P.kTurn.Right, @(world_state, P)inlane_policy(world_state, id, goal_state, P));
distribution.samples(1).likelihood = .8;

goal_state.lane_id = 2;
goal_state = set_to_center(goal_state, P);

distribution.samples(2).action = init_action(P.kTurn.Left, @(world_state, P)inlane_policy(world_state, id, goal_state, P));
distribution.samples(2).likelihood = .2;
