% autor: Ondrej Tkadlec
% CVUT FEL
% bakalarska prace
% vypocet KZK rovnice
% model audioreflektoru

clear;clc;

tic
%% inicializace frekvenci
global M harm_generative;
M = 140; % pocet harmonickych
harm_generative = 40:100; % rozsah budicich harmonickych, melo by byt symetricke okolo nosne frekvence [kHz]
harmonics = 1:M;
F = 1000;
o = 2*pi*F; % frekvence obalky
w = 2*pi*F*70; % frekvence nosne
global m;
m = 1;

%% zdroj zvuku
pm = 6.4; % amplituda pm
p0 = 2e-005; % prah slysitelnosti
v0 = 4.85616689e-8; % prah slysitelnosti prevedeny na akustickou rychlost

%% prubeh buzeni
t = linspace(0,4,2000); % normalni cas --- pro vykresleni prubehu buzeni
f = 0; % vysledna funkce
global peak;
peak = zeros(1,M); % amplitudy jednotlivych harmonickych
tlak = zeros(1,M);

%% pocatecni rozdeleni - gauss * koeficienty pro harmonicke
for harm = harm_generative
    coeff_a = an(o,w,harm);
    coeff_b = bn(o,w,harm);
    deviation = coeff_a*cos(harm*o*t)+coeff_b*sin(harm*o*t);
    pole = [coeff_a coeff_b];
    [~, idx] = max(abs(pole));
    sgn = sign(pole(idx));
%     peak(harm) = sgn*sqrt(coeff_a^2+coeff_b^2);
%     tlak(harm) = sqrt(coeff_a^2+coeff_b^2)*pm;
    peak(harm) = sgn*max(deviation);
    tlak(harm) = sgn*max(deviation)*pm;
    f = f + deviation;
end

%Lp = 20*log10(max((tlak/p0),1)); % logaritmus Lp = 20*log(p/p0)
Lp = 20*log(tlak/p0);

%g = sqrt(1-m*(cos(o*t)+cos(2*o*t)));
g = sqrt(1+m*(cos(o*t)));
h = -g;

%% ----------PLOT------------
figure
hold on
grid on
plot(t,f,t,g,t,h)
xlabel('\tau´ [-]')
ylabel('w [-]')
figure
hold on
grid on
bar(harmonics, Lp, 'red')
xlabel('frekvence [kHz]')
ylabel('L_p [dB]')


%% inicializace site - sirka, delka, kroky,... 
global zmax rmax
zmax = 300; % delka oblasti v podelnem smeru, normalizovana Rayleighovym polomerem
rmax = 20; % sirka oblasti v kolmem smeru
global imax jmax
imax = 200; % pocet kroku v kolmem smeru
jmax = 5000; % pocet kroku v podelnem smeru
f_step = linspace(0,zmax,jmax);
s_step = linspace(0,rmax,imax);


%% inicializace pocatecnich hodnot G a H
Amp = a_vals(imax,M);
Pha = p_vals(imax,M);
G = zeros(M,imax);
H = zeros(M,imax);
for k=1:M
    for i=1:imax
       G(k,i) = imag(Amp(k,i),Pha(k,i));
       H(k,i) = real(Amp(k,i),Pha(k,i));
    end
end

G
H
%error('stuj')

%% velikosti kroku
sigma = f_step(2)-f_step(1);
eps = s_step(2)-s_step(1);
R = sigma/eps^2;

%% nastaveni parametru prostredi a zdroje - slaba interakce
vm = 0.03;
a = 0.3;
f = 1000; % nosna frekvence


%% spocitani koeficientu
Gf = 100; % fokusacni cinitel
environment = "vzduch";
if environment == "vzduch"
    env_num = 1;
else
    env_num = 2;
end
[G0, r0, a1, alpha, ln, l_dis,A] = calc_coeffs(vm, a, f, environment);

G0
r0
a1
alpha
ln
l_dis
A

%error('tu')

%% tridiagonalni matice
diag = zeros(1,imax); % na diagonale
u_diag = zeros(1,imax-1); % under
a_diag = zeros(1,imax-1); % above

for i=1:imax
    if i == 1
        diag(i) = -2;
        u_diag(i) = 0.5;
        a_diag(i) = 2;
    elseif i>1 && i<imax
        diag(i) = -2;
        u_diag(i) = 1-1/(2*(i));
        a_diag(i) = 1+1/(2*(i-1));
    else
        diag(i) = -2;
    end
end

%% pomocne promenne gn, gf, hn, hf
gf = zeros(1, imax);
hf = zeros(1, imax);

gn = zeros(1,imax);
hn = zeros(1,imax);

% %% harmonicke a hladina akustickeho tlaku
% harmonics = 1:M;
% amplitudes = zeros(1,M);

