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

%% sensor selection for each Re

% 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
c = ones(M,1); Pe = 0.90; 
Revec = [0.07,0.1:0.05:0.25]; 
iters = length(Revec);

W =[];
h = waitbar(0,'Please wait...');
for iter=1:iters
    Re = Revec(iter);
    lambda = (2/Re^2)*(1/(1-Pe));

% cvx_begin sdp 
%     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 

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

%wsel = randomizedrounding(wlog,Smat, Amat,N,M,lambda,options);
wsel=zeros(M,1);
ind_ = find(wlog >=0.001); wsel(ind_)=1;
W = [W wsel];
waitbar(iter / iters)
end
close(h);
%plotting
figure(1);
set(gca,'Fontsize', 16);
himg = imagesc(fliplr(W(:,1:end-1)));
xlabel('Re [cm]');  ylabel('sensor index');      
set(gca,'xtick', linspace(0.5,3+0.5,3+1));
set(gca,'XTickLabel',{'20','15','10','7'}); 
%set(gca,'YTickLabel',{'3','4','5','6','7','8','9','10','11'}); 
set(gca,'xgrid', 'on', 'gridlinestyle', '-', 'xcolor', 'k', 'ycolor', 'k');
axis xy; box off; 

% annotation('textbox',...
%     [0.130359662162825 0.928571428571429 0.164283194980032 0.0569264610871168],...
%     'String',{'rmse =6.63 cm'},...
%     'FontSize',12,...
%     'FitBoxToText','on',...
%     'LineStyle','none');

annotation('textbox',...
    [0.142187888198756 0.924182474368754 0.191964285714286 0.0619047619047619],...
    'String',{'rmse =6.86 cm'},...
    'FontSize',12,...
    'LineStyle','none');

annotation('textbox',...
    [0.345961180124219 0.927233850724453 0.191964285714286 0.0619047619047619],...
    'String',{'rmse =5.43 cm'},...
    'FontSize',12,...
    'LineStyle','none');

annotation('textbox',...
    [0.53886490683229 0.92766245922301 0.191964285714286 0.0619047619047619],...
    'String',{'rmse =3.74 cm'},...
    'FontSize',12,...
    'LineStyle','none');

annotation('textbox',...
    [0.729936335403723 0.927662459223013 0.191964285714286 0.0619047619047619],...
    'String',{'rmse =2.65 cm'},...
    'FontSize',12,...
    'LineStyle','none');

print('-depsc', 'solutionpath.eps');

%% rmse and crb
close all; clear all; clc;
load solutionpath_v2.mat;
%senLoctrue = [20.25;16.75];

senLoctrue = [19.3290;17.4282]; %15 + (30-15)*rand(2,1);
%usually use this
%SenLocEst = 15 + (30-15)*rand(2,1);
%one realization to generate the plots in the paper
SenLocEst = [20.24;16.7];
options.MeasurementType = 'Range';

for iter = 1: iters
    
al1 = find(W(:,iter)==1);
count(iter) = length(al1);
anchorLoc = Amat(:,al1); Msel = size(anchorLoc,2);

% Compute CRB, RMSE for a point within the target area
% noiseless measurements
LocDelta = repmat(senLoctrue,1,Msel) - anchorLoc;
distance   = sqrt(sum((-LocDelta).^2 , 1)).';
bearing    = atan(LocDelta(2,:)./LocDelta(1,:)).';
%noise variances
sigma2range = 1.78*10^-5./(distance.^-2);
sigma2bearing = 10^-4;

Err = zeros(1,1);
for mc=1:2000
distanceNoisy = distance + sqrt(sigma2range).*randn(Msel,1);
bearingNoisy  = bearing + sqrt(sigma2bearing).*randn(Msel,1);

switch lower(options.MeasurementType)
    case {lower('Range')}       
        measurement = distanceNoisy;
    case {lower('RangeBearing')}
        measurement = [distanceNoisy;bearingNoisy];
    case {lower('Bearing')}
        measurement = bearingNoisy;
   
end

targetLocEst = GNlocalization(measurement,anchorLoc,SenLocEst,options).';
Err = Err + norm(targetLocEst-senLoctrue,2)^2;
end
% Cramer-Rao bound
FIM = zeros(2,2);
for m=1:Msel
    FIM = FIM + FmPassive(senLoctrue,anchorLoc(:,m),options);
end

% rmse(iter) = sqrt((Err)./(2000));
% crb(iter) = sqrt(trace(inv(FIM)));

