clear all
close all

vzdalenost = 2;             %vzdalenost pole a zdroje
pocet_mikrofonu_m = 3:1:50; %parametry mic pole
frek = 5000;               %vyhodnocovaná frekvence
prumer1 = 0.6;              %prumer 1
prumer2 = 0.8;              %prumer 2
pootoceni = 3;            %pootoceni kriznic
pom =0;

mse = pocet_mikrofonu_m;
for aaa=1:1:length(pocet_mikrofonu_m)
    pocet_mikrofonu = pocet_mikrofonu_m(aaa);
    vzorkovani = 100000;    %vzorkovaci frekvence
    x_pole = 0.5*(0:0.5:3);
    y_pole = 0.5*(0:0.5:3);
    z_pole = 0;
    
    x_pole_interp = 0.5*(0:0.1:3);
    y_pole_interp = 0.5*(0:0.1:3);
    
    x_stred = 0.75;
    y_stred = 0.75;
    
    x_zdroj=1;          %souradnice zdroje
    y_zdroj=1;
    
    cas = 0:1/vzorkovani:3/frek;        %interval dlouhy 3 preriody
    uhel = cas.*frek.*(2*pi);           %jemu odpovidajici uhly
    
    signaly1 = ones(pocet_mikrofonu,(3/frek)/(1/vzorkovani)+pom);
    signaly2 = ones(pocet_mikrofonu,(3/frek)/(1/vzorkovani)+pom);
    while((length(signaly1)~=length(uhel)))
        if((length(signaly1)>length(uhel)))
            pom = pom-1;
        end
        if((length(signaly1)<length(uhel)))
            pom = pom+1;
        end
        signaly1 = ones(pocet_mikrofonu,(3/frek)/(1/vzorkovani)+pom);
        signaly2 = ones(pocet_mikrofonu,(3/frek)/(1/vzorkovani)+pom);
    end
    
    for i=1:1:pocet_mikrofonu
         signaly1(i,:) = sin(uhel);   %3 periody sinusovek1 
         signaly2(i,:) = sin(uhel);   %3 periody sinusovek2 
    end
    
    x_souradnice1(1:1:pocet_mikrofonu) = 1;     %x souradnice mikrofonu
    y_souradnice1(1:1:pocet_mikrofonu) = 1;     %y souradnice mikrofonu       
    faze1(1:1:pocet_mikrofonu) = 1;             %faze posunuti signalu
    x_souradnice2(1:1:pocet_mikrofonu) = 1;      
    y_souradnice2(1:1:pocet_mikrofonu) = 1;
    faze2(1:1:pocet_mikrofonu) = 1;
    for i=1:1:pocet_mikrofonu
        x_souradnice1(i) = x_stred+cos(i*2*pi/pocet_mikrofonu)*prumer1/2;   %urceni x souradnic mikrofonu
        y_souradnice1(i) = y_stred+sin(i*2*pi/pocet_mikrofonu)*prumer1/2;   %urceni y souradnic mikrofonu
        faze1(i) = (sqrt((x_souradnice1(i)-x_zdroj)^2+(y_souradnice1(i)-y_zdroj)^2+(vzdalenost)^2))/343*frek*2*pi; %posunuti signalu na mikrofonech ve °
        signaly1(i,:) = sin(uhel+faze1(i));        %3 periody sinusovky posunute
    
        x_souradnice2(i) = x_stred+cos(i*2*pi/pocet_mikrofonu+pootoceni)*prumer2/2;
        y_souradnice2(i) = y_stred+sin(i*2*pi/pocet_mikrofonu+pootoceni)*prumer2/2;
        faze2(i) = (sqrt((x_souradnice2(i)-x_zdroj)^2+(y_souradnice2(i)-y_zdroj)^2+(vzdalenost)^2))/343*frek*2*pi;
        signaly2(i,:) = sin(uhel+faze2(i));      
    end
    
    vysl = ones(length(x_pole),length(y_pole));     %matice pro vysledky
    [m n] = size(vysl);
    
    signaly_uhel1 = asin(signaly1);
    signaly_uhel2 = asin(signaly2);
    signalyB1 = signaly1;
    signalyB2 = signaly2;
    tau1(1:1:pocet_mikrofonu) = 1;                  %zpozdeni signalu v s
    tau2(1:1:pocet_mikrofonu) = 1;
    for i=1:1:m
        for j=1:1:n
            for k=1:1:pocet_mikrofonu
                tau1(k) = (sqrt((x_souradnice1(k)-x_pole(i))^2+(y_souradnice1(k)-y_pole(j))^2+(vzdalenost)^2))/343;
                tau2(k) = (sqrt((x_souradnice2(k)-x_pole(i))^2+(y_souradnice2(k)-y_pole(j))^2+(vzdalenost)^2))/343;
            end
            k=0;
            minimum1 = min(tau1);       
            tau1 = tau1-minimum1;                   %aby nejmensi tau bylo 0
            minimum2 = min(tau2);
            tau2 = tau2-minimum2;
    
            for k=1:1:pocet_mikrofonu
                signalyB1(k,:) = sin(signaly_uhel1(k,:)+tau1(k)*2*pi*frek);
                signalyB2(k,:) = sin(signaly_uhel2(k,:)+tau2(k)*2*pi*frek);
            end  
    
            vysl_signal1 = sum(signalyB1,1);
            vysl_signal2 = sum(signalyB2,1);
            vysl_signal = vysl_signal1 + vysl_signal2;
            %vysl_signal = abs(vysl_signal);
            vysl(i,j) = max(vysl_signal);           %ulozeni max vysledku
        end
        j=0;
    end
    
    vysl_max = max(max(vysl));
    vysl = vysl/(pocet_mikrofonu*2);
    sp_vysl = zeros(7,7);
    sp_vysl(5,5) = 1; 
    rozdil = (vysl-sp_vysl);
    rozdil2 = rozdil.^2;
    rozdil3 = sum(sum(rozdil2));
    mse(aaa) = sqrt(rozdil3);
end
plot(pocet_mikrofonu_m,mse*100);
xlabel('n[-]');
ylabel('MSE[%]');
grid on;