%=============================================================================%
%=                         TOA UWB ALGORITHM                                 =%
%=                                                                           =%
%= Programmed by Jasurbek Khodjaev                                           =%
%= Yeungnam University Mobile Communication Lab.                             =%
%= MCL 2006                                                                  =%
%=============================================================================%

clear all;
clc;

%--------------- Initialization -----------------------------------------
%-------------------------------------------

% Speed of Light
light_speed = 3e8;

% Coordinates of APs
AP = [0 0; 0 10; 10 0]; % in meters

% Number of Access Points (AP)
num_ap = length(AP);

% Tag's initial coordinate
% Tag = [5 4];



    
    
% Pulse shape
pulse_order = 1; % 0-Gaussian pulse, 1-First derivative of Gaussian pulse, 2 - Second derivative;

% Number of bits
num_bits = 1000;

% Pulse repetition interval, PRI
pri = 200e-9;

% The SNR range (in dB)
% EbNo = -10

fs = 20e9; %sample rate-10 times the highest frequency in GHz
ts = 1/fs; %sample period
t = [(-1.5E-9-ts):ts:(1.5E-9-ts)]; %vector with sample instants
t1 = .5E-9; %pulse width(0.5 nanoseconds)
%-------------------------------------------------------------------------


%----------------- PRI -------------------------------------------

A =1;%positive value gives negative going monopulse;neg value gives
   %positive going monopulse
[y] = monocycle(fs, ts, t, t1, A, pulse_order); % Generate Gaussian pulse

n_pulse_pri = round(pri/ts);               % Sampling of PRI
sig = zeros(1,n_pulse_pri);    
sig(1:length(y)) = y;                 % One pulse in one PRI
%-----------------------------------------------------------------
shift_const = 40;

kkk = 0;
for EbNo = -15:5:5
    kkk = kkk + 1;
    
        % Tag's initial coordinate
    a = round(rand * 10);
    b = round(rand * 10);
    Tag = [a b];
    
% Distance calculation between each AP and the Tag, IDEAL case
for ii = 1:num_ap
    dist_ap_tag(ii) = dist_t(AP(ii,:), Tag);
    % Time from each AP to Tag
    time_ap_tag(ii) = dist_ap_tag(ii)/light_speed;
end




    %++++++++++++++++ TRANSMISSION +++++++++++++++++++++++++++++++

    
    for jj = 1:num_bits
% % % % % %             % From TAG to AP1
% % % % % % %             del_sample_ap_tag = round(time_ap_tag(1)/ts);
% % % % % % %             xx = zeros(1,del_sample_ap_tag);
% % % % % %             h = uwb_channel(dist_ap_tag(1),time_ap_tag(1));
% % % % % %             conv_data = conv(y, h);
% % % % % %             ap1_tag_chan(jj, :) = conv_data(1:length(sig));
% % % % % % 
% % % % % %             % From TAG to AP2
% % % % % % %             del_sample_ap_tag = round(time_ap_tag(2)/ts);
% % % % % % %             xx = zeros(1,del_sample_ap_tag);
% % % % % %             h = uwb_channel(dist_ap_tag(2), time_ap_tag(2));
% % % % % %             conv_data = conv(y, h);
% % % % % %             ap2_tag_chan(jj, :) = conv_data(1:length(sig));
% % % % % %             
% % % % % %             % From TAG to AP3
% % % % % % %             del_sample_ap_tag = round(time_ap_tag(3)/ts);
% % % % % % %             xx = zeros(1,del_sample_ap_tag);
% % % % % %             h = uwb_channel(dist_ap_tag(3), time_ap_tag(3));
% % % % % %             conv_data = conv(y, h);
% % % % % %             ap3_tag_chan(jj, :) = conv_data(1:length(sig));

            % From TAG to AP1
            del_sample_ap_tag = round(time_ap_tag(1)/ts);
            xx = zeros(1,del_sample_ap_tag);
            % Delayed signals from AP1 to Tag
            del_sig_ap1_tag(jj,:) = [xx sig(1:end-length(xx))];
            h = uwb_channel(dist_ap_tag(1));
            kk = 0;
            for ll = 1:length(h)
               add_chnl = conv(del_sig_ap1_tag(jj,:), h(ll));
               added(ll,:) = [zeros(1,shift_const*kk) add_chnl(1:end - shift_const*kk)];
               kk = kk + 1;
            end
            ap1_tag_chan(jj, :) =  sum(added);


    %         chan_ap1_tag(jj,:) = ap1_tag_chan(1:length(del_sig_ap1_tag));

            % From TAG to AP2
            del_sample_ap_tag = round(time_ap_tag(2)/ts);
            xx = zeros(1,del_sample_ap_tag);
            % Delayed signals from AP1 to Tag
            del_sig_ap2_tag(jj,:) = [xx sig(1:end-length(xx))];
            h = uwb_channel(dist_ap_tag(2));
            kk = 0;
            for ll = 1:length(h)
               add_chnl = conv(del_sig_ap2_tag(jj,:), h(ll));
               added(ll,:) = [zeros(1,shift_const*kk) add_chnl(1:end - shift_const*kk)];
               kk = kk + 1;
            end
            ap2_tag_chan(jj, :) =  sum(added);

            
            
            % From TAG to AP3
            del_sample_ap_tag = round(time_ap_tag(3)/ts);
            xx = zeros(1,del_sample_ap_tag);
            % Delayed signals from AP1 to Tag
            del_sig_ap3_tag(jj,:) = [xx sig(1:end-length(xx))];
            h = uwb_channel(dist_ap_tag(3));
            kk = 0;
            for ll = 1:length(h)
               add_chnl = conv(del_sig_ap3_tag(jj,:), h(ll));
               added(ll,:) = [zeros(1,shift_const*kk) add_chnl(1:end - shift_const*kk)];
               kk = kk + 1;
            end
            ap3_tag_chan(jj, :) =  sum(added);


    end

    %-------------------------------------------------------
    % Additive White Gaussian Noise (AWGN) Channel ---------