rmse(iter) = sqrt((Err)./(2000));
crb(iter) =  sqrt(trace(inv(FIM)));

end
constr = sqrt((1-Pe)*Revec.^2).*100;
% plotting
figure(1);
set(gca,'Fontsize', 16);
h1 = plot(Revec(1:end-1).*100,rmse(1:end-1)*100,'--k','LineWidth',1.01,...
            'MarkerEdgeColor','k',...
            'MarkerSize',6,'DisplayName','RMSE');
hold on;  
h2 = plot(Revec(1:end-1)*100,crb(1:end-1)*100,'--sk','LineWidth',1.01,...
            'MarkerEdgeColor','k',...
            'MarkerSize',6,'DisplayName','CRB');
        
h3 = plot(Revec(1:end-1)*100,constr(1:end-1),'-','LineWidth',1.01,...
    'MarkerEdgeColor','k',...
    'color', [0,0,0]+0.2,...
    'MarkerSize',6,'DisplayName','constraint');

xlabel('Re [cm]');  ylabel('RMSE [cm]');
%set(gca,'XLim',[5 25]);       
set(gca,'YTick',[2,3,4,5,6,7]);  
set(gca,'XDir','reverse');

annotation('textarrow',[0.471428571428571 0.357142857142857],...
    [0.75952380952381 0.65952380952381],'TextEdgeColor','none','FontSize',16,...
    'String',{'Performance constraint'});

hleg = legend([h1,h2,h3],'RMSE','root-CRB','sqrt(1-Pe)Rep2'...
                  ,'Location','Best');              
%print('-depsc', 'rmsecrb.eps');


%% random placement
% rndSel = unidrnd(M,1,12);  % pick K sensors uniformly at random [70,29,16,12,8]
% r = zeros(M,1); r(rndSel) = 1;
% anchorLoc = Amat(:,rndSel); Msel = size(anchorLoc,2);
% % noiseless measurements
% LocDelta = repmat(senLoctrue,1,Msel) - anchorLoc;
% distance   = sqrt(sum((-LocDelta).^2 , 1)).';
% bearing    = atan(LocDelta(2,:)./LocDelta(1,:)).';
% %noise variances
% sigma2range = 1.78*10^-5./(distance.^-2);
% sigma2bearing = 10^-4;
% 
% %
% Err = zeros(2,1);
% for mc=1:2000
% distanceNoisy = distance + sqrt(sigma2range).*randn(Msel,1);
% bearingNoisy  = bearing + sqrt(sigma2bearing).*randn(Msel,1);
% 
% switch lower(options.MeasurementType)
%     case {lower('Range')}       
%         measurement = distanceNoisy;
%     case {lower('RangeBearing')}
%         measurement = [distanceNoisy;bearingNoisy];
%     case {lower('Bearing')}
%         measurement = bearingNoisy;
%    
% end
% SenLocEst = [29.2939;24.0764];
% targetLocEst = GNlocalization(measurement,anchorLoc,SenLocEst,options).';
% Err = Err + (targetLocEst-senLoctrue).^2;
% end
% 
% % Cramer-Rao bound
% FIM = zeros(2,2);
% for m=1:Msel
%     FIM = FIM + FmPassive(senLoctrue,anchorLoc(:,m),options);
% end
% rmseRnd = sqrt(sum(Err)./2000);
% CRBRnd = sqrt(trace(inv(FIM)));
% 
% %fprintf(1,'Random placement RMSE is %f [m] and CRB is %f [m]\n', rmseRnd,CRBRnd);
% 
% %plotting
% % figure(4); set(gca,'Fontsize', 16);
% % h1 = stem(abs(wsel),'--r','LineWidth',1,...
% %             'MarkerEdgeColor','r',...
% %             'MarkerFaceColor','r',...
% %             'MarkerSize',6,'DisplayName','LMI constraints (randomized rounding)'); 
% % hold on; 
% % h2 = stem(r,'--ms','LineWidth',1,...
% %             'MarkerEdgeColor','m',...
% %             'MarkerSize',6,'DisplayName','Random placement');
% % xlabel('sensors');  ylabel('w');
% % legend([h1,h2],'LMI constraints (randomized rounding)','Random placement'...
% %                 ,'Location','Best');
% % print('-depsc', 'CompRnd.eps');
% %
% 
% %data:
% %rmsernd = [0.0233 0.0384 0.0481 0.0647];
%rmse = [0.0171 0.023 0.045 0.053];
%crb = [0.0153 0.0213 0.0421 0.0493]