/* -*- c++ -*- */

/* 
 * Copyright (c) 2015, Dennis Glatting.
 *
 * This file is the successor of the others, which were test
 * constructions. This file is meant to more closely resemble
 * dc_blocker in GNURadio 3.7.8 as a potential replacement.
 *
 *
 * This 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.
 * 
 * This software 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 this software; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 *
 *
 * $Log: main_dc.cc,v $
 * Revision 1.1  2015/07/24 06:04:03  dennisg
 * Initial revision
 *
 */

extern "C" {

#include <assert.h>
#include <stdlib.h>

}

#include <complex>
#include <cstddef>
#include <cstdlib>
#include <deque>
#include <iostream>
#include <memory>
#include <vector>

#include <gnuradio/gr_complex.h>
#include <gnuradio/high_res_timer.h>


// Cut down on some of the typing.
//

typedef float DTYPE;
typedef std::complex<DTYPE> CPLX;


// This template defines the moving average class.
//

template<typename T>
class moving_average_t {

private:

  // Relationships of the following variables:
  //
  //  d_len   = d_delay_line.size().
  //  d_index = (d_index % (d_len-1))
  //
  
  // The length of the moving average queue and the current index into
  // it.
  //
  
  size_t d_len, d_index;

  // The moving average delay line.
  //
  // The delay line is a vector of type T, length d_len-1, and where
  // d_index points to the oldest entry.
  //
  
  std::vector<T> d_delay_line;

  T d_out, d_out_d2;
  
  // Rather than returning a constructed sample from filter(), which
  // may have construction/destruction overhead, store the sample here
  // and return a const reference.
  //

  T d_temp;

  // Set the length of the delay line, reinitializing content.
  //

  void set_len( size_t len );
  
public:

           moving_average_t( void ) = delete;
           moving_average_t( size_t len );
  virtual ~moving_average_t( void );

  moving_average_t( const moving_average_t& t ) = delete;
  moving_average_t& operator=( const moving_average_t& t ) = delete;

  // Add a sample to the delay line and return the oldest and averaged
  // sample (removed from the delay line).
  //
  
  const T& filter( const T& t );
  
  // The oldest, unadjusted (i.e., averaged) signal in the delay line.
  //
  
  const T& delayed_sig( void ) const noexcept;
  
};

template<typename T>
moving_average_t<T>::moving_average_t( size_t len )
  : d_len( 0 ), d_index( 0 ),
    d_out( 0 ), d_out_d2( 0 ),
    d_temp( 0 ) {

  set_len( len );
}

template<typename T>
moving_average_t<T>::~moving_average_t( void ) {}
  
template<typename T>
void
moving_average_t<T>::set_len( size_t len ) {

  d_len   = len;
  d_index = 0;
  d_temp  = 0;
  
  d_out = d_out_d2 = T( 0 );

  d_delay_line.clear();
  for( size_t i = 0; i < d_len - 1; ++i )
    d_delay_line.push_back( T(0));

}

template<typename T>
inline const T&
moving_average_t<T>::delayed_sig( void ) const noexcept {

  return d_out;
}

template<typename T>
inline const T&
moving_average_t<T>::filter( const T& t ) {

  T d_out_d1 ( d_out );

  // Cache the oldest signal used in the average.
  //
  
  d_out = d_delay_line[d_index];

  // Stuff the passed sample into the delay line and bump the index.
  //

  d_delay_line[d_index] = t;
  d_index = (( d_index + 1 ) % ( d_len - 1 ));

  // Do the math.
  //

  d_out_d2 = (t - d_out_d1 + d_out_d2 );
  d_temp   = d_out_d2 / (float)(d_len);

  return d_temp;
}

template<typename T>
class dc_blocker_t {

private:

  // The length of the delay line and an index into it. Unused in
  // short form because the delay line is unused in short form.
  //
  
  size_t d_len, d_index;

  // The moving average stages. Only half are used in short form.
  //
  
  moving_average_t<T> d_ma_0;
  moving_average_t<T> d_ma_1;
  moving_average_t<T> d_ma_2;
  moving_average_t<T> d_ma_3;

