%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
% Paper: S.P. Chepuri and G. Leus. Sparsity-Promoting Sensor Selection 
%        for Non-linear Measurement Models. IEEE Trans. on Signal 
%        Processing, 63(3): 684-698, Feb. 2015.
% Author: Sundeep Prabhakar Chepuri, TU Delft
% Date: Sep 2013
%
% 
%
% Needs CVX toolbox
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all; clc; close all;
% x= -1.2:0.01:1.2; y = -1.2:0.01:1.2; s = [-0.15;0.15];
%
% for ii =1 : length(x)
%     for jj=1:length(x)
%
%     h(ii,jj) = 0.001 / (0.001 + norm(s - [x(ii);y(jj)],2)^2);
%     end
% end
%
% [XS,YS] = meshgrid(-0.4:0.1:0.4,-0.4:0.1:0.4); Smat = [XS(:) YS(:)]';
%
% [XA1,YA1] = meshgrid(-1:0.1:1,-1); [XA2,YA2] = meshgrid(-1:0.1:1,1);
% [XA3,YA3] = meshgrid(-1,-1:0.1:1);  [XA4,YA4] = meshgrid(1,-1:0.1:1);
% Amat = [XA1(:) YA1(:);XA2(:) YA2(:);...
%     XA3(:) YA3(:);XA4(:) YA4(:)]';

x= -1:0.1:42; y = -1:0.1:42; s = [25;25];

for ii =1 : length(x)
    for jj=1:length(x)
        
        h(ii,jj) = 1 / (1 + norm(s - [x(ii);y(jj)],2)^2);
    end
end

% Initialization
L=2; delA=2; delS=1.75;
%Sensors
[XA1,YA1] = meshgrid(1:delA:41,41); [XA2,YA2] = meshgrid(1:delA:41,1);
[XA3,YA3] = meshgrid(1,3:delA:39);  [XA4,YA4] = meshgrid(41,3:delA:39);
Amat = [XA1(:) YA1(:);XA2(:) YA2(:);...
    XA3(:) YA3(:);XA4(:) YA4(:)]';
%target area
[XS,YS] = meshgrid(15:delS:30,15:delS:30); Smat = [XS(:) YS(:)]';

M= size(Amat,2); N=size(Smat,2);
beta = 1;

%%covariance matrix
sigma2rss = 2*10^-5;  rho_horz = 0.5; rho_vert = 0;
Sigma2_horz = sigma2rss*((1-rho_horz)*eye(42) + rho_horz*ones(42,42));
Sigma2_vert = sigma2rss*((1-rho_vert)*eye(38) + rho_vert*ones(38,38));

% Sigma2_horz = toeplitz(rho_horz.^(0:42-1));
% Sigma2_vert = toeplitz(rho_vert.^(0:38-1));
Sigma2  = blkdiag(Sigma2_horz,Sigma2_vert);

a = 0.3*sigma2rss; ainv = 1/a;
Scorr = Sigma2 - a*eye(M);

