% funzione per il calcolo della posizione del centro dei raggi sul piano 
% immagine che uniscono il punto focale della telecamera ai marker visibili

function [centri num_pixel_centri] = calc_centri(n, e, pixel_dim, pixel_vec)
%% inizializzazioni
% centri: vettore contenente i centri
% n: numero di marker totali
% pixel_vec: vettore dei pixel del piano immagine della telecamera
% e: numero di zeri massimo che divide due serie di uno per considerarle
% indicare lo stesso marker
% pixel_dim: dimensione del pixel in millimetri

% possiamo avere al massimo la proiezione di n marker sul piano immagine
% della telecamera
centri_p = zeros(1,n); %vettore provvisorio dei centri
num_pixel = zeros(1,n); %nel posto i contiene il num di pixel del centro i

%% algoritmo per il calcolo dei centri
% questo algoritmo mi calcola i centri di tutte le serie di uni consecutivi
% presenti nel vettore pixel_vec

e=e+1;
j = 1; %contatore del numero di centri
flag = 0;
h = 0;
l = length(pixel_vec);
for i = 1:l
    if flag == 1
        if pixel_vec(i) == 0 || i == l
            flag = 0; 
            centri_p(j) = (h+((i-h-1)/2)+0.5*(i==l)*(pixel_vec(i)==1));
            num_pixel(j) = i-h;
            if i == l
                num_pixel(j) = num_pixel(j)+1*(pixel_vec(i)==1);
            end
            j = j + 1;
        end
    else
        if pixel_vec(i) == 1
            flag = 1;
            h = i;
            if i == l
                centri_p(j) = i;
                num_pixel(j) = 1;
                j=j+1;
            end
        end
    end
end
centri_num = j - 1;

%% 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

c = 0;
scambio = zeros(1,n);
i = 1;
agg_centri = centri_num;
while i < centri_num
    num= 0;
    contr_1 = centri_p(i) + (num_pixel(i)/2 - 0.5);    %var. temporanee per 
    contr_2 = centri_p(i+1) - (num_pixel(i+1)/2 -0.5); %controllare dist.
    if contr_2-contr_1<e                               %fra due serie di 1
        scambio(i) = centri_p(i)*num_pixel(i);
        c = i+1;
        scambio(c) = centri_p(c)*num_pixel(c);
        den=num_pixel(i) +num_pixel(c);
        f = 0;
        while f == 0
            if c == centri_num
                m = c-i+1;
                for j = 1:m
                    num = num + scambio(i-1+j);
                end
                centri_p(i) = num/den;
                num_pixel(i) = den;
                f = 1;
                for j = i+1:c
                    centri_p(j) = 0;
                    num_pixel(j) = 0;
                end
                agg_centri = agg_centri - c+i;
                c=c+1;
            elseif (centri_p(c+1)-(num_pixel(c+1)/2-0.5))-(centri_p(c)+num_pixel(c)/2-0.5)<= e
                c = c + 1;
                scambio(c) = centri_p(c)*num_pixel(c);
                den = den + num_pixel(c);
            else
                m = c-i+1;
                for j = 1:m
                    num = num + scambio(i-1+j);
                end
                centri_p(i) = num/den;
                num_pixel(i) = den;
                f = 1;
                for j = i+1:c
                    centri_p(j) = 0;
                    num_pixel(j) = 0;
                end
                agg_centri = agg_centri - c+i;
                c=c+1;
            end
        end
    end
    if centri_p(i+1)-centri_p(i)>e
        c=i+1;
    end
    i = c;
end
centri_num = agg_centri;

%% vettore dei centri in uscita
% i due cicli seguenti sono necessari per eliminare i centri composti da un
% solo 1 nel piano immagine, quelli cio� generati da errore
for i = 1:n
    if num_pixel(i) == 1
        centri_num = centri_num -1;
    end
end

centri = zeros(1,centri_num); %vettore dei centri in uscita
num_pixel_centri = zeros(1, centri_num); %vettore contenente il numero di 
                                         %pixel del rispettivo centro
g = 1; %contatore dei centri
for i = 1:n
    if centri_p(i) ~= 0 && num_pixel(i) > 1
        centri(g)= centri_p(i);
        num_pixel_centri(g) = num_pixel(i);
        g = g+1;
    end
end
centri = centri*pixel_dim;
        
