%% sources of sound

clear all
clc

%aperture
x=linspace(1,0,40);
y=linspace(1,0,40);

%%source's positions
% source 1
x1=0.4;
y1=0.2;

%source 2
x2=0.8;
y2=0.55;

%source 3
x3=0.5;
y3=0.65;

%source's properties 
f=1000; %Hz frequency
A1=1;%magnitude
w=2*pi*f;% angular speed
c=343; %m/s speed of sound in air
lambda=c/f; %m wave length
k0=w/c; %wave number
pref=2*10^-5; %Pa 
p0=1.275; %kg/m3 air density


%distance matrix, source plane
for m=1:length(y)
    for n=1:length(x)
        r1source(m,n)=sqrt((x(n)-x1)^2+(y(m)-y1)^2); 
        r2source(m,n)=sqrt((x(n)-x2)^2+(y(m)-y2)^2);
        r3source(m,n)=sqrt((x(n)-x3)^2+(y(m)-y3)^2);
    end    
end

%distance matrix, measured plane, distance lambda/3 from source
for m=1:length(y)
    for n=1:length(x)
        r1meas(m,n)=sqrt((x(n)-x1)^2+(y(m)-y1)^2+(lambda/3)^2); 
        r2meas(m,n)=sqrt((x(n)-x2)^2+(y(m)-y2)^2+(lambda/3)^2);
        r3meas(m,n)=sqrt((x(n)-x3)^2+(y(m)-y3)^2+(lambda/3)^2);
    end    
end

%distance matrix, reconstructed forward plane, distance 2lambda/3 from source
for m=1:length(y)
    for n=1:length(x)
        r1rec(m,n)=sqrt((x(n)-x1)^2+(y(m)-y1)^2+(2*lambda/3)^2); 
        r2rec(m,n)=sqrt((x(n)-x2)^2+(y(m)-y2)^2+(2*lambda/3)^2);
        r3rec(m,n)=sqrt((x(n)-x3)^2+(y(m)-y3)^2+(2*lambda/3)^2);
    end    
end

%sount pressure on measured plane
p1meas=(A1.*exp(-j*k0.*r1meas))./r1meas; %pressure from source 1
p2meas=(A1.*exp(-j*k0.*r2meas))./r2meas; %preassure from source 2
p3meas=(A1.*exp(-j*k0.*r3meas))./r3meas; %pressure from source 3
Pm=p1meas+p2meas+p3meas;%total pressure from all three sourcec
pmeas=abs(p1meas+p2meas+p3meas); %total abs pressure without noise, abs
Lmeas=20*log10((pmeas)/pref); %pressure level without noise

%noise definition
pmin=min(min(pmeas)); %determination of minimal pressure value
s1=pmin*1;%sqrt noise variance level 1
s2=pmin*10;%sqrt noise variance level 2
s3=pmin*20;%sqrt noise variance level 3


pmeas_noise1=Pm + s1*randn(size(Pm)); %prassure on measured plane with noise1
Lmeas_noise1=20*log10(abs(pmeas_noise1)/pref);%pressure level on measured plane with noise1
pmeas_noise2=Pm + s2*randn(size(Pm)); %prassure on measured plane with noise2
Lmeas_noise2=20*log10(abs(pmeas_noise2)/pref);%pressure level on measured plane with noise2
pmeas_noise3=Pm + s3*randn(size(Pm)); %prassure on measured plane with noise3
Lmeas_noise3=20*log10(abs(pmeas_noise3)/pref);%pressure level on measured plane with noise3


%acoustic pressure on source plane
p1source=(A1.*exp(-j*k0.*r1source))./r1source;%pressure from source 1
p2source=(A1.*exp(-j*k0.*r2source))./r2source;%pressure from source 2
p3source=(A1.*exp(-j*k0.*r3source))./r3source;%pressure from source 3
Ps=p1source+p2source+p3source;%total pressure from three sources
psource=abs(p1source+p2source+p3source); %total abs pressure without noise, abs
Lsource=20*log10((psource)/pref); %pressure level without noise  
psource_noise1=Ps + s1*randn(size(Ps));%pressure level with noise 1
Lsource_noise1=20*log10(abs(psource_noise1)/pref);%pressure level with noise 1
psource_noise2=Ps + s2*randn(size(Ps));%pressure level with noise 2
Lsource_noise2=20*log10(abs(psource_noise2)/pref);%pressure level with noise 2
psource_noise3=Ps + s3*randn(size(Ps));%pressure level with noise 3
Lsource_noise3=20*log10(abs(psource_noise3)/pref);%pressure level with noise 3