%     noise_var   = 10^(-EbNo/10);
    for jj = 1:num_bits
%         ap1_tag_chan_wgn(jj,:) = ap1_tag_chan(jj,:)/std(ap1_tag_chan(jj,:)) + randn(1,length(ap1_tag_chan(jj,:))) .* sqrt(noise_var);
%         ap2_tag_chan_wgn(jj,:) = ap2_tag_chan(jj,:)/std(ap2_tag_chan(jj,:)) + randn(1,length(ap2_tag_chan(jj,:))) .* sqrt(noise_var);
%         ap3_tag_chan_wgn(jj,:) = ap3_tag_chan(jj,:)/std(ap3_tag_chan(jj,:)) + randn(1,length(ap3_tag_chan(jj,:))) .* sqrt(noise_var);
        ap1_tag_chan_wgn(jj,:) = cp0801_Gnoise1(ap1_tag_chan(jj,:), EbNo, 1);
        ap2_tag_chan_wgn(jj,:) = cp0801_Gnoise1(ap2_tag_chan(jj,:), EbNo, 1);
        ap3_tag_chan_wgn(jj,:) = cp0801_Gnoise1(ap3_tag_chan(jj,:), EbNo, 1);
        
    end
    
% %     %-------------------------------------------------------
% % 
% %     received_signl_ap1 = sum(ap1_tag_chan_wgn)/num_bits;
% %     xc = cp0804_corrsyn(received_signl_ap1, sig, fs);
% %     [a,delay1]=max(xc);
% %     TOA_1 = (length(sig) - delay1) * ts;
% %     
% %     %-----------------------------------
% %     
% %     received_signl_ap2 = sum(ap2_tag_chan_wgn)/num_bits;
% %     xc = cp0804_corrsyn(received_signl_ap2, sig, fs);
% %     [a,delay2]=max(xc);
% %     TOA_2 = (length(sig) - delay2) * ts;
% %     
% %     %-----------------------------------
% %     
% %     received_signl_ap3 = sum(ap2_tag_chan_wgn)/num_bits;
% %     xc = cp0804_corrsyn(received_signl_ap3, sig, fs);
% %     [a,delay3]=max(xc);
% %     TOA_3 = (length(sig) - delay3) * ts;
    
    

    %------------------- AP1 receiver ------------------------------
    % Correlator
    received_signl_ap1 = sum(ap1_tag_chan_wgn)/num_bits;
    xc = xcorr(y, received_signl_ap1);
    [a,delay1]=max(xc);
    TOA_1 = (length(sig) - delay1) * ts;

    %------------------- AP2 receiver ------------------------------
    % Correlator
    received_signl_ap2 = sum(ap2_tag_chan_wgn)/num_bits;
    xc = xcorr(y, received_signl_ap2);
    [a,delay2]=max(xc);
    TOA_2 = (length(sig) - delay2) * ts;

    %------------------- AP3 receiver ------------------------------
    % Correlator
    received_signl_ap3 = sum(ap3_tag_chan_wgn)/num_bits;
    xc = xcorr(y, received_signl_ap3);
    [a,delay3] = max(xc);
    TOA_3 = (length(sig) - delay3) * ts;
% 
%     %---------------------------------------------------------------
%     time_ap_tag = time_ap_tag;
    time_dur = [TOA_1 TOA_2 TOA_3];
    toa_error(1,kkk) = toa(AP, Tag, time_dur, light_speed);
    
end
% 
plot(-15:5:5, toa_error);