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

clear

P = setup_problem(8, @multilane_roads);

% car 1
world_state.car_states(1) = init_state(1, 5, 5);
P.cars(1).goal_state.lane_id = 4;

% car 2
world_state.car_states(2) = init_state(2, 20, 5);
P.cars(2).goal_state.lane_id = 4;

% car 3
world_state.car_states(3) = init_state(3, 2, 5);
world_state.car_states(3).lane_id = 2;
P.cars(3).v_nominal = 6;
P.cars(3).goal_state.lane_id = 4;

% car 4
world_state.car_states(4) = init_state(4, 15, 5);
world_state.car_states(4).lane_id = 2;
P.cars(4).v_nominal = 6;
P.cars(4).goal_state.lane_id = 4;

% car 5
world_state.car_states(5) = init_state(5, 3, 4);
world_state.car_states(5).lane_id = 3;
P.cars(5).goal_state.lane_id = 1;

% car 6
world_state.car_states(6) = init_state(6, 20, 4);
world_state.car_states(6).lane_id = 3;
P.cars(6).goal_state.lane_id = 1;

% car 7
world_state.car_states(7) = init_state(7, -10, 6);
world_state.car_states(7).lane_id = 4;
P.cars(7).goal_state.lane_id = 1;

% car 8
world_state.car_states(8) = init_state(8, -20, 5);
world_state.car_states(8).lane_id = 4;
P.cars(8).goal_state.lane_id = 1;

for i=1:length(P.cars)
  world_state.car_states(i) = set_to_center(world_state.car_states(i), P);  
  P.cars(i).goal_state = set_to_center(P.cars(i).goal_state, P);  
end

world_state.car_controls = [];

P.enable_turns = 0;

% choose different actions sets to compare performance
%
P.cars(1).feasible_actions = @feasible_actions;
% P.cars(1).feasible_actions = @simple_velocity_lane_actions;
% P.cars(1).feasible_actions = @simple_acceleration_lane_actions;

for k=2:length(P.cars)
  P.cars(k).action_distribution = @(world_state,P)manual_drive_distribution(world_state, k, ...
                                                    P.cars(k).goal_state, P);
end

P.animate_motion = 1;
P.visualize_each_horizon = true;

fig1 = figure;

P.action_duration = 1;

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

[trace, violations, 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 = manual_drive_distribution(world_state, id, goal_state, P)

state = world_state.car_states(id);

len = state.v*3;

distribution.samples(1).likelihood = 1;

if 0

if goal_state.lane_id < state.lane_id || goal_state.lane_id > state.lane_id
  start_state = state;
  
  if goal_state.lane_id < state.lane_id
    start_state.lane_id = state.lane_id - 1;
  else
    start_state.lane_id = state.lane_id + 1;
  end
  
  start_state.x = start_state.x - P.cars(id).length;
  start_state = set_to_center(start_state, P);
  
  if region_free(world_state, start_state, len, P) < eps
    goal_state.lane_id = start_state.lane_id;
    goal_state = set_to_center(goal_state, P);
    distribution.samples(1).action = ...
        init_action(P.kInlane, @(world_state, P)inlane_policy(world_state, id, goal_state, P));
    return
  end
  
  if 0
  acc_state = state;
  if goal_state.lane_id < state.lane_id 
    acc_state.lane_id = state.lane_id - 1;
  else 
    acc_state.lane_id = state.lane_id + 1;
  end
  acc_state.x = acc_state.x + P.cars(id).length;
  
  desired_state = set_to_center(state, P);
  desired_state.x = inf;
  
  vmax = region_free(world_state, acc_state, len, P);
  
  if vmax < state.v
    desired_state.v = state.v + 5;
    % create a goal state with zero velocity
  else
    desired_state.v = max(state.v - 5, 0);
  end
  
  distribution.samples(1).action = ...
      init_action(P.kSpeed.Zero, @(world_state, P)acc_policy(world_state, id, ...
                                                    desired_state, P));
  return
  
  end
end

end

goal_state.ey = nan;

distribution.samples(1).action = ...
    init_action(P.kInlane, @(world_state, P)inlane_policy(world_state, id, goal_state, P));
