function [trace, violations, terminal] = rollout(world_state, action_policies, duration, P)
% performs a simulation (rollout) starting at given world state using a 
% vector of action policies of each entity for given duration and problem P;
% returns a sequence (trace) of computed decision states, any violations, and
% whether the rollout reached a terminal state

% decision state accumulator
trace = [];

violations = [];
terminal = false;

% make sure s is logically consistent
world_state = update_logic(world_state, P);

initialized = false;

% if world_state contains valid previous controls, then we will use the "action" to
% initialiate sampling of the control policies, otherwise will sample new actions
use_previous_actions = (isfield(world_state, 'car_controls') && ~isempty(world_state.car_controls));
use_previous_actions = false;

% run either to time_final or until terminal_state
for t=world_state.car_states(1).t:P.dt:world_state.car_states(1).t + duration
  
  % sample actions for all entities
  for k=1:length(P.cars)
    
    action_policy = action_policies{k};    
    if ~initialized && use_previous_actions
      % take the last chosen actions (if any) from the decision state
      actions(k) = world_state.car_controls(k).action;
    else    
      if (~initialized && ~use_previous_actions) || actions(k).termination(world_state, P)
        actions(k) = action_policy(world_state, P);      
      end
    end
    
    assert(~isempty(actions(k).policy));
  end
  initialized = true;
  
  world_state = transition(world_state, actions, P);
  
  % actional external callback, e.g. for display
  P.callback(world_state, P);

  trace = [trace, world_state];
  
  % restrict constraint checking to hero only
  if in_collision(world_state, P.hero_id, P)
    violations(end + 1).type = P.kCollision;
    violations(end).world_state = world_state;
    break;
  end
    
  if ~correct(world_state, P.hero_id, P)
    violations(end + 1).type = P.kIncorrect;
    violations(end).world_state = world_state;
    break;
  end
  
  if terminal_state(world_state, P)
    terminal = true;
    break;
  end  

end


function world_state = transition(world_state, actions, P)
% time-stepping of the decision state s for a given problem P.
% Decision state s includes hero state and other cars states,
% and possibly their histories, as well as action history. 
% P describes the problem: roads, goals, motion constraints, 
% chosen policies for all agents, etc...


%%%%% 1) Motion update using each entity policy %%%%%
for id=1:length(P.cars)
  policy = actions(id).policy;
  control = policy(world_state, P);
  control.action = actions(id); % record the chosen action in the control
  if isempty(control)
    disp('invalid control')
    world_state = [];
    return;
  end
  
  car_states(id) = step_motion(world_state.car_states(id), control, P.dt, true, P);
  car_controls(id) = control;   % add action to history
end

world_state.car_states = car_states;
world_state.car_controls = car_controls;


%%%%% 2) Logic update  %%%%%%
world_state = update_logic(world_state, P);



function world_state = update_logic(world_state, P)

% 2.1) Decoupled Logic update: logic that only depends on the vehicle state itself
for id=1:length(P.cars)
  % collect relevant information around us
  world_state.car_states(id).symbols = scan_road(world_state, id, P);
  world_state.car_states(id) = update_self_logic(world_state.car_states(id), P);
end

% 2.2) Coupled Logic update: update that depends on other vehicle states
% the self_logic update above must be run before the coupled update
for id=1:length(P.cars)
  state = world_state.car_states(id);
  other_states = world_state.car_states([world_state.car_states.id] ~= id);
  world_state.car_states(id) = update_coupled_logic(state, other_states, P);
end



function state = update_self_logic(state, P)
% individual vehicle state update along given route, for problem P
% does not depend on other 

% start by assuming not yielding
state.yield = 0;

road = P.roads(state.road_id);

state.stopped = (abs(state.v) < P.v_tol);

car = P.cars(state.id);

% s-coordinate of the car front bumper
front_s = car_front_s(state, car);

% here at_intersection means "inside intersection stop_region" where stop_region
% is a short region (e.g. 1 meter along the s-direction) right before the intersection
state.at_intersection = (front_s >= road.intersection.s0 - P.stop_region_length && ...
                         front_s < road.intersection.s0);

now_at_stop = (road.intersection.sign == P.kStop && state.at_intersection);

% if previously at stop but not now after stop region
if state.at_stop && ~now_at_stop
  state.after_stop = true;
end
state.at_stop = now_at_stop;

% if first time at stop-sign
if state.stopped && state.at_stop && isnan(state.time_stopped_at_stop)
  state.time_stopped_at_stop = state.t;
  state.yield = 1;
end


in_intersection =  (front_s >= road.intersection.s0 && ...
                    state.x <= road.intersection.sf);

% if we remain in the intersection, then we should commit to our chosen turn
if state.in_intersection
  state.committed_turn = in_intersection;
end
state.in_intersection =  in_intersection;

% yield before an yield sign
if road.intersection.sign == P.kYield && state.v > 0 && front_s < road.intersection.s0
  state.yield = 1;
end



function state = update_coupled_logic(state, other_states, ~)
% state here is the individual vehicle state

% if still not at stop-sign don't need to yield yet
if isnan(state.time_stopped_at_stop)
  return
end

% indices of other crossing cars that have stopped at stop signs and are still in intersection
other_stopped_in_intersection = find(~isnan([other_states.time_stopped_at_stop]) & ...
                                     [other_states.in_intersection] & ...
                                     [other_states.road_id] ~= state.road_id);

if ~isempty(other_stopped_in_intersection)
  min_time_stopped_at_stop = min([other_states(other_stopped_in_intersection).time_stopped_at_stop]);
  
  % yield if there are cars stopped earlier than us
  if state.time_stopped_at_stop > min_time_stopped_at_stop
    state.yield = 1;
  end
end



function f = correct(world_state, id, ~)
% logical constraint evaluation

f = true;
state = world_state.car_states(id);

% should have stopped at that stop sign
if state.after_stop && isnan(state.time_stopped_at_stop)
  f = false;
end