%% inicializace HDF souboru
% jmeno souboru podle parametru
filename = strcat('gaussian_M',num2str(M),'_',num2str(F),'Hz','_',num2str(harm_generative(1)),'_',num2str(harm_generative(end)),'_jmax',num2str(jmax),'_imax',num2str(imax),'_G',num2str(Gf),'.h5');
% atributy
fcpl = H5P.create('H5P_FILE_CREATE');
fapl = H5P.create('H5P_FILE_ACCESS');
% vytvor soubor, s temito atributy se stejny soubor vzdy prepise
fid = H5F.create(filename,'H5F_ACC_TRUNC',fcpl,fapl);
% vytvor groups G, H, Params
Gid = H5G.create(fid,'G',0);
Hid = H5G.create(fid,'H',0);
Pid = H5G.create(fid, 'Params',0);

%% zapis parametru do souboru a slozky Params
HDF5_add_to_group(Pid, 1, 'sigma', sigma);
HDF5_add_to_group(Pid, 1, 'eps', eps);
HDF5_add_to_group(Pid, 1, 'R', R);
HDF5_add_to_group(Pid, 1, 'G0', G0);
HDF5_add_to_group(Pid, 1, 'Gf', Gf);
HDF5_add_to_group(Pid, 1, 'jmax', jmax);
HDF5_add_to_group(Pid, 1, 'imax', imax);
HDF5_add_to_group(Pid, 1, 'rmax', rmax);
HDF5_add_to_group(Pid, 1, 'zmax', zmax);
HDF5_add_to_group(Pid, 1, 'r0', r0);
HDF5_add_to_group(Pid, 1, 'ln', ln);
HDF5_add_to_group(Pid, 1, 'l_dis', l_dis);
HDF5_add_to_group(Pid, 1, 'alpha', alpha);
HDF5_add_to_group(Pid, 1, 'a1', a1);
HDF5_add_to_group(Pid, 1, 'A', A);
HDF5_add_to_group(Pid, 1, 'vm', vm);
HDF5_add_to_group(Pid, 1, 'o', o);
HDF5_add_to_group(Pid, 1, 'M', M);
HDF5_add_to_group(Pid, 1, 'prostredi', env_num);


% nastaveni pro groups G a H
% H5P.set_chunk(plist,[imax M]); % nechunkuju = nedelim data na mensi segmenty
% level = 6; %level komprese
% H5P.set_deflate(plist,level);

%% vlastni vypocet
for j=1:jmax % pres vsechny body v podelnem smeru
    j

    %% zapis G a H do souboru
    if mod(j,10) == 1
        HDF5_add_to_group(Gid, 2, strcat('j', num2str(j)), G);
        HDF5_add_to_group(Hid, 2, strcat('j', num2str(j)), H);
    end
    
    %% prubeh HAT ve vzdalenosti j/jmax*r0*zmax
%     if j == 20
%         for k=1:M
%            W(k,1) = sqrt(G(k,1)^2+H(k,1)^2); %amplitude
%            amplitudes(k) = W(k,1)*pm;
%         end
%         break % dal uz nepocitej
%     end 
    
    %% cyklus pres vsechny harmonicke
    for k = 1:M
        
        dn = 1+k^2*A/(2*G0)*sigma;
        
        %% prvni cast
        for i=1:imax
           gf(i) = G(k,i)+sigma*k*A*sumG(G, H, k, i, M)/4;
           hf(i) = H(k,i)+sigma*k*A*sumH(G, H, k, i, M)/4;
        end
                
        for i=1:imax
            gn(i) = G(k,i);
            hn(i) = H(k,i);
        end
        
        
        %% druha cast
        tmp_G = multiply(diag, u_diag, a_diag, hn);
        for i=1:imax
           mult_g = tmp_G(i);
           gn(i) = 1/dn*(gf(i)+R*mult_g/(4*k*Gf));
        end
        
        tmp_H = multiply(diag, u_diag, a_diag, gn);
        for i=1:imax
           mult_h = tmp_H(i);
           hn(i) = 1/dn*(hf(i)-R*mult_h/(4*k*Gf));
        end
        
        %% korekce harmonickych - vyhlazeni prubehu, nahrada za nedostatek harmonickych
        for i=1:imax
           G(k,i) = gn(i);%*sin(k/100)/(k/100);
           H(k,i) = hn(i);%*sin(k/100)/(k/100);
        end
        
    end %harmonicke
end % podelne

H5F.close(fid);

toc %kolik trvalo
%% vykresleni
distance = -5; % ve vzdalenosti
plotting(filename, distance, r0, M, jmax, imax, zmax, rmax, pm);


%% ----------FUNKCE-----------

