/* A simple approach to FMradio.
 *
 * The code was addapted from Marco Cornero Aug'06 
 * for Acotes project. This version ensures to have
 * the same result that the stream one for plain C.
 * 
 * Author: David Rodenas-Pico, BSC
 * Creation Date: March 2007
 */


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


#define KSMPS 1024
#define FM_MAX       5
#define FM_LIMIT     (65536/2)-1 




/* *********************************************** complex operations   */

typedef struct
{
  float real;
  float imag;
} complex;

#define M_PI   3.14159265358979323846  /* pi */
#define GAIN_LMR  2000

#define complex_conj(s, d)			\
  {						\
    (d)->real = (s)->real;			\
    (d)->imag = -1 * (s)->imag;			\
  }

#define complex_arg(x)				\
  atan2((x)->imag, (x)->real)

#define complex_mul(a, b, d)						\
  {									\
    (d)->real = (a)->real * (b)->real - ((a)->imag * (b)->imag);	\
    (d)->imag = (a)->real * (b)->imag + (a)->imag * (b)->real;		\
  }


/* *********************************************** auxiliar functions   */

short 
dac_cast_trunc_and_normalize_to_short(float f)
{
  short x;
  f = (f / FM_MAX)*FM_LIMIT;
  x = f;
  return x;
}

#if 0
/* *********************************************** file filters         */

void
file_to_stream(FILE* file, stream_t* output)
{
  int i;
  int room;
  float value;

  room= stream_room(output);
  for (i= 0; i < room; i++) {
    if (fread(&value, sizeof(value), 1, file)) {
      stream_push(output, &value);
    }
  }
}

void
stream_to_file(stream_t* input, FILE* file)
{
  int i;
  int count;
  float value;
  short res_int;
  
  count= stream_count(input);
  for (i= 0; i < count; i++)
    {
      stream_pop(input, &value);
      //fwrite(&value, sizeof(value), 1, file);
      res_int= dac_cast_trunc_and_normalize_to_short(value);
      fwrite(&res_int, sizeof(res_int), 1, file);
    }
}
#endif


/* *********************************************** fm_quad_demod        */

typedef struct {
  float history[2];
} fm_quad_demod_filter;

void
fm_quad_demod_init(fm_quad_demod_filter* filter)
{
  filter->history[0]= 0;
  filter->history[1]= 0;
}

void
fm_quad_demod(fm_quad_demod_filter* filter, float i1, float i2, float* result)
{
  complex x_N;
  complex x_N_1;
  complex x_N_1_conj;
  complex y_N;
  float demod, d_gain = 0.5;

  /*
   *
   * y(n) = angle(x(n)*conj(x(n-1))
   *
   */

  /* read two complex data */
  x_N.real = i2; x_N.imag = i1;
  x_N_1.real = filter->history[1]; x_N_1.imag = filter->history[0];
  
  /* compute */
  complex_conj(& x_N_1, & x_N_1_conj);
  complex_mul(& x_N_1_conj, & x_N, & y_N); 
  demod = d_gain * complex_arg(& y_N); 

  filter->history[0]= i1;
  filter->history[1]= i2;

  *result= demod;
}



/* *********************************************** multiply_square      */

void 
multiply_square(float i1, float i2, float* result)
{
  *result= GAIN_LMR * i1 * i2 * i2;
}


/* *********************************************** ntaps_filter         */

typedef struct ntaps_filter_conf
{
  double* coeff;
  int decimation;
  int taps;
  float*  history;
  int next;
} ntaps_filter_conf;

#define WIN_HAMMING     0
#define WIN_HANNING     1
#define WIN_BLACKMAN    2

void 
compute_window(unsigned int ntaps, double *taps, unsigned int type)
{
  int n;
  int M = ntaps - 1;    // filter order

  if(type == WIN_HAMMING)
    {
      for (n = 0; n < ntaps; n++)
        taps[n] = 0.54 - 0.46 * cos ((2 * M_PI * n) / M);
    }
  else if(type == WIN_HANNING)
    {
      for (n = 0; n < ntaps; n++)
        taps[n] = 0.5 - 0.5 * cos ((2 * M_PI * n) / M);
    }
  else if(type == WIN_BLACKMAN)
    {
      for (n = 0; n < ntaps; n++)
        taps[n] = 0.42 - 0.50 * cos ((2*M_PI * n) / (M-1)) - 0.08 * cos ((4*M_PI * n) / (M-1));
    }
}

int 
compute_ntaps (float sampling_freq,
	       float transition_width,
	       int window_type)
{
  /* Mormalized transition width */
  float delta_f = transition_width / sampling_freq;

  float width_factor[3] = { 3.3, 3.1, 5.5 };

  /* compute number of taps required for given transition width */
  int ntaps = (int) (width_factor[window_type]/ delta_f + 0.5);

  if ((ntaps & 1) == 0)
    ntaps++;

  return ntaps;
}

