function [action_sequence, trace, status] = planner(start_world_state, id, time_horizon, P)
% Compute an optimal suquence of actions (policies), each applied for P.action_duration sec.
% also return the physical decision trajectory ss (sequence of integrated decision states)
% and the maximum total rewards. An exhaustive search over all possible sequences is performed.

time_segments = round(time_horizon / P.action_duration);

action_policies = {P.cars.action_policy};

% start node
node_id = 1;
root = init_node(node_id, start_world_state, 0, [], [], false);

best_node_id = 0;
max_total_reward = -inf;
pruned_nodes = 0;

% tree stored as a map
tree = containers.Map('KeyType', 'uint32', 'ValueType', 'any');

% list of open (unexpanded) nodes, as a max-priority queue
open_list = PriorityQueue;
open_list.insert(node_priority(root), root);

start_time = start_world_state.car_states(1).t;

% add and remove new nodes from open list according to priority
while ~open_list.isEmpty()
  % pop new node from end of open list
  [priority, node] = open_list.pop();
  world_state = node.trace(end);
  
  if ~node.terminal && world_state.car_states(1).t < start_time + time_horizon - eps
    % expand children by executing feasible actions
    actions = P.cars(id).feasible_actions(world_state, id, P.cars(id).goal_state, P);
    
    for i=1:length(actions)      
      action_policies{id} = @(world_state, P)fixed_action_policy(world_state, actions(i), P);
      [action_trace, violations, terminal] = rollout(world_state, action_policies, P.action_duration, P);
      
      if ~isempty(violations)
        % disp('infeasible...')
        % violations.s.car_states(1)
        continue
      end
      
      r = 0;
      for k=1:length(action_trace)
        r = r + reward(action_trace(k), 1, P);
      end
      
      total_reward = node.total_reward + r;
      if total_reward < max_total_reward
        pruned_nodes = pruned_nodes + 1;
        continue
      end
      
      child = init_node(node_id + 1, action_trace, r, node, actions(i), terminal);
      node_id = node_id + 1;
      
      open_list.insert(node_priority(child), child);
    end
  else 
    % this is a terminal node
    if node.total_reward > max_total_reward
      best_node_id = node.id;
      max_total_reward = node.total_reward;
    end
  end

  % add processed node to tree
  tree(node.id) = node;
end

action_sequence = [];
trace = [];

status.max_total_reward = max_total_reward;
status.tree_size = tree.length();
status.best_node_id = best_node_id;
status.pruned_nodes = pruned_nodes;

% if no solution found
if ~best_node_id
  return
end

% reconstruct the optimal sequence from the best leaf node up the tree
while best_node_id ~= 1
  best_node = tree(best_node_id);
  action_sequence = [best_node.action, action_sequence];  
  trace = [best_node.trace, trace];
  best_node_id = best_node.parent_id;
end

function node = init_node(id, trace, reward, parent, action, terminal)

node.id = id;

% sequence of high-frequency states from parent to node
node.trace = trace;

 % reward r(s,a,s') where s' corresponds to this node, i.e. reward for applied action from parent
node.reward = reward;

if ~isempty(parent)
  node.parent_id = parent.id;
  % total reward = "reward to come"
  node.total_reward = parent.total_reward + reward;
  node.depth = parent.depth + 1;
else
  node.parent_id = 0;
  node.total_reward = 0;
  node.depth = 0;
end

% value = "reward to go"
node.value = 0;

% which action/action was executed
node.action = action;

% is this a terminal node
node.terminal = terminal;


function q = node_priority(node)

% heurist function determining the order of node processing
q = node.depth*100000 + node.total_reward;
