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

P = setup_problem(8);

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

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

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

world_state.car_states(3) = init_state(3, -6, 4, 2);
world_state.car_states(3).road_id = 2;

for i=4:length(P.cars)
  world_state.car_states(i) = world_state.car_states(3);
  world_state.car_states(i).id = i;
  world_state.car_states(i).x = world_state.car_states(i-1).x - 8;  
  world_state.car_states(i).road_id = 1;
  if mod(i,2)
    world_state.car_states(i).ey = 2;
  else
    world_state.car_states(i).ey = -2;  
  end
end


P.animate_motion = 1;

fig1 = figure;
lane_action = init_action(P.kLane.Right, @(world_state, P)lane_change_policy(world_state, 1, 2, P), ...
                          @(world_state,P)final_time_termination(world_state, inf, P));

%lane_action = init_action(P.kLane.Right, @(world_state, P)lane_change_policy(world_state, 1, 2, P), ...
%                          @(world_state,P)final_time_termination(world_state, inf, P));

lane_action = init_action(P.kLane.Right, @(world_state, P)lane_change_policy(world_state, 1, 2, P), ...
                          @(world_state,P)lane_change_termination(world_state, 1, 2, P));


P.cars(1).action_policy = @(world_state,P)fixed_action_policy(world_state, lane_action, P);
[lane_trace, violations, terminal] = rollout(world_state, {P.cars.action_policy}, 2, P);

world_state = lane_trace(end);
world_state.car_controls = [];  % remove car_controls in order to start with a new action next

goto_action = init_action(P.kInlane, @(world_state, P)inlane_policy(world_state, 1, P.cars(1).goal_state, P));
P.cars(1).action_policy = @(world_state,P)fixed_action_policy(world_state, goto_action, P);
[trace, violations, terminal] = rollout(world_state, {P.cars.action_policy}, 10, P);

trace = [lane_trace, trace];

fig2 = figure;
plot_results(trace, P);

close(P.video)