/* -*- c++ -*- */
/*
 * Copyright 2004 Free Software Foundation, Inc.
 * 
 * This file is part of GNU Radio
 * 
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 * 
 * GNU Radio 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 General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

/*
 * config.h is generated by configure.  It contains the results
 * of probing for features, options etc.  It should be the first
 * file included in your .cc file.
 */
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <stdio.h>

#include <gnuradio/io_signature.h>
#include "ofdm_ffe_all_in_one_impl.h"
#include <gnuradio/math.h>

namespace gr {
  namespace dab {
/*
 * Create a new instance of dab_ofdm_ffe_all_in_one and return
 * a boost shared_ptr.  This is effectively the public constructor.
 */
ofdm_ffe_all_in_one::sptr
ofdm_ffe_all_in_one::make(unsigned int symbol_length, unsigned int fft_length, unsigned int num_symbols, float alpha, unsigned int sample_rate)
{
  return gnuradio::get_initial_sptr
    (new ofdm_ffe_all_in_one_impl(symbol_length, fft_length, num_symbols, alpha, sample_rate));
}

ofdm_ffe_all_in_one_impl::ofdm_ffe_all_in_one_impl(unsigned int symbol_length, unsigned int fft_length, unsigned int num_symbols, float alpha, unsigned int sample_rate)
  : gr::sync_block("ofdm_ffe_all_in_one",
             gr::io_signature::make (1, 1, sizeof(gr_complex)),
             gr::io_signature::make (1, 1, sizeof(float))),
  d_symbol_length(symbol_length), d_fft_length(fft_length), d_num_symbols(num_symbols), d_alpha(alpha), d_sample_rate(sample_rate), d_cur_symbol(num_symbols), d_cur_sample(0), d_ffs_error_sum(0), d_estimated_error(0), d_estimated_error_per_sample(0)
{
  assert(symbol_length<=2*fft_length); /* cyclic prefix can not be longer than fft_length .. */
  set_history(symbol_length+1);
  set_tag_propagation_policy(TPP_DONT);
}

float 
ofdm_ffe_all_in_one_impl::calc_ffe_estimate(const gr_complex *in) {
  gr_complex sum = 0;
  int cp_length = d_symbol_length - d_fft_length;

  // for (int i=-cp_length;i<0;i++)
  //   sum += in[i-d_fft_length+d_symbol_length] * conj(in[i+d_symbol_length]);
  for (int i=0;i<cp_length;i++)
    sum += in[i] * conj(in[i+d_fft_length]);

  return gr::fast_atan2f(sum);
}


int
ofdm_ffe_all_in_one_impl::work(int noutput_items,
          gr_vector_const_void_star &input_items,
          gr_vector_void_star &output_items)
{
  const gr_complex *iptr = (const gr_complex *) input_items[0];
  float *optr = (float *) output_items[0];

  
  float new_estimate;

  std::vector<int> tag_positions;
  int next_tag_position = -1;
  int next_tag_position_index = -1;

  // Get all stream tags with key "dab_sync", and make a vector of the positions.
  // "next_tag_position" contains the position within "iptr where the next "dab_sync" stream tag is found
  std::vector<tag_t> tags;
  get_tags_in_range(tags, 0, nitems_read(0), nitems_read(0) + noutput_items, pmt::mp("dab_sync"));
  for(int i=0;i<tags.size();i++) {
      int current;
      current = tags[i].offset - nitems_read(0);
      tag_positions.push_back(current);
      next_tag_position_index = 0;
  }
  if(next_tag_position_index >= 0) {
      next_tag_position = tag_positions[next_tag_position_index];
  }


  for (int i=0; i<noutput_items; i++) {
    if (next_tag_position == i) { /* new frame starts */
      // Action when stream tags is found:
      d_cur_symbol = 0;
      d_cur_sample = 0;
      d_ffs_error_sum = 0;
      //

      next_tag_position_index++;
      if (next_tag_position_index == tag_positions.size()) {
        next_tag_position_index = -1;
        next_tag_position = -1;
      }
      else {
        next_tag_position = tag_positions[next_tag_position_index];
      }
    }

    
    if (d_cur_sample==d_symbol_length) { /* new symbol starts */
      d_cur_sample = 0;

      if (d_cur_symbol<d_num_symbols) {
        new_estimate = calc_ffe_estimate(iptr);

        if (d_cur_symbol>0) {
          if (d_ffs_error_sum < 0 && new_estimate > 0 && new_estimate - d_ffs_error_sum/d_cur_symbol > M_PI)
            new_estimate -= 2*M_PI;
          else if (d_ffs_error_sum > 0 && new_estimate < 0 && d_ffs_error_sum/d_cur_symbol - new_estimate > M_PI)
            new_estimate += 2*M_PI;
        }

        d_ffs_error_sum += new_estimate;
      }

      if (d_cur_symbol == d_num_symbols-1) { /* update estimated error */
        d_ffs_error_sum /= d_num_symbols; /* average */

        /* if the offset is close to half of the subcarrier bandwidth, it may
         * jump from some large positive value to some large negative value.
         * with averaging, this is a problem - we have to detect it (although
         * it really only makes a difference when the offset is very close to
         * half the subcarrier bandwidth)
         
         * note: if there is an offset of one subcarrier bandwidth, the phase
         * offset in fft_length samples is 2pi */
        if (d_estimated_error < 0 && d_ffs_error_sum > 0 && d_ffs_error_sum - d_estimated_error > M_PI) {
          fprintf(stderr, "ofdm_ffe_all_in_one: switch detected: neg -> pos\n");
          d_estimated_error += 2*M_PI; 
        } else if (d_estimated_error > 0 && d_ffs_error_sum < 0 && d_estimated_error - d_ffs_error_sum > M_PI) {
          fprintf(stderr, "ofdm_ffe_all_in_one: switch detected: pos -> neg\n");
          d_estimated_error -= 2*M_PI; 
        }

        /* the following distinction is not really needed; but without it,
         * simulation would need to run much longer, because the
         * synchronisation would need time to adjust to the offset */
        if (d_estimated_error == 0)
          d_estimated_error = d_ffs_error_sum; /* first time -> fast adjustment */
        else
          d_estimated_error = d_alpha*d_ffs_error_sum + (1-d_alpha)*d_estimated_error; /* slow adjustment */

        d_estimated_error_per_sample = d_estimated_error / (float)d_fft_length;
//        fprintf(stderr, "ofdm_ffe_all_in_one: d_estimated_error: %f (%3.2f Hz)\n", d_estimated_error, d_estimated_error_per_sample*d_sample_rate/(2*M_PI));
      }

      d_cur_symbol++;
    } 

    d_cur_sample++;

    *optr++ = d_estimated_error_per_sample;
    iptr++;
  }

  return noutput_items;
}

}
}
