% funzione calc_centri_3D che permette di calcolare i centri di un piano
% immagine nel caso tridimensionale

function [centri_x, centri_y, num_pixel_centri] = calc_centri_3D(pixel_matrix, pixel_dim_x, pixel_dim_y, h)
%% inizializzazioni
% USCITE:
% centri_x: vettore contenente le coordinate dei centri sull'asse x
% centri_y: vettore contenente le coordinate dei centri sull'asse y
% num_pixel_centri: vettore contenente il numero di pixel che caratterizza
% il centro del vettore centri nella stessa posizione

% INGRESSI:
% h: numero di marker totali (serve solo per le inizializzazioni)
% pixel_matrix: matrice dei pixel rappresentante il piano immagine della 
% telecamera
% pixel_dim_x: dimensione della base dei pixel
% pixel_dim_y: dimensione dell'altezza dei pixel

centri_p_x = zeros(1,h);
centri_p_y = zeros(1,h);
num_pixel = zeros(1,h);

[n r] = size(pixel_matrix);

%% algoritmo
% questo algoritmo, mediante l'appoggio delle funzioni alg_centri_3D e
% alg_ricorsivo, permette di calcolare le coordinate x ed y dei centri dei
% marker sul piano immagine in ingresso

matrice = zeros(n,n);
c=0; %contatore del numero dei centri trovati
for i = 1:n
    for j =1:n
        if pixel_matrix(i,j) == 1
           [matrice] = alg_ricorsivo(i, j, pixel_matrix, matrice, n);
           [centro_x, centro_y, num] = alg_centri_3D(matrice, pixel_dim_x, pixel_dim_y);
           c=c+1; 
           centri_p_x(c) = centro_x;
           centri_p_y(c) = centro_y;
           num_pixel(c) = num;
           pixel_matrix = pixel_matrix - matrice;
           matrice = zeros(n,n);
        end
    end
end

% inizializzazione dei valori in uscita
centri_x = zeros(1,c);
centri_y = zeros(1,c);
num_pixel_centri = zeros(1,c);

% salvataggio dei valori in uscita
for i = 1:c
    centri_x(i) = centri_p_x(i);
    centri_y(i) = centri_p_y(i);
    num_pixel_centri(i) = num_pixel(i);
end
