/* -*- 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 <gnuradio/io_signature.h>
#include "magnitude_equalizer_vcc_impl.h"

namespace gr {
  namespace dab {

magnitude_equalizer_vcc::sptr
magnitude_equalizer_vcc::make(unsigned int vlen, unsigned int num_symbols)
{
  return gnuradio::get_initial_sptr
    (new magnitude_equalizer_vcc_impl(vlen, num_symbols));
}

magnitude_equalizer_vcc_impl::magnitude_equalizer_vcc_impl(unsigned int vlen, unsigned int num_symbols)
  : gr::sync_block("magnitude_equalizer_vcc",
             gr::io_signature::make (1, 1, sizeof(gr_complex)*vlen),
             gr::io_signature::make (1, 1, sizeof(gr_complex)*vlen)),
  d_vlen(vlen), d_num_symbols(num_symbols)
{
  assert(d_num_symbols>0);

  d_equalizer = new float[vlen];
  for (unsigned int i=0;i<vlen;i++)
    d_equalizer[i] = 1;

  set_history(d_num_symbols);
  set_tag_propagation_policy(TPP_DONT); // We need to handle the tag propagation "manually"
                                        // because of set_history(d_num_symbols)
  d_add_item_tag_at = -1;
}

magnitude_equalizer_vcc_impl::~magnitude_equalizer_vcc_impl(void)
{
  delete [] d_equalizer;
}

void magnitude_equalizer_vcc_impl::update_equalizer(const gr_complex *in) 
{
  for (unsigned int i=0; i<d_vlen; i++) 
    d_equalizer[i] = std::abs(in[i]);

  if (d_num_symbols>1) {
    for (unsigned int i=1; i<d_num_symbols; i++) {
      in += d_vlen;
      for (unsigned int j=0; j<d_vlen; j++) 
        d_equalizer[j] += std::abs(in[j]);
    }
  }

  for (unsigned int i=0; i<d_vlen; i++) 
    d_equalizer[i] = (float)d_num_symbols/d_equalizer[i];
  
  // for (unsigned int i=0; i<d_vlen; i++) 
    // printf("%e, ", d_equalizer[i]);
  // printf(" <-- equalizer\n");
}

int 
magnitude_equalizer_vcc_impl::work(int noutput_items,
                        gr_vector_const_void_star &input_items,
                        gr_vector_void_star &output_items)
{
  gr_complex const *in = (const gr_complex *) input_items[0];
  gr_complex *out = (gr_complex *) output_items[0];

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

  // If there were not enough samples to be produced in the previous call to work(..),
  // we need to add tags in the following call:
  if (d_add_item_tag_at >= 0) {
      if (d_add_item_tag_at < noutput_items) {
          add_item_tag(0, nitems_written(0) + d_add_item_tag_at, pmt::intern("first"), pmt::intern(""), pmt::intern("magnitude_equalizer"));
          d_add_item_tag_at = -1;
      }
      else {
          d_add_item_tag_at = d_add_item_tag_at - noutput_items;
      }
  }
  //

  // Get all stream tags with key "first", and make a vector of the positions.
  std::vector<tag_t> tags;
  get_tags_in_range(tags, 0, nitems_read(0), nitems_read(0) + noutput_items, pmt::mp("first"));
  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) { /* there was a trigger signal d_num_symbols-1 symbols before -> update equalizer */
      // Action when stream tags is found:
      update_equalizer(in);
      if ((i + d_num_symbols-1) < noutput_items)
          add_item_tag(0, nitems_written(0) + i+d_num_symbols-1, pmt::intern("first"), pmt::intern(""), pmt::intern("magnitude_equalizer"));
      else {
          d_add_item_tag_at = (i + d_num_symbols -1) - noutput_items;
      }
      //

      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];
      }
    }
  
    for (unsigned int j=0;j<d_vlen;j++)
      out[j] = in[j]*d_equalizer[j];
   
    in  += d_vlen;
    out += d_vlen;
  }
    
  return noutput_items;
}

}
}
