%% GFSK Fixed-point implementation

%% Transmission

%%% Transmission/modulation parameters

% fixed-point precision
prec_op_TX = 32;           % precision to be used for the operations performed on the considered signal
prec_cst = 8;           % precision for the constants

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 = 1e04;          % Number of symbols used for the simulation
reso = 8;               % Resolution factor (number of samples by symbol period)
dfppm = 20;


%%% 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
preamble = fi([1;-1;1;-1;1;-1;1;-1], 1, prec_cst, prec_cst-1);
Lpreamble = length(preamble);
N_symb = N_symb-Lpreamble;

Lnoisestart = 92;
N_symb = N_symb-Lnoisestart;

%%% Generation of the sequence of symbols (I)
bits = fi(round(rand(N_symb,1)), 1, prec_cst, prec_cst-1,'ProductMode','SpecifyPrecision','ProductFractionLength',16-1,'ProductWordLength',16,'SumMode', 'SpecifyPrecision','SumFractionLength',16-1,'SumWordLength',16);
I0 = bits-0.5;
I1 = I0*2;
I1(1) = fi(1, 1, prec_cst, prec_cst-1,'ProductMode','SpecifyPrecision','ProductFractionLength',16-1,'ProductWordLength',16,'SumMode', 'SpecifyPrecision','SumFractionLength',16-1,'SumWordLength',16);
I1(2) = fi(1, 1, prec_cst, prec_cst-1,'ProductMode','SpecifyPrecision','ProductFractionLength',16-1,'ProductWordLength',16,'SumMode', 'SpecifyPrecision','SumFractionLength',16-1,'SumWordLength',16);
I = fi([preamble;I1], 1, prec_cst, prec_cst-1,'ProductMode','SpecifyPrecision','ProductFractionLength',16-1,'ProductWordLength',16,'SumMode', 'SpecifyPrecision','SumFractionLength',16-1,'SumWordLength',16);
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;                    % Here, we will always have Tb=Ts
K = sqrt(log(2));           % Parameter for the filter amplitude
g = g_GMSK(time,K,Tb,BTb);  % Filter generation, using my function "g_GMSK"
Lg = length(g);

% g_GMSK can be precomputed and tabled since it is constant, so it does not
% have to be calculated in fixed-point, only the final values are
% converted.
gfi = fi(g,1,16,16-1,'ProductMode','SpecifyPrecision','ProductFractionLength',16-1,'ProductWordLength',16,'SumMode', 'SpecifyPrecision','SumFractionLength',16-1,'SumWordLength',16);