void 
ntaps_filter_ffd_init(ntaps_filter_conf *conf, 
		      double cutoff_freq, 
		      double transition_band,
		      double gain,
		      int decimation,
		      double sampling_rate, 
		      int window_type)
{
  /* Taken from the GNU software radio .. */

  int n;
  int i;
  unsigned int ntaps;
  double* w;
  int M;
  double fwT0;
  double fmax;

  ntaps = compute_ntaps(sampling_rate, transition_band, window_type);
  conf->coeff = malloc(ntaps * sizeof(double));
  w = malloc(ntaps * sizeof(double));
  conf->taps = ntaps;
  conf->decimation = decimation;

  compute_window(ntaps, w, window_type);

  M = (ntaps - 1) / 2;

  fwT0 = 2 * M_PI * cutoff_freq / sampling_rate;

  for (n=0; n<ntaps; n++)
    conf->coeff[n]=0.0;

  for (n = -M; n <= M; n++) {
    if (n == 0)
      conf->coeff[n + M] = fwT0 / M_PI * w[n + M];
    else
      conf->coeff[n + M] =  sin (n * fwT0) / (n * M_PI) * w[n + M];
  }

  fmax = conf->coeff[0 + M];

  for (n = 1; n <= M; n++)
    fmax += 2 * conf->coeff[n + M];

  gain /= fmax; // normalize

  for (i = 0; i < ntaps; i++)
    conf->coeff[i] *= gain;
  
  // init history
  conf->next= 0;
  conf->history= (float*)malloc(sizeof(float) * conf->taps);
  for (n= 0; n < conf->taps; n++)
    {
      conf->history[n]= 0;
    }

  free(w);
}


void
ntaps_filter_ffd(ntaps_filter_conf* conf
		 , int input_size, float input[]
		 , float* result)
{
  int i;
  float sum;
  
  assert(input_size == conf->decimation);

  for (i= 0; i < conf->decimation; i++)
    {
      conf->history[conf->next]= input[i];
      conf->next= (conf->next + 1) % conf->taps;
    }
  
  sum= 0.0;
  for (i= 0; i < conf->taps; i++)
    {
      sum= sum 
	+ conf->history[(conf->next + i) % conf->taps] 
	* conf->coeff[conf->taps - i - 1];
    }

  *result= sum;
}


/* *********************************************** stereo_sum           */

void
stereo_sum(float data_spm, float data_smm, float* left, float* right)
{
  *left = (data_spm+data_smm);
  *right= (data_spm-data_smm);
}


/* *********************************************** subctract            */

void 
subctract(float i1, float i2, float* result)
{
  *result= i1 - i2;
}


/* *********************************************** program options      */

int
read_opts(int argc, char* argv[], 
	  FILE** input_file, 
	  FILE** output_file, 
	  FILE** text_file,
	  int* final_audio_frequency)
{
  char  input_filename[4096];
  char  output_filename[4096];
  char  text_filename[4096];


  /* arguments parsing */
  if ((argc < 1) || (argc > 3)) 
    {
      printf("Usage: fm_rcv [sample_file_name [audio_frequency]]\n");
      return 1;
    }
  if (argc == 1)
    {
      strcpy(input_filename, "input.dat");
    }
  else
    {
      strcpy(input_filename, argv[1]);
    }
  sprintf(output_filename,"%s.raw",argv[0]);
  sprintf(text_filename,"%s.txt",argv[0]);
  if(argc == 3)
    *final_audio_frequency = atoi(argv[2]);


  *input_file= fopen(input_filename, "r");
  if (*input_file == NULL) {
    printf("Cannot open input file %s\n", input_filename);
    return 1;
  }
  *output_file= fopen(output_filename, "w");
  if (*output_file == NULL) {
    printf("Cannot open output file %s\n", output_filename);
    return 1;
  }
  *text_file= fopen(text_filename, "w");
  if (*text_file == NULL) {
    printf("Cannot open output file %s\n", text_filename);
    return 1;
  }

  
  return 0;
}



/* *********************************************** main                 */

