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

close all;clear all;
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);
    
    for nn=1:N
    Err = zeros(2,1);
    for mc=1:100
        % Compute CRB, RMSE for a point within the target area
        % noiseless measurements
        senLoctrue = Smat(:,nn); %15 + (30-15)*rand(2,1);
        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;
        
        SenLocEst = senLoctrue;
        
        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 + (targetLocEst-senLoctrue).^2;
    end
    % Cramer-Rao bound
    FIM = zeros(2,2);
    for m=1:Msel
        FIM = FIM + FmPassive(senLoctrue,anchorLoc(:,m),options);
    end
    
    rmse(nn,iter) = sqrt(sum(Err)./(100));
    crb(nn,iter) = sqrt(trace(inv(FIM)));
    end
end

%%
[rmseMax, rmseMax_idx] =max(rmse(:,1:end-1),[],1);
%crbMax = [crb(rmseMax_idx(1),1), crb(rmseMax_idx(2),2),crb(rmseMax_idx(3),3),crb(rmseMax_idx(4),4)];
[crbMax, crbMax_idx] = max(crb(:,1:end-1),[],1);
AvgCRB = sum(crb,1)/N;  AvgRmse = sum(rmse,1)/N;

%plotting
constr = sqrt((1-Pe)*Revec.^2).*100;
figure(1);
set(gca,'Fontsize', 16);
h1 = plot(Revec(1:end-1).*100,rmseMax*100,'--k','LineWidth',1.01,...
            'MarkerEdgeColor','k',...
            'MarkerSize',6,'DisplayName','Max. RMSE');
hold on;  
h2 = plot(Revec(1:end-1)*100,crbMax*100,'--sk','LineWidth',1.01,...
            'MarkerEdgeColor','k',...
            'MarkerSize',6,'DisplayName','Max. CRB');
        
        
h3 = plot(Revec(1:end-1)*100,constr(1:end-1),'-r','LineWidth',1.01,...
    'MarkerEdgeColor','k',...
    'MarkerSize',6,'DisplayName','constraint');

h4 = plot(Revec(1:end-1)*100,AvgRmse(1:end-1)*100,':k','LineWidth',1.2,...
            'MarkerEdgeColor','k',...
            'MarkerSize',6,'DisplayName','Avg. RMSE');

h5 = plot(Revec(1:end-1)*100,AvgCRB(1:end-1)*100,':sk','LineWidth',1.2,...
            'MarkerEdgeColor','k',...
            'MarkerSize',6,'DisplayName','Avg. CRB');

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.507142857142857 0.355357142857143],...
    [0.569047619047619 0.523809523809524],'TextEdgeColor','none','FontSize',16,...
    'String',{'Performance constraint'});


hleg = legend([h1,h2,h3,h4,h5],'Max. RMSE','Max. root-CRBAAA','sqrt(1-Pe)Rep2'...
    ,'Avg. RMSE', 'Avg. root-CRBAAA'...
                  ,'Location','NorthEast');              
print('-deps', 'rmsecrb.eps');