%=============================================================================%
%=                Asynchronous UWB position location system                  =%
%= 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 20; 20 0]; % in meters

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

% Tag's initial coordinate
Tag = [14 12];

% 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 = 10000;

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)
%-------------------------------------------------------------------------


%----------------- Transmitter -------------------------------------------

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

% 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

% Distance from AP1 to other APs
% AP1 is always master AP
for ii = 2:num_ap
    dist_ap_ap(ii-1) = dist_t(AP(1,:), AP(ii,:));
    time_ap_ap(ii-1) = dist_ap_ap(ii-1)/light_speed;
end

shift_const = 40;

kkk = 0;
for EbNo = -40:5:-30
    kkk = kkk + 1;

for nloop = 1:1

%++++++++++++++++ TRANSMISSION +++++++++++++++++++++++++++++++
   
for jj = 1:num_bits
    % From AP1 to TAG
    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));
    conv_data = conv(del_sig_ap1_tag(jj,:), h);
    ap1_tag_chan(jj, :) = conv_data(1:length(sig));

% %     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);


    % From AP1 to AP2
    del_sample_ap_tag = round(time_ap_ap(1)/ts);
    xx = zeros(1,del_sample_ap_tag);
    % Delayed signals from AP1 to AP2
    del_sig_ap2_tag(jj,:) = [xx sig(1:end-length(xx))];
    h = uwb_channel(dist_ap_ap(1));
    conv_data = conv(del_sig_ap2_tag(jj,:), h);
    ap1_ap2_chan(jj, :) = conv_data(1:length(sig));

% %     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
% %     ap1_ap2_chan(jj, :) =  sum(added);


    % From AP1 to AP3
    del_sample_ap_tag = round(time_ap_ap(2)/ts);
    xx = zeros(1,del_sample_ap_tag);
    % Delayed signals from AP1 to AP3
    del_sig_ap3_tag(jj,:) = [xx sig(1:end-length(xx))];
    h = uwb_channel(dist_ap_ap(2));
   	conv_data = conv(del_sig_ap3_tag(jj,:), h);
    ap1_ap3_chan(jj, :) = conv_data(1:length(sig));
% %     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
% %     ap1_ap3_chan(jj, :) =  sum(added);

end
EbNo
%-------------------------------------------------------
% Additive White Gaussian Noise (AWGN) Channel ---------
noise_var   = 0.5 * 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);
    ap1_ap2_chan_wgn(jj,:) = ap1_ap2_chan(jj,:)/std(ap1_ap2_chan(jj,:)) + randn(1,length(ap1_ap2_chan(jj,:))) .* sqrt(noise_var);
    ap1_ap3_chan_wgn(jj,:) = ap1_ap3_chan(jj,:)/std(ap1_ap3_chan(jj,:)) + randn(1,length(ap1_ap3_chan(jj,:))) .* sqrt(noise_var);
end
%-------------------------------------------------------


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



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);
    % Delayed signals from AP1 to Tag
    del_sig_ap1_tag(jj,:) = [xx sig(1:end-length(xx))];
    h = uwb_channel(dist_ap_tag(1));
    conv_data = conv(del_sig_ap1_tag(jj,:), h);
    tag_ap1_chan(jj, :) = conv_data(1:length(sig));

% %     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
% %     tag_ap1_chan(jj, :) =  sum(added);
    
    % 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));
    conv_data = conv(del_sig_ap2_tag(jj,:), h);
    tag_ap2_chan(jj, :) = conv_data(1:length(sig));

% %     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
% %     tag_ap2_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));
    conv_data = conv(del_sig_ap3_tag(jj,:), h);
    tag_ap3_chan(jj, :) =  conv_data(1:length(sig));
    
% %     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
% %     tag_ap3_chan(jj, :) =  sum(added);
    
end

%-------------------------------------------------------
% Additive White Gaussian Noise (AWGN) Channel ---------
noise_var   = 0.5 * 10^(-EbNo/10);
for jj = 1:num_bits
	tag_ap1_chan_wgn(jj,:) = tag_ap1_chan(jj,:)/std(tag_ap1_chan(jj,:)) + randn(1,length(tag_ap1_chan(jj,:))) .* sqrt(noise_var);
    tag_ap2_chan_wgn(jj,:) = tag_ap2_chan(jj,:)/std(tag_ap2_chan(jj,:)) + randn(1,length(tag_ap2_chan(jj,:))) .* sqrt(noise_var);
    tag_ap3_chan_wgn(jj,:) = tag_ap3_chan(jj,:)/std(tag_ap3_chan(jj,:)) + randn(1,length(tag_ap3_chan(jj,:))) .* sqrt(noise_var);
end
%-------------------------------------------------------

%------------------- AP 1's RECEIVER -------------------
% Correlator
received_tag_ap1 = sum(tag_ap1_chan_wgn)/num_bits;
xc = xcorr(y, received_tag_ap1);
[a,delay1]=max(xc);
TOA_ap1 = (length(sig) - delay1) * ts;

TOA_1 = (TOA_ap1 + TOA_tag)/2;

%------------------- AP 2's RECEIVER -------------------
% Correlator
received_ap1_ap2 = sum(ap1_ap2_chan_wgn) / num_bits;
received_tag_ap2 = sum(tag_ap2_chan_wgn) / num_bits;

xc = xcorr(y, received_ap1_ap2);
[a,delay2]=max(xc);
delay_from_ap1_sampl = round(length(sig) - delay2);
delayed_from_ap1 = delay_from_ap1_sampl * ts;
timing = delayed_from_ap1 - time_ap_ap(1);
y2 = [zeros(1,round(timing/ts)) y];


xc = xcorr(y2, received_tag_ap2);
[a,delay]=max(xc);
timeDuration_ap1_tag = (length(sig) - delay) * ts;
TOA_2 = timeDuration_ap1_tag - TOA_1 + TOA_tag;

%------------------- AP3 receiver ------------------------------
% Averaging received signals over time
received_tag_ap3 = sum(tag_ap3_chan_wgn) ./ num_bits;
received_ap1_ap3 = sum(ap1_ap3_chan_wgn) ./ num_bits;
% % received_ap3 = sum(tag_ap3_chan_wgn) ./ num_bits;
% % received_from_ap1 = sum(ap1_ap3_chan_wgn) ./ num_bits;

xc3 = xcorr(y, received_ap1_ap3);
[a3, delay3] = max(xc3);
delay_from_ap1_sampl = round(length(sig) - delay3);
delayed_from_ap1 = delay_from_ap1_sampl * ts;
timing = delayed_from_ap1 - time_ap_ap(2);
y3 = [zeros(1,round(timing/ts)) y];


xc = xcorr(y3, received_tag_ap3);
[a,delay]=max(xc);
timeDuration_ap1_tag = (length(sig) - delay) * ts;
TOA_3 = timeDuration_ap1_tag - TOA_1 + TOA_tag;

time_dur = [TOA_1 TOA_2 TOA_3];
toa_error(kkk,nloop) = toa(AP, Tag, time_dur, light_speed)



end % nloop

end % EbNo =