int 
main(int argc, char* argv[])
{
  ntaps_filter_conf lp_2_conf;
  ntaps_filter_conf lp_11_conf, lp_12_conf;
  ntaps_filter_conf lp_21_conf, lp_22_conf;
  ntaps_filter_conf lp_3_conf;
  fm_quad_demod_filter fm_qd_conf;

  int final_audio_frequency = 64*KSMPS;
  float input_rate = 512 * KSMPS;

  float inout_ratio;

  FILE* input_file;
  FILE* output_file;
  FILE* text_file;

  float read_buffer[16];
  float fm_qd_buffer[8];
  float ffd_buffer[8];

  float band_2;
  float band_3;
  float band_11[8];
  float band_12[8];
  float resume_1[8];
  float band_21[8];
  float band_22[8];
  float resume_2[8];
  
  float output1, output2;
  short output_short[2];

  int i=0;
  int count=0;


  if (read_opts(argc, argv,&input_file,&output_file,&text_file, &final_audio_frequency))
    {
      return 1;
    }

  inout_ratio = ((int) input_rate)/final_audio_frequency;

  ntaps_filter_ffd_init(&lp_2_conf,  15000, 4000, 0.5, inout_ratio, input_rate, WIN_HANNING);
  ntaps_filter_ffd_init(&lp_11_conf, 53000, 4000, 1,   1,           input_rate, WIN_HANNING);
  ntaps_filter_ffd_init(&lp_12_conf, 23000, 4000, 1,   1,           input_rate, WIN_HANNING);
  ntaps_filter_ffd_init(&lp_21_conf, 21000, 2000, 1,   1,           input_rate, WIN_HANNING);
  ntaps_filter_ffd_init(&lp_22_conf, 17000, 2000, 1,   1,           input_rate, WIN_HANNING);
  ntaps_filter_ffd_init(&lp_3_conf,  15000, 4000, 1.0, inout_ratio, input_rate, WIN_HANNING);
  fm_quad_demod_init (&fm_qd_conf);


#pragma omp parallel num_threads (2) default (none) \
  shared (input_file, output_file, text_file, lp_2_conf, lp_11_conf, lp_12_conf, lp_21_conf, lp_22_conf, lp_3_conf, fm_qd_conf) \
  private (read_buffer, fm_qd_buffer, ffd_buffer, band_2, band_3, band_11, band_12, resume_1, band_21, band_22, resume_2, output1, output2, output_short, i, count)
  {
#pragma omp single
    {
      while (16 == fread (read_buffer, sizeof(float), 16, input_file))
	{

#pragma omp task firstprivate (read_buffer) output (fm_qd_buffer) shared (fm_qd_conf) private (i)
	  for (i = 0; i < 8; i++)
	    fm_quad_demod (&fm_qd_conf, read_buffer[2*i], read_buffer[2*i + 1], &fm_qd_buffer[i]);

#pragma omp task input (fm_qd_buffer) output (band_11) shared (lp_11_conf) private (i) num_threads (2)
	  for (i = 0; i < 8; i++)
	    ntaps_filter_ffd (&lp_11_conf, 1, &fm_qd_buffer[i], &band_11[i]);

#pragma omp task input (fm_qd_buffer) output (band_12) shared (lp_12_conf) private (i)
	  for (i = 0; i < 8; i++)
	    ntaps_filter_ffd (&lp_12_conf, 1, &fm_qd_buffer[i], &band_12[i]);

#pragma omp task input (band_11, band_12) output (resume_1) private (i)
	  for (i = 0; i < 8; i++)
	    subctract (band_11[i], band_12[i], &resume_1[i]);



#pragma omp task input (fm_qd_buffer) output (band_21) shared (lp_21_conf) private (i)
	  for (i = 0; i < 8; i++)
	    ntaps_filter_ffd (&lp_21_conf, 1, &fm_qd_buffer[i], &band_21[i]);

#pragma omp task input (fm_qd_buffer) output (band_22) shared (lp_22_conf) private (i)
	  for (i = 0; i < 8; i++)
	    ntaps_filter_ffd (&lp_22_conf, 1, &fm_qd_buffer[i], &band_22[i]);

#pragma omp task input (band_21, band_22) output (resume_2) private (i)
	  for (i = 0; i < 8; i++)
	    subctract (band_21[i], band_22[i], &resume_2[i]);

#pragma omp task input (resume_1, resume_2) output (ffd_buffer) private (i)
	  for (i = 0; i < 8; i++)
	    multiply_square (resume_1[i], resume_2[i], &ffd_buffer[i]);


#pragma omp task input (fm_qd_buffer) output (band_2) shared (lp_2_conf)
	  ntaps_filter_ffd (&lp_2_conf, 8, fm_qd_buffer, &band_2);

#pragma omp task input (ffd_buffer) output (band_3) shared (lp_3_conf)
	  ntaps_filter_ffd (&lp_3_conf, 8, ffd_buffer, &band_3);

#pragma omp task input (band_2, band_3) output (output1, output2)
	  stereo_sum (band_2, band_3, &output1, &output2);

#pragma omp task input (output1, output2) shared (output_file, text_file) private (output_short)
	  {
	    output_short[0] = dac_cast_trunc_and_normalize_to_short (output1);
	    output_short[1] = dac_cast_trunc_and_normalize_to_short (output2);
	    fwrite (output_short, sizeof(short), 2, output_file);
	    //fprintf (text_file, "%-10.5f %-10.5f\n", output1, output2);
	    fprintf (text_file, "%-10.5f %-10.5f \t %10d %10d\n", output1, output2, output_short[0], output_short[1]);
	  }
	}
    }
  }
  fclose (input_file);
  fclose (output_file);
  fclose (text_file);

  return 0;
}