%% Funzione ricostruzione  punti centralizzata Totale

% In ingresso posizione assolute dei centri del piano immagine
function [punti_finali Punti_tot] = Ricostruzione_centralizzato(x_centri,y_centri,z_centri,camera_pos,m,distanza_minima,distanza_media)

Punti_tot =[];

epsilon = distanza_minima;


l = 1;

for s = 1 : m - 1 
    % Centri telecamera 1
    x_centri1 = [x_centri{s}];
    y_centri1 = [y_centri{s}];
    z_centri1 = [z_centri{s}];

    for q = s + 1 : m
        x_centri2 = [x_centri{q}];
        y_centri2 = [y_centri{q}];
        z_centri2 = [z_centri{q}];
        
        % numero centri tele 1
        n = length(x_centri1);
        %numero centri tele 2
        w = length(x_centri2);
        
        for i = 1 : n
            for k = 1 : w
                % Coordinate marker nel sistema di riferimento assoluto
                x_0 = x_centri1(i);
                y_0 = y_centri1(i);
                z_0 = z_centri1(i);
                
                % Posizione del centro del piano immagine telecamera
                x_1 = camera_pos(s,1);
                y_1 = camera_pos(s,2);
                z_1 = camera_pos(s,3);
                
                x_2 = x_centri2(k);
                y_2 = y_centri2(k);
                z_2 = z_centri2(k);
                
                x_3 = camera_pos(q,1);
                y_3 = camera_pos(q,2);
                z_3 = camera_pos(q,3);
                
                
                
                % Prima retta passante per due punti
                % x = x_0 + (x_1-x_0)*t;
                % y = y_0 + (y_1-y_0)*t;
                % z = z_0 + (z_1-z_0)*t;
                
                m1=(x_1-x_0);
                m2=(y_1-y_0);
                m3=(z_1-z_0);
                
                % Seconda retta passante per due punti
                % x2 = x_3 + (x_2-x_3)*t;
                % y2 = y_3 + (y_2-y_3)*t;
                % z2 = z_3 + (z_2-z_3)*t;
                
                m4=(x_2-x_3);
                m5=(y_2-y_3);
                m6=(z_2-z_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 + (x_1-x_0)*t1;
                y1 = y_0 + (y_1-y_0)*t1;
                z1 = z_0 + (z_1-z_0)*t1;
                
                Punti1 = [x1 y1 z1];
                
                x2 = x_3 + (x_2-x_3)*t2;
                y2 = y_3 + (y_2-y_3)*t2;
                z2 = z_3 + (z_2-z_3)*t2;
                
                Punti2 = [x2 y2 z2];
                
                Punti = (Punti2'+Punti1')/2;
                
                distanza = Punti2' - Punti1';
                Distanza_minima = (distanza(1)^(2)+distanza(2)^2+distanza(3)^(2))^(0.5);
                
                if (Distanza_minima < epsilon)&&(Punti(1)<10000)&&(Punti(2)<10000)&&(Punti(3)<10000)&&(Punti(1)>0)&&(Punti(2)>0)&&(Punti(3)>0)
                    Punti_tot(1,l) = Punti(1);
                    Punti_tot(2,l) = Punti(2);
                    Punti_tot(3,l) = Punti(3);
                    Punti_tot(4,l) = Distanza_minima;
                    l = l + 1;
                end
            end
        end
    end
end 
punti_finali = media_punti_gen(distanza_media, Punti_tot);

