clear all
% close all

%%
%
%Code used to generate the figures of the 
%paper 'Illumination strategies for intensity-only imaging'
%
%%
%Imaging configuration
lambda = 0.01; % wavelength

la = 100*lambda; % length of array
h = 1*lambda; % sampling interval of array

L = 100*lambda; % distance from array to image window

s = 1*lambda; % sampling interval of image window
nx = 30; % number of pixels along x
ny = nx; % Image Window is square


[gg,ca,cimx,cimy,DD] = imagconfig(la,h,L,s,nx,ny,lambda); %gg: green's functions

Na = length(ca); % # of transducers
n=nx*ny;         % # of pixels in IW



K=input('What is the number of scatterers? (by default =7)');
if isempty(K)==1
   K = 7;   % # of non-zero components 
end
    

perc_noi=input('What is the fraction of noise? (by default =.2)');
if isempty(perc_noi)==1
    perc_noi = 0.2; % percentage 'in amplitudes'
end

xref = zeros(n,1);
ipos = randperm(n);
xref(ipos(1:K)) =  ((rand(1,K))+1.2).*exp(1i*2*pi*rand(1,K));

ptx = floor(ipos(1:K)/nx) + 1; ptx(ptx==0) = 1;
pty = mod(ipos(1:K),nx); pty(pty==nx) = nx;

%% Uncomment the next 9 lines to see the original image
% figure(1); imagesc(reshape(abs(xref),ny,nx)); colorbar, hold on % Plot of original points
% plot(ptx, pty, 'o', 'MarkerEdgeColor', 'white', 'MarkerSize', 2), hold off
% xlabel('x (\lambda units)','FontSize',18)
% ylabel('y','FontSize',18)
% set(gca,'XTick',0.5:5:30.5)
% set(gca,'XTickLabel',{'85','90','95','100','105','110','115'},'FontSize',16)
% set(gca,'YTick',0.5:5:30.5)
% set(gca,'YTickLabel',{'15','10','5','0','-5','-10', '-15'},'FontSize',16)

%% uncomment the next line to plot the image
% print('-depsc2',['REF_M',num2str(K),'_Noisep',num2str(10*perc_noi),'.eps'])


%% Matrix M=P^*P

M=zeros(Na,Na); 
Mfreenoise = M;
P=zeros(Na,Na); 
b=zeros(Na,1);
noi=zeros(Na,1);

Aaux = zeros(Na,n);
b_Iaux = zeros(Na,1);

