function f = test_mdp_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(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 = .3;

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

% for this scenario we do not need to consider left/right turn actions
P.cars(1).feasible_actions = @nonturn_feasible_actions;
P.cars(4).action_policy = @(world_state,P)rand_action_policy(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 = 30;
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);


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 = .5;

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 = .5;

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

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

  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
  
  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


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);
