%% Simulates GFSK communication and plots the performance in terms of BER
%
% For the CFO correction, the samples are processed "one by one" to match 
% the real behavior of the radio.
%

%% Transmission

%%% Transmission/modulation parameters

f_c = 2.45e09;          % Carrier frequency
r_symb = 1e06;          % Symbol rate
Ts = 1/r_symb;          % Symbol period
m = 0.5;                % Modulation index
BTb = 0.5;              % Bandwidth - Symbol perdiod product


N_symb = 1e06;          % Number of symbols used for the simulation
reso = 8;               % Resolution factor (number of samples by symbol period at TX)
dfppm = 30;				% CFO expressed in ppm wrt the carrier frequency


%%% For BER vs Eb/N0 plot and noise variance (N0) generation
Nsnr = 16;              % Number of different values for the plot
Eb_N0_dB = linspace(0,Nsnr-1,Nsnr);
Eb_N0 = 10.^(Eb_N0_dB/10);
Eb = 1;
N0 = Eb./Eb_N0;


%%% Generation of the preamble + noise before packet parameter
preamble = [1;-1;1;-1;1;-1;1;-1];
Lpreamble = length(preamble);

Lnoisestart = 92;		% Length of the noise portion preceding the packet
N_symb = N_symb-Lpreamble-Lnoisestart;


%%% Generation of the sequence of symbols (I)
M = 2;
N_bits_by_symb = log2(M);
I = pammod(N_symb,M);	% "pammod" is a function I wrote for the PAM modulation
I(1) = 1;   			% Because the first bit of the Access Address is always the opposite of the last one of the preamble.
I = [preamble;I];
LI = length(I);


%%% Gaussian pulse-shape filter (g) generation
N_Ts = 3;                   % Number of symbol periods for the filter support
time = -N_Ts/2*Ts : Ts/reso : N_Ts/2*Ts;    % Filter support
Tb = Ts/N_bits_by_symb;     % Here, we will always have Tb=Ts since M=2
K = sqrt(log(2));           % Parameter for the filter amplitude, set to a value giving a unitary amplitude
g = g_GMSK(time,K,Tb,BTb);  % Filter generation, using my function "g_GMSK"
Lg = length(g);


%%% Convolution (convo) between the symbol sequence and the Gaussian filter
Iup = upsample(I, reso);
convIupg = conv(Iup,g);
tmin = (reso*(LI-1)+Lg+1)/2 - (reso*LI)/2;
tmax = (reso*(LI-1)+Lg+1)/2 + (reso*LI)/2;
convo = convIupg(tmin:tmax);
Lconvo = length(convo);


%%% Integration (integ) of the convolution
integ = zeros(Lconvo-1,1);
incr = zeros(Lconvo-1,1);
integ(1) = (convo(1)+convo(2))/2 * (Ts/reso);
for i = 2:Lconvo-1
    incr(i) = (convo(i)+convo(i+1))/2 * (Ts/reso);
    integ(i) = integ(i-1) + incr(i);
end

% In practice the values in ak are tabled (they can be precomputed).
% They are used for the CFO correction.
integ2 = integ(1+reso:end) - integ(1:end-reso);
a = integ2 * pi*m/Tb;
ak = a(reso:reso+2*reso-1);


%%% Phase (phi) generation
phi = pi*m/Tb * integ;
Lphi = length(phi);


%%% Transmitted signal (s) generation
s = exp(1j*phi);
Ls = length(s);






%% Reception


OSF = 8;                % Oversampling factor (number of samples by symbol period at RX)


