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

clear

P = setup_problem(2);

world_state.car_states(1) = init_state(1, 0, 8, -2);
world_state.car_states(1).road_id = 1;
P.cars(1).v_nominal = 9;
P.roads(1).intersection.sign = P.kYield;

world_state.car_states(2) = init_state(2, -6, 8, -2);
world_state.car_states(2).road_id = 2;
P.cars(2).v_nominal = 10;
%P.roads(2).intersection.sign = P.kStop;

P.animate_motion = 1;
fig1 = figure;

%P.cars(3).action_policy = @(s,P)rand_action_policy(s, 4, P.cars(4).goal_state, P);
%P.cars(1).feasible_actions = @nonturn_feasible_actions;
P.cars(2).action_policy = @(world_state,P)rand_action_distribution(world_state, 2, P.cars(2).goal_state, P);

P.visualize_each_horizon = 1;

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

[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 of the executed pat
for i=1:length(trace)
  animate_callback(trace(i), P);
end

fig2 = figure;
plot_results(trace, P);

figure
plot_stats(stats, P);



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

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

goal_state.road_id = 1;
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 = 1;

% 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;
    distribution.samples(end+1).action = 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));
    distribution.samples(end).likelihood = 1;
  end
  
  if ~isempty(road.right_connections)
    desired_state = road.right_connections(1).state;
    desired_state.v = P.cars(id).v_nominal;
    distribution.samples(end+1).action = 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

normalizer = sum([distribution.samples.likelihood]);
for i=1:length(distribution.samples)
  distribution.samples(i).likelihood = distribution.samples(i).likelihood/normalizer;
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);