for i=1:Na
  source = i; % transducer that fires
  f = zeros(Na,1); f(source) = 1; % illumination vector
    
  Af = repmat((gg.'*f).',Na,1).*gg; % matrix of the linear system
  b = Af*xref;
  noise = perc_noi*randn(Na,1);
  P(:,i) = b.*(1 + sqrt(noise)/2);
  b2 = abs(b).^2;
  Mfreenoise(i,i) = sum(b2);
  b2 = b2.*(1 + noise);
  b2 = max(0,b2);
  M(i,i) = sum(b2);
 
  
  Aaux = [Aaux; Af];
  b_Iaux = [b_Iaux; b2];
end

for i=2:Na
  for j=1:i-1

    f = zeros(Na,1); f(i) = 1; f(j) = 1;% illumination vector
    Af = repmat((gg.'*f).',Na,1).*gg; % matrix of the linear system
    b =Af*xref;
    noise = perc_noi*randn(Na,1);
    b2 = abs(b).^2;
    rebfreenoise = (sum(b2) - Mfreenoise(i,i) - Mfreenoise(j,j))/2;
    b2 = b2.*(1 + noise);
    b2 = max(0,b2);
    reb = (sum(b2) - M(i,i) - M(j,j))/2;
   

    f = zeros(Na,1); f(i) = 1; f(j) = sqrt(-1);% illumination vector
    Af = repmat((gg.'*f).',Na,1).*gg; % matrix of the linear system
    b =Af*xref;
    noise = perc_noi*randn(Na,1);
    b2 = abs(b).^2;
    imbfreenoise = (sum(b2) - Mfreenoise(i,i) - Mfreenoise(j,j))/2;
    b2 = b2.*(1 + noise);
    b2 = max(0,b2);
    imb = (sum(b2) - M(i,i) - M(j,j))/2;

    Mfreenoise(i,j)=rebfreenoise-sqrt(-1)*imbfreenoise;
    Mfreenoise(j,i)=rebfreenoise+sqrt(-1)*imbfreenoise;
    
    M(i,j)=reb-sqrt(-1)*imb;
    M(j,i)=reb+sqrt(-1)*imb;

  end
end

%% SVD
[U, S, V]=svd(M);


%% MUSIC

functional = zeros(1,ny*nx);
illum_window = functional;
for ik=K+1:Na
  f = V(:,ik);
  illum_window = transpose(f)*gg;
  functional = functional + abs(illum_window);
end

aux = min(functional);
functional = aux./functional;
figure(2); imagesc(reshape(functional,ny,nx)); colorbar, hold on
title('MUSIC. Small dots are true locations')
plot(ptx, pty, 'o', 'MarkerEdgeColor', 'white', 'MarkerSize', 2), hold off
xlabel('x (\lambda units)','FontSize',18)
ylabel('y','FontSize',18)
set(gca,'XTick',0.5:5:30.5)
set(gca,'XTickLabel',{'85','90','95','100','105','110','115'},'FontSize',16)
set(gca,'YTick',0.5:5:30.5)
set(gca,'YTickLabel',{'15','10','5','0','-5','-10', '-15'},'FontSize',16)

%% uncomment the next line to plot the image
print('-depsc2',['MUSIC_M',num2str(K),'_Noisep',num2str(10*perc_noi),'.eps'])



%% MMV

% First step. Effective sources

Nillum = K;
allMMV = zeros(n, Nillum);
dat_m = zeros(Na,Nillum);
dat_m = conj(V(:,1:Nillum))*sqrt(S(1:Nillum,1:Nillum));

params.exactSol = xref;
params.numOfIter = 10000;
params.display = false;% make it true if you want to see performance of Gelma

Xrho = GelmaMMV(gg, dat_m, params);

aux = mean(abs(Xrho),2); aux = aux/max(aux(:));

%% Uncomment the next 9 lines to see the image without correct reflectivities
%figure(3); imagesc(reshape(aux,ny,nx)); colorbar, caxis([0 1]), hold on
%plot(ptx, pty, 'o', 'MarkerEdgeColor', 'white', 'MarkerSize', 2), hold off
%title('MMV. Effective sources')
%set(gca,'XTick',0.5:5:30.5)
%set(gca,'XTickLabel',{'85','90','95','100','105','110','115'},'FontSize',16)
%set(gca,'YTick',0.5:5:30.5)
%set(gca,'YTickLabel',{'15','10','5','0','-5','-10', '-15'},'FontSize',16)
%xlabel('x (\lambda units)','FontSize',16)
%ylabel('y','FontSize',16)

%% uncomment the next line to plot the image
%print('-depsc2',['MMV0_M',num2str(K),'_Noisep',num2str(10*perc_noi),'.eps'])



% Second step. Recovering reflectivities

Xrhoafter = zeros(size(Xrho));

mmv_sol = zeros(nx,ny);
for ik = 1:Nillum
    mmv_min = Xrho(:,ik);
    mmv_min = reshape(mmv_min,ny,nx);
    
    fillum = V(:, ik);
    illum_window = reshape(transpose(fillum)*gg,ny,nx);
    Xrhoafter(:,ik) = abs(Xrho(:,ik))./abs(transpose(gg)*fillum);
end
mmv_sol = sum(abs(Xrhoafter),2)/Nillum;
mmv_sol = reshape(mmv_sol,ny,nx);


%% Uncomment the next 9 lines to see the image without optimization
% figure(4); imagesc(mmv_sol); colorbar, hold on
% plot(ptx, pty, 'o', 'MarkerEdgeColor', 'white', 'MarkerSize', 2), hold off
% title('MMV. Reflectivities without regularization')
% xlabel('x (\lambda units)','FontSize',18)
% ylabel('y','FontSize',18)
% set(gca,'XTick',0.5:5:30.5)
% set(gca,'XTickLabel',{'85','90','95','100','105','110','115'},'FontSize',16)
% set(gca,'YTick',0.5:5:30.5)
% set(gca,'YTickLabel',{'15','10','5','0','-5','-10', '-15'},'FontSize',16)

%% uncomment the next line to plot the image
% print('-depsc2',['MMV1_M',num2str(K),'_Noisep',num2str(10*perc_noi),'.eps'])


% Recovering the reflectivities with regularization

Xrhoafter_th = zeros(size(Xrho));
counts = zeros(ny*nx,1);
for ik = 1:Nillum
    
    fillum = V(:, ik);
    illum_window = transpose(fillum)*gg;
    max_illum = max(abs(illum_window(:)));
    indhighillum = abs(illum_window) > 0.7*max_illum;
    
    aux = abs(illum_window(indhighillum))';
    counts(indhighillum) = counts(indhighillum) + 1;
    Xrhoafter_th(indhighillum,ik) = abs(Xrho(indhighillum,ik))./aux;
    
end
mmv_sol_th = sum(Xrhoafter_th,2);
mmv_sol_th = mmv_sol_th./counts;
mmv_sol_th = reshape(mmv_sol_th,ny,nx);

figure(5); imagesc(mmv_sol_th); colorbar, hold on
plot(ptx, pty, 'o', 'MarkerEdgeColor', 'white', 'MarkerSize', 2), hold off
title('MMV with optimization of reflectivities')
xlabel('x (\lambda units)','FontSize',18)
ylabel('y','FontSize',18)
set(gca,'XTick',0.5:5:30.5)
set(gca,'XTickLabel',{'85','90','95','100','105','110','115'},'FontSize',16)
set(gca,'YTick',0.5:5:30.5)
set(gca,'YTickLabel',{'15','10','5','0','-5','-10', '-15'},'FontSize',16)

%% uncomment the next line to plot the image
% print('-depsc2',['MMV1th_M',num2str(K),'_Noisep',num2str(10*perc_noi),'.eps'])