%acoustic pressure on reconstructed plane
p1rec=(A1.*exp(-j*k0.*r1rec))./r1rec;%pressure from source 1
p2rec=(A1.*exp(-j*k0.*r2rec))./r2rec;%pressure from source 2
p3rec=(A1.*exp(-j*k0.*r3rec))./r3rec;%pressure from source 3
Pr=p1rec+p2rec+p3rec;%total pressure from all three sources
prec=abs(p1rec+p2rec+p3rec); %total abs pressure without noise, abs
Lrec=20*log10((prec)/pref); %pressure level without noise
prec_noise1=Pr + s1*randn(size(Pr));%pressure level with noise 1
Lrec_noise1=20*log10(abs(prec_noise1)/pref);%pressure level with noise 1
prec_noise2=Pr + s2*randn(size(Pr));%pressure level with noise  2
Lrec_noise2=20*log10(abs(prec_noise2)/pref);%pressure level noise 2
prec_noise3=Pr + s3*randn(size(Pr));%pressure level with noise 3
Lrec_noise3=20*log10(abs(prec_noise3)/pref);%pressure level with noise 3

%%
%%SONAH 

%Source plane
Bsource=Ps; %matrix of Elementary Wave Functions for source plane
Asource=Bsource.';
eps_source0=18;%regularization parameter for reconstruction without noise
cross_source=zeros(length(x));
for ii=1:length(x)     
    for nn=1:length(y)
       cross_source(ii,:)=cross_source(ii,:)+(conj(Bsource(ii,nn))*Bsource(nn,:)); 
    end       
end

prsource=abs((Pm.')*inv((Asource')*Asource+eps_source0*eye(length(x)))*cross_source);
Lrsource=20*log10(prsource/pref); % without noise 

eps_source1=20;%regularization parameter for reconstruction with noise 1
eps_source2=35;%regularization parameter for reconstruction with noise 2
eps_source3=40;%regularization parameter for reconstruction with noise 3
prsource_noise1=abs((pmeas_noise1.')*inv((Asource')*Asource+eps_source1*eye(length(x)))*cross_source);% with noise 1
prsource_noise2=abs((pmeas_noise2.')*inv((Asource')*Asource+eps_source2*eye(length(x)))*cross_source);% with noise 2
prsource_noise3=abs((pmeas_noise3.')*inv((Asource')*Asource+eps_source3*eye(length(x)))*cross_source);% with noise 3

%Reconstruction plane
Brec=Pr; %matrix of Elementary Wave Functions for reconstructed plane
Arec=Brec.';
crossrec=zeros(length(x));
eps_rec0=25;%regularization parameter for reconstruction without noise
for ii=1:length(x)  
    for nn=1:length(y)
        crossrec(ii,:)=crossrec(ii,:)+(conj(Brec(ii,nn))*Brec(nn,:));
    end
end
prrec=abs((Pm.')*inv((Arec')*Arec+eps_rec0*eye(length(x),length(y)))*crossrec); %without noise
Lrrec=20*log10(prrec/pref); 

eps_rec1=25;%regularization parameter for reconstruction with noise 1
eps_rec2=29;%regularization parameter for reconstruction with noise 2
eps_rec3=28;%regularization parameter for reconstruction with noise 3
prrec_noise1=abs((pmeas_noise1.')*inv((Arec')*Arec+eps_rec1*eye(length(x)))*crossrec);% with noise 1
prrec_noise2=abs((pmeas_noise2.')*inv((Arec')*Arec+eps_rec2*eye(length(x)))*crossrec);% with noise 2
prrec_noise3=abs((pmeas_noise3.')*inv((Arec')*Arec+eps_rec3*eye(length(x)))*crossrec);% with noise 3

%%
%REGULARISATION, Finding the regularization parameter

Hs=ones(length(x),length(y))*(j*w*p0);%spatial transfer function, made into matrix

sigma1=sqrt(length(x)*length(y))*s1;
sigma2=sqrt(length(x)*length(y))*s2;
sigma3=sqrt(length(x)*length(y))*s3;

s=1000000;%service variable

