/* -*- c++ -*- */
/*
 * Copyright 2007,2010,2011 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 <vocoder_cvsd_decode_bs.h>
#include <gr_io_signature.h>
#include <limits.h>

/*
 * Create a new instance of vocoder_cvsd_decode_bs and return
 * a boost shared_ptr.  This is effectively the public constructor.
 */
vocoder_cvsd_decode_bs_sptr
vocoder_make_cvsd_decode_bs (short min_step, short max_step, double step_decay,
			     double accum_decay,  int K, int J,
			     short pos_accum_max, short neg_accum_max)
{
  return gnuradio::get_initial_sptr(new vocoder_cvsd_decode_bs (min_step, max_step,
								step_decay, accum_decay, K, J,
								pos_accum_max, neg_accum_max));
}

vocoder_cvsd_decode_bs::vocoder_cvsd_decode_bs (short min_step, short max_step, double step_decay,
						double accum_decay, int K, int J,
						short pos_accum_max, short neg_accum_max)
  : gr_sync_interpolator ("vocoder_cvsd_decode_bs",
			  gr_make_io_signature (1, 1, sizeof (unsigned char)),
			  gr_make_io_signature (1, 1, sizeof (short)),
			  8),
    d_min_step (min_step), d_max_step(max_step), d_step_decay(step_decay),
    d_accum_decay(accum_decay), d_K(K), d_J(J),
    d_pos_accum_max(pos_accum_max), d_neg_accum_max(neg_accum_max),
    d_accum(0),
    d_loop_counter(1),
    d_runner(0),
    d_runner_mask(0),
    d_stepsize(min_step)

{
  assert(d_K <= 32);
  assert(d_J <= d_K);
}


vocoder_cvsd_decode_bs::~vocoder_cvsd_decode_bs ()
{
  // nothing else required in this example
}

unsigned char vocoder_cvsd_decode_bs::cvsd_bitwise_sum (unsigned int input)
{
  unsigned int temp=input;
  unsigned char bits=0;

  while(temp) {
    temp=temp&(temp-1);
    bits++;
  }
  return bits;
}

int vocoder_cvsd_decode_bs::cvsd_round (double input)
{
  double temp;
  temp=input+0.5;
  temp=floor(temp);

  return (int)temp;
}

unsigned int vocoder_cvsd_decode_bs::cvsd_pow (short radix, short power)
{
  double d_radix = (double) radix;
  int i_power = (int) power;
  double output;

  output=pow(d_radix,i_power);
  return ( (unsigned int) cvsd_round(output));
}


int
vocoder_cvsd_decode_bs::work (int noutput_items,
			      gr_vector_const_void_star &input_items,
			      gr_vector_void_star &output_items)
{


  const unsigned char *in = (const unsigned char *) input_items[0];
  short *out = (short *) output_items[0];

  int i=0;
  short output_short=0;	         // 2 bytes 0 .. 65,535
  unsigned char bit_count=0;	 // 1 byte, 0 .. 255
  unsigned int mask=0;		 // 4 bytes, 0 .. 4,294,967,295
  unsigned char input_byte=0;	 //  1 bytes
  unsigned char input_bit=0;	 // 1 byte, 0 .. 255

  // Loop through each input data point
  for(i = 0; i < noutput_items/8.0; i++) {

    input_byte = in[i];
    // Initiliaze bit counter
    bit_count=0;

    while(bit_count<8) {
      // Compute the Appropriate Mask
      mask=cvsd_pow(2,7-bit_count);

      // Pull off the corresponding bit
      input_bit = input_byte & mask;

      // Update the bit counter
      bit_count++;

      // Update runner with the next input bit
      // Runner is a shift-register; shift left, add on newest output bit
      d_runner = (d_runner<<1) | ((unsigned int) input_bit);

      // Run this only if you have >= J bits in your shift register
      if (d_loop_counter>=d_J) {
	// Update Step Size
	d_runner_mask=(cvsd_pow(2,d_J)-1);
	if ((cvsd_bitwise_sum(d_runner & d_runner_mask)>=d_J)||(cvsd_bitwise_sum((~d_runner) & d_runner_mask)>=d_J)) {
	  // Runs of 1s and 0s
	  d_stepsize = std::min( (short) (d_stepsize + d_min_step), d_max_step);
	}
	else {
	  // No runs of 1s and 0s
	  d_stepsize = std::max( (short) cvsd_round(d_stepsize*d_step_decay), d_min_step);
	}
      }

      // Update Accum (i.e. the reference value)
      if (input_bit) {
	d_accum=d_accum+d_stepsize;
      }
      else {
	d_accum=d_accum-d_stepsize;
      }

      // Multiply by Accum_Decay
      d_accum=(cvsd_round(d_accum*d_accum_decay));

      // Check for overflow
      if (d_accum >=((int) d_pos_accum_max)) {
	d_accum=(int)d_pos_accum_max;
      }
      else if (d_accum <=((int) d_neg_accum_max)) {
	d_accum=(int)d_neg_accum_max;
      }

      // Find the output short to write to the file
      output_short=((short) d_accum);

      if (d_loop_counter <= d_K) {
	d_loop_counter++;
      }

      *(out++) = output_short;
    } // while ()

  } // for()

  return noutput_items;
}