%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%
% 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;

% 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(:)]';
%Number of sensors and target grid points 
M= size(Amat,2); N=size(Smat,2); 
options.MeasurementType = 'Range'; %RangeBearing
c = ones(M,1);
%stacking FIM as blkdiag matrices
for m=1:M
    for n=1:N
        Faux(:,:,n) = FmPassive(Smat(:,n),Amat(:,m),options);
    end
    temp=num2cell(Faux,[1 2]); Fm(:,:,m) = blkdiag(temp{:});
end

%% l1-norm min. eigenvalue constraint

% Re=1 with Bearing
% RangeBearing Re=0.2, sigma2bearing = 10^-2; Pe = 0.95;
Re = 0.2; Pe = 0.9; 
lambda = (2/Re^2)*(1/(1-Pe));
cvx_solver sedumi;
tic;
cvx_begin sdp quiet
    variable w(M);
    minimize (c'*w);
    subject to
            Fblk = zeros(2*N,2*N);
            for m=1:M
                Fblk = Fblk + w(m).*Fm(:,:,m);
                0<=w(m)<=1;
            end;
            Fblk >= lambda*eye(2*N);                
cvx_end 
toc;
%figure;stem(w)
%% Sparsity-enchancing iterative algorithm (Concave surrogate) 
NUM_RUNS = 10;
%nnzs = [];
delta = 1e-8;
U = ones(M,1); % initial weights
%disp([char(10) 'Log-based heuristic:']);
for k = 1:NUM_RUNS
    cvx_begin sdp quiet
    variable wlog(M); 
    minimize(sum( U.*abs(wlog)))
    subject to
    Fblk = zeros(2*N,2*N);
            for m=1:M
                Fblk = Fblk + wlog(m).*Fm(:,:,m);
                0<=wlog(m)<=1;
            end;
            Fblk >= lambda*eye(2*N);                
    cvx_end
    
    % display new number of nonzeros in the solution vector
    %nnz = length(find( abs(wlog) > delta ));
    %nnzs = [nnzs nnz];
    % adjust the weights and re-iterate
    U = 1./(delta + abs(wlog));
end

%% plotting - selection
figure(2); set(gca,'Fontsize', 16);           
h1 = stem(abs(w),'ko','LineWidth',1.5,...
            'MarkerEdgeColor','k',...
            'MarkerSize',6,'DisplayName','l1-norm'); 
hold on; 
h2 = stem(wlog,'--k^','LineWidth',1.01,...
            'MarkerSize',7,'DisplayName','Concave surrogate (log-based)');
%set(h2, 'Color', [0.7,0.3,0]);        

% randomized rounding
% randomization rounding
%wlog_sel = randomizedrounding(wlog,Smat, Amat,N,M,lambda,options);

h3 = stem(wlog_sel,'-.k^','LineWidth',1.01,...
            'MarkerSize',6,'DisplayName','Randomized rounding (Proj. subgradient)');
%set(h3, 'Color', [0.7,0.3,0]); 
set(h3, 'MarkerFaceColor', 'black');        
set(gca,'YLim',[0 1.01]);

xlabel('sensors');  ylabel('w');
hleg2= legend([h1,h2,h3],'l1-norm','Concave surrogate (log-based)', 'Randomized rounding (Concave surrogate)AAA'...
                ,'Location','Best');
set(hleg2,...
    'Position',[0.216118243243258 0.130231352085316 0.628630530226275 0.161914143119817]);
print('-deps', 'logbased.eps');

%% plotting-sensor topology
figure(1); set(gca,'Fontsize', 16);           
hp1 = plot(Amat(1,:),Amat(2,:),'bs','LineWidth',1,...
            'MarkerEdgeColor','b',...
            'MarkerSize',6);
hold on; 
hp2= plot(Smat(1,:),Smat(2,:),'ko','LineWidth',1,...
            'MarkerEdgeColor','k',...
            'MarkerFaceColor','k',...
            'MarkerSize',5);
wsel_idx = (wlog_sel == 1);
hold on;
hp3 = plot(Amat(1,wsel_idx),Amat(2,wsel_idx),'rs','LineWidth',1,...
    'MarkerEdgeColor','r',...
    'MarkerFaceColor','r',...
    'MarkerSize',6);
hp = legend([hp1,hp2,hp3],'Sensors','Target area','Selected sensors'...
                ,'Location','Best');
xlabel('x');  ylabel('y');
set(gca,'YLim',[0 42]); set(gca,'XLim',[0 42]);                       
grid off; box off;
set(hp,...
    'Position',[0.211267857142857 0.715977452249615 0.412559101654846 0.161914143119817]);

print('-deps', 'SenTopPassive.eps');


%% scalar trace constraints
lambdasc1 = 4/((1-Pe)*Re^2);
% cvx_begin sdp 
%  %cvx_precision low
%     variable wtrace(M);
%     minimize (c'*wtrace);
%     subject to
%     for jj=1:N
%         F = zeros(2,2);
%         for ii=1:M
%             F = F + wtrace(ii).*FmPassive(Smat(:,jj),Amat(:,ii),options);
%             0<=wtrace(ii)<=1;
%         end
%         trace(F) >= lambdasc1; 
%     end
% cvx_end 
% t=zeros(M,N);
% for jj=1:N
%     for ii=1:M
%         t(ii,jj) = trace(FmPassive(Smat(:,jj),Amat(:,ii),options));
%         
%     end
%     
% end
% cvx_begin  
%  %cvx_precision low
%     variable wtrace(M);
%     minimize (c'*wtrace);
%     subject to
%     for jj=1:N
%          (wtrace'*t(:,jj)) >= lambdasc1;
%         
%     end
%     0<=wtrace<=1;
% cvx_end

lambdasc_crb = (1-Pe)*Re^2;
cvx_begin 
variable wtrace(M);
minimize (c'*wtrace);
subject to
for jj=1:N
    F = zeros(2,2);
    for ii=1:M
        F = F + wtrace(ii).*FmPassive(Smat(:,jj),Amat(:,ii),options);
        0<=wtrace(ii)<=1;
    end
    trace_inv(F) <= lambdasc_crb;
end
cvx_end


%% plotting
figure(3); set(gca,'Fontsize', 16);           
h1 = stem(abs(w),'k','LineWidth',1.01,...
            'MarkerEdgeColor','k','MarkerFaceColor','none',...
            'MarkerSize',6,'DisplayName','LMI constraints (l1-norm)'); 
hold on; 
h2 = stem(wtrace,'--ks','LineWidth',1.01,...
            'MarkerEdgeColor','k',...
            'MarkerSize',6,'DisplayName','trace constraints (l1-norm)');
%set(gca,'ytick', 0:0.1:1); 
set(gca,'YLim',[0 1.01]);
xlabel('sensors');  ylabel('w');
hleg = legend([h1,h2],'Min. eigenvalue (l1-norm)','trace (l1-norm)'...
                ,'Location','Best');
set(hleg,...
    'Position',[0.329125000000002 0.506245778453221 0.416250375248602 0.110722728807835]);
            
print('-depsc', 'tracePassive.eps');

% %% scalar determinant constraints
% lambdasc2 = (2/((1-Pe)*Re^2));
%  cvx_begin sdp quiet
%  %cvx_precision low
%     variable wdet(M);
%     minimize (c'*wdet);
%     subject to
%        for jj=1:N
%             F = zeros(2,2);
%             for ii=1:M
%                 F = F + wdet(ii).*FmPassive(Smat(:,jj),Amat(:,ii),options);
%                 0<=wdet(ii)<=1;
%             end;
%             det_rootn(F)>= lambdasc2;
%        end                    
% cvx_end 
% 
% %plotting
% figure(4); set(gca,'Fontsize', 16);           
% h1 = stem(abs(w),'k','LineWidth',1.01,...
%             'MarkerEdgeColor','k',...
%             'MarkerSize',6,'DisplayName','LMI constraints (l1-norm)'); 
% hold on; 
% h2 = stem(wdet,'--rd','LineWidth',1.01,...
%             'MarkerEdgeColor','r',...
%             'MarkerSize',6,'DisplayName','trace constraints (l1-norm)');
% set(gca,'YLim',[0 1.01]);
% xlabel('sensors');  ylabel('w');
% pos = [0.1,0.1,0.1,0.5];
% hleg1= legend([h1,h2],'Min. eigenvalue (l1-norm)','determinant (l1-norm)'...
%                  ,'Location','Best'); 
% set(hleg1,...
%     'Position',[0.266214984614811 0.548686254643693 0.504712887913243 0.110722728807835]);
% print('-depsc', 'detPassive.eps');


%% Projected subgradient algorithm
% solve using subgradient method
w_sg = ones(M,1); maxiter = 1000; ep = c'*w;%1e-3; 
g = zeros(M,1); fbest = (c'*w_sg)*ones(maxiter,1);
tic;
for k = 1:maxiter
    %F = lambda*eye(2);
    %w_sgm1 = w_sg;
    F=zeros(2*N,2*N);
    for m=1:M 
        F = F + w_sg(m).*Fm(:,:,m);  
    end
    
    [V,D] = eig(F); lambdaMin = D(1,1); vtilde = V(:,1);
    %[lambdaMin,vtilde] = mineig(F);
    if (lambdaMin < lambda)
        for m = 1:M 
            g(m) = vtilde'*Fm(:,:,m)*vtilde; 
        end
        %Polyak step-size
        %alpha = ((lambdaMin + ep)/(g'*g));
        %alpha = 1/sqrt(k);
        % Polyak estimate
        gamma = 10/(10+k);
        alpha = (lambdaMin + fbest(k) + gamma)/(g'*g);
        w_sg = w_sg + alpha*g;
        
        %projection
        w_sg((w_sg<=0))=0; w_sg((w_sg>=1))=1;
    else
        if (c'*w_sg < fbest(k))
            fbest(k:end) = c'*w_sg;
        end
         w_sg = w_sg - (1/sqrt(k))*c;
         %projection
         w_sg((w_sg<=0))=0; w_sg((w_sg>=1))=1;
    end
end
toc;

%% plotting
figure(5); set(gca,'Fontsize', 16);           
h1 = stem(abs(w),'k','LineWidth',1.01,'MarkerFaceColor','none',...
            'MarkerSize',6,'DisplayName','LMI constraints (l1-norm)'); 
hold on; 
h2 = stem(w_sg,'--k+','LineWidth',1.2,...
            'MarkerEdgeColor','k',...
            'MarkerSize',8,'DisplayName','Proj. subgradient (l1-norm)');

% randomization rounding
%wsg_sel = randomizedrounding(w_sg,Smat, Amat,N,M,lambda);       
set(gca,'YLim',[0 1.01]);
xlabel('sensors');  ylabel('w');
hleg5= legend([h1,h2],'SeDuMi','Projected subgradientAAAAAAAAAAAA'...
                                     ,'Location','Best'); 
set(hleg5,...
    'Position',[0.280500698900524 0.70582911178655 0.504712887913243 0.110722728807835]);
print('-depsc', 'ProjSub.eps');

figure(6); set(gca,'Fontsize', 16);  
hsem1 = semilogy((fbest-c'*w)/(c'*w),'k');
xlabel('k'); ylabel('fbestmf');
set(gca,'YLim',[0.01 50]);
%legend(hsem1, 'epsilon10000000000000000','Location','Best');
print('-depsc', 'ProjSubPerformance.eps');



