% This program computes the DOAs of targets observed by a MIMO radar system
% using ESPRIT
clear
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 = [-5 5];
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:M_r-1);
%d_r=[0 sort((M_r-1)*rand(1,M_r-2)) M_r-1]; 

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)
% ---------- 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:2,:);
s_mimo = A_t.'*phi;
%
load MAT;
A_beam1 = Xsqrt1.'*A_t
s_beam1 = sqrt(M_t/2)*A_beam1.'*phi_beam;

A_beam2 = Xsqrt2.'*A_t
s_beam2 = sqrt(M_t/2)*A_beam2.'*phi_beam;

P_noise = 1;
SNR = [-20:5:20];
MSE = zeros(length(SNR), 3);
montcarlo = 500;
for itr = 1:montcarlo, itr
    Beta = sqrt(0.5)*(randn(L_tar,T)+1i*randn(L_tar,T));
    Z_new = [];
    Z_mimo = [];
    for n_pulse = 1:T;
        z = sqrt(P_noise/2)*(randn(M_r,N)+1i*randn(M_r,N));
        z2 = z*conj(phi_beam.');% z2 = sqrt(1/var(z2))*z2;
        Z_new = [Z_new reshape(z2,2*M_r,1)];
        %
        z1 = z*conj(phi.');% z1 = sqrt(1/var(z1))*z1;
        Z_mimo = [Z_mimo reshape(z1,M_t*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;
    
    for snr = 1:length(SNR)
        P_tar = 10^(SNR(snr)/10);
        Beta_tar = sqrt(P_tar/2)*Beta;
        x_beam1 = [];
        x_beam2 = [];
        x_mimo = [];
        for n_pulse = 1:T;
            % compute signals observed at targets
            s_tar_beam1 = diag(Beta_tar(:,n_pulse))*s_beam1;
            x_tar_beam1 = A_r*s_tar_beam1;
            
            %            %
            s_tar_beam2 = diag(Beta_tar(:,n_pulse))*s_beam2;
            x_tar_beam2 = A_r*s_tar_beam2;
            %
            s_tar_mimo = diag(Beta_tar(:,n_pulse))*s_mimo;
            x_tar_mimo = A_r*s_tar_mimo;
            %
            x_beam1 = [x_beam1 reshape(x_tar_beam1*conj(phi_beam.'), 2*M_r, 1)];
            x_beam2 = [x_beam2 reshape(x_tar_beam2*conj(phi_beam.'), 2*M_r, 1)];
            x_mimo = [x_mimo reshape(x_tar_mimo*conj(phi.'), M_t*M_r, 1)];
        end
        x_mimo = x_mimo + Z_mimo;
        R_mimo = (1/T)*x_mimo*x_mimo';
        %
       
        %
        x_beam1 = x_beam1 + Z_new;
        R_beam1 = (1/T)*x_beam1*x_beam1';
              
        x_beam2 = x_beam2 + Z_new;
        R_beam2 = (1/T)*x_beam2*x_beam2';

       
        psi1 = LS_ESPRIT(R_beam1, L_tar, .5, 0);
        angles1=angle_extract(angle(psi1),-pi/18,pi/18,10,10,Xsqrt1,0);
        disp(angles1)
        DOA_beam1 = sort(angles1);
        MSE(snr, 1) = MSE(snr, 1) + mean((Theta(:) - DOA_beam1(:)).^2);

        psi2 = LS_ESPRIT(R_beam2, L_tar, .5, 0);
        angles2=angle_extract(angle(psi2),-pi/18,pi/18,10,10,Xsqrt2,1);
        disp(angles2)
        DOA_beam2 = sort(angles2);
        MSE(snr,2) = MSE(snr,2) + mean((Theta(:) - DOA_beam2(:)).^2);
    
        psi = LS_ESPRIT(R_mimo, L_tar, .5, 1);
        DOA = asin((-1/(d_t*2*pi))*angle(psi))*180/pi;
        DOA_mimo = sort(DOA);
        MSE(snr, 3) = MSE(snr, 3) + mean((Theta(:) - DOA_mimo(:)).^2);
    %[DOA_beam DOA_beam1 DOA_twoA DOA_twoB DOA_mimo]
    end
end
SNR, RMSE = sqrt(MSE/montcarlo)
% plot(Th_grid,spectrum_mimo/max(spectrum_mimo), Th_grid,spectrum_beam/max(spectrum_beam),'r', Th_grid,spectrum_beam1/max(spectrum_beam1),'g', Th_grid,spectrum_twoA/max(spectrum_twoA),'k', Th_grid,spectrum_twoB/max(spectrum_twoB),'c'),grid
% axis([-10 10 -.1 1.1])
semilogy(SNR,RMSE)
legend('Best Achievable Tx Beamspace W_0', 'Tx Beamspace W_0 U', 'Traditional MIMO RADAR')

save Example1_Workspace_RMSE