clc
clear
tic
M_t = 10;                                              % Total number of transmitting antennas
M_r = 10;                                              % Total number of receiving antennas
K = 4;                 % Number of transmit beams
d_t = 0.5;% transmitter spacings in wavelength
Theta = [-5 9];
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]';
%
PassSector = [-15 15];   % desired sector
TransWidth = 10;         % transition width, i.e., are of uncontroled beampattern.
delta = 0.1;      % parameter that controls the passband accuracey
%[Wsph Wconvex] = TxEnrgFocusWeights(M_t, K, d_t, PassSector, TransWidth, delta);



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:4,:);
s_mimo = A_t.'*phi;

load Wconvex;
MATTT = Wconvex;
C = trace(MATTT'*MATTT);
MATTT = (MATTT/sqrt(C))*sqrt(10);
A_beam = (MATTT)'*A_t
s_beam = sqrt(1)*A_beam.'*phi_beam;
%
P_noise = 1;
SNR = [20:2.5:20];
MSE = zeros(length(SNR), 3);
montcarlo = 2;
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,4*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_beam = [];
        x_mimo = [];
        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;
            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)];
        end
        x_mimo = x_mimo + Z_mimo;
        R_mimo = (1/T)*x_mimo*x_mimo';

        x_beam = x_beam +Z_new;
        y_beam = [x_beam(1:(K-1)*M); x_beam(2:K-1)*M)];
        R_beam = (1/T)*y_beam*y_beam';
        
        
  
        psi1 = LS_ESPRIT(R_beam, L_tar, .5, 0);
        DOA = asin((-1/(d_t*2*pi))*angle(psi))*180/pi;
        DOA_beam = sort(DOA);
        %
        psi = LS_ESPRIT(R_mimo, L_tar, .5, 1);
        DOA = asin((-1/(d_t*2*pi))*angle(psi))*180/pi;
        DOA_mimo = sort(DOA);
        
       
        disp([DOA_beam DOA_mimo])
    end
end