%This file compares the performance of the proposed method with the methods
%of [15] and conventional MIMO in terms of the probability of resolving two
%closely located targets. This file produces angle estimates in exactly the
%same way as RMSE.m.
clc
clear
tic
K = 2;
M_t = 10;                                              % Total number of transmitting antennas
M_r = 10;                                              % Total number of receiving antennas
d_t = 0.5;                                             % transmitter spacings in wavelength
Theta = [-9 -8];
Theta_rad = Theta*pi/180;                               % Directions of targets to the broadside of the array
L_tar = length(Theta);
A_t = exp(j*d_t*2*pi*(0:M_t-1)'*sin(Theta_rad));        % Uplink steering vectors
d_r=[ 0; 0.088751705948312; 0.521021743058417; 1.250017937612549 ; 1.824886967042458; 3.175813189953003;
    3.692431862918508; 7.318498475733820; 8.042845778221803; 9.000000000000000]';

A_r = exp(j*pi*d_r'*sin(Theta_rad));                     % Downlink steering vectors; Tx and Rx are assumed to be close to each other
T = 50;                                                  % number of radar pulses (slow-time)
t1=(-15)/180*pi;
t2=(0)/180*pi;
t3=(0)/180*pi;
t4=(15)/180*pi;

% ---------- Orthogonal waveforms ------
N = 1024;                                               % number of smaples within one radar pulse (fast-time)
phi = [];
for kk = 1:M_t
    phi = [phi; (1/sqrt(N))*exp(j*2*pi*(kk/N)*(0:N-1))];
end
phi_beam = phi(1:4,:);
phi_beam_cvx = phi(1:2,:);
s_mimo = A_t.'*phi;

load W_n_opt;
MATTT=W_n_opt;
C = trace(MATTT'*MATTT);
MATTT = (MATTT/sqrt(C))*sqrt(10);
A_beam = (MATTT)'*A_t
s_beam = sqrt(1)*A_beam.'*phi_beam;
load Wconvex
C1 = trace(Wconvex'*Wconvex);
Wconvex = Wconvex/sqrt(C1)*sqrt(10);
A_beam_cvx = Wconvex'*A_t;
s_beam_cvx = sqrt(1)*A_beam_cvx.'*phi_beam_cvx;

P_noise = 1;
SNR = [-30:3:30];
MSE = zeros(length(SNR), 3);
montcarlo = 25;
resolve_beam = zeros(length(SNR),1);
resolve_mimo = zeros(length(SNR),1);
resolve_cvx  = zeros(length(SNR),1);

for itr = 1:montcarlo, itr
    Beta = sqrt(0.5)*(randn(L_tar,T)+1i*randn(L_tar,T));
    Z_new = [];
    Z_mimo = [];
    Z_new_cvx = [];
    for n_pulse = 1:T;
        z = sqrt(P_noise/2)*(randn(M_r,N)+1i*randn(M_r,N));
        z2 = z*conj(phi_beam.');
        Z_new = [Z_new reshape(z2,4*M_r,1)];
        z1 = z*conj(phi.');
        Z_mimo = [Z_mimo reshape(z1,M_t*M_r,1)];
        z3 = z*conj(phi_beam_cvx.');
        Z_new_cvx = [Z_new_cvx reshape(z3,2*M_r,1)];
    end
    Var_new = diag(1./sqrt(var(Z_new.')));
    Z_new = Var_new*Z_new;
    
    Var_mimo = diag(1./sqrt(var(Z_mimo.')));
    Z_mimo = Var_mimo*Z_mimo;
    
    Var_cvx = diag(1./sqrt(var(Z_new_cvx.')));
    Z_new_cvx = Var_cvx*Z_new_cvx;

    for snr = 1:length(SNR)
        P_tar = 10^(SNR(snr)/10);
        Beta_tar = sqrt(P_tar)*Beta;
        x_beam = [];
        x_mimo = [];
        x_beam_cvx = [];
        for n_pulse = 1:T;
            % compute signals observed at targets
            s_tar_beam = diag(Beta_tar(:,n_pulse))*s_beam;
            x_tar_beam = A_r*s_tar_beam;

            s_tar_mimo = diag(Beta_tar(:,n_pulse))*s_mimo;
            x_tar_mimo = A_r*s_tar_mimo;
            
            s_tar_cvx = diag(Beta_tar(:,n_pulse))*s_beam_cvx;
            x_tar_beam_cvx = A_r*s_tar_cvx;
           
            x_beam = [x_beam reshape(x_tar_beam*conj(phi_beam.'), 4*M_r, 1)];
            x_mimo = [x_mimo reshape(x_tar_mimo*conj(phi.'), M_t*M_r, 1)];
            x_beam_cvx = [x_beam_cvx reshape(x_tar_beam_cvx*conj(phi_beam_cvx.'), 2*M_r, 1)];
        end
        x_mimo = x_mimo + Z_mimo;
        R_mimo = (1/T)*x_mimo*x_mimo'; %cross-correlation matrix for MIMO

        x_beam = x_beam +Z_new;
        [mm,nn]=size(x_beam);
        y_beam1=x_beam(1:10,:)+x_beam(11:20,:);
        y_beam2=x_beam(21:30,:)+ x_beam(31:40,:);
        y_beam=[y_beam1;y_beam2];
        R_beam = (1/T)*y_beam*y_beam'; %cross-correlation matrix for proposed method
        
        x_beam_cvx = x_beam_cvx + Z_new_cvx;
        y_beam_cvx = [x_beam_cvx(1:(K-1)*M_t, :); x_beam_cvx(M_t+1:K*M_t, :)];
        R_cvx = (1/T)*y_beam_cvx*y_beam_cvx'; %cross-correlation matrix for method of [15]
        
        %discon_find finds a discontinuity in the phase profile
        %corresponding to a given beamspace matrix if it exists. unwrapkon.m, which is
        %called by angle_extract uses this information to either unwrap the
        %phase profile or not. The returned phase profile is then used in
        %comparison with the phase argument from ESPRIT to provide the DOA
        %estimates
        [T1,min1] = discon_find(t1,t2,MATTT);
        [T2,min2] = discon_find(t3,t4,MATTT);
%       

        [TT1,min_1_cvx] = discon_find(t1,t2,Wconvex);
        [TT2,min_2_cvx] = discon_find(t3,t4,Wconvex);

        
        psi = LS_ESPRIT(R_beam, L_tar, 0); %ESPRIT
        %target could be in 2 possible places (left, or right side of the sector). 
        %We have two targets, we divide our sector in two, therefore there are 4 possible target
        %arrangements which will be parsed using MUSIC.
        angles1(1)=angle_extract(angle(psi(1)),t1,t2,10,10,MATTT,T1,min1);
        angles1(2)=angle_extract(angle(psi(1)),t3,t4,10,10,MATTT,T2,min1);
        
        psi = LS_ESPRIT(R_beam, L_tar, 0); %ESPRIT
        angles2(1)=angle_extract(angle(psi(2)),t1,t2,10,10,MATTT,T1,min1);
        angles2(2)=angle_extract(angle(psi(2)),t3,t4,10,10,MATTT,T2,min1);
        
        psi = LS_ESPRIT(R_cvx, L_tar, 0); %ESPRIT
        angles_cvx_1(1)=angle_extract(angle(psi(1)),t1,t2,10,10,Wconvex,TT1,min_1_cvx);
        angles_cvx_1(2)=angle_extract(angle(psi(1)),t3,t4,10,10,Wconvex,TT2,min_2_cvx);
        
        psi = LS_ESPRIT(R_cvx, L_tar, 0);
        angles_cvx_2(1)=angle_extract(angle(psi(2)),t1,t2,10,10,Wconvex,TT1,min_1_cvx);
        angles_cvx_2(2)=angle_extract(angle(psi(2)),t3,t4,10,10,Wconvex,TT2,min_2_cvx);
        
        %----------------Parsing Target Arrangements with MUSIC----------%
        [A,B]=eig(R_mimo);
        G=A(:,3:100);
        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles1(1)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles1(1)/180*pi));
        v=kron(A_T,A_R);
        S1= real(v'*G*G'*v);

        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles1(2)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles1(2)/180*pi));
        v=kron(A_T,A_R);
        S2= real(v'*G*G'*v);
        
                if S1 <= S2
                    angles(1) = angles1(1);
                else
                    angles(1) = angles1(2);
                end


        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles2(1)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles2(1)/180*pi));
        v = kron(A_T,A_R);
        S1 = real(v'*G*G'*v);

        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles2(2)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles2(2)/180*pi));
        v=kron(A_T,A_R);
        S2= real(v'*G*G'*v);
        
                if S1 <= S2
                    angles(2) = angles2(1);
                else
                    angles(2) = angles2(2);
                end


                
                
        [A_cvx,B_cvx]=eig(R_mimo);
        G=A_cvx(:,3:100);
        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_cvx_1(1)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles_cvx_1(1)/180*pi));
        v = kron(A_T,A_R);
        S1 = real(v'*G*G'*v);


        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_cvx_1(2)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles_cvx_1(2)/180*pi));
        v = kron(A_T,A_R);
        S2 = real(v'*G*G'*v);
        
                if S1 <= S2
                    angles_cvx(1) = angles_cvx_1(1);
                else
                    angles_cvx(1) = angles_cvx_1(2);
                end


        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_cvx_2(1)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles_cvx_2(1)/180*pi));
        v = kron(A_T,A_R);
        S1 = real(v'*G*G'*v);

        A_T = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_cvx_2(2)/180*pi));        % Uplink steering vectors
        A_R = exp(j*pi*d_r'*sin(angles_cvx_2(2)/180*pi));
        v = kron(A_T,A_R);
        S2= real(v'*G*G'*v);
        
                if S1 <= S2
                    angles_cvx(2) = angles_cvx_2(1);
                else
                    angles_cvx(2) = angles_cvx_2(2);
                end
        %------------Display DOA and calculate MSE---------------------%        
        DOA_beam = sort(angles);
        DOA_beam_cvx = sort(angles_cvx);
        MSE(snr, 1) = MSE(snr, 1) + mean((Theta(:) - DOA_beam(:)).^2);
        %
        psi = LS_ESPRIT(R_mimo, L_tar, 1);
        DOA = asin((-1/(d_t*2*pi))*angle(psi))*180/pi;
        DOA_mimo = sort(DOA);
        MSE(snr, 2) = MSE(snr, 2) + mean((Theta(:) - DOA_mimo(:)).^2);
        
        MSE(snr, 3) = MSE(snr, 3) + mean((Theta(:) - DOA_beam_cvx(:)).^2);        
%         
        disp([DOA_beam DOA_mimo' DOA_beam_cvx])
        
        %-----------Source Resolution Calculation------------------------%
        d_theta1 = abs(DOA_beam(1) - Theta(1));
        d_theta2 = abs(DOA_beam(2) - Theta(2)) ;

        if (d_theta1 <= abs(Theta(1) - Theta(2))/2) & (d_theta2 <= abs(Theta(1) - Theta(2))/2)
            resolve_beam(snr) = resolve_beam(snr) + 1;
        end

        d_theta1 = abs(DOA_mimo(1) - Theta(1));
        d_theta2 = abs(DOA_mimo(2) - Theta(2)) ;

        if (d_theta1 <= abs(Theta(1) - Theta(2))/2) & (d_theta2 <= abs(Theta(1) - Theta(2))/2)
            resolve_mimo(snr) = resolve_mimo(snr) + 1;
        end
        
        d_theta1 = abs(DOA_beam_cvx(1) - Theta(1));
        d_theta2 = abs(DOA_beam_cvx(2) - Theta(2)) ;

        if (d_theta1 <= abs(Theta(1) - Theta(2))/2) & (d_theta2 <= abs(Theta(1) - Theta(2))/2)
            resolve_cvx(snr) = resolve_cvx(snr) + 1;
        end
        
    end
end
p_resolve_beam = resolve_beam/montcarlo;
p_resolve_mimo = resolve_mimo/montcarlo;
p_resolve_cvx  = resolve_cvx/montcarlo;

figure
hold on
axis([-30.1 30 0 1.5])
xlabel('SNR of Received Signal')
ylabel('Probability of Resolution of Two Closely Located Targets')
plot(SNR,p_resolve_mimo,'-sqm',SNR, p_resolve_cvx ,'v-r',SNR, p_resolve_beam,'o-b','LineWidth',2,'MarkerSize',10)
grid
hold on
legend('Traditional MIMO radar', 'Method of [15]', 'Best Achievable Tx Beamspace')
hFig = gcf;
saveas(hFig,['Figurex','.eps'],'epsc')

save Example3_Workspace_Resolution.mat