%%% Convolution (convo) between the symbol sequence and the Gaussian filter
tmin = (reso*(LI-1)+Lg+1)/2 - (reso*LI)/2;
tmax = (reso*(LI-1)+Lg+1)/2 + (reso*LI)/2;
Iup = upsample(I, reso);
convfiIupgfi = convfi(Iup,gfi',16,prec_op_TX);
convofi = convfiIupgfi(tmin:tmax);
Lconvo = length(convofi);


%%% Integration (integfi) of the convolution (trapezium method)
% First, we divide everything by 256 as it is the max value the integral
% can output considering a packet of +-300 bits. This way, saturation is
% avoided.
convofi = bitsra(convofi, 8);
integfi = fi(zeros(Lconvo-1,1),1,32,32-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_TX-1,'ProductWordLength',prec_op_TX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_TX-1,'SumWordLength',prec_op_TX);
integfi(1) = bitsra(convofi(1)+convofi(2), log2(2*reso));  % Division is performed via arithmetic shift right
for i = 2:Lconvo-1
    integfi(i) = integfi(i-1) + bitsra(convofi(i)+convofi(i+1), log2(2*reso));
end
% To avoid saturation resulting from multiplication by pi/2, the signal
% should be divided by 2.
integfi = bitsra(integfi, 1);


%%% Phase (phi) generation
fipi_2 = fi(pi/2, 1, prec_cst, prec_cst-2,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_TX-1,'ProductWordLength',prec_op_TX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_TX-1,'SumWordLength',prec_op_TX);
phifi = fipi_2 * integfi;
Lphifi = length(phifi);


%%% Transmitted signal (s) generation
s = exp(1j*double(phifi)*2^9);
Ls = length(s);





%% Reception


% Fixed-point precision
prec_op_RX = 16;
precRX = 16;


% Table of values for the CFO correction:
ak = [-0.894221291841193;...
      -0.823171345055248;...
      -0.624515667660133;...
      -0.335010157734548;...
                       0;...
       0.335010157734548;...
       0.624515667660133;...
       0.823171345055248;...
       0.894221291841193;...
       0.823171345055248;...
       0.624515667660133;...
       0.335010157734548;...
                       0;...
      -0.335010157734548;...
      -0.624515667660133;...
      -0.823171345055248];
sinak = sin(ak);
cosak = cos(ak);
sinakfi = fi(sinak,1,prec_cst,prec_cst-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
cosakfi = fi(cosak,1,prec_cst,prec_cst-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
pow_cosak2pifi_m1 = fi(1./(cosak*2*pi),1,prec_cst,prec_cst-1);


BERcorrfi = zeros(Nsnr,1);
BERfi = zeros(Nsnr,1);
df_hatfi = zeros(Nsnr,1);
ind_trig = zeros(Nsnr,1);
SFindi = zeros(Nsnr,1);
% Loop for each different value of the noise variance
% for i = 1:Nsnr
SNR = 13;
for i = SNR+1

    
    %%% AWGN (n) generation
    % noise can remain in floating-point, it doesn't matter as it will be
    % converted as soon as it will be added to the signal.
    N = N0(i)*reso;
    n = awgn(Lnoisestart*reso+Ls, 1, N);
    
    
    %%% Received signal (r) generation
    r = [zeros(Lnoisestart*reso, 1) ; s] + n;
    Lr = length(r);

    
        %%% CFO        
        df = dfppm/1e06 * f_c;
        time = (1/(r_symb*reso) : 1/(r_symb*reso) : (Lr)/(r_symb*reso))';
        rcfo = exp(1j*2*pi*df*time).*r;
        rcfofi0 = fi(rcfo,1,precRX,precRX-4,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        % /!\ abs(rcfof) can go up to 13 (result from 100000 iterations 
        % with 300 bits and SNR=13dB) => a normalization by 16 should be
        % performed.
        rcfofi = fi(bitsra(rcfofi0,4),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        Lrcfo = length(rcfo);
        
       
        %%% Filtering of the received CFO'ed signal
        % This is done in digital, so from now on signals must be in
        % fixed-point. But the filter can be precomputed, so only its final
        % coefficient values have to be converted.
        % Moreover, as the filter coefficients are <= 1, there is no need
        % for normalization. But one is done nonetheless as rcfo needs one.
        fcut = 0.6e06;
        Nper = 12;
        h0 = LPF('Hamming', fcut, reso, Nper);
        h0fi = fi(h0,1,prec_cst,prec_cst-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        rconvfih0fi0 = convfi(rcfofi,h0fi',prec_op_RX,prec_op_RX);
        rconvfih0fi = rconvfih0fi0(1+Nper/2*reso:end-Nper/2*reso);
        rficfof = rconvfih0fi;
        
        


        %%% Preamble detection
        % Hermitian product
        OSR = reso;
        Hprodfi0 = rficfof(1+OSR:end).*conj(rficfof(1:end-OSR));
        % IIR filter
        IIRb = 1;
        IIRa = zeros(OSR+1,1);
        IIRa(1) = 1;
        IIRa(OSR+1) = 0.875;
        impu = eye(500,1);
        impuresp = filter(IIRb, IIRa, impu);
        % impuresp can be precomputed and tabled, so only the final values
        % need to be converted into fixed-point.
        impurespfi = fi(impuresp,1,prec_cst,prec_cst-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        % /!\ abs(IIRHprod) can go up to 350 (result from 100000 iterations 
        % with 300 bits and SNR=13dB) => a normalization by 512 should be
        % performed, normalization of Hprodfi. But it is already 16*16 = 
        % 256 times smaller due to the previous normalization, so it needs 
        % an additional normalization factor of only 2 to reach 512!
        Hprodfi = fi(bitsra(Hprodfi0,1),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        IIRHprodfi0 = convfi(Hprodfi,impurespfi,precRX,prec_op_RX);
        IIRHprodfi = IIRHprodfi0(1:end-(length(impu)-1));
        absIIRHprodfi = abs(IIRHprodfi);
        LabsIIRHprodfi = length(absIIRHprodfi);

        
        % Search for preamble
        if (SNR==13)
            thresh = 37.5;
        elseif (SNR==14)
            thresh = 32.5;
        end
        % Due to the normalization, the comparison must be done wrt a
        % threshold that has undergone the same normalization.
        threshfi = fi(thresh/2^(4+4+1),1,prec_cst,prec_cst-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_cst-1,'ProductWordLength',prec_cst,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_cst-1,'SumWordLength',prec_cst);
        p = 1;
        booly = false;
        while(booly == false)
            if (absIIRHprodfi(p) > threshfi)
                booly = true;
            elseif (p >= LabsIIRHprodfi)
                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;
        
        
        %%% Symbol timing recovery
        Noffset = 5;
        offset = Noffset*reso;
        maxxtemp = absIIRHprodfi(ind_trig(i)+offset);
        indtemp = ind_trig(i)+offset;
        kk = 1;
        while (kk < 1*reso)
            if (absIIRHprodfi(ind_trig(i)+offset+kk) > maxxtemp)
                maxxtemp = absIIRHprodfi(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;
        afikind = zeros(Nperiod*reso,1);
        symbrfi = fi(zeros(Nperiod*reso,1),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        symbrfiold = fi(zeros(Nperiod*reso,1),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);        
        hermifi_unnorm = fi(zeros(Nperiod*reso,1),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        hermifi = fi(zeros(Nperiod*reso,1),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        demodsymbrfirfiold = fi(zeros(Nperiod*reso,1),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        dkfi = fi(zeros(Nperiod*reso,1),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        dfhfi = fi(zeros(Nperiod*reso,1),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        dfh1fi0 = fi(zeros(Nperiod*reso,1),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        dfh1fi = fi(zeros(Nperiod*reso,1),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        while(b <= Nperiod*reso)
            
            symbrfi(b) = rficfof(b+ind_trig(i)+reso-1);
            symbrfiold(b) = rficfof(b+ind_trig(i)+reso-1-reso);
            
            hermifi_unnorm(b) = symbrfi(b)*conj(symbrfiold(b));
            % The division for the normalization is somehow problematic,
            % as I don't know how to handle this in Matlab, I "cheat" by
            % using a double value for the divider.
            hermifi(b) = hermifi_unnorm(b)/double(abs(hermifi_unnorm(b)));
            demodsymbrfirfiold(b) = imag(hermifi(b));

            dkfi0 = demodsymbrfirfiold(b);
            % Must be divided by 2 to avoid saturation when subtracting it
            % with sinakfik
            dkfi(b) = bitsra(dkfi0,1);
            
            % Simulated the index to fetch the values from the LUTs.
            afikind(b) = mod(b,2*reso);
            if (afikind(b) == 0)
                afikind(b) = 2*reso;
            end
            
            sinakfik0 = sinakfi(afikind(b));
            % Must be divided by 2 to avoid saturation when subtracting it
            % from dkfi            
            sinakfik = bitsra(sinakfik0,1);

            dfh1fi0(b) = (dkfi(b)-sinakfik)*pow_cosak2pifi_m1(afikind(b));
            
            % Here, I accumulate Nperiod*reso (48) values, so I have to
            % normalize by this value beforehand (48->64).
            dfh1fi(b) = bitsra(dfh1fi0(b),6);
            df_hat_temp1 = df_hat_temp1+dfh1fi(b);
            dfhfi(b) = df_hat_temp1/counter;
            counter = counter + 1;
                                            
            b = b+1;
            
        end
        

        df_hatfi(i) = dfhfi(end);
        normfact = 2^7/Tb;

        % Correction is done in RF, and the denormalization is performed at
        % the same time using the factor normfact.
        rcorr = exp(-1j*2*pi*(double(df_hatfi(i))*normfact)*time((Lnoisestart+Lpreamble)*reso+1 : end)).*rcfo((Lnoisestart+Lpreamble)*reso+1 : end);
        rcorrfi0 = fi(rcorr, 1, precRX);
        % Looks like convfi(rcorrfi,h0fi) can go up to 10, so it seems
        % appropriate to normalize by 16.
        rcorrfi = fi(bitsra(rcorrfi0, 4),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
        rcorrficonvfih0fi0 = convfi(rcorrfi,h0fi',precRX,prec_op_RX);
        rcorrficonvfih0fi = rcorrficonvfih0fi0(1+Nper/2*reso:end-Nper/2*reso);
        rcorrfif = rcorrficonvfih0fi;
               
        
    %%% Filtering of the received signal (for the CFO-free case)
    rfi0 = fi(r, 1, precRX);
    % Looks like convfi(rfi,h0fi) can go up to 12, so it seems appropriate
    % to normalize by 16.
    rfi = fi(bitsra(rfi0, 4),1,precRX,precRX-1,'ProductMode','SpecifyPrecision','ProductFractionLength',prec_op_RX-1,'ProductWordLength',prec_op_RX,'SumMode', 'SpecifyPrecision','SumFractionLength',prec_op_RX-1,'SumWordLength',prec_op_RX);
    rficonvfih0fi0 = convfi(rfi,h0fi',precRX,prec_op_RX);
    rficonvfih0fi = rficonvfih0fi0(1+Nper/2*reso:end-Nper/2*reso);
    rfif = rficonvfih0fi;        
    

    %%% Demodulation
    
    indidown = mod(SFindi(i)+2,reso);
    if (indidown == 0)
        indidown = reso;
    end       
    
    rcorrfifdown = downsample(rcorrfif(indidown:end),reso/2);
    Lrcorrfifdown = length(rcorrfifdown);
    if (mod(Lrcorrfifdown,2)==0)
        demodficorr = imag(rcorrfifdown(2:2:end).*conj(rcorrfifdown(1:2:end-1)));
    else
        demodficorr = imag(rcorrfifdown(2:2:end-1).*conj(rcorrfifdown(1:2:end-2)));
    end    
    Ldemodficorr = length(demodficorr);


    rfifdown = downsample(rfif(indidown:end),reso/2);
    Lrfifdown = length(rfifdown);
    if (mod(Lrfifdown,2)==0)
        demodfi = imag(rfifdown(2:2:end).*conj(rfifdown(1:2:end-1)));
    else
        demodfi = imag(rfifdown(2:2:end-1).*conj(rfifdown(1:2:end-2)));
    end
    demodfi = demodfi(1+Lpreamble+Lnoisestart:end);
    Ldemodfi = length(demodfi);
    

    %%% Decision
    Icorrfi_hat = sign(demodficorr);
    BERcorrfi(i) = sum(sign(I(1+Lpreamble:Lpreamble+Ldemodficorr))~=Icorrfi_hat)/N_symb;
    Ifi_hat = sign(demodfi);
    BERfi(i) = sum(sign(I(1+Lpreamble:Lpreamble+Ldemodfi))~=Ifi_hat)/N_symb;
    

end


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