  // My own delay line? That's now five delay lines in long form
  // (unused in short form).
  //
  
  std::vector<T> d_delay_line;

  // Long or short form? Specifically, half the moving averages?
  //
  
  bool d_long_form;
  
public:
  
           dc_blocker_t( void ) = delete;
           dc_blocker_t( size_t len, bool long_form = true );
  virtual ~dc_blocker_t( void );


  dc_blocker_t( const dc_blocker_t& d ) = delete;
  dc_blocker_t& operator=( const dc_blocker_t& d ) = delete;
  
  void work( const std::vector<T>& in, T* out );

  int group_delay( void ) const noexcept;
  
};
  
template<typename T>
dc_blocker_t<T>::dc_blocker_t( size_t len, bool long_form )
  : d_len( len ), d_index( 0 ),
    d_ma_0( len ), d_ma_1( len ), d_ma_2( len ), d_ma_3( len ),
    d_long_form( long_form ) {

  d_delay_line.clear();
  for( size_t i = 0; i < len - 1; ++i )
    d_delay_line.push_back( T(0));

}

template<typename T>
dc_blocker_t<T>::~dc_blocker_t( void ) {}
  
template<typename T>
void
dc_blocker_t<T>::work( const std::vector<T>& in, T* out ) {

  assert( in.size());
  assert( out );

  if( d_long_form ) {
    for( size_t i = 0; i < in.size(); ++i ) {
      
      T y1, y2, y3, y4, d;
      
      y1 = d_ma_0.filter( in[i]);
      y2 = d_ma_1.filter( y1 );
      y3 = d_ma_2.filter( y2 );
      y4 = d_ma_3.filter( y3 );
      
      d = d_delay_line[d_index];
      
      d_delay_line[d_index] = d_ma_0.delayed_sig();
      d_index = (( d_index + 1 ) % ( d_len - 1 ));
      
      out[i] = d - y4;
    }
  } else {
    for( size_t i = 0; i < in.size(); ++i ) {
      
      T y1, y2;
      
      y1 = d_ma_0.filter( in[i]);
      y2 = d_ma_1.filter( y1 );
      
      out[i] = d_ma_0.delayed_sig() - y2;
      
    }
  }
}

template<typename T>
inline int
dc_blocker_t<T>::group_delay( void ) const noexcept {

  if( d_long_form )
    return (2*d_len-2);
  else
    return d_len - 1;
}



// A copy of the old code to compare converted to a template to
// compare the newer templates against.
//

template<typename T>
class moving_averager_xx {
  
private:

  int d_length;

  T  d_out, d_out_d1, d_out_d2;
  std::deque<T> d_delay_line;

public:
  
   moving_averager_xx(int D);
  ~moving_averager_xx();

  const T  filter( const T& x );
  const T& delayed_sig() const { return d_out; }

};

template<typename T>
moving_averager_xx<T>::moving_averager_xx( int D )
  : d_length(D), d_out(0), d_out_d1(0), d_out_d2(0),
    d_delay_line( D - 1, T( 0 )) {

}

template<typename T>
moving_averager_xx<T>::~moving_averager_xx() {}

template<typename T>
inline const T
moving_averager_xx<T>::filter( const T& x ) {

  d_out_d1 = d_out;
  d_delay_line.push_back(x);
  d_out = d_delay_line[0];
  d_delay_line.pop_front();

  T y = x - d_out_d1 + d_out_d2;
  d_out_d2 = y;

  return (y / (float)(d_length));
  
}


class dc_blocker_cc_deque {

private:

  int  d_length;
  bool d_long_form;

  moving_averager_xx<CPLX>* d_ma_0;
  moving_averager_xx<CPLX>* d_ma_1;
  moving_averager_xx<CPLX>* d_ma_2;
  moving_averager_xx<CPLX>* d_ma_3;
  
  std::deque<CPLX> d_delay_line;

public:

   dc_blocker_cc_deque( int D, bool long_form = true );
  ~dc_blocker_cc_deque();

  int group_delay();

  void work( const std::vector<CPLX>& in, CPLX* out );

};

