function buf_id = swarm_planner_rank_to_id( ...
    x_id1, y_id1, psi_id1, ...
    x_id2, y_id2, psi_id2, ...
    x_id3, y_id3, psi_id3, ...
    ranking, L, N_REPLAN)
%SWARM_PLANNER_RANK_TO_ID
%
% Input:
%   stati in ordine ID fisso: id1, id2, id3
%
% ranking:
%   ranking(1) = ID del primo classificato
%   ranking(2) = ID del secondo classificato
%   ranking(3) = ID del terzo classificato
%
% Output:
%   buf_id in ordine ID fisso:
%   slot id1, slot id2, slot id3

x   = [x_id1;   x_id2;   x_id3];
y   = [y_id1;   y_id2;   y_id3];
psi = [psi_id1; psi_id2; psi_id3];

first  = ranking(1);
second = ranking(2);
third  = ranking(3);

%% Chiamo il vecchio helper in ordine classifica
% slot 1 = primo classificato
% slot 2 = secondo classificato
% slot 3 = terzo classificato
buf_rank = swarm_planner_helper2( ...
    x(first),  y(first),  psi(first), ...
    x(second), y(second), psi(second), ...
    x(third),  y(third),  psi(third), ...
    L, N_REPLAN);

%% Rimappo da ordine classifica a ordine ID fisso
buf_id = remap_rank_buffer_to_id_buffer(buf_rank, ranking, L);

end

function buf_id = remap_rank_buffer_to_id_buffer(buf_rank, ranking, L)

buf_id = zeros(6*L + 11, 1);

%% Dati rank1
px_rank1 = buf_rank(1:L);
py_rank1 = buf_rank(L+1:2*L);
n_rank1  = buf_rank(2*L+1);

%% Dati rank2
px_rank2 = buf_rank(2*L+2:3*L+1);
py_rank2 = buf_rank(3*L+2:4*L+1);
n_rank2  = buf_rank(4*L+2);

%% Dati rank3
px_rank3 = buf_rank(4*L+3:5*L+2);
py_rank3 = buf_rank(5*L+3:6*L+2);
n_rank3  = buf_rank(6*L+3);

goal_flag = buf_rank(6*L+4);
pct_known = buf_rank(6*L+5);

mu_rank1 = buf_rank(6*L+6);
mu_rank2 = buf_rank(6*L+7);
mu_rank3 = buf_rank(6*L+8);

unk_rank1 = buf_rank(6*L+9);
unk_rank2 = buf_rank(6*L+10);
unk_rank3 = buf_rank(6*L+11);

%% ranking(1) dice quale ID riceve i dati rank1
buf_id = write_rank_slot_to_id_slot( ...
    buf_id, ranking(1), px_rank1, py_rank1, n_rank1, mu_rank1, unk_rank1, L);

%% ranking(2) dice quale ID riceve i dati rank2
buf_id = write_rank_slot_to_id_slot( ...
    buf_id, ranking(2), px_rank2, py_rank2, n_rank2, mu_rank2, unk_rank2, L);

%% ranking(3) dice quale ID riceve i dati rank3
buf_id = write_rank_slot_to_id_slot( ...
    buf_id, ranking(3), px_rank3, py_rank3, n_rank3, mu_rank3, unk_rank3, L);

%% Info globali
buf_id(6*L+4) = goal_flag;
buf_id(6*L+5) = pct_known;

end