%% Funzione ricostruzione pesata per il secondo livello dell'albero 

function [Punti_utili Rette] = minima_distanza_pesata_secpass(M1,x_t11,M2,x_t22,distanza_minima)
% L'info del numero di pixel delle rette entra come la quarta componente
% del vettore M

[o  p] = size(M1);
[y  h] = size(M2);

Matrice = zeros(o,y);
epsilon = distanza_minima;

% Inizializzo
Punti_utili = [];

% Numeri punti utili
s = 1;

% Rette non utilizzate
Rette = [];
r = 1;


for i = 1 : o
    m_1 = [M1{i}];
    
    m1 = m_1(1);
    m2 = m_1(2);
    m3 = m_1(3);
    
    % Numero di pixel della prima retta
    num_pixel_tmp1 = m_1(4);
    
    x_t1 = [x_t11{i}];
    
    x_0 = x_t1(1);
    y_0 = x_t1(2);
    z_0 = x_t1(3);
       
    for j = 1 : y
        
        m_2 = [M2{j}];
        
        m4 = m_2(1);
        m5 = m_2(2);
        m6 = m_2(3);
        
        % Numero di pixel della seconda retta
        num_pixel_tmp2 = m_2(4);
        
        % Somma del numero di pixel
        num_pixel_tot = num_pixel_tmp1 + num_pixel_tmp2;
        
        x_t2 = [x_t22{j}];
        
        x_3 = x_t2(1);
        y_3 = x_t2(2);
        z_3 = x_t2(3);
        
        
        M = [-(m1^2+m2^2+m3^2)      (m1*m4+m2*m5+m3*m6);
            -(m1*m4+m2*m5+m3*m6)     (m4^2+m5^2+m6^2)];
        
        R = [-m1*(x_3-x_0)-m2*(y_3-y_0)-m3*(z_3-z_0);-m4*(x_3-x_0)-m5*(y_3-y_0)-m6*(z_3-z_0)];
        
        T = inv(M)*R;
        
        t1=T(1);
        t2=T(2);
        
        x1 = x_0 + m1*t1;
        y1 = y_0 + m2*t1;
        z1 = z_0 + m3*t1;
        
        Punti1 = [x1 y1 z1];
        
        x2 = x_3 + m4*t2;
        y2 = y_3 + m5*t2;
        z2 = z_3 + m6*t2;
        
        Punti2 = [x2 y2 z2];
        
        % Punto pesato
        Punti = ((Punti2')*(num_pixel_tmp2/num_pixel_tot)+(Punti1')*(num_pixel_tmp1/num_pixel_tot));
       
        d_1 = Punti - Punti1';
        
        d_2 = Punti - Punti2';
        
        % Distanza minima punto 1
        Dist_min_1 = (d_1(1)^2 + d_1(2)^2 + d_1(3)^2)^(0.5);
        
        % Distanza minima punto 2
        Dist_min_2 = (d_2(1)^2 + d_2(2)^2 + d_2(3)^2)^(0.5);
        
        if (Dist_min_1 < epsilon)&&(Dist_min_2 < epsilon)&&(Punti(1)<10000)&&(Punti(2)<10000)&&(Punti(3)<10000)&&(Punti(1)>0)&&(Punti(2)>0)&&(Punti(3)>0)
            Punti_utili(1,s) = Punti(1);
            Punti_utili(2,s) = Punti(2);
            Punti_utili(3,s) = Punti(3);
            Punti_utili(4,s) = max(Dist_min_1,Dist_min_2);
            s = s + 1;
            Matrice(i,j) = 1;
        end
    end
end


% Verifico matrice adiacenze
w = 1;
for i = 1 : o
    p = 0;
    for j = 1 : y
        if (Matrice(i,j) == 0)
            p = p+1;
        end
        if (p == y)
            m_1 = [M1{i}];
            Rette(1,w) = m_1(1);
            Rette(2,w) = m_1(2);
            Rette(3,w) = m_1(3);
            Rette(4,w) = m_1(4);
            
            Rette(1,w+1) = x_t1(1);
            Rette(2,w+1) = x_t1(2);
            Rette(3,w+1) = x_t1(3);
            
            w = w + 2 ;
        end        
    end
end

for j = 1 : y
    g = 0;
    for i = 1 : o
        if (Matrice(i,j) == 0)
            g = g+1;
        end
        if (g == o)
            m_2 = [M2{j}];
            Rette(1,w) = m_2(1);
            Rette(2,w) = m_2(2);
            Rette(3,w) = m_2(3);
            Rette(4,w) = m_2(4);
            
            Rette(1,w+1) = x_t2(1);
            Rette(2,w+1) = x_t2(2);
            Rette(3,w+1) = x_t2(3);
           
            w = w + 2;
        end        
    end
end