/*---------------------------------------------------------------------------*\

  FILE........: test_cohpsk_ch.c
  AUTHOR......: David Rowe
  DATE CREATED: April 2015

  Tests for the C version of the coherent PSK FDM modem with channel
  impairments generated by Octave.

\*---------------------------------------------------------------------------*/

/*
  Copyright (C) 2015 David Rowe

  All rights reserved.

  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU Lesser General Public License version 2, as
  published by the Free Software Foundation.  This program is
  distributed in the hope that it will be useful, but WITHOUT ANY
  WARRANTY; without even the implied warranty of MERCHANTABILITY or
  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public
  License for more details.

  You should have received a copy of the GNU Lesser General Public License
  along with this program; if not, see <http://www.gnu.org/licenses/>.
*/

#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "codec2_cohpsk.h"
#include "octave.h"
#include "comp_prim.h"
#include "noise_samples.h"
#include "cohpsk_defs.h"
#include "cohpsk_internal.h"

#define FRAMES      100
#define SYNC_FRAMES 12     /* sync state uses up extra log storage as we reprocess several times */

/* defaults with no arguments */

#define FOFF_HZ      0.0
#define ES_NO_DB     8.0
#define HF_DELAY_MS  2.0

#define CH_BUF_SZ (4*COHPSK_NOM_SAMPLES_PER_FRAME)

/* This file gets generated using the function write_noise_file in tcohpsk.m.  You have to run
   tcohpsk first (any variant) to load the function into Octave, e.g.:

  octave:17> tcohpsk
  octave:18> write_noise_file("../raw/fast_fading_samples.float", 7500, 7500*60)
  octave:19> write_noise_file("../raw/slow_fading_samples.float", 75000, 7500*60)
*/

#define FADING_FILE_NAME "../../raw/fading_samples.float"

