function f = test_acc_policy()
% 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, 4, 1, -2);
world_state.car_states(1).road_id = 1;

world_state.car_states(2) = init_state(2, 4, 1, -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 = 20;

P.animate_motion = 1;
fig1 = figure;

desired_state = init_state(1, inf, 10, -2);

hero_action = init_action(1, @(world_state,P) acc_policy(world_state, 1, desired_state, P));

P.cars(1).action_policy = @(world_state,P) fixed_action_policy(world_state, hero_action, P);

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


fig2 = figure;

plot_results(trace, P);

