clc
clear
tic
M_t = 10;                                              % Total number of transmitting antennas
M_r = 10;                                              % Total number of receiving antennas
d_t = 0.5;% transmitter spacings in wavelength
shift = 0;
Theta = [-12 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]';

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+shift)/180*pi;
t2=(0+shift)/180*pi;
t3=(0+shift)/180*pi;
t4=(15+shift)/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,:);
s_mimo = A_t.'*phi;

load Xopt;
MATTT2 = Xopt;
C = trace(MATTT2'*MATTT2);
MATTT2 = (MATTT2/sqrt(C))*sqrt(10);
load W_n_opt;
%load MATTT;
%MATTT = WW;
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 X_sub_opt;
% %load MATTT;
% %MATTT = WW;
MATTT1 = X_sub_opt;
C1 = trace(MATTT1'*MATTT1);
MATTT1 = (MATTT1/sqrt(C1))*sqrt(10);
A_beam_so = (MATTT1)'*A_t
s_beam_so = sqrt(1)*A_beam_so.'*phi_beam;
P_noise = 1;
SNR = [-30:3:30];
MSE = zeros(length(SNR), 3);
montcarlo = 5;
for itr = 1:montcarlo, itr
    Beta = sqrt(0.5)*(randn(L_tar,T)+1i*randn(L_tar,T));
    Z_new_so = [];
    Z_new = [];
    Z_mimo = [];
    for n_pulse = 1:T;
        z = sqrt(P_noise/2)*(randn(M_r,N)+1i*randn(M_r,N));
        z3 = z*conj(phi_beam.');% z2 = sqrt(1/var(z2))*z2;
        Z_new_so = [Z_new reshape(z3,4*M_r,1)];
        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_so = diag(1./sqrt(var(Z_new_so.')));
    Z_new_so = Var_new_so*Z_new_so;
    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_so = [];
        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_beam_so = diag(Beta_tar(:,n_pulse))*s_beam_so;
            x_tar_beam_so = A_r*s_tar_beam_so;
            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_beam_so = [x_beam_so reshape(x_tar_beam_so*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;
        [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';
        
        x_beam_so = x_beam_so + Z_new_so;
        [mm,nn]=size(x_beam_so);
        y_beam_so_1=x_beam_so(1:10,:)+x_beam_so(11:20,:);
        y_beam_so_2=x_beam_so(21:30,:)+ x_beam_so(31:40,:);
        y_beam_so=[y_beam_so_1;y_beam_so_2];
        R_beam_so = (1/T)*y_beam_so*y_beam_so';
        
        [TTT1,min1] = discon_find(t1,t2,MATTT);
        [TTT2,min2] = discon_find(t3,t4,MATTT);
        
        [TTTT1,min_so_1] = discon_find(t1,t2,MATTT1);
        [TTTT2,min_so_2] = discon_find(t3,t4,MATTT1);

        psi = LS_ESPRIT(R_beam, L_tar, .5, 0);
        angles1(1)=angle_extract(angle(psi(1)),t1,t2,10,10,MATTT,TTT1,min1);
        angles1(2)=angle_extract(angle(psi(2)),t3,t4,10,10,MATTT,TTT2,min2);

        angles2(1)=angle_extract(angle(psi(1)),t3,t4,10,10,MATTT,TTT2,min2);
        angles2(2)=angle_extract(angle(psi(2)),t1,t2,10,10,MATTT,TTT1,min1);
        
        psi1 = LS_ESPRIT(R_beam_so, L_tar, .5, 0);
        angles_so_1(1)=angle_extract(angle(psi1(1)),t1,t2,10,10,MATTT1,TTTT1,min_so_1);
        angles_so_1(2)=angle_extract(angle(psi1(2)),t3,t4,10,10,MATTT1,TTTT2,min_so_2);

        angles_so_2(1)=angle_extract(angle(psi1(1)),t3,t4,10,10,MATTT1,TTTT2,min_so_2);
        angles_so_2(2)=angle_extract(angle(psi1(2)),t1,t2,10,10,MATTT1,TTTT1,min_so_1);

%         sayan=0;
%         [A,B]=eig(R_mimo);
%         G=A(:,3:100);
%         for teta=-40/180*pi:.01:40/180*pi
%             sayan=sayan+1;
%             A_t = exp(j*d_t*2*pi*(0:M_t-1)'*sin(teta));        % Uplink steering vectors
%             A_r = exp(j*pi*d_r'*sin(teta));
%             v=kron(A_t,A_r);
%             zzz(sayan)=v'*G*G'*v;
%             ttt(sayan)=teta;
%         end
%         plot(ttt/pi*180,zzz)
% 

        [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));
        v1=kron(A_T,A_R);

        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));
        v2=kron(A_T,A_R);
        S1= real(v1'*G*G'*v1+v2'*G*G'*v2);


        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));
        v1=kron(A_T,A_R);

        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));
        v2=kron(A_T,A_R);
        S2= real(v1'*G*G'*v1+v2'*G*G'*v2);
        
                if S1 <= S2
                    angles =angles1;
                else
                    angles =angles2;
                end

        [A_so,B_so]=eig(R_mimo);
        G_so=A_so(:,3:100);
        A_T_so = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_so_1(1)/180*pi));        % Uplink steering vectors
        A_R_so = exp(j*pi*d_r'*sin(angles_so_1(1)/180*pi));
        v_so_1=kron(A_T_so,A_R_so);

        A_T_so = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_so_1(2)/180*pi));        % Uplink steering vectors
        A_R_so = exp(j*pi*d_r'*sin(angles_so_1(2)/180*pi));
        v_so_2=kron(A_T,A_R);
        S_so_1= real(v_so_1'*G_so*G_so'*v_so_1+v_so_2'*G_so*G_so'*v_so_2);


        A_T_so = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_so_2(1)/180*pi));        % Uplink steering vectors
        A_R_so = exp(j*pi*d_r'*sin(angles_so_2(1)/180*pi));
        v_so_1=kron(A_T_so,A_R_so);

        A_T_so = exp(j*d_t*2*pi*(0:M_t-1)'*sin(angles_so_2(2)/180*pi));        % Uplink steering vectors
        A_R_so = exp(j*pi*d_r'*sin(angles_so_2(2)/180*pi));
        v_so_2=kron(A_T_so,A_R_so);
        S_so_2= real(v_so_1'*G_so*G_so'*v_so_1+v_so_2'*G_so*G_so'*v_so_2);
        
                if S_so_1 <= S_so_2
                    angles_so =angles_so_1;
                else
                    angles_so =angles_so_2;
                end
                
%         disp(angles)
        DOA_beam = sort(angles);
        MSE(snr, 1) = MSE(snr, 1) + mean((Theta(:) - DOA_beam(:)).^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, 2) = MSE(snr, 2) + mean((Theta(:) - DOA_mimo(:)).^2);
        
        DOA_beam_so = sort(angles_so);
        MSE(snr, 3) = MSE(snr, 3) + mean((Theta(:) - DOA_beam_so(:)).^2);
        
        disp([DOA_beam DOA_mimo' DOA_beam_so])
    end
end
CM = MATTT;
CM_so = MATTT1;
crb_beam = crb(A_t, A_r, Theta_rad, CM, SNR, T, d_r, d_t, M_t, M_r)
crb_mimo = crb(A_t, A_r, Theta_rad, eye(M_t), SNR, T, d_r, d_t, M_t, M_r)
crb_beam_so = crb(A_t, A_r, Theta_rad, CM_so, SNR, T, d_r, d_t, M_t, M_r)
crb_beam_nonuni = crb(A_t, A_r, Theta_rad, MATTT2, SNR, T, d_r, d_t, M_t, M_r)
SNR, RMSE = sqrt(MSE/montcarlo)
figure
semilogy(SNR,RMSE,SNR,crb_beam,'r--',SNR,crb_mimo,'g--',SNR,crb_beam_so,'m--'),grid
hold on
xlabel('SNR (dB)')
ylabel('RMSE in degrees')
figure(3)
semilogy(SNR,crb_beam,SNR,crb_beam_nonuni,'r--')


