% funzione per il calcolo delle coordinate x e y del centro di un singolo
% marker luminoso, funziona solamente se in pixel_matrix sono presenti i
% pixel rappresentanti un solo marker

function [centro_x, centro_y, num_pixel] = alg_centri_3D(pixel_matrix, pixel_dim_x, pixel_dim_y)
%% inizializzazioni
% USCITE:
% centro_x: coordinata x del centri del marker sul piano immagine
% centro_y: coordinata x del centri del marker sul piano immagine
% num_pixel: numero di pixel caratterizzanti il centro trovato relativo al
% marker contenuto nel piano immagine

% INGRESSI:
% pixel_matrix: matrice dei pixel rappresentante il piano immagine della 
% telecamera, affinchè l'algoritmo funzioni nel piano immagine in ingresso
% devono esserci i pixel di un solo marker
% pixel_dim_x: dimensione della base dei pixel
% pixel_dim_y: dimensione dell'altezza dei pixel

% possiamo avere al massimo la proiezione di n marker sul piano immagine
% della telecamera
[r n] = size(pixel_matrix); %la matrice e' quadrata, quindi numero di righe
                            %e di colonne coincidono
centri_1 = zeros(n,n); %matrice contenente i centri rispetto all'asse x
centri_2 = zeros(n,n); %matrice contenente i centri rispetto all'asse y

centri_1_num = zeros(n,n); %matrice conten. il num. dei pixel di centri_1
centri_2_num = zeros(n,n); %matrice conten. il nume. dei pixel di centri_2

c_1 = ones(1,n); %vettori conten. in posizione i il numero di centri della
c_2 = ones(1,n); %riga i(c_1) e della colonna i(c_2)

%% algoritmo
for j = 1:n
    h_1 = 0;
    h_2 = 0;
    flag_1 = 0;
    flag_2 = 0;
    for i = 1:n
        if flag_1 == 1
            if pixel_matrix(j,i) == 0 || i == n
                flag_1 = 0; 
                centri_1(j,c_1(j)) = h_1+((i-h_1-1)/2)+0.5*(i==n)*pixel_matrix(j,i);
                c_1(j) = c_1(j) + 1;
                centri_1_num(j,c_1(j)) = i-h_1;
                if i == n
                    centri_1_num(j,c_1(j)) = centri_1_num(j,c_1(j)) + 1*(pixel_matrix(j,i)==1);
                end
            end
        elseif flag_1 == 0
            if pixel_matrix(j,i) == 1
                flag_1 = 1;
                h_1 = i;
                if i == n
                    centri_1(j,c_1(j)) = i;
                    centri_1_num(j,c_1(j)) = 1;
                end
            end
        end
        if flag_2 == 1
            if pixel_matrix(i,j) == 0 || i == n
                flag_2 = 0; 
                centri_2(c_2(j),j) = h_2+((i-h_2-1)/2)+0.5*(i==n)*pixel_matrix(i,j);
                c_2(j) = c_2(j) + 1;
                centri_2_num(c_2(j),j) = i-h_2;
                if i == n
                    centri_2_num(c_2(j),j) = centri_2_num(c_2(j),j) + 1*(pixel_matrix(i,j)==1);
                end
            end
        elseif flag_2 == 0
            if pixel_matrix(i,j) == 1
                flag_2 = 1;
                h_2 = i;
                if i == n
                    centri_2(c_2(j),j) = i;
                    centri_2_num(c_2(j),j) = 1;
                end
            end
        end
    end
end
c_1 = c_1 - ones(1,n);
c_2 = c_2 - ones(1,n);

%% algoritmo per sistemare i centri vicini
% questo algoritmo tiene conto del fatto che due serie di uno troppo vicini
% hanno una alta probabilità di riferirsi allo stesso marker, per questo
% motivo viene calcolato un nuovo centro pesato, sia per l'asse delle x che
% per l'asse delle y

% calcolo della posizione del centro sull'asse delle x

% calcoliamo c, numero di centri effettivi fra cui fare la media per
% calcolare la posizione x del centro
c = 0;
for i = 1:n
    if c_1(i) ~= 0
        c = c + 1;
    end
end
centri_x = zeros(1,c);

% sistemazione dei centri vicini
c = 0;
for j = 1:n
    num = 0;
    den = 0;
    if c_1(j) >= 1
        for i = 1:c_1(j)
            num = num + centri_1(j,i)*centri_1_num(j,i+1);
            den = den + centri_1_num(j,i+1);
        end
        c = c + 1;
        centri_x(c) = num/den;
    end
end

% calcolo della posizione del centro sull'asse delle y

% calcoliamo c, numero di centri effettivi fra cui fare la media per
% calcolare la posizione y del centro
c = 0;
for i = 1:n
    if c_2(i) ~= 0
        c = c + 1;
    end
end
centri_y = zeros(1,c);

% sistemazione dei centri vicini
c = 0;
for j = 1:n
    num = 0;
    den = 0;
    if c_2(j) >= 1
        for i = 1:c_2(j)
            num = num + centri_2(i,j)*centri_2_num(i+1,j);
            den = den + centri_2_num(i+1,j);
        end
        c = c + 1;
        centri_y(c) = num/den;
    end
end

%% calcolo delle uscite

% posizione del centro sull'asse delle x
centro_x = mean(centri_x)*pixel_dim_x;

% posizione del centro sull'asse dell y
centro_y = mean(centri_y)*pixel_dim_y;

% numero di pixel totali che formano il marker sul piano
num_pixel = sum(sum(centri_1_num));