Scorr = 0.5*(Scorr + Scorr.');

Scorr_inv = inv(Scorr);%Scorr\eye(size(Scorr));
Scorr_inv = 0.5*(Scorr_inv + Scorr_inv.');

Re = 0.2;  c = ones(M,1); Pe = 0.8;
lambda = (2/Re^2)*(1/(1-Pe));




if  eig(Scorr_inv) + ainv >0
    
    disp('OK')
    
else
    disp('Not OK')
end


%% Correlated MIL
% lambda computation
for jj=1:N
    for ii=1:M
        d = norm(Smat(:,jj)-Amat(:,ii),2);
        Jmat(ii,:,jj) = (-2*beta/(beta+d^2)^2)* (Smat(:,jj)-Amat(:,ii))';
        %           Jmat(ii,:,jj) =  (Smat(:,jj)-Amat(:,ii))'/norm(Smat(:,jj)-Amat(:,ii),2);
    end
    
    lam_min(jj) = min(eig(Jmat(:,:,jj)'*inv(Sigma2)*Jmat(:,:,jj)));
    
end

min_lam_min = min(lam_min);

%% N=1 case


% cvx_solver mosek
% cvx_begin sdp
% 
% variable w(M);
% variable t;
% expression Jmat(M,2);
% expression L(2,M)
% 
% 
% for ii=1:M
%     d = norm(Smat(:,1)-Amat(:,ii),2);
%     Jmat(ii,:) = (-2*beta/(beta+d^2)^2)* (Smat(:,1)-Amat(:,ii))';
%     %         Jmat(ii,:) =  (Smat(:,1)-Amat(:,ii))'/norm(Smat(:,1)-Amat(:,ii),2);
% end
% 
% L (:,:) = (Jmat(:,:)'*Scorr_inv) ;
% 
% 
% 
% % minimize norm(w,1)
% maximize t
% subject to
% [(Jmat(:,:)'*Scorr_inv*Jmat(:,:)) - t*eye(2), L(:,:)  ;
%     L(:,:)' , Scorr_inv + ainv*diag(w) ] >= zeros(M+2);
% w >= 0;
% w <= 1;
% sum(w) == 4;
% cvx_end
% 
% 
% figure;stem(w)

%%
% lambda = 0.5*min_lam_min;
% 
% %cvx_solver sedumi
% cvx_begin sdp
% 
% variable w(M);
% variable t;
% expression Jmat(M,2,N);
% expression L(2,M,N)
% 
% for jj=1:N
% %     for ii=1:M
% %         d = norm(Smat(:,jj)-Amat(:,ii),2);
% %         Jmat(ii,:,jj) = (-2*beta*d/(beta+d^2)^2)* (Smat(:,jj)-Amat(:,ii))';
% %         %         Jmat(ii,:,jj) =  (Smat(:,jj)-Amat(:,ii))'/norm(Smat(:,jj)-Amat(:,ii),2);
% %     end
%     
%     L (:,:,jj) = (Jmat(:,:,jj)'*Scorr_inv) ;
%     
% end
% 
% % minimize norm(w,1)
% maximize t
% subject to
% for jj=1:N
%     
%     [(Jmat(:,:,jj)'*Scorr_inv*Jmat(:,:,jj)) - t*eye(2), L(:,:,jj)  ;
%         L(:,:,jj)' , Scorr_inv + ainv*diag(w) ] >= 0;
%     
% end
% w >= 0;
% w <= 1;
% sum(w)==10;
% cvx_end
% 
% 
% figure;stem(w)


%%

lambda = 0.25*min_lam_min;

Re = sqrt(2/(lambda*(1-Pe)));

NUM_RUNS = 10;
cvx_solver sedumi
% cvx_solver MOSEK
nnzs = [];
delta = 1e-6;
U = ones(M,1); % initial weights
disp([char(10) 'Log-based heuristic:']);
for k = 1:NUM_RUNS
    
    cvx_begin sdp quiet
    variable wlog(M);
    %     variable t;
    expression L(2,M,N)
    
    for jj=1:N
        
%         for ii=1:M
%             d = norm(Smat(:,jj)-Amat(:,ii),2);
%             Jmat(ii,:,jj) = (-2*beta/(beta+d^2)^2)* (Smat(:,jj)-Amat(:,ii))';
%             %         Jmat(ii,:,jj) =  (Smat(:,jj)-Amat(:,ii))'/norm(Smat(:,jj)-Amat(:,ii),2);
%         end
        
        L (:,:,jj) = (Jmat(:,:,jj)'*Scorr_inv) ;
        
        L2(:,:,jj) = (Jmat(:,:,jj).'*Scorr_inv*Jmat(:,:,jj)) - lambda*eye(2);
        L2(:,:,jj) = 0.5*(L2(:,:,jj)+L2(:,:,jj).');
       
        
    end
    
    minimize (sum( U.*(wlog)));
    subject to
    for jj=1:N
        
        [L2(:,:,jj), L(:,:,jj)  ;
            L(:,:,jj).' , Scorr_inv + ainv*diag(wlog) ] >= 0;
        
    end
    
    wlog >= 0;
    wlog <= 1;
    cvx_end
    
    % display new number of nonzeros in the solution vector
    nnz = length(find( abs(wlog) > delta ));
    nnzs = [nnzs nnz];
    fprintf(1,' found a solution with %d nonzeros...\n', nnz);
    % adjust the weights and re-iterate
    U = 1./(delta + abs(wlog));
end
fprintf(1,' Re is %d \n', Re);
figure; stem(wlog)

%% plotting - selection
close all; %w=wlog;
figure(2); set(gca,'Fontsize', 16);
% h1 = stem(abs(w),'k','LineWidth',1.5,...
%     'MarkerEdgeColor','k',...
%     'MarkerSize',6,'DisplayName','l1-norm');
hold on;
h2 = stem(wlog,'--^','LineWidth',1.01,...
    'MarkerSize',7,'DisplayName','Concave surrogate (log-based)');
set(h2, 'Color', 'black');
set(gca,'YLim',[0 1.01]);
set(gca,'XLim',[1 80]);
xlabel('sensors');  ylabel('w');
hleg2= legend([h2],'Concave surrogate (log-based)AAA',...
    'Location','Best');
box on;
% set(hleg2,...
%     'Position',[0.212546814671829 0.761183733037684 0.628630530226275 0.161914143119817]);
print('-deps', 'feildestimation_correlated.eps');

% plotting-sensor topology
figure(1); set(gca,'Fontsize', 16);
imagesc(x,y,h); hold on;
hp1 = plot(Amat(1,1:42),Amat(2,1:42),'wd','LineWidth',1,...
    'MarkerEdgeColor','w',...
    'MarkerSize',8);
hp1 = plot(Amat(1,43:80),Amat(2,43:80),'ws','LineWidth',1,...
    'MarkerEdgeColor','w',...
    'MarkerSize',7);
hp2= plot(Smat(1,:),Smat(2,:),'wo','LineWidth',1,...
    'MarkerEdgeColor','w',...
    'MarkerSize',6);
wsel_idx = find(wlog >= 0.01);

hp3 = plot(Amat(1,wsel_idx),Amat(2,wsel_idx),'w*','LineWidth',2,...
    'MarkerEdgeColor','w',...
    'MarkerFaceColor','w',...
    'MarkerSize',10);

xlabel('x');  ylabel('y');
set(gca,'YLim',[-1 42]); set(gca,'XLim',[-1 42]);
grid off; %box off;
print('-deps', 'feildestimation_correlated_horz.eps');