BERcorr = zeros(Nsnr,1);
BER = zeros(Nsnr,1);
df_hat = zeros(Nsnr,1);
ind_trig = zeros(Nsnr,1);
SFindi = zeros(Nsnr,1);
% Loop for each different value of the noise variance
% Since the non-idealities mitigations are sized for a SNR value of 13 or
% 14 dB, the loop should only take those values.
% for i = 1:Nsnr
SNR = 13;
for i = SNR+1
    
    %%% AWGN (n) generation
    N = N0(i)*OSF;
    n = awgn(Lnoisestart*OSF+Ls, 1, N);     % My function awgn generates the noise
    
    
    %%% Received signal (r) generation
    r = [zeros(Lnoisestart*OSF, 1) ; s] + n;
    Lr = length(r);


        %%% CFO        
        df = dfppm/1e06 * f_c;
        time = (1/(r_symb*OSF) : 1/(r_symb*OSF) : (Lr)/(r_symb*OSF))';
        rcfo = exp(1j*2*pi*df*time).*r;
        
       
        %%% Filtering of the received CFO'ed signal
        fcut = 0.6e06;
        Nper = 8;
        [h0] = LPF('Hamming', fcut, OSF, Nper);
        Lh0 = length(h0);
        rconvh00 = conv(rcfo,h0);
        rconvh0 = rconvh00(1+Nper/2*OSF:end-Nper/2*OSF);
        rcfof = rconvh0;


        %%% Preamble detection
        % Hermitian product
        OSR = OSF;
        Hprod = rcfof(1+OSR:end).*conj(rcfof(1:end-OSR));
        % IIR filter
        IIRb = 1;
        IIRa = zeros(OSR+1,1);
        IIRa(1) = 1;
        IIRa(OSR+1) = 0.875;
        IIRHprod = filter(IIRb,IIRa,Hprod);
        absIIRHprod = abs(IIRHprod);
        LabsIIRHprod = length(absIIRHprod);
        % Search for preamble
        if (SNR == 13)
            thresh = 37.5;
        elseif (SNR == 14)
            thresh = 32.5;
        end
        p = 1;
        booly = false;
        while(booly == false)
            if (absIIRHprod(p) > thresh)
                booly = true;
            elseif (p >= LabsIIRHprod)
                booly = true;
                fprintf('Error, after 1000 iterations, there is still no value that has reached the threshold!');
            else
                p = p+1;
            end
        end
        ind_trig(i) = p;    % This is the index that triggered the detection.
        
		
        %%% Symbol timing recovery
        Noffset = 5;        % We need to wait for 5 periods after the 
                            % preamble detection in order to target the 
                            % max of the 6th preamble period.
        offset = Noffset*OSF;
        maxxtemp = absIIRHprod(ind_trig(i)+offset);
        indtemp = ind_trig(i)+offset;
        kk = 1;
        while (kk < 1*OSF)
            if (absIIRHprod(ind_trig(i)+offset+kk) > maxxtemp)
                maxxtemp = absIIRHprod(ind_trig(i)+offset+kk);
                indtemp = ind_trig(i)+offset+kk;
            end
            kk = kk+1;
        end
        SFindi(i) = indtemp;

       
        %%% CFO correction        
        df_hat_temp1 = 0;
        Nperiod = 6;
        b = 1;
        counter = 1;
        symbr = zeros(Nperiod*OSF,1);
        symbrold = zeros(Nperiod*OSF,1);
        dfh = zeros(Nperiod*OSF,1);
        dfh1 = zeros(Nperiod*OSF,1);
        demodsymbrrold = zeros(Nperiod*OSF,1);
        dk = zeros(Nperiod*OSF,1);
        akind = zeros(Nperiod*OSF,1);
        hermi_unnorm = zeros(Nperiod*OSF, 1);
        hermi = zeros(Nperiod*OSF, 1);
        while(b <= Nperiod*OSF)
            
            % The first sample used for the CFO correction is the one 1
            % period after the preamble detection.
            symbr(b) = rcfof(b+ind_trig(i)+OSF-1);
            symbrold(b) = rcfof(b+ind_trig(i)+OSF-1-OSF);
            
            hermi_unnorm(b) = symbr(b)*conj(symbrold(b));
            hermi(b) = hermi_unnorm(b)/abs(hermi_unnorm(b));
            dk(b) = imag(hermi(b));

            % Simulates the index used to fetch the tabled values from the
            % LUTs.
            akind(b) = mod(b,2*OSF);
            if (akind(b) == 0)
                akind(b) = 2*OSF;
            end
            
            sinak = sin(ak(akind(b)));
            cosak = cos(ak(akind(b)));
            dfh1(b) = (dk(b)-sinak)/cosak /(2*pi*Tb);   % CFO estimate
            
            df_hat_temp1 = df_hat_temp1+dfh1(b);
            dfh(b) = df_hat_temp1/counter;      % Averaging of the estimate
            counter = counter + 1;
                                            
            b = b+1;
            
        end

        df_hat(i) = dfh(end);   % Final CFO estimate.

        
        rcorr = exp(-1j*2*pi*df_hat(i)*time((Lnoisestart+Lpreamble)*OSF+1 : end)).*rcfo((Lnoisestart+Lpreamble)*OSF+1 : end);
        % Filtering of the corrected signal
        rcorrconvh0 = conv(rcorr,h0);
        rcorrconvh0 = rcorrconvh0(1+Nper/2*OSF:end-Nper/2*OSF);
        rcorrf = rcorrconvh0;
        Lrcorrf = length(rcorrf);  
        

        
    %%% Filtering of the received signal (to simulate the case without CFO)
    rconvh0 = conv(r,h0);
    rconvh0 = rconvh0(1+Nper/2*OSF:end-Nper/2*OSF);
    rf = rconvh0;
    Lrf = length(rf);
        
        
    %%% Demodulation
    
    % Index for the decimation prior to the demodulation
    indidown = mod(SFindi(i)+2,OSF);
    if (indidown == 0)
        indidown = OSF;
    end
    
    rcorrfdown = downsample(rcorrf(indidown:end),OSF/2);
    Lrcorrfdown = length(rcorrfdown);
    if (mod(Lrcorrfdown,2)==0)
        demodcorr = imag(rcorrfdown(2:2:end).*conj(rcorrfdown(1:2:end-1)));
    else
        demodcorr = imag(rcorrfdown(2:2:end-1).*conj(rcorrfdown(1:2:end-2)));
    end
    Ldemodcorr = length(demodcorr);
    
    
    rfdown = downsample(rf(indidown:end),OSF/2);
    Lrfdown = length(rfdown);
    if (mod(Lrfdown,2)==0)
        demod = imag(rfdown(2:2:end).*conj(rfdown(1:2:end-1)));
    else
        demod = imag(rfdown(2:2:end-1).*conj(rfdown(1:2:end-2)));
    end
    demod = demod(1+Lpreamble+Lnoisestart:end);
    Ldemod = length(demod);
    
    
    %%% Decision
    Icorr_hat = sign(demodcorr);
    BERcorr(i) = sum(I(1+Lpreamble:Lpreamble+Ldemodcorr)~=Icorr_hat)/N_symb;
    I_hat = sign(demod);
    BER(i) = sum(I(1+Lpreamble:Lpreamble+Ldemodcorr)~=I_hat)/N_symb;



end



% BER vs Eb/N0 plot
figure;
semilogy(Eb_N0_dB,BERcorr,'xg','LineWidth',1.5,'MarkerSize',8);
hold on;
semilogy(Eb_N0_dB,BER,'xk','LineWidth',1.5,'MarkerSize',8);
axis([0 Nsnr 10/N_symb 1]);
grid;
legend('After CFO correction', 'Without CFO');
title( strcat('BER -- CFO = ', num2str(df)) );
xlabel('E_{b}/N_{0} [dB]');
ylabel('BER');