function amp=a_vals(n,M)
    global rmax;
    global peak;
    amp = zeros(M,n);
    t = linspace(0,rmax,n);
    for i=1:n
        for k=1:M
        	%amp(1,i) = exp(-(2*t(i)).^2);
            amp(k,i) = peak(k)*exp(-(2*t(i)).^2);
            %amp(k,i) = peak(k)./(1+exp(2.*(5*t(i)-5)));
        end
    end
end

function pha = p_vals(n,M)
    global harm_generative;
    pha = zeros(M,n);
    for i=1:n
        %pha(1,i) = -90 + 0*t(i);
        for k=harm_generative
            pha(k,i) = -90;
        end
    end
end

function re = real(a,f)
    re = a*cos(pi/180*f);
end

function im = imag(a,f)
    im = -a*sin(pi/180*f);
end


function result = multiply(dg, udg, adg, gh)
    global imax;
    result = zeros(1, imax);
    for i=1:imax
        if i == 1
            result(i) = dg(i)*gh(i)+adg(i)*gh(i+1);
        elseif i == imax
            result(i) = udg(i-1)*gh(i-1)+dg(i)*gh(i);
        else
            result(i) = udg(i-1)*gh(i-1)+dg(i)*gh(i)+adg(i)*gh(i+1); 
        end        
    end
end

function sum = sumG(G, H, n, i, M)
    sum = 0;
    for k=1:n-1
        sum = sum+G(n-k,i)*G(k,i)-H(n-k,i)*H(k,i);
    end
    sum = 0.5*sum;
    for k=(n+1):M
        sum = sum-G(k-n,i)*G(k,i)-H(k-n,i)*H(k,i);
    end
end


function sum = sumH(G, H, n, i, M)
    sum = 0;
    for k=1:n-1
        sum = sum+G(n-k,i)*H(k,i)+H(n-k,i)*G(k,i);
    end
    sum = 0.5*sum;
    for k=(n+1):M
        sum = sum+H(k-n,i)*G(k,i)-G(k-n,i)*H(k,i);
    end
end

function [G0, r0, a1, alpha, ln, l_dis, A] = calc_coeffs(vm, a, f, environment)
    if environment == "vzduch"
        c0 = 345.22;
        rho0 = 1.193;
        cp = 1004;
        cv = 716.1;
        gamma = cp/cv;
        ni2 = 1.096e-5;
        ni1 = 1.827e-5;
        k = 25.87e-3;
    else
        c0 = 1503;
        rho0 = 1000;
        cp = 4184;
        gamma = 6;
        cv = 4184;
        ni2 = 3.04e-3;
        ni1 = 1.0e-3;
        k = 5.98e-1;
    end
        beta = (gamma+1)/2;
        omega = 2*pi*f;
        b = ni2+4/3*ni1+k*(1/cv-1/cp);
        r0 = (omega*a^2)/(2*c0);
        G0 = (2*beta*vm*rho0*c0)/(b*omega);
        ln = c0^2/(beta*vm*omega);
        alpha = b*omega^2/(2*rho0*c0^3);
        a1 = alpha*r0;
        l_dis = 1/alpha;
        A = 2*r0/ln;
end

%% koeficienty pred sin() pro buzeni


%% HDF5
function HDF5_add_to_group(group_id, group_name, name, data)
    global M imax;
    plist = H5P.create('H5P_DATASET_CREATE');
    if group_name == 1
        space_id = H5S.create_simple(1,1,1);
    else
        dims = fliplr([M imax]);
        space_id = H5S.create_simple(2, dims, dims);
    end
    dataset_id = H5D.create(group_id, name, 'H5T_NATIVE_DOUBLE', space_id, plist);
    H5D.write(dataset_id, 'H5T_NATIVE_DOUBLE', 'H5S_ALL', 'H5S_ALL', 'H5P_DEFAULT', data);
    H5S.close(space_id);
    H5D.close(dataset_id);
end

function sum = an(o,w,n)
    global m;
    dt = 0.000001;
    t = 0:dt:2*pi/o;
    sum = 0;
    for i=1:length(t)
        sum = sum + dt*sqrt(1+m*(cos(o*t(i))))*sin(w*t(i))*cos(n*o*t(i));
        %sum = sum + dt*(sqrt(1-m*(cos(o*t(i))+cos(2*o*t(i)))))*sin(w*t(i))*cos(n*o*t(i));
    end
    sum = sum*o/pi;
end

function sum = bn(o,w,n)
    global m;
    dt = 0.000001;
    t = 0:dt:2*pi/o;
    sum = 0;
    for i=1:length(t)
        sum = sum + dt*sqrt(1+m*(cos(o*t(i))))*sin(w*t(i))*sin(n*o*t(i));
    end
    sum = sum*o/pi;
end