for ii=0:100
    eps=ii;
    ra=(inv(eps*eye(length(x))+(Hs')*Hs))*(Hs'); 
    P=Hs*ra*Pm; %wanted pressure matrix P from measured pressure matrix Pm
    n=norm(P-Pm)-sigma1; 
    if (n<s)  
        s=n;
        eps_reg_meas0=ii;
    end
end

for ii=0:100
    eps=ii;
    ra=(inv(eps*eye(length(x))+(Hs')*Hs))*(Hs');
    P=Hs*ra*pmeas_noise1;%wanted pressure matrix P from measured pressure matrix pmeas_noise1
    n=norm(P-pmeas_noise1)-sigma1;
    if (n<s)
        s=n;
        eps_reg_meas1=ii;
    end
end

for ii=0:100
    eps=ii;
    ra=(inv(eps*eye(length(x))+(Hs')*Hs))*(Hs');
    P=Hs*ra*pmeas_noise2;%wanted pressure matrix P from measured pressure matrix pmeas_noise2
    n=norm(P-pmeas_noise2)-sigma2;
    if (n<s)
        s=n;
        eps_reg_meas2=ii;
    end
end

for ii=0:100
    eps=ii;
    ra=(inv(eps*eye(length(x))+(Hs')*Hs))*(Hs');
    P=Hs*ra*pmeas_noise3;%wanted pressure matrix P from measured pressure matrix pmeas_noise3
    n=norm(P-pmeas_noise3)-sigma3;
    if (n<s)
        s=n;
        eps_reg_meas3=ii;
    end
end

prsource_mor=abs((Pm.')*(inv(Asource'*Asource+eps_reg_meas0*eye(length(x))))*cross_source);
Lrsource_mor=20*log10(prsource_mor/pref);
prsource_noise1_mor=abs((pmeas_noise1.')*(inv(Asource'*Asource+eps_reg_meas1*eye(length(x))))*cross_source);
Lrsource1_mor=20*log10(prsource_noise1_mor/pref);
prsource_noise2_mor=abs((pmeas_noise2.')*(inv(Asource'*Asource+eps_reg_meas2*eye(length(x))))*cross_source);
Lrsource2_mor=20*log10(prsource_noise2_mor/pref);
prsource_noise3_mor=abs((pmeas_noise3.')*(inv(Asource'*Asource+eps_reg_meas3*eye(length(x))))*cross_source);
Lrsource3_mor=20*log10(prsource_noise3_mor/pref);

prrec_mor=abs((Pm.')*(inv(Arec'*Arec+eps_reg_meas0*eye(length(x))))*crossrec);
Lrrec_mor=20*log10(prrec_mor/pref);
prrec_noise1_mor=abs((pmeas_noise1.')*(inv(Arec'*Arec+eps_reg_meas1*eye(length(x))))*crossrec);
Lrrec1_mor=20*log10(prrec_noise1_mor/pref);
prrec_noise2_mor=abs((pmeas_noise2.')*(inv(Arec'*Arec+eps_reg_meas2*eye(length(x))))*crossrec);
Lrrec2_mor=20*log10(prrec_noise2_mor/pref);
prrec_noise3_mor=abs((pmeas_noise3.')*(inv(Arec'*Arec+eps_reg_meas3*eye(length(x))))*crossrec);
Lrrec3_mor=20*log10(prrec_noise3_mor/pref);

figure
pcolor(x,y,Lrsource_mor);
colorbar;
shading interp;
title ('L [dB], source plane, morozov');xlabel('x [m]'); ylabel('y [m]');

figure
pcolor(x,y,Lrsource1_mor);
colorbar;
shading interp;
title ('L [dB], source plane,noise1, morozov');xlabel('x [m]'); ylabel('y [m]');

figure
pcolor(x,y,Lrsource2_mor);
colorbar;
shading interp;
title ('L [dB], source plane, noise2, morozov');xlabel('x [m]'); ylabel('y [m]');

figure
pcolor(x,y,Lrsource3_mor);
colorbar;
shading interp;
title ('L [dB], source plane, noise3, morozov');xlabel('x [m]'); ylabel('y [m]');

figure
pcolor(x,y,Lrrec_mor);
colorbar;
shading interp;
title ('L [dB], reconstruction plane, morozov');xlabel('x [m]'); ylabel('y [m]');

figure
pcolor(x,y,Lrrec1_mor);
colorbar;
shading interp;
title ('L [dB], reconstruction plane, noise1, morozov');xlabel('x [m]'); ylabel('y [m]');

figure
pcolor(x,y,Lrrec2_mor);
colorbar;
shading interp;
title ('L [dB], reconstruction plane, noise2, morozov');xlabel('x [m]'); ylabel('y [m]');

figure
pcolor(x,y,Lrrec3_mor);
colorbar;
shading interp;
title ('L [dB], reconstruction plane, noise3, morozov');xlabel('x [m]'); ylabel('y [m]');

