% test_qpsk.m
% David Rowe Feb 2014
%
% QPSK modem simulation, initially based on code by Bill Cowley
% Generates curves BER versus E/No curves for different modems.
% Design to test coherent demodulation ideas on HF channels without
% building a full blown modem.  Uses 'genie provided' estimates for
% timing estimation, frame sync.
%

1;

% main test function 

function sim_out = ber_test(sim_in, modulation)
    Fs = 8000;

    verbose          = sim_in.verbose;
    framesize        = sim_in.framesize;
    Ntrials          = sim_in.Ntrials;
    Esvec            = sim_in.Esvec;
    phase_offset     = sim_in.phase_offset;
    phase_est        = sim_in.phase_est;
    w_offset         = sim_in.w_offset;
    plot_scatter     = sim_in.plot_scatter;
    Rs               = sim_in.Rs;
    hf_sim           = sim_in.hf_sim;
    Nhfdelay         = floor(sim_in.hf_delay_ms*1000/Fs);
    hf_phase_only    = sim_in.hf_phase_only;
    hf_mag_only      = sim_in.hf_mag_only;

    bps              = 2;
    Nsymb            = framesize/bps;
    prev_sym_tx      = qpsk_mod([0 0]);
    prev_sym_rx      = qpsk_mod([0 0]);
    rx_symb_log      = [];

    Np               = sim_in.Np;  % number of pilot symbols to use in phase est
    Ns               = sim_in.Ns;  % spacing of pilot symbols, so (Nps-1) data symbols between every pilot
    Nps              = Np*Ns;
    r_delay_line     = zeros(1,Nps+1);
    s_delay_line     = zeros(1,Nps+1);
    spread_main_phi  = 0;
    spread_delay_phi = 0;
    spread_main_phi_log = [];

    ldpc_code = sim_in.ldpc_code;

    if ldpc_code
        % Start CML library

        currentdir = pwd;
        addpath '/home/david/tmp/cml/mat'    % assume the source files stored here
        cd /home/david/tmp/cml
        CmlStartup                           % note that this is not in the cml path!
        cd(currentdir)
  
        % Our LDPC library

        ldpc;

        rate = 3/4; 
        mod_order = 4; 
        modulation = 'QPSK';
        mapping = 'gray';

        demod_type = 0;
        decoder_type = 0;
        max_iterations = 100;

        code_param = ldpc_init(rate, framesize, modulation, mod_order, mapping);
        code_param.code_bits_per_frame = framesize;
        code_param.symbols_per_frame = framesize/bps;
    else
        rate = 1;
    end

    % convert "spreading" samples from 1kHz carrier at Fs to complex
    % baseband, generated by passing a 1kHz sine wave through PathSim
    % with the ccir-poor model, enabling one path at a time.
    
    Fc = 1000;
    fspread = fopen("../raw/sine1k_2Hz_spread.raw","rb");
    spread1k = fread(fspread, "int16")/10000;
    fclose(fspread);
    fspread = fopen("../raw/sine1k_2ms_delay_2Hz_spread.raw","rb");
    spread1k_2ms = fread(fspread, "int16")/10000;
    fclose(fspread);

    % down convert to complex baseband
    spreadbb = spread1k.*exp(-j*(2*pi*Fc/Fs)*(1:length(spread1k))');
    spreadbb_2ms = spread1k_2ms.*exp(-j*(2*pi*Fc/Fs)*(1:length(spread1k_2ms))');

    % remove -2000 Hz image
    b = fir1(50, 5/Fs);
    spread = filter(b,1,spreadbb);
    spread_2ms = filter(b,1,spreadbb_2ms);

    % discard first 1000 samples as these were near 0, probably as
    % PathSim states were ramping up

    spread    = spread(1000:length(spread));
    spread_2ms = spread_2ms(1000:length(spread_2ms));

    % Determine "gain" of HF channel model, so we can normalise
    % carrier power during HF channel sim to calibrate SNR.  I imagine
    % different implementations of ccir-poor would do this in
    % different ways, leading to different BER results.  Oh Well!

    hf_gain = 1.0/sqrt(var(spread)+var(spread_2ms));

    % design root nyquist (root raised cosine) filter and init tx and rx filter states

    alpha = 0.5; T=1/Fs; Nfiltsym=7; M=Fs/Rs;
    if floor(Fs/Rs) != Fs/Rs
        printf("oversampling ratio must be an integer\n");
        return;
    end
    hrn = gen_rn_coeffs(alpha, T, Rs, Nfiltsym, M);
    Nfilter = length(hrn);
    tx_filter_memory = zeros(1, Nfilter);
    rx_filter_memory = zeros(1, Nfilter);
    s_delay_line_filt = zeros(1,Nfiltsym);
    tx_bits_delay_line_filt = zeros(1,Nfiltsym*bps);
    hf_sim_delay_line = zeros(1,M+Nhfdelay);

    for ne = 1:length(Esvec)
        Es = Esvec(ne);
        EsNo = 10^(Es/10);
    
        % Given Es/No, determine the variance of a normal noise source:
        %
        %   Es = C/Rs where C is carrier power (energy per unit time) and Rs is the symbole rate
        %   N  = NoB where N is the total noise power, No is the Noise spectral density is W/Hz
        %        and B is the bandwidth of the noise which is Fs
        %   No = N/Fs
        %
        % equating Es/No we get:
        %
        %   Es/No = (C/Rs)/(No/Fs)
        %   No    = CFs/(Rs(Es/No))

        variance = Fs/(Rs*EsNo);
        Terrs = 0;  Tbits = 0;  Terrsldpc = 0;  Tbitsldpc = 0; Ferrsldpc = 0;
        if verbose > 1
            printf("EsNo (dB): %f EsNo: %f variance: %f\n", Es, EsNo, variance);
        end
        
        % init HF channel
        sc = 1;

        tx_filt_log = [];
        rx_filt_log = [];
        rx_baseband_log = [];
        tx_baseband_log = [];
        noise_log = [];
        hf_angle_log = [];
        tx_phase = rx_phase = 0;
        tx_data_buffer = zeros(1,2*framesize);
        s_data_buffer = zeros(1,2*Nsymb);
        C_log = [];

        for nn = 1: Ntrials
                  
            tx_bits = round( rand( 1, framesize*rate ) );
            %tx_bits = [1 0 zeros(1,framesize*rate-2)];

            % modulate

            if ldpc_code
                [tx_bits, s] = ldpc_enc(tx_bits, code_param);
                t2 = tx_bits;
                s2 = s;
            else
                s = zeros(1, Nsymb);
                for i=1:Nsymb
                    tx_symb = qpsk_mod(tx_bits(2*(i-1)+1:2*i));
                    %printf("shift: %f prev_sym: %f  ", tx_symb, prev_sym_tx);
                    if strcmp(modulation,'dqpsk')
                        tx_symb *= prev_sym_tx;
                        %printf("tx_symb: %f\n", tx_symb);
                        prev_sym_tx = tx_symb;
                    end 
                    s(i) = tx_symb;
                end
            end
            s_ch = s;

            % root nyquist filter symbols

            for k=1:Nsymb

               % tx filter symbols

               tx_filt = zeros(1,M);

               % tx filter each symbol, generate M filtered output samples for each symbol.
               % Efficient polyphase filter techniques used as tx_filter_memory is sparse

               tx_filter_memory(Nfilter) = s_ch(k);

               for i=1:M
                   tx_filt(i) = M*tx_filter_memory(M:M:Nfilter) * hrn(M-i+1:M:Nfilter)';
               end
               tx_filter_memory(1:Nfilter-M) = tx_filter_memory(M+1:Nfilter);
               tx_filter_memory(Nfilter-M+1:Nfilter) = zeros(1,M);
               
               % HF channel simulation

               if hf_sim

                   hf_sim_delay_line(1:Nhfdelay) = hf_sim_delay_line(M+1:M+Nhfdelay);
                   hf_sim_delay_line(Nhfdelay+1:M+Nhfdelay) = tx_filt;

                   % disable as a wrap around will cause a nasty phase jump.  Best to generate
                   % longer files
                   %if ((sc+M) > length(spread)) || ((sc+M) > length(spread_2ms))
                   %    sc =1 ;
                   %end
                   comb = conj(spread(sc:sc+M-1))' + conj(spread_2ms(sc:sc+M-1))';
                   if hf_phase_only
                        tx_filt = tx_filt.*exp(j*angle(comb));
                   else
                       if hf_mag_only
                           comb = conj(spread(sc:sc+M-1))' + conj(spread_2ms(sc:sc+M-1))';
                           tx_filt = tx_filt.*abs(comb);   
                       else
                           % regular HF channel sim
                           tx_filt = tx_filt.*conj(spread(sc:sc+M-1))' + hf_sim_delay_line(1:M).*conj(spread_2ms(sc:sc+M-1))';
                       end
                   end
                   sc += M;
 
                   % normalise so average HF power C=1

                   if hf_phase_only == 0   % C already 1 if we are just tweaking phase
                       tx_filt *= hf_gain;
                   end
                   C_log = [C_log abs(comb)*hf_gain];
               end
               tx_filt_log = [tx_filt_log tx_filt];
               hf_angle_log = [hf_angle_log angle(comb)];

               % AWGN noise and phase/freq offset channel simulation
               % 0.5 factor ensures var(noise) == variance , i.e. splits power between Re & Im

               noise = sqrt(variance*0.5)*( randn(1,M) + j*randn(1,M) );
               noise_log = [noise_log noise];
               rx_baseband = tx_filt.*exp(j*phase_offset) + noise;
               phase_offset += w_offset;
               
               % rx filter symbol

               rx_filter_memory(Nfilter-M+1:Nfilter) = rx_baseband;
               rx_filt = rx_filter_memory * hrn';
               rx_filter_memory(1:Nfilter-M) = rx_filter_memory(1+M:Nfilter);
               rx_filt_log = [rx_filt_log rx_filt];

               % delay in tx symbols to compensate for filtering
               % delay, as tx symbols are used as pilot symbols input
               % to phase est

               s_delay_line_filt(1:Nfiltsym-1) = s_delay_line_filt(2:Nfiltsym);
               s_delay_line_filt(Nfiltsym) = s(k);
               s(k) = s_delay_line_filt(1);  

               % delay in tx bits to compensate for filtering delay

               tx_bits_delay_line_filt(1:(Nfiltsym-1)*bps) = tx_bits_delay_line_filt(bps+1:Nfiltsym*bps);
               tx_bits_delay_line_filt((Nfiltsym-1)*bps+1:Nfiltsym*bps) = tx_bits((k-1)*bps+1:k*bps);
               tx_bits((k-1)*bps+1:k*bps) = tx_bits_delay_line_filt(1:bps);

               s_ch(k) = rx_filt;               
            end

            % coherent demod phase estimation and correction using pilot symbols
            % cheating a bit here, we use fact that all tx-ed symbols are known

            if phase_est
                for i=1:Nsymb

                    % delay line for phase est window

                    r_delay_line(1:Nps-1) = r_delay_line(2:Nps);
                    r_delay_line(Nps) = s_ch(i);

                    % delay in tx data to compensate data for phase est window

                    s_delay_line(1:Nps-1) = s_delay_line(2:Nps);
                    s_delay_line(Nps) = s(i);
                    tx_bits(2*(i-1)+1:2*i) = qpsk_demod(s_delay_line(floor(Nps/2)+1));

                    % estimate phase from surrounding known pilot symbols and correct

                    corr = 0; centre = floor(Nps/2)+1;
                    for k=1:Ns:(Nps+1)
                        if (k != centre)
                            corr += s_delay_line(k) * r_delay_line(k)';
                        end
                    end
                    s_ch(i) = r_delay_line(centre).*exp(j*angle(corr));
               end    
               %printf("corr: %f angle: %f\n", corr, angle(corr));
            end

            % de-modulate

            rx_bits = zeros(1, framesize);
            for i=1:Nsymb
                rx_symb = s_ch(i);
                if strcmp(modulation,'dqpsk')
                    tmp = rx_symb;
                    rx_symb *= conj(prev_sym_rx/abs(prev_sym_rx));
                    prev_sym_rx = tmp;
                end
                rx_bits((2*(i-1)+1):(2*i)) = qpsk_demod(rx_symb);
                rx_symb_log = [rx_symb_log rx_symb];
            end

            % Measure BER

            % discard bits from first 2*Nfiltsym+Nps+1 symbols as tx
            % and rx filter and phase est memories not full

            skip = bps*(2*Nfiltsym+1+Nps+1);
            if nn == 1
                tx_bits_tmp = tx_bits(skip:length(tx_bits));
                rx_bits_tmp = rx_bits(skip:length(rx_bits));
            else
                tx_bits_tmp = tx_bits;
                rx_bits_tmp = rx_bits;
           end

            error_positions = xor( rx_bits_tmp, tx_bits_tmp );
            Nerrs = sum(error_positions);
            Terrs += Nerrs;
            Tbits += length(tx_bits_tmp);

            % Optionally LDPC decode
            
            if ldpc_code
                % filter memories etc screw up frame alignment so we need to buffer a frame

                tx_data_buffer(1:framesize) = tx_data_buffer(framesize+1:2*framesize);
                s_data_buffer(1:Nsymb) = s_data_buffer(Nsymb+1:2*Nsymb);
                tx_data_buffer(framesize+1:2*framesize) = tx_bits;
                s_data_buffer(Nsymb+1:2*Nsymb) = s_ch;

                offset = Nfiltsym-1;
                if (phase_est)
                    offset += floor(Nps/2);
                end
                st_tx = offset*bps+1;
                st_s = offset;

                detected_data = ldpc_dec(code_param, max_iterations, demod_type, decoder_type, ...
                                s_data_buffer(st_s+1:st_s+Nsymb), min(100,EsNo));

                % ignore first frame as filter, phase est memories filling up
                if nn != 1
                    error_positions = xor( detected_data(1:framesize*rate), ...
                                           tx_data_buffer(st_tx:st_tx+framesize*rate-1) );
                    Nerrs = sum(error_positions);
                    if Nerrs
                        Ferrsldpc++;
                    end
                    Terrsldpc += Nerrs;
                    Tbitsldpc += framesize*rate;
                end
            end

        end
    
        TERvec(ne) = Terrs;
        BERvec(ne) = Terrs/Tbits;
        if ldpc_code
            TERldpcvec(ne) = Terrsldpc;
            FERldpcvec(ne) = Ferrsldpc;
            BERldpcvec(ne) = Terrsldpc/Tbitsldpc;
        end

        if verbose 
            printf("EsNo (dB): %f  Terrs: %d BER %f BER theory %f", Es, Terrs,
                   Terrs/Tbits, 0.5*erfc(sqrt(EsNo/2)));
            if ldpc_code
                printf(" LDPC: Terrs: %d BER: %f Ferrs: %d FER: %f", 
                       Terrsldpc, Terrsldpc/Tbitsldpc, Ferrsldpc, Ferrsldpc/(Ntrials-1));
            end
            printf("\n");
        end
        if verbose > 1
            printf("Terrs: %d BER %f BER theory %f C %f N %f Es %f No %f Es/No %f\n\n", Terrs,
                   Terrs/Tbits, 0.5*erfc(sqrt(EsNo/2)), var(tx_filt_log), var(noise_log),
                   var(tx_filt_log)/Rs, var(noise_log)/Fs, (var(tx_filt_log)/Rs)/(var(noise_log)/Fs));
        end
    end
    
    Ebvec = Esvec - 10*log10(bps);
    sim_out.BERvec          = BERvec;
    sim_out.Ebvec           = Ebvec;
    sim_out.TERvec          = TERvec;
    if ldpc_code
        sim_out.BERldpcvec  = BERldpcvec;
        sim_out.TERldpcvec  = TERldpcvec;
        sim_out.FERldpcvec  = FERldpcvec;
    end

    if plot_scatter
        figure(2);
        clf;
        scat = rx_symb_log(2*Nfiltsym:length(rx_symb_log)) .* exp(j*pi/4);
        plot(real(scat), imag(scat),'+');

        figure(3);
        clf;

        subplot(211)
        plot(C_log);
        subplot(212)
        plot(hf_angle_log);
        axis([1 10000 min(hf_angle_log) max(hf_angle_log)])
    end
endfunction

% Gray coded QPSK modulation function

function symbol = qpsk_mod(two_bits)
    two_bits_decimal = sum(two_bits .* [2 1]); 
    switch(two_bits_decimal)
        case (0) symbol =  1;
        case (1) symbol =  j;
        case (2) symbol = -j;
        case (3) symbol = -1;
    endswitch
endfunction

% Gray coded QPSK demodulation function

function two_bits = qpsk_demod(symbol)
    if isscalar(symbol) == 0
        printf("only works with scalars\n");
        return;
    end
    bit0 = real(symbol*exp(j*pi/4)) < 0;
    bit1 = imag(symbol*exp(j*pi/4)) < 0;
    two_bits = [bit1 bit0];
endfunction

% Start simulations ---------------------------------------

more off;
sim_in.verbose = 2;

sim_in.Esvec            = 5; 
sim_in.Ntrials          = 100;
sim_in.framesize        = 100;
sim_in.Rs               = 400;
sim_in.phase_offset     = 0;
sim_in.phase_est        = 0;
sim_in.w_offset         = 0;
sim_in.plot_scatter     = 1;
sim_in.hf_delay_ms      = 2;
sim_in.hf_sim           = 1;
sim_in.Np               = 6;
sim_in.Ns               = 5;
sim_in.hf_phase_only    = 0;
sim_in.hf_mag_only      = 1;
sim_in.ldpc_code        = 0;

Ebvec = sim_in.Esvec - 10*log10(2);
BER_theory = 0.5*erfc(sqrt(10.^(Ebvec/10)));

sim_qpsk           = ber_test(sim_in, 'qpsk');

figure(1); 
clf;
semilogy(Ebvec, BER_theory,'r;QPSK theory;')
hold on;
semilogy(sim_qpsk.Ebvec, sim_qpsk.BERvec,'g;QPSK;')
hold off;
xlabel('Eb/N0')
ylabel('BER')
grid("minor")


if 0
sim_in.hf_mag_only      = 1;
sim_qpsk_mag            = ber_test(sim_in, 'qpsk');

sim_in.hf_mag_only      = 0;
sim_in.hf_phase_only    = 1;
sim_in.phase_est        = 1;
sim_qpsk_phase          = ber_test(sim_in, 'qpsk');

sim_in.hf_phase_only    = 0;
sim_qpsk_coh_6_5        = ber_test(sim_in, 'qpsk');

sim_in.phase_est        = 0;
sim_dqpsk               = ber_test(sim_in, 'dqpsk');

figure(1); 
clf;
semilogy(Ebvec, BER_theory,'r;QPSK theory;')
hold on;
semilogy(sim_qpsk_mag.Ebvec, sim_qpsk_mag.BERvec,'g;QPSK CCIR poor mag;')
semilogy(sim_qpsk_phase.Ebvec, sim_qpsk_phase.BERvec,'k;QPSK CCIR poor phase;')
semilogy(sim_qpsk_coh_6_5.Ebvec, sim_qpsk_coh_6_5.BERvec,'c;QPSK CCIR poor Np=6 Ns=5;')
semilogy(sim_dqpsk.Ebvec, sim_dqpsk.BERvec,'b;DQPSK CCIR poor;')
%semilogy(sim_qpsk_coh_5_24.Ebvec, sim_qpsk_coh_5_24.BERvec,'k;QPSK Ns=5 Np=24;')
%semilogy(sim_qpsk_coh_2_12.Ebvec, sim_qpsk_coh_2_12.BERvec,'c;QPSK Ns=2 Np=12;')
hold off;
xlabel('Eb/N0')
ylabel('BER')
grid("minor")
axis([min(Ebvec)-1 max(Ebvec)+1 1E-2 1])
end
