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

P = setup_problem(2);

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

P.cars(1).v_nominal = 8;


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

world_state.car_states(2) = init_state(2, -6, 4, -2);
world_state.car_states(2).road_id = 2;
P.cars(2).v_nominal = 4;

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

P.time_final = 10;

P.animate_motion = 1;
fig1 = figure;

P.cars(1).action_policy = @(world_state,P)right_turn_action_policy(world_state, 1, P.cars(1).goal_state, P);

P.cars(2).action_policy = @(world_state,P)right_turn_action_policy(world_state, 2, P.cars(2).goal_state, P);

[trace, violations, terminal] = rollout(world_state, {P.cars.action_policy}, 10, P);

fig2 = figure;
plot_results(trace, P);



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

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

i = find([actions.id] == P.kTurn.Right);
if isempty(i)
  action = init_action(P.kInlane, @(world_state, P)inlane_policy(world_state, id, goal_state, P));
else
  action = actions(i);
end

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

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

i = find([actions.id] == P.kTurn.Left);
if isempty(i)
  action = init_action(P.kInlane, @(world_state, P)inlane_policy(world_state, id, goal_state, P));
else
  action = actions(i);
end