dc_blocker_cc_deque::dc_blocker_cc_deque(int D, bool long_form)
  : d_length(D), d_long_form(long_form) {

  d_ma_0 = new moving_averager_xx<CPLX>(D);
  d_ma_1 = new moving_averager_xx<CPLX>(D);
  d_ma_2 = new moving_averager_xx<CPLX>(D);
  d_ma_3 = new moving_averager_xx<CPLX>(D);
  d_delay_line = std::deque<CPLX>();

  for( int i = 0; i < d_length-1; ++i )
    d_delay_line.push_back( CPLX( 0.0, 0.0 ));

}

dc_blocker_cc_deque::~dc_blocker_cc_deque() {

  delete d_ma_0;
  delete d_ma_1;
  delete d_ma_2;
  delete d_ma_3;

}

void
dc_blocker_cc_deque::work( const std::vector<CPLX>& in, CPLX* out ) {

  assert( in.size());
  assert( out );

  if(d_long_form) {
    
    CPLX y1, y2, y3, y4, d;
    
    for(size_t i = 0; i < in.size(); i++) {
      
      y1 = d_ma_0->filter(in[i]);
      y2 = d_ma_1->filter(y1);
      y3 = d_ma_2->filter(y2);
      y4 = d_ma_3->filter(y3);
      
      d_delay_line.push_back(d_ma_0->delayed_sig());
      d = d_delay_line.front();
      d_delay_line.pop_front();
      
      out[i] = d - y4;

    }
  } else {

    CPLX y1, y2;

    for( size_t i = 0; i < in.size(); i++) {

      y1 = d_ma_0->filter(in[i]);
      y2 = d_ma_1->filter(y1);
      
      out[i] = d_ma_0->delayed_sig() - y2;

    }
  }
}


class dc_blocker_ff_deque {

private:

  int  d_length;
  bool d_long_form;

  moving_averager_xx<float>* d_ma_0;
  moving_averager_xx<float>* d_ma_1;
  moving_averager_xx<float>* d_ma_2;
  moving_averager_xx<float>* d_ma_3;

  std::deque<float> d_delay_line;

public:

   dc_blocker_ff_deque( int D, bool long_form = true );
  ~dc_blocker_ff_deque();

  int group_delay();

  void work( const std::vector<float>& in, float* out );

};

dc_blocker_ff_deque::dc_blocker_ff_deque(int D, bool long_form)
  : d_length(D), d_long_form(long_form) {

  d_ma_0 = new moving_averager_xx<float>(D);
  d_ma_1 = new moving_averager_xx<float>(D);
  d_ma_2 = new moving_averager_xx<float>(D);
  d_ma_3 = new moving_averager_xx<float>(D);
  d_delay_line = std::deque<float>();

  for( int i = 0; i < d_length-1; ++i )
    d_delay_line.push_back( 0.0 );

}

dc_blocker_ff_deque::~dc_blocker_ff_deque() {

  delete d_ma_0;
  delete d_ma_1;
  delete d_ma_2;
  delete d_ma_3;

}

void
dc_blocker_ff_deque::work( const std::vector<float>& in, float* out ) {

  assert( in.size());
  assert( out );

  if( d_long_form ) {
    
    float y1, y2, y3, y4, d;
    
    for(size_t i = 0; i < in.size(); i++) {
      
      y1 = d_ma_0->filter(in[i]);
      y2 = d_ma_1->filter(y1);
      y3 = d_ma_2->filter(y2);
      y4 = d_ma_3->filter(y3);
      
      d_delay_line.push_back(d_ma_0->delayed_sig());
      d = d_delay_line.front();
      d_delay_line.pop_front();

      out[i] = d - y4;

    }
  } else {

    float y1, y2;

    for( size_t i = 0; i < in.size(); i++) {

      y1 = d_ma_0->filter(in[i]);
      y2 = d_ma_1->filter(y1);

      out[i] = d_ma_0->delayed_sig() - y2;
      
    }
  }
}




// Silly little routine to consolidate common code. All this routine
// does is print timing information of a test run. Big whoop.
//

inline void
timing( const gr::high_res_timer_type& t_start,
	const gr::high_res_timer_type& t_stop,
	int loops ) {

  const gr::high_res_timer_type t_span = t_stop - t_start;
  const double t_sec  = (double)t_span/(double)gr::high_res_timer_tps();

  std::cout << "Done: " << "total_t: " << t_span << ", "
	    << "sec_t: " << t_sec << ", "
	    << "t/ea: " << t_sec/(float)(loops)
	    << std::endl << std::endl;

}

	   
int
main( void ) {

#define NUM_LOOPS     5
#define NUM_NUMS  10000
#define NUM_ELEM     32

  // I don't know why, here, I care about dangling pointers but I do.
  //

  std::unique_ptr<CPLX> dc_template_cc
    ((CPLX*)malloc(( NUM_NUMS + 2 ) * sizeof( CPLX )));
  std::unique_ptr<CPLX> dc_deque_cc
    ((CPLX*)malloc(( NUM_NUMS + 2 ) * sizeof( CPLX )));

  assert( dc_template_cc );
  assert( dc_deque_cc );

  std::unique_ptr<float> dc_template_ff
    ((float*)malloc(( NUM_NUMS + 2 ) * sizeof( float )));
  std::unique_ptr<float> dc_deque_ff
    ((float*)malloc(( NUM_NUMS + 2 ) * sizeof( float )));

  assert( dc_template_ff );
  assert( dc_deque_ff );

  
  std::vector<CPLX>  data_cc;
  std::vector<float> data_ff;
  
  std::cout << "Building number data..." << std::endl;
  for( int i = 0; i < NUM_NUMS; ++i )
    data_cc.push_back( CPLX( std::rand(), std::rand()));
  for( int i = 0; i < NUM_NUMS; ++i )
    data_ff.push_back( std::rand());
  std::cout << "Done." << std::endl << std::endl;


  // A little data.
  //

  std::cout << "GNURadio hi-res clock tps: "
	    << gr::high_res_timer_tps() << std::endl;
  std::cout << "GNURadio sizeof(gr_complex): "
	    << sizeof(gr_complex) << std::endl;
  std::cout << "GNURadio sizeof(CPLX): "
	    << sizeof(CPLX) << std::endl << std::endl;


  // Run the tests.
  //
  
  gr::high_res_timer_type t_start;


  // Long form.
  //
  
  { dc_blocker_t<CPLX> dc( NUM_ELEM );
    
    std::cout << "complex template: size=" << sizeof( dc ) << std::endl;
    
    t_start = gr::high_res_timer_now();
    for( int i = 0; i < NUM_LOOPS; ++i )
      for( int j = 0; j < NUM_NUMS; ++j )
	dc.work( data_cc, dc_template_cc.get());
    timing( t_start, gr::high_res_timer_now(), NUM_LOOPS*NUM_NUMS );

  }
  
  { dc_blocker_cc_deque dc( NUM_ELEM );
  
    std::cout << "complex deque: size=" << sizeof( dc ) << std::endl;

    t_start = gr::high_res_timer_now();
    for( int i = 0; i < NUM_LOOPS; ++i )
      for( int j = 0; j < NUM_NUMS; ++j ) 
	dc.work( data_cc, dc_deque_cc.get());
    timing( t_start, gr::high_res_timer_now(), NUM_LOOPS*NUM_NUMS );

  }

  { dc_blocker_t<float> dc( NUM_ELEM );

    std::cout << "float template: size=" << sizeof( dc ) << std::endl;

    t_start = gr::high_res_timer_now();
    for( int i = 0; i < NUM_LOOPS; ++i )
      for( int j = 0; j < NUM_NUMS; ++j )
	dc.work( data_ff, dc_template_ff.get());
    timing( t_start, gr::high_res_timer_now(), NUM_LOOPS*NUM_NUMS );

  }

  { dc_blocker_ff_deque dc( NUM_ELEM );

    std::cout << "float deque: size=" << sizeof( dc ) << std::endl;

    t_start = gr::high_res_timer_now();
    for( int i = 0; i < NUM_LOOPS; ++i )
      for( int j = 0; j < NUM_NUMS; ++j )
	dc.work( data_ff, dc_deque_ff.get());
    timing( t_start, gr::high_res_timer_now(), NUM_LOOPS*NUM_NUMS );

  }


  // A final check on long form: The contents of these data structures
  // should be the same.
  //

  std::cout << "Complex: "
	    << "deque:    " << *dc_deque_cc.get() << std::endl << "         " 
	    << "template: " << *dc_template_cc.get() << std::endl;
  for( int i = 0; i < NUM_NUMS; ++i ) 
    if(( *(dc_deque_cc.get()+i) != *(dc_template_cc.get()+i))) {
      
      std::cout << "Data error i=" << i << std::endl;
      break;
      
    }

  std::cout << "Float:   "
	    << "deque:    " << *dc_deque_ff.get() << std::endl << "         "
	    << "template: " << *dc_template_ff.get() << std::endl;
  for( int i = 0; i < NUM_NUMS; ++i )
    if(( *(dc_deque_ff.get()+i) != *(dc_template_ff.get()+i))) {

      std::cout << "Data error i=" << i << std::endl;
      break;

    }


  std::cout << std::endl << std::endl;
  

  // Short form.
  //

  { dc_blocker_t<CPLX> dc( NUM_ELEM, false );

    std::cout << "complex template: size=" << sizeof( dc ) << std::endl;

    t_start = gr::high_res_timer_now();
    for( int i = 0; i < NUM_LOOPS; ++i )
      for( int j = 0; j < NUM_NUMS; ++j )
	dc.work( data_cc, dc_template_cc.get());
    timing( t_start, gr::high_res_timer_now(), NUM_LOOPS*NUM_NUMS );

  }

  { dc_blocker_cc_deque dc( NUM_ELEM, false );

    std::cout << "complex deque: size=" << sizeof( dc ) << std::endl;

    t_start = gr::high_res_timer_now();
    for( int i = 0; i < NUM_LOOPS; ++i )
      for( int j = 0; j < NUM_NUMS; ++j )
	dc.work( data_cc, dc_deque_cc.get());
    timing( t_start, gr::high_res_timer_now(), NUM_LOOPS*NUM_NUMS );

  }

  { dc_blocker_t<float> dc( NUM_ELEM, false );

    std::cout << "float template: size=" << sizeof( dc ) << std::endl;

    t_start = gr::high_res_timer_now();
    for( int i = 0; i < NUM_LOOPS; ++i )
      for( int j = 0; j < NUM_NUMS; ++j )
	dc.work( data_ff, dc_template_ff.get());
    timing( t_start, gr::high_res_timer_now(), NUM_LOOPS*NUM_NUMS );

  }

  { dc_blocker_ff_deque dc( NUM_ELEM, false );

    std::cout << "float deque: size=" << sizeof( dc ) << std::endl;

    t_start = gr::high_res_timer_now();
    for( int i = 0; i < NUM_LOOPS; ++i )
      for( int j = 0; j < NUM_NUMS; ++j )
	dc.work( data_ff, dc_deque_ff.get());
    timing( t_start, gr::high_res_timer_now(), NUM_LOOPS*NUM_NUMS );

  }
  

  // A final check. The contents of these data structures should be
  // the same.
  //

  std::cout << "Complex: "
	    << "deque:    " << *dc_deque_cc.get() << std::endl << "         "
	    << "template: " << *dc_template_cc.get() << std::endl;
  for( int i = 0; i < NUM_NUMS; ++i )
    if(( *(dc_deque_cc.get()+i) != *(dc_template_cc.get()+i))) {

      std::cout << "Data error i=" << i << std::endl;
      break;

    }

  std::cout << "Float:   "
	    << "deque:    " << *dc_deque_ff.get() << std::endl << "         "
	    << "template: " << *dc_template_ff.get() << std::endl;
  for( int i = 0; i < NUM_NUMS; ++i )
    if(( *(dc_deque_ff.get()+i) != *(dc_template_ff.get()+i))) {

      std::cout << "Data error i=" << i << std::endl;
      break;

    }
    
  
  return 0;
}


//  LocalWords:  GNURadio deque