%% Estimates AoD in SU-MIMO systems with spatial sigma-delta quantizer at the BS

%%%%% Input parameters
% H_true : True channel (to obtain the received signal at the BS)
% SNR : uplink SNR at the BS
% u_grid : search grid for AoD
% T_s : number of times the same precoder is used at the UE
% aoa_est_sd :  estimated AoAs from step 1
% L : number of paths
% d_t :  inter-element spacing at the UE
% d_r :  inter-element spacing at the BS
% c : voltage level of clipper (Equation : )
% ang_steer : enable/disable angle steering during AoD estimation
% 0 implies angle steering is disabled. 1 implies angle steering is enabled

%%% Output
% dod_est_dc_ind : index of the estimated AoD (1 \times L)


function dod_est_dc_ind = Dod_est_SD(H_true, SNR, u_grid, T_s, aoa_est_sd, L, d_t, c, d_r,ang_steer)


[N_r, N_t] = size(H_true);
A_RX_est = gen_a(N_r, d_r, aoa_est_sd);
dod_est_dc_ind = zeros(1,L);
M = length(u_grid);
K = 2; %  Beams per stage
Nstage = log2(M); % Number of stages

doa_check_sd = zeros(K,1);

for iter=1:L
    
    d_index = 1; 
    w_mrc = A_RX_est(:,iter);
    if(ang_steer == 1)
        psi_3 = aoa_est_sd(iter); % steering angle of sigma-delta quantizer for step 3
    elseif(ang_steer == 0)
        psi_3 = 0; % steer to broadside
    end
    
    a_arg = 2*pi*d_r*sind(psi_3);
    lev_corr_fact = 2 - abs(cos(a_arg)) -  abs(sin(a_arg));
    b = c/lev_corr_fact; % Eq: 
    % choosing quantization level (b) based on clipping level (c) to avoid overloading
    
    for s = 1:Nstage 
        
        W = get_code_TX(N_t,d_t,M,K,s); % set of precoders for stage-s
        pos_req(1:K) = 2*(d_index-1)+(1:K); 

        for j=1:K
            f_tx = W(:,pos_req(j));
            N_noise = sqrt(1/2).*(randn(N_r,T_s) + 1j.*randn(N_r,T_s));
            X_rec = sqrt(SNR(1)).*H_true*repmat(f_tx,1,T_s) + N_noise;   
            Y = sigma_delta_ADC(X_rec, psi_3,d_r,b);
         
            doa_check_sd(j) = abs(mean(w_mrc'*Y))^2; % received power
        end

        [val c_index] = max(doa_check_sd);
        d_index = 2*(d_index-1) + c_index;

    end
    dod_est_dc_ind(iter) = (d_index);
end
dod_est_dc_ind = sort(dod_est_dc_ind);

end


