clear all
close all
clc
tic

c = 344;    %rychlost zvuku ve vzduchu v m/s

d = 0.05;   %vzdalenost mezi mikrofony v mrizovem poli

N = 2;      %rad Langrangeova interpolatoru

p_kanalu = 8;   %pocet mikrofonu
p_sloupcu = 4;  %pocet sloupcu v mrizovem poli mikrofonu
p_radku = 2;  %pocet radku v mrizovem poli mikrofonu
p_bodu = 180;  %pocet mericich bodu v horizontalni a vertikalni rovine

r_m_hor = [0 0.05 0.10 0.15];    %vzdalenosti mikrofonu od referencniho v HORIZONTALNI rovine
r_m_vert = [0 0.05];                  %vzdalenosti mikrofonu od referencniho ve VERTIKALNI rovine
uhel = linspace(1,179,p_bodu);    %uhly natoceni pole

preprocessing(p_kanalu);
for n=1:p_kanalu
    nazev = ['mic',int2str(n),'.wav'];
    [p(:,n),Fvz,Nbits]=wavread(nazev);
end

p_vzorku = size(p,1);         %pocet vzorku signalu

w = ones(1,p_kanalu);              %vahy mikrofonu

max_delay_vzorku_hor = round((max(r_m_hor)/c)*Fvz);
max_delay_vzorku_vert = round((max(r_m_vert)/c)*Fvz);

p_delay = zeros(max_delay_vzorku_hor+max_delay_vzorku_vert+p_vzorku,p_kanalu);
p_delay_vzorku = max_delay_vzorku_hor+max_delay_vzorku_vert+p_vzorku;

%------------- Lagrangeuv Interpolator ------------------------------------
U = zeros(N+1);
hodnota_prvku = 0;
for n=1:N+1
    mocnina = 0;
    for m=1:N+1
        U(n,m) = hodnota_prvku^mocnina;
        mocnina = mocnina + 1;
    end
    hodnota_prvku = hodnota_prvku + 1;
end
Q = inv(U);

%--------------------------------------------------------------------------

y = zeros(1,p_delay_vzorku)';
energie = zeros(p_bodu);
amplituda = zeros(p_bodu);

for i=1:p_bodu
    
    k_vert = r_m_vert*cos(uhel(i)*(pi/180));
    delay_vert = (k_vert/c)*Fvz;
    delay_vzorku_vert = round(delay_vert);
    
    for j=1:p_bodu

        k_horiz = r_m_hor*cos(uhel(j)*(pi/180));
        delay_hor = (k_horiz/c)*Fvz;
        delay_vzorku_hor = round(delay_hor);
    
        p_delay(:,:) = 0; 
        
        kanal = 0;
        for m=1:p_radku
            for n=1:p_sloupcu
                kanal = kanal + 1;
                
                if (uhel(j) <= 90 && uhel(i) <= 90)
                    p_delay(delay_vzorku_hor(n)+delay_vzorku_vert(m)+1:...
                        delay_vzorku_hor(n)+delay_vzorku_vert(m)+p_vzorku, kanal) = p(:,kanal);
                elseif (uhel(j) <= 90 && uhel(i) > 90)
                    p_delay(delay_vzorku_hor(n)+...
                        max_delay_vzorku_vert+delay_vzorku_vert(m)+1:...
                        delay_vzorku_hor(n)+...
                        max_delay_vzorku_vert+delay_vzorku_vert(m)+p_vzorku, kanal) = p(:,kanal);
                elseif (uhel(j) > 90 && uhel(i) <= 90)
                    p_delay(max_delay_vzorku_hor+delay_vzorku_hor(n)+...
                        delay_vzorku_vert(m)+1:...
                        max_delay_vzorku_hor+delay_vzorku_hor(n)+...
                        delay_vzorku_vert(m)+p_vzorku, kanal) = p(:,kanal);
                elseif (uhel(j) > 90 && uhel(i) > 90)
                    p_delay(max_delay_vzorku_hor+delay_vzorku_hor(n)+...
                        max_delay_vzorku_vert+delay_vzorku_vert(m)+1:...
                        max_delay_vzorku_hor+delay_vzorku_hor(n)+...
                        max_delay_vzorku_vert+delay_vzorku_vert(m)+p_vzorku, kanal) = p(:,kanal);
                end
                
                delta = delay_vert(m) + delay_hor(n) - (delay_vzorku_hor(n)+delay_vzorku_vert(m));
                H = zeros(1,N+1);
                for k=1:N+1
                    for l=1:N+1
                       H(l) = H(l) + Q(k,l)*delta^(k-1);
                    end
                end
                
                p_delay(:,kanal) = conv(p_delay(:,kanal),H,'same');
                
            end
        end

        y(:)= 0;
        for m=1:p_kanalu
            y = y + w(m).*p_delay(:,m);
        end
        Y = sum(y.^2)/p_delay_vzorku;

        energie(i,j) = Y;
        amplituda(i,j) = max(y);
    end
end

energieNorm = energie / max(max(energie));
logEnergie = 10*log10(energieNorm/1);

% for m = 1:(p_bodu^2)
%     if (energieNorm(m)<0.95)
%         energieNorm(m) = 0;
%     end
% end

%%%%%%%%%%%%%%%%%%%%% GRAFY %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(6)
vertUhel = fliplr(uhel);
horizUhel = uhel;%fliplr(uhel)
imagesc(horizUhel,vertUhel,energieNorm)%logEnergie|energie
set(gca,'xlim',[0 180],'XTickLabel',[180 160 140 120 100 80 60 40 20 0])
set(gca,'ylim',[0 180],'YTickLabel',[0 20 40 60 80 100 120 140 160 180])
xlabel('\leftarrow  {\it{\alpha}}  [°]')
ylabel('\leftarrow  {\it{\beta}}  [°]')
text(190,190,'{\it{E_n}} [-] \uparrow');
colorbar
set(gca,'layer','top','box','on')


figure(5)
vertUhel = fliplr(uhel);
horizUhel = uhel;%fliplr(uhel)
imagesc(horizUhel,vertUhel,logEnergie)%logEnergie|energie
set(gca,'xlim',[0 180],'XTickLabel',[180 160 140 120 100 80 60 40 20 0])
set(gca,'ylim',[0 180],'YTickLabel',[0 20 40 60 80 100 120 140 160 180])
xlabel('\leftarrow  {\it{\alpha}}  [°]')
ylabel('\leftarrow  {\it{\beta}}  [°]')
text(190,190,'{\it{L_{E_n}}} [dB] \uparrow');
colorbar
set(gca,'layer','top','box','on')

toc
time=toc;