function P = setup_problem(number_cars, varargin)
% setup a problem with a total of number_cars (including hero)

% generic undefined id
P.kNone = 0;

% types of road symbols
P.kYieldRegion = 1;
P.kStopRegion = 2;
P.kCrossRegion = 3;
P.kCar = 4;

P.symbol_ids = [P.kYieldRegion, P.kStopRegion, P.kCrossRegion, P.kCar];
P.symbol_names = {'Yield', 'Stop', 'Cross', 'Car'};


% action/action classes
P.kInlane = 1;
P.kLane.Left = 2;
P.kLane.Right = 3;
P.kSpeed.Zero = 4;
P.kSpeed.Coast = 5;
P.kSpeed.Nominal = 6;
P.kTurn.Left = 7;
P.kTurn.Right = 8;
P.action_ids = [P.kNone, P.kInlane, P.kLane.Left, P.kLane.Right, ...
                P.kSpeed.Zero, P.kSpeed.Coast, P.kSpeed.Nominal, ...,
                P.kTurn.Left, P.kTurn.Right];
P.action_labels = {'None', 'Goto', 'Lane.Left', 'Lane.Right', ...
                   'Speed.Zero', 'Speed.Coast', 'Speed.Nominal', ...
                   'Turn.Left', 'Turn.Right'};


% action internal modes -- these can provide more info about a chosen action
P.kStop = 1;
P.kYield = 2;
P.kFollow = 3;
P.kGoal = 4;
P.kStraight = 5;
P.kLeft = 6;
P.kRight = 7;
P.kCoast = 8;
P.kSlower = 9;
P.kFaster = 10;
P.mode_ids = [P.kNone, P.kStop, P.kYield, P.kFollow, P.kGoal, P.kLeft, P.kRight, P.kCoast, P.kSlower, P.kFaster];
P.mode_labels = {'None', 'Goto.Stop', 'Goto.Yield', 'Goto.Follow', 'Goto.Goal', 'Straight', 'Left', 'Right', 'Speed.Coast', 'Speed.Slower', 'Speed.Faster'};


% constraint violations
P.kCollision = 1;  % physical obstacle collision
P.kIncorrect = 2;  % logical rule correctness violations

%load roads
if nargin < 2
  P = default_roads(P);
else
  roads = varargin{1};
  P = roads(P);
end

enable_obstacles = 0;

if enable_obstacles
  P.roads(1).obstacles(1).state = init_state(0, 20, 0, 5);
  P.roads(1).obstacles(1).state.road_id = 1;
  P.roads(1).obstacles(1).length = 6;
  P.roads(1).obstacles(1).width = 3;
end

% set hero to the first car by default: 
P.hero_id = 1;

% hero entity, chosen driving style
P.cars(1).v_nominal = 10;
P.cars(1).acc_nominal = 1;
P.cars(1).acc_min = -4;
P.cars(1).acc_max = 2;
P.cars(1).steering_angle_min = -0.5;
P.cars(1).steering_angle_max = 0.5;
P.cars(1).name = 'car_1';

P.cars(1).max_jerk = inf;

P.cars(1).goal_state = init_state(1, 120, 10);
P.cars(1).goal_state.t = inf;
P.cars(1).goal_state.lane_id = nan;

% what actions we allow for this car
P.cars(1).feasible_actions = @feasible_actions;

% stores computed optimal state sequence for display purposes
P.cars(1).traces = [];

P.cars(1).length = 4;
P.cars(1).width = 2;
P.cars(1).wheel_base = 3.5;


% create additional cars, here we just replicate hero
% but they can have different properties if needed
for id=2:number_cars
  P.cars(id) = P.cars(1);
  P.cars(id).name = ['car_' num2str(id)];  
  P.cars(id).goal_state = set_to_center(P.cars(id).goal_state, P);
end

% default policy for all cars is the goto policy
for id=1:length(P.cars)
  P.cars(id).action_distribution = @(s, P)nominal_action_distribution(s, id, P.cars(id).goal_state, ...
                                                    P);

  action = init_action(P.kInlane, @(s, P)inlane_policy(s, id, P.cars(id).goal_state, P));
  P.cars(id).action_policy = @(world_state, P)fixed_action_policy(world_state, action, P);
end


% under nominal driving, yielding is activated
% if hero is about to arrive at intersection within the interval
% [car_time_in - yield_time_margin_in, car_time_out + yield_time_margin_out]
% where car_time_in/out is the time car enters/exits the same intersection
P.yield_time_margin_in = 3; % seconds
P.yield_time_margin_out = 2; % seconds

% distance at which to follow other vehicles
P.follow_distance = 2;

% velocity and position tolerances
P.v_tol = 0.1;
P.s_tol = 0.1;
P.ey_tol = 0.1;
P.stop_region_length = 1;

% nominal lateral velocity for lane changing
P.vy_nominal = 2;
% use linear vs cubic interpolation for lateral motion
P.linear_ey = 1;
% nominal turn rate
P.w_nominal = .5;


% execution horizon and time-step
P.time_final = 20;
P.dt = 0.1;

P.callback = @animate_callback;
P.animate_motion = 0;
P.animate_real_time = 0;

% experimental feature for regularizing acceleration
P.acc_prior = 0;

% Planner actions
P.number_samples = 1;
P.action_duration = 1;

for i=1:length(P.roads)
  P.roads(i).frame_inv = se2_inv(P.roads(i).frame);
end

P.video = VideoWriter(['path_' datestr(now, 'dd-mm-yyyy-HH:MM:SS') '.avi']);
open(P.video);

P.I = diag([0 1]);
P.k1 = diag([0 1]);
P.k2 = diag([0 1]);

P.k_v = 1;
P.k_ey1 = 2;
P.k_ey2 = 2;

P.enable_turns = 1;

P.tree = [];

P.importance_sampling = 1;

P.obstacles(1).state = init_state(1, 32, 2, -4);
P.obstacles(1).width = 2;
P.obstacles(1).length = 4;

P.obstacles(2).state = init_state(2, 26, 2, -4);
P.obstacles(2).width = 2;
P.obstacles(2).length = 4;

P.obstacles(3).state = init_state(3, 50, 2, 4);
P.obstacles(3).width = 2;
P.obstacles(3).length = 4;

P.obstacles(4).state = init_state(4, 70, 2, -4);
P.obstacles(4).width = 2;
P.obstacles(4).length = 4;


P.sensor_range = 15;
P.obstacle_buffer = 0.5;

%P.passive_state = init_state()

function u = null_policy(s, P)

u = init_control(P.kNone);


function distribution = nominal_action_distribution(s, id, goal_state, P)
% nominal action policy which always returns a goto control policy

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


function states = passive_entities(t, P)
