%% Ricostruzione marker 3D con distanza pesata con il numero di pixels

function [Tempo Punti_pesati Rette] = Ricostruzione_punti_pesati_3D(x_p_as,y_p_as,z_p_as,num_pixel,camera_pos,m,distanza_minima,epsilon)

tmp_tot = tic;

Punti = [];

Rette_tmp = cell([1,1]);

j = 1;
k = 1;
h = 1;

tmp = tic;
for i = 1 : 2 : m
    Tempo_tmp = tic;
    [m_1 x_1] = calcola_parametri([x_p_as{i}],[y_p_as{i}],[z_p_as{i}],camera_pos(i,:));
    [m_2 x_2] = calcola_parametri([x_p_as{i+1}],[y_p_as{i+1}],[z_p_as{i+1}],camera_pos(i+1,:));
    num_pixel1 = num_pixel{i};
    num_pixel2 = num_pixel{i+1};
    
    % Oltre ai coeffiecienti rette non utilizzate restituisce anche il loro
    % numero di pixels
    [Punti_utili Rette_non_ut] = minima_distanza_pesata_3D(m_1,x_1,m_2,x_2,num_pixel1,num_pixel2,distanza_minima);
    
    % Punti 
    Punti = [Punti  Punti_utili];
    
    % Rette
    Rette_tmp{j} = Rette_non_ut;
    j = j + 1;
    disp('Tempo Algorimo pesato primo livello')
    toc(Tempo_tmp)
end

% Tempo del primo ramo (Tempo massimo)
tempo1 = toc(tmp);

% Numero dei livelli di albero
p = j - 1;

tmp1 = tic;
while(p > 2)
    for i = 1 : 2 : p
        Tempo_tmp1 = tic;
        [m1 x1] = Rette_celle_con_peso(Rette_tmp{i});
        [m2 x2] = Rette_celle_con_peso(Rette_tmp{i+1});
        
        % Controllo se valori nulli
        contr1 = [m1{1}];
        contr2 = [m2{1}];
        
        if ((length(contr1)==0)||(length(contr2))==0)
            % Non fa nulla
        else
            [Punti_utili2 Rette2] = minima_distanza_pesata_secpass(m1,x1,m2,x2,distanza_minima);
            
            Punti = [Punti Punti_utili2];
            
            Rette_tmp{k} = Rette2;
            k = k + 1;
        end
        p = p - 2;
        k = 1;
        
    disp('Tempo Algorimo pesato secondo in poi livello')
    toc(Tempo_tmp1)   
    end
end

tempo2 = toc(tmp1);

Tempo_tmp3 = tic;

% Ultimo passaggio albero
[m1 x1] = Rette_celle_con_peso(Rette_tmp{1});
[m2 x2] = Rette_celle_con_peso(Rette_tmp{2});

% Controllo se valori nulli
contr1 = [m1{1}];
contr2 = [m2{1}];

if ((length(contr1)==0)||(length(contr2))==0)
    Rette = Rette_tmp;
else
    [Punti_utili3 Rette] = minima_distanza_pesata_secpass(m1,x1,m2,x2,distanza_minima);
    Punti = [Punti Punti_utili3];
end
tempo3 = toc(tmp_tot);
disp('Tempo Algorimo pesato ultimo livello')
toc(Tempo_tmp3)

Punti_pesati = media_punti_gen(epsilon,Punti);
Tempo = [tempo1 tempo2 tempo3];