%%
% %GRAPHS 
% figure
% subplot(4,3,1);
% pcolor(x,y,Lsource);
% colorbar;
% shading interp;
% title ('L [dB], source plane'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(4,3,2);
% pcolor(x,y,Lmeas);
% colorbar;
% shading interp;
% title ('L [dB], measurement plane');xlabel('x [m]'); ylabel('y [m]');
% 
% subplot(4,3,3);
% pcolor(x,y,Lrec);
% colorbar;
% shading interp;
% title ('L [dB], reconstruction plane');xlabel('x [m]'); ylabel('y [m]');
% 
% subplot(4,3,4);
% pcolor(x,y,Lsource_noise1);
% colorbar;
% shading interp;
% title ('Source with noise s1'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(4,3,5);
% pcolor(x,y,Lmeas_noise1);
% colorbar;
% shading interp;
% title('Measure with noise s1'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(4,3,6);
% pcolor(x,y,Lrec_noise1);
% colorbar;
% shading interp;
% title ('Reconstruction with noise s1'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(4,3,7);
% pcolor(x,y,Lsource_noise2);
% colorbar;
% shading interp;
% title ('Source with noise s2'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(4,3,8);
% pcolor(x,y,Lmeas_noise2);
% colorbar;
% shading interp;
% title('Measure with noise s2'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(4,3,9);
% pcolor(x,y,Lrec_noise2);
% colorbar;
% shading interp;
% title ('Reconstruction with noise s2'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(4,3,10);
% pcolor(x,y,Lsource_noise3);
% colorbar;
% shading interp;
% title ('Source with noise s3'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(4,3,11);
% pcolor(x,y,Lmeas_noise3);
% colorbar;
% shading interp;
% title('Measure with noise s3'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(4,3,12);
% pcolor(x,y,Lrec_noise3);
% colorbar;
% shading interp;
% title ('Reconstruction with noise s3'); xlabel('x [m]');ylabel('y [m]');

%%
% %GRAPHS RECONSTRUCTED SOURCE PLANE
% figure
% subplot(2,4,1);
% pcolor(x,y,Lsource);
% colorbar;
% shading interp;
% title ('L [dB], source plane'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(2,4,2);
% pcolor(x,y,Lsource_noise1);
% colorbar;
% shading interp;
% title ('Source with noise s1'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(2,4,3);
% pcolor(x,y,Lsource_noise2);
% colorbar;
% shading interp;
% title ('Source with noise s2'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(2,4,4);
% pcolor(x,y,Lsource_noise3);
% colorbar;
% shading interp;
% title ('Source with noise s3'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(2,4,5);
% pcolor(x,y,Lrsource);
% colorbar;
% shading interp;
% title ('L [dB], source plane without noise SONAH'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(2,4,6);
% pcolor(x,y,20*log10(prsource_noise1/pref));
% colorbar;
% shading interp;
% title ('L [dB], source plane with noise s1 SONAH ');xlabel('x [m]'); ylabel('y [m]');
% 
% subplot(2,4,7);
% pcolor(x,y,20*log10(prsource_noise2/pref));
% colorbar;
% shading interp;
% title ('L [dB], source plane with noise s2 SONAH ');xlabel('x [m]'); ylabel('y [m]');
% 
% subplot(2,4,8);
% pcolor(x,y,20*log10(prsource_noise3/pref));
% colorbar;
% shading interp;
% title ('L [dB], source plane with s3 noise SONAH ');xlabel('x [m]'); ylabel('y [m]');
% 
% %GRAPHS RECONSTRUCTED PLANE REC
% figure
% subplot(2,4,1);
% pcolor(x,y,Lrec);
% colorbar;
% shading interp;
% title ('L [dB], rec plane'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(2,4,2);
% pcolor(x,y,Lrec_noise1);
% colorbar;
% shading interp;
% title ('Rec with noise s1'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(2,4,3);
% pcolor(x,y,Lrec_noise2);
% colorbar;
% shading interp;
% title ('Rec with noise s2'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(2,4,4);
% pcolor(x,y,Lrec_noise3);
% colorbar;
% shading interp;
% title ('Rec with noise s3'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(2,4,5);
% pcolor(x,y,Lrrec);
% colorbar;
% shading interp;
% title ('L [dB], rec plane without noise SONAH'); xlabel('x [m]');ylabel('y [m]');
% 
% subplot(2,4,6);
% pcolor(x,y,20*log10(prrec_noise1/pref));
% colorbar;
% shading interp;
% title ('L [dB], rec plane with noise s1 SONAH ');xlabel('x [m]'); ylabel('y [m]');
% 
% subplot(2,4,7);
% pcolor(x,y,20*log10(prrec_noise2/pref));
% colorbar;
% shading interp;
% title ('L [dB], rec plane with noise s2 SONAH ');xlabel('x [m]'); ylabel('y [m]');
% 
% subplot(2,4,8);
%  pcolor(x,y,20*log10(prrec_noise3/pref));
%  colorbar;
%  shading interp;
%  title ('L [dB], rec plane with s3 noise SONAH ');xlabel('x [m]'); ylabel('y [m]');