int main(int argc, char *argv[])
{
    struct COHPSK *coh;
    int            tx_bits[COHPSK_BITS_PER_FRAME];
    COMP           tx_fdm[COHPSK_NOM_SAMPLES_PER_FRAME];
    COMP           ch_fdm[COHPSK_NOM_SAMPLES_PER_FRAME];
    COMP           ch_buf[CH_BUF_SZ];
    float          rx_bits_sd[COHPSK_BITS_PER_FRAME];
    char           rx_bits_char[COHPSK_BITS_PER_FRAME];
    float         *rx_amp_log;
    float         *rx_phi_log;
    COMP          *rx_symb_log;

    int            f, r, i;
    COMP           phase_ch;
    int            noise_r, noise_end;
    int            bit_errors;
    int            state, nerrors, nbits, reliable_sync_bit;
    float          EsNo, variance;
    COMP           scaled_noise;
    float          EsNodB, foff_hz;
    int            fading_en, nhfdelay, ret, nin_frame, frames, framesl;
    float          hf_gain;
    COMP          *ch_fdm_delay, aspread, aspread_2ms, delayed, direct;
    FILE          *ffading, *fout;
    int            ch_buf_n;
    float          tx_pwr, rx_pwr, noise_pwr;
    short          error_pattern[COHPSK_BITS_PER_FRAME];
    int            log_data_r, c, j, tmp;

    EsNodB = ES_NO_DB;
    foff_hz =  FOFF_HZ;
    fading_en = 0;
    frames = FRAMES;
    if (argc == 5) {
        EsNodB = atof(argv[1]);
        foff_hz = atof(argv[2]);
        fading_en = atoi(argv[3]);
        frames = atoi(argv[4]);
    }
    fprintf(stderr, "EsNodB: %4.2f foff: %4.2f Hz fading: %d\n", EsNodB, foff_hz, fading_en);

    coh = cohpsk_create();
    assert(coh != NULL);

    framesl = SYNC_FRAMES*frames;
    coh->ch_symb_log_col_sz = COHPSK_NC*ND;
    coh->ch_symb_log = (COMP *)malloc(sizeof(COMP)*NSYMROWPILOT*framesl*coh->ch_symb_log_col_sz);
    assert(coh->ch_symb_log != NULL);

    rx_amp_log = (float *)malloc(sizeof(float)*frames*NSYMROW*COHPSK_NC*ND);
    assert(rx_amp_log != NULL);
    rx_phi_log = (float *)malloc(sizeof(float)*frames*NSYMROW*COHPSK_NC*ND);
    assert(rx_phi_log != NULL);
    rx_symb_log = (COMP *)malloc(sizeof(COMP)*frames*NSYMROW*COHPSK_NC*ND);
    assert(rx_symb_log != NULL);

    phase_ch.real = 1.0; phase_ch.imag = 0.0;
    noise_r = 0;
    noise_end = sizeof(noise)/sizeof(COMP);
    ch_buf_n = 0;

    log_data_r = 0;

    /*  each carrier has power = 2, total power 2Nc, total symbol rate
        NcRs, noise BW B=Fs Es/No = (C/Rs)/(N/B), N = var =
        2NcFs/NcRs(Es/No) = 2Fs/Rs(Es/No) */

    EsNo = pow(10.0, EsNodB/10.0);
    variance = 2.0*COHPSK_FS/(COHPSK_RS*EsNo);

    tx_pwr = rx_pwr = noise_pwr = 0.0;

    nerrors = 0;
    nbits = 0;

    /* init HF fading model */
    ffading = NULL;
    ch_fdm_delay = NULL;

    if (fading_en) {
        ffading = fopen(FADING_FILE_NAME, "rb");
        if (ffading == NULL) {
            printf("Can't find fading file: %s\n", FADING_FILE_NAME);
            exit(1);
        }
        nhfdelay = floor(HF_DELAY_MS*COHPSK_FS/1000);
        ch_fdm_delay = (COMP*)malloc((nhfdelay+COHPSK_NOM_SAMPLES_PER_FRAME)*sizeof(COMP));
        assert(ch_fdm_delay != NULL);
        for(i=0; i<nhfdelay+COHPSK_NOM_SAMPLES_PER_FRAME; i++) {
            ch_fdm_delay[i].real = 0.0;
            ch_fdm_delay[i].imag = 0.0;
        }

        /* first values in file are HF gains */

        for (i=0; i<4; i++)
            ret = fread(&hf_gain, sizeof(float), 1, ffading);
        printf("hf_gain: %f\n", hf_gain);
    }

    /* Main Loop ---------------------------------------------------------------------*/

    nin_frame = COHPSK_NOM_SAMPLES_PER_FRAME;
    for(f=0; f<frames; f++) {

	/* --------------------------------------------------------*\
	                          Mod
	\*---------------------------------------------------------*/

        cohpsk_get_test_bits(coh, tx_bits);
	cohpsk_mod(coh, tx_fdm, tx_bits);
        cohpsk_clip(tx_fdm);

        for(r=0; r<COHPSK_NOM_SAMPLES_PER_FRAME; r++) {
            tx_pwr += pow(tx_fdm[r].real, 2.0) + pow(tx_fdm[r].imag, 2.0);
        }

	/* --------------------------------------------------------*\
	                          Channel
	\*---------------------------------------------------------*/

        fdmdv_freq_shift(ch_fdm, tx_fdm, foff_hz, &phase_ch, COHPSK_NOM_SAMPLES_PER_FRAME);

        /* optional HF fading -------------------------------------*/

        if (fading_en) {

            /* update delayed signal buffer */

            for(i=0; i<nhfdelay; i++)
                ch_fdm_delay[i] = ch_fdm_delay[i+COHPSK_NOM_SAMPLES_PER_FRAME];
            for(j=0; j<COHPSK_NOM_SAMPLES_PER_FRAME; i++, j++)
                ch_fdm_delay[i] = ch_fdm[j];

            /* combine direct and delayed paths, both multiplied by
               "spreading" (doppler) functions */

            for(i=0; i<COHPSK_NOM_SAMPLES_PER_FRAME; i++) {
                ret = fread(&aspread, sizeof(COMP), 1, ffading);
                assert(ret == 1);
                ret = fread(&aspread_2ms, sizeof(COMP), 1, ffading);
                assert(ret == 1);
                //printf("%f %f %f %f\n", aspread.real, aspread.imag, aspread_2ms.real, aspread_2ms.imag);

                direct    = cmult(aspread, ch_fdm[i]);
                delayed   = cmult(aspread_2ms, ch_fdm_delay[i]);
                ch_fdm[i] = fcmult(hf_gain, cadd(direct, delayed));
            }
        }

        for(i=0; i<COHPSK_NOM_SAMPLES_PER_FRAME; i++) {
            rx_pwr += pow(ch_fdm[i].real, 2.0) + pow(ch_fdm[i].imag, 2.0);
        }

        /* AWGN noise ------------------------------------------*/

        for(r=0; r<COHPSK_NOM_SAMPLES_PER_FRAME; r++) {
            scaled_noise = fcmult(sqrt(variance), noise[noise_r]);
            ch_fdm[r] = cadd(ch_fdm[r], scaled_noise);
            noise_pwr += pow(noise[noise_r].real, 2.0) + pow(noise[noise_r].imag, 2.0);
            noise_r++;
            if (noise_r > noise_end) {
                noise_r = 0;
                //fprintf(stderr, "  [%d] noise wrap\n", f);
            }

        }

        /* buffer so we can let demod Fs offset code do it's thing */

        memcpy(&ch_buf[ch_buf_n], ch_fdm, sizeof(COMP)*COHPSK_NOM_SAMPLES_PER_FRAME);
        ch_buf_n += COHPSK_NOM_SAMPLES_PER_FRAME;
        assert(ch_buf_n < CH_BUF_SZ);

 	/* --------------------------------------------------------*\
	                          Demod
	\*---------------------------------------------------------*/

        coh->frame = f;
        tmp = nin_frame;
 	cohpsk_demod(coh, rx_bits_sd, &reliable_sync_bit, ch_buf, &tmp);

        ch_buf_n -= nin_frame;
        //printf("nin_frame: %d tmp: %d ch_buf_n: %d\n", nin_frame, tmp, ch_buf_n);
        assert(ch_buf_n >= 0);
        if (ch_buf_n)
            memcpy(ch_buf, &ch_buf[nin_frame], sizeof(COMP)*ch_buf_n);
        nin_frame = tmp;

	
    	for (int i = 0; i < COHPSK_BITS_PER_FRAME; i++)
	{
           rx_bits_char[i] = rx_bits_sd[i];
	}

        cohpsk_put_test_bits(coh, &state, error_pattern, &bit_errors, rx_bits_char);
        nerrors += bit_errors;
        nbits   += COHPSK_BITS_PER_FRAME;

        if (state) {
            for(r=0; r<NSYMROW; r++, log_data_r++) {
                for(c=0; c<COHPSK_NC*ND; c++) {
                    rx_amp_log[log_data_r*COHPSK_NC*ND+c] = coh->amp_[r][c];
                    rx_phi_log[log_data_r*COHPSK_NC*ND+c] = coh->phi_[r][c];
                    rx_symb_log[log_data_r*COHPSK_NC*ND+c] = coh->rx_symb[r][c];
                }
            }
        }

    }

    printf("%4.3f %d %d\n", (float)nerrors/nbits, nbits, nerrors);
    printf("tx var: %f noise var: %f rx var: %f\n",
           tx_pwr/(frames*COHPSK_NOM_SAMPLES_PER_FRAME),
           noise_pwr/(frames*COHPSK_NOM_SAMPLES_PER_FRAME),
           rx_pwr/(frames*COHPSK_NOM_SAMPLES_PER_FRAME)
           );
    if (fading_en) {
        free(ch_fdm_delay);
        fclose(ffading);
    }
    cohpsk_destroy(coh);

    fout = fopen("test_cohpsk_ch_out.txt","wt");
    octave_save_complex(fout, "ch_symb_log_c", (COMP*)coh->ch_symb_log, coh->ch_symb_log_r, COHPSK_NC*ND, COHPSK_NC*ND);
    octave_save_float(fout, "rx_amp_log_c", (float*)rx_amp_log, log_data_r, COHPSK_NC*ND, COHPSK_NC*ND);
    octave_save_float(fout, "rx_phi_log_c", (float*)rx_phi_log, log_data_r, COHPSK_NC*ND, COHPSK_NC*ND);
    octave_save_complex(fout, "rx_symb_log_c", (COMP*)rx_symb_log, log_data_r, COHPSK_NC*ND, COHPSK_NC*ND);
    fclose(fout);

    return 0;
}

