/*
 * Copyright (C) 2002-2004 Erik de Castro Lopo <erikd@mega-nerd.com>
 *
 * This program 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 2 of the License, or
 * (at your option) any later version.
 *
 * This program 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 program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
 */

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

#include "float_cast.h"
#include "samplerate2.h"
#include "common.h"

#define	SINC_MAGIC_MARKER MAKE_MAGIC (' ', 's', 'i', 'n', 'c', ' ')

#define MAKE_INCREMENT_T(x) ((increment_t) (x))

#define SHIFT_BITS 12
#define FP_ONE ((double) (((increment_t) 1) << SHIFT_BITS))
#define INV_FP_ONE (1.0 / FP_ONE)

typedef int32_t increment_t;
typedef sr2_float_t coeff_t;

#include "fastest_coeffs.h"
#include "mid_qual_coeffs.h"
#include "high_qual_coeffs.h"

#ifdef SRCNEWCOEFFS
#include "slow_mid_qual_coeffs.h"
#include "slow_high_qual_coeffs.h"
#endif

typedef struct
{
  long sinc_magic_marker;
  long channels;
  long in_count, in_used;
  long out_count, out_gen;
  long coeff_half_len, index_inc;
  double src_ratio, input_index;
  coeff_t const	*coeffs;
  
  long 	b_current, b_end, b_real_end, b_len;

  /* Sure hope noone does more than 128 channels at once. */
  sr2_float_t left_calc [128], right_calc [128];
  
  /* C99 struct flexible array. */
  sr2_float_t buffer [];
} SR2_(SINC_FILTER);

static inline increment_t double_to_fp (double x)
{
  // if(sizeof (increment_t) == 8) return (llrint ((x) * FP_ONE));
  return (lrint ((x) * FP_ONE));
}

static inline increment_t int_to_fp (int x)
{
  return (((increment_t) (x)) << SHIFT_BITS);
}

static inline int fp_to_int (increment_t x)
{
  return (((x) >> SHIFT_BITS));
}

static inline increment_t fp_fraction_part (increment_t x)
{
  return ((x) & ((((increment_t) 1) << SHIFT_BITS) - 1));
}

static inline double fp_to_double (increment_t x)
{
  return fp_fraction_part (x) * INV_FP_ONE;
}

static int SR2_(prepare_data)(SR2_(SINC_FILTER) * filter,
			       SR2_(SRC_DATA) *data, long half_filter_chan_len)
{
  long len = 0;
  
  if(filter->b_real_end >= 0) return 0;
  
  if(filter->b_current == 0)
    {
      /* Initial state. Set up zeros at the start of the buffer and
      ** then load new data after that.
      */
      len = filter->b_len - 2 * half_filter_chan_len;
      
      filter->b_current = filter->b_end = half_filter_chan_len;
    }
  else if(filter->b_end + half_filter_chan_len + filter->channels < filter->b_len)
	{	/*  Load data at current end position. */
	  len = MAX (filter->b_len - filter->b_current - half_filter_chan_len, 0);
	}
  else
    {
      /* Move data at end of buffer back to the start of the buffer. */
      len = filter->b_end - filter->b_current;
      memmove (filter->buffer, filter->buffer + filter->b_current - half_filter_chan_len,
	       (half_filter_chan_len + len) * sizeof (filter->buffer [0]));
      
      filter->b_current = half_filter_chan_len;
      filter->b_end = filter->b_current + len;
      
      /* Now load data at current end of buffer. */
      len = MAX (filter->b_len - filter->b_current - half_filter_chan_len, 0);
    }
  
  len = MIN (filter->in_count - filter->in_used, len);
  len -= (len % filter->channels);
  
  if  (len < 0 || filter->b_end + len > filter->b_len)
    return SRC_ERR_SINC_PREPARE_DATA_BAD_LEN ;

  memcpy (filter->buffer + filter->b_end, data->data_in + filter->in_used,
	  len * sizeof (filter->buffer [0]));
  
  filter->b_end += len;
  filter->in_used += len;
  
  if(filter->in_used == filter->in_count &&
     filter->b_end - filter->b_current < 2 * half_filter_chan_len && data->end_of_input)
    {
      /* Handle the case where all data in the current buffer has been
      ** consumed and this is the last buffer.
      */
      
      if(filter->b_len - filter->b_end < half_filter_chan_len + 5)
	{
	  /* If necessary, move data down to the start of the buffer. */
	  len = filter->b_end - filter->b_current;
	  memmove (filter->buffer, filter->buffer + filter->b_current - half_filter_chan_len,
		   (half_filter_chan_len + len) * sizeof (filter->buffer [0]));
	  
	  filter->b_current = half_filter_chan_len;
	  filter->b_end = filter->b_current + len;
	};
      
      filter->b_real_end = filter->b_end;
      len = half_filter_chan_len + 5;
      
      memset (filter->buffer + filter->b_end, 0, len * sizeof (filter->buffer [0]));
      filter->b_end += len;
    };
  
  return 0;
}

// main src codes

static inline sr2_float_t
SR2_(calc_output_single) (SR2_(SINC_FILTER) *filter, increment_t increment, increment_t start_filter_index)
{	sr2_float_t		fraction, left, right, icoeff ;
	increment_t	filter_index, max_filter_index ;
	int			data_index, coeff_count, indx ;

	/* Convert input parameters into fixed point. */
	max_filter_index = int_to_fp (filter->coeff_half_len) ;

	/* First apply the left half of the filter. */
	filter_index = start_filter_index ;
	coeff_count = (max_filter_index - filter_index) / increment ;
	filter_index = filter_index + coeff_count * increment ;
	data_index = filter->b_current - coeff_count ;

	left = 0.0 ;
	do
	{	fraction = fp_to_double (filter_index) ;
		indx = fp_to_int (filter_index) ;

		icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;

		left += icoeff * filter->buffer [data_index] ;

		filter_index -= increment ;
		data_index = data_index + 1 ;
		}
	while (filter_index >= MAKE_INCREMENT_T (0)) ;

	/* Now apply the right half of the filter. */
	filter_index = increment - start_filter_index ;
	coeff_count = (max_filter_index - filter_index) / increment ;
	filter_index = filter_index + coeff_count * increment ;
	data_index = filter->b_current + 1 + coeff_count ;

	right = 0.0 ;
	do
	{	fraction = fp_to_double (filter_index) ;
		indx = fp_to_int (filter_index) ;

		icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;

		right += icoeff * filter->buffer [data_index] ;

		filter_index -= increment ;
		data_index = data_index - 1 ;
		}
	while (filter_index > MAKE_INCREMENT_T (0)) ;

	return (left + right) ;
} /* calc_output_single */

static long
SR2_(sinc_mono_vari_process) (SR2_(SRC_PRIVATE) *psrc, SR2_(SRC_DATA) *data)
{	SR2_(SINC_FILTER) *filter ;
	sr2_float_t	input_index, src_ratio, count, float_increment, terminate, rem ;
	increment_t	increment, start_filter_index ;
	int			half_filter_chan_len, samples_in_hand ;

	if (psrc->private_data == NULL)
		return SRC_ERR_NO_PRIVATE ;

	filter = (SR2_(SINC_FILTER)*) psrc->private_data ;

	/* If there is not a problem, this will be optimised out. */
	if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0]))
		return SRC_ERR_SIZE_INCOMPATIBILITY ;

	filter->in_count = data->input_frames * filter->channels ;
	filter->out_count = data->output_frames * filter->channels ;
	filter->in_used = filter->out_gen = 0 ;

	src_ratio = psrc->last_ratio ;

	/* Check the sample rate ratio wrt the buffer len. */
	count = (filter->coeff_half_len + 2.0) / filter->index_inc ;
	if (MIN (psrc->last_ratio, data->src_ratio) < 1.0)
		count /= MIN (psrc->last_ratio, data->src_ratio) ;

	/* Maximum coefficientson either side of center point. */
	half_filter_chan_len = filter->channels * (lrint (count) + 1) ;

	input_index = psrc->last_position ;
	float_increment = filter->index_inc ;

	rem = SR2_(fmod_one) (input_index) ;
	filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
	input_index = rem ;

	terminate = 1.0 / src_ratio + 1e-20 ;

	/* Main processing loop. */
	while (filter->out_gen < filter->out_count)
	{
		/* Need to reload buffer? */
		samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;

		if (samples_in_hand <= half_filter_chan_len)
		  {	if ((psrc->error = SR2_(prepare_data) (filter, data, half_filter_chan_len)) != 0)
				return psrc->error ;

			samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;
			if (samples_in_hand <= half_filter_chan_len)
				break ;
			} ;

		/* This is the termination condition. */
		if (filter->b_real_end >= 0)
		{	if (filter->b_current + input_index + terminate >= filter->b_real_end)
				break ;
			} ;

		if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10)
			src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ;

		float_increment = filter->index_inc * 1.0 ;
		if (src_ratio < 1.0)
			float_increment = filter->index_inc * src_ratio ;

		increment = double_to_fp (float_increment) ;

		start_filter_index = double_to_fp (input_index * float_increment) ;

		data->data_out [filter->out_gen] = (sr2_float_t) ((float_increment / filter->index_inc) *
							    SR2_(calc_output_single) (filter, increment, start_filter_index)) ;
		filter->out_gen ++ ;

		/* Figure out the next index. */
		input_index += 1.0 / src_ratio ;
		rem = SR2_(fmod_one) (input_index) ;

		filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
		input_index = rem ;
		} ;

	psrc->last_position = input_index ;

	/* Save current ratio rather then target ratio. */
	psrc->last_ratio = src_ratio ;

	data->input_frames_used = filter->in_used / filter->channels ;
	data->output_frames_gen = filter->out_gen / filter->channels ;

	return SRC_ERR_NO_ERROR ;
} /* sinc_mono_vari_process */

static inline void
SR2_(calc_output_stereo) (SR2_(SINC_FILTER) *filter, increment_t increment, increment_t start_filter_index, sr2_float_t scale, sr2_float_t * output)
{	sr2_float_t	fraction, left [2], right [2], icoeff ;
	increment_t	filter_index, max_filter_index ;
	int			data_index, coeff_count, indx ;

	/* Convert input parameters into fixed point. */
	max_filter_index = int_to_fp (filter->coeff_half_len) ;

	/* First apply the left half of the filter. */
	filter_index = start_filter_index ;
	coeff_count = (max_filter_index - filter_index) / increment ;
	filter_index = filter_index + coeff_count * increment ;
	data_index = filter->b_current - filter->channels * coeff_count ;

	left [0] = left [1] = 0.0 ;
	do
	{	fraction = fp_to_double (filter_index) ;
		indx = fp_to_int (filter_index) ;

		icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;

		left [0] += icoeff * filter->buffer [data_index] ;
		left [1] += icoeff * filter->buffer [data_index + 1] ;

		filter_index -= increment ;
		data_index = data_index + 2 ;
		}
	while (filter_index >= MAKE_INCREMENT_T (0)) ;

	/* Now apply the right half of the filter. */
	filter_index = increment - start_filter_index ;
	coeff_count = (max_filter_index - filter_index) / increment ;
	filter_index = filter_index + coeff_count * increment ;
	data_index = filter->b_current + filter->channels * (1 + coeff_count) ;

	right [0] = right [1] = 0.0 ;
	do
	{	fraction = fp_to_double (filter_index) ;
		indx = fp_to_int (filter_index) ;

		icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;

		right [0] += icoeff * filter->buffer [data_index] ;
		right [1] += icoeff * filter->buffer [data_index + 1] ;

		filter_index -= increment ;
		data_index = data_index - 2 ;
		}
	while (filter_index > MAKE_INCREMENT_T (0)) ;

	output [0] = scale * (left [0] + right [0]) ;
	output [1] = scale * (left [1] + right [1]) ;
} /* calc_output_stereo */

static long
SR2_(sinc_stereo_vari_process) (SR2_(SRC_PRIVATE) *psrc, SR2_(SRC_DATA) *data)
{	SR2_(SINC_FILTER) *filter ;
	sr2_float_t	input_index, src_ratio, count, float_increment, terminate, rem ;
	increment_t	increment, start_filter_index ;
	int			half_filter_chan_len, samples_in_hand ;

	if (psrc->private_data == NULL)
		return SRC_ERR_NO_PRIVATE ;

	filter = (SR2_(SINC_FILTER)*) psrc->private_data ;

	/* If there is not a problem, this will be optimised out. */
	if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0]))
		return SRC_ERR_SIZE_INCOMPATIBILITY ;

	filter->in_count = data->input_frames * filter->channels ;
	filter->out_count = data->output_frames * filter->channels ;
	filter->in_used = filter->out_gen = 0 ;

	src_ratio = psrc->last_ratio ;

	/* Check the sample rate ratio wrt the buffer len. */
	count = (filter->coeff_half_len + 2.0) / filter->index_inc ;
	if (MIN (psrc->last_ratio, data->src_ratio) < 1.0)
		count /= MIN (psrc->last_ratio, data->src_ratio) ;

	/* Maximum coefficientson either side of center point. */
	half_filter_chan_len = filter->channels * (lrint (count) + 1) ;

	input_index = psrc->last_position ;
	float_increment = filter->index_inc ;

	rem = SR2_(fmod_one) (input_index) ;
	filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
	input_index = rem ;

	terminate = 1.0 / src_ratio + 1e-20 ;

	/* Main processing loop. */
	while (filter->out_gen < filter->out_count)
	{
		/* Need to reload buffer? */
		samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;

		if (samples_in_hand <= half_filter_chan_len)
		  {	if ((psrc->error = SR2_(prepare_data) (filter, data, half_filter_chan_len)) != 0)
				return psrc->error ;

			samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;
			if (samples_in_hand <= half_filter_chan_len)
				break ;
			} ;

		/* This is the termination condition. */
		if (filter->b_real_end >= 0)
		{	if (filter->b_current + input_index + terminate >= filter->b_real_end)
				break ;
			} ;

		if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10)
			src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ;

		float_increment = filter->index_inc * 1.0 ;
		if (src_ratio < 1.0)
			float_increment = filter->index_inc * src_ratio ;

		increment = double_to_fp (float_increment) ;

		start_filter_index = double_to_fp (input_index * float_increment) ;

		SR2_(calc_output_stereo) (filter, increment, start_filter_index, float_increment / filter->index_inc, data->data_out + filter->out_gen) ;
		filter->out_gen += 2 ;

		/* Figure out the next index. */
		input_index += 1.0 / src_ratio ;
		rem = SR2_(fmod_one) (input_index) ;

		filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
		input_index = rem ;
		} ;

	psrc->last_position = input_index ;

	/* Save current ratio rather then target ratio. */
	psrc->last_ratio = src_ratio ;

	data->input_frames_used = filter->in_used / filter->channels ;
	data->output_frames_gen = filter->out_gen / filter->channels ;

	return SRC_ERR_NO_ERROR ;
} /* sinc_stereo_vari_process */

static inline void
SR2_(calc_output_quad) (SR2_(SINC_FILTER) *filter, increment_t increment, increment_t start_filter_index, sr2_float_t scale, sr2_float_t * output)
{	sr2_float_t	fraction, left [4], right [4], icoeff ;
	increment_t	filter_index, max_filter_index ;
	int			data_index, coeff_count, indx ;

	/* Convert input parameters into fixed point. */
	max_filter_index = int_to_fp (filter->coeff_half_len) ;

	/* First apply the left half of the filter. */
	filter_index = start_filter_index ;
	coeff_count = (max_filter_index - filter_index) / increment ;
	filter_index = filter_index + coeff_count * increment ;
	data_index = filter->b_current - filter->channels * coeff_count ;

	left [0] = left [1] = left [2] = left [3] = 0.0 ;
	do
	{	fraction = fp_to_double (filter_index) ;
		indx = fp_to_int (filter_index) ;

		icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;

		left [0] += icoeff * filter->buffer [data_index] ;
		left [1] += icoeff * filter->buffer [data_index + 1] ;
		left [2] += icoeff * filter->buffer [data_index + 2] ;
		left [3] += icoeff * filter->buffer [data_index + 3] ;

		filter_index -= increment ;
		data_index = data_index + 4 ;
		}
	while (filter_index >= MAKE_INCREMENT_T (0)) ;

	/* Now apply the right half of the filter. */
	filter_index = increment - start_filter_index ;
	coeff_count = (max_filter_index - filter_index) / increment ;
	filter_index = filter_index + coeff_count * increment ;
	data_index = filter->b_current + filter->channels * (1 + coeff_count) ;

	right [0] = right [1] = right [2] = right [3] = 0.0 ;
	do
	{	fraction = fp_to_double (filter_index) ;
		indx = fp_to_int (filter_index) ;

		icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;

		right [0] += icoeff * filter->buffer [data_index] ;
		right [1] += icoeff * filter->buffer [data_index + 1] ;
		right [2] += icoeff * filter->buffer [data_index + 2] ;
		right [3] += icoeff * filter->buffer [data_index + 3] ;

		filter_index -= increment ;
		data_index = data_index - 4 ;
		}
	while (filter_index > MAKE_INCREMENT_T (0)) ;

	output [0] = scale * (left [0] + right [0]) ;
	output [1] = scale * (left [1] + right [1]) ;
	output [2] = scale * (left [2] + right [2]) ;
	output [3] = scale * (left [3] + right [3]) ;
} /* calc_output_quad */

static long
SR2_(sinc_quad_vari_process) (SR2_(SRC_PRIVATE) *psrc, SR2_(SRC_DATA) *data)
{	SR2_(SINC_FILTER) *filter ;
	sr2_float_t	input_index, src_ratio, count, float_increment, terminate, rem ;
	increment_t	increment, start_filter_index ;
	int			half_filter_chan_len, samples_in_hand ;

	if (psrc->private_data == NULL)
		return SRC_ERR_NO_PRIVATE ;

	filter = (SR2_(SINC_FILTER)*) psrc->private_data ;

	/* If there is not a problem, this will be optimised out. */
	if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0]))
		return SRC_ERR_SIZE_INCOMPATIBILITY ;

	filter->in_count = data->input_frames * filter->channels ;
	filter->out_count = data->output_frames * filter->channels ;
	filter->in_used = filter->out_gen = 0 ;

	src_ratio = psrc->last_ratio ;

	/* Check the sample rate ratio wrt the buffer len. */
	count = (filter->coeff_half_len + 2.0) / filter->index_inc ;
	if (MIN (psrc->last_ratio, data->src_ratio) < 1.0)
		count /= MIN (psrc->last_ratio, data->src_ratio) ;

	/* Maximum coefficientson either side of center point. */
	half_filter_chan_len = filter->channels * (lrint (count) + 1) ;

	input_index = psrc->last_position ;
	float_increment = filter->index_inc ;

	rem = SR2_(fmod_one) (input_index) ;
	filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
	input_index = rem ;

	terminate = 1.0 / src_ratio + 1e-20 ;

	/* Main processing loop. */
	while (filter->out_gen < filter->out_count)
	{
		/* Need to reload buffer? */
		samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;

		if (samples_in_hand <= half_filter_chan_len)
		  {	if ((psrc->error = SR2_(prepare_data) (filter, data, half_filter_chan_len)) != 0)
				return psrc->error ;

			samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;
			if (samples_in_hand <= half_filter_chan_len)
				break ;
			} ;

		/* This is the termination condition. */
		if (filter->b_real_end >= 0)
		{	if (filter->b_current + input_index + terminate >= filter->b_real_end)
				break ;
			} ;

		if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10)
			src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ;

		float_increment = filter->index_inc * 1.0 ;
		if (src_ratio < 1.0)
			float_increment = filter->index_inc * src_ratio ;

		increment = double_to_fp (float_increment) ;

		start_filter_index = double_to_fp (input_index * float_increment) ;

		SR2_(calc_output_quad) (filter, increment, start_filter_index, float_increment / filter->index_inc, data->data_out + filter->out_gen) ;
		filter->out_gen += 4 ;

		/* Figure out the next index. */
		input_index += 1.0 / src_ratio ;
		rem = SR2_(fmod_one) (input_index) ;

		filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
		input_index = rem ;
		} ;

	psrc->last_position = input_index ;

	/* Save current ratio rather then target ratio. */
	psrc->last_ratio = src_ratio ;

	data->input_frames_used = filter->in_used / filter->channels ;
	data->output_frames_gen = filter->out_gen / filter->channels ;

	return SRC_ERR_NO_ERROR ;
} /* sinc_quad_vari_process */

static inline void
SR2_(calc_output_hex) (SR2_(SINC_FILTER) *filter, increment_t increment, increment_t start_filter_index, sr2_float_t scale, sr2_float_t * output)
{	sr2_float_t	fraction, left [6], right [6], icoeff ;
	increment_t	filter_index, max_filter_index ;
	int			data_index, coeff_count, indx ;

	/* Convert input parameters into fixed point. */
	max_filter_index = int_to_fp (filter->coeff_half_len) ;

	/* First apply the left half of the filter. */
	filter_index = start_filter_index ;
	coeff_count = (max_filter_index - filter_index) / increment ;
	filter_index = filter_index + coeff_count * increment ;
	data_index = filter->b_current - filter->channels * coeff_count ;

	left [0] = left [1] = left [2] = left [3] = left [4] = left [5] = 0.0 ;
	do
	{	fraction = fp_to_double (filter_index) ;
		indx = fp_to_int (filter_index) ;

		icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;

		left [0] += icoeff * filter->buffer [data_index] ;
		left [1] += icoeff * filter->buffer [data_index + 1] ;
		left [2] += icoeff * filter->buffer [data_index + 2] ;
		left [3] += icoeff * filter->buffer [data_index + 3] ;
		left [4] += icoeff * filter->buffer [data_index + 4] ;
		left [5] += icoeff * filter->buffer [data_index + 5] ;

		filter_index -= increment ;
		data_index = data_index + 6 ;
		}
	while (filter_index >= MAKE_INCREMENT_T (0)) ;

	/* Now apply the right half of the filter. */
	filter_index = increment - start_filter_index ;
	coeff_count = (max_filter_index - filter_index) / increment ;
	filter_index = filter_index + coeff_count * increment ;
	data_index = filter->b_current + filter->channels * (1 + coeff_count) ;

	right [0] = right [1] = right [2] = right [3] = right [4] = right [5] = 0.0 ;
	do
	{	fraction = fp_to_double (filter_index) ;
		indx = fp_to_int (filter_index) ;

		icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;

		right [0] += icoeff * filter->buffer [data_index] ;
		right [1] += icoeff * filter->buffer [data_index + 1] ;
		right [2] += icoeff * filter->buffer [data_index + 2] ;
		right [3] += icoeff * filter->buffer [data_index + 3] ;
		right [4] += icoeff * filter->buffer [data_index + 4] ;
		right [5] += icoeff * filter->buffer [data_index + 5] ;

		filter_index -= increment ;
		data_index = data_index - 6 ;
		}
	while (filter_index > MAKE_INCREMENT_T (0)) ;

	output [0] = scale * (left [0] + right [0]) ;
	output [1] = scale * (left [1] + right [1]) ;
	output [2] = scale * (left [2] + right [2]) ;
	output [3] = scale * (left [3] + right [3]) ;
	output [4] = scale * (left [4] + right [4]) ;
	output [5] = scale * (left [5] + right [5]) ;
} /* calc_output_hex */

static long
SR2_(sinc_hex_vari_process) (SR2_(SRC_PRIVATE) *psrc, SR2_(SRC_DATA) *data)
{	SR2_(SINC_FILTER) *filter ;
	sr2_float_t	input_index, src_ratio, count, float_increment, terminate, rem ;
	increment_t	increment, start_filter_index ;
	int			half_filter_chan_len, samples_in_hand ;

	if (psrc->private_data == NULL)
		return SRC_ERR_NO_PRIVATE ;

	filter = (SR2_(SINC_FILTER)*) psrc->private_data ;

	/* If there is not a problem, this will be optimised out. */
	if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0]))
		return SRC_ERR_SIZE_INCOMPATIBILITY ;

	filter->in_count = data->input_frames * filter->channels ;
	filter->out_count = data->output_frames * filter->channels ;
	filter->in_used = filter->out_gen = 0 ;

	src_ratio = psrc->last_ratio ;

	/* Check the sample rate ratio wrt the buffer len. */
	count = (filter->coeff_half_len + 2.0) / filter->index_inc ;
	if (MIN (psrc->last_ratio, data->src_ratio) < 1.0)
		count /= MIN (psrc->last_ratio, data->src_ratio) ;

	/* Maximum coefficientson either side of center point. */
	half_filter_chan_len = filter->channels * (lrint (count) + 1) ;

	input_index = psrc->last_position ;
	float_increment = filter->index_inc ;

	rem = SR2_(fmod_one) (input_index) ;
	filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
	input_index = rem ;

	terminate = 1.0 / src_ratio + 1e-20 ;

	/* Main processing loop. */
	while (filter->out_gen < filter->out_count)
	{
		/* Need to reload buffer? */
		samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;

		if (samples_in_hand <= half_filter_chan_len)
		  {	if ((psrc->error = SR2_(prepare_data) (filter, data, half_filter_chan_len)) != 0)
				return psrc->error ;

			samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;
			if (samples_in_hand <= half_filter_chan_len)
				break ;
			} ;

		/* This is the termination condition. */
		if (filter->b_real_end >= 0)
		{	if (filter->b_current + input_index + terminate >= filter->b_real_end)
				break ;
			} ;

		if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10)
			src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ;

		float_increment = filter->index_inc * 1.0 ;
		if (src_ratio < 1.0)
			float_increment = filter->index_inc * src_ratio ;

		increment = double_to_fp (float_increment) ;

		start_filter_index = double_to_fp (input_index * float_increment) ;

		SR2_(calc_output_hex) (filter, increment, start_filter_index, float_increment / filter->index_inc, data->data_out + filter->out_gen) ;
		filter->out_gen += 6 ;

		/* Figure out the next index. */
		input_index += 1.0 / src_ratio ;
		rem = SR2_(fmod_one) (input_index) ;

		filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
		input_index = rem ;
		} ;

	psrc->last_position = input_index ;

	/* Save current ratio rather then target ratio. */
	psrc->last_ratio = src_ratio ;

	data->input_frames_used = filter->in_used / filter->channels ;
	data->output_frames_gen = filter->out_gen / filter->channels ;

	return SRC_ERR_NO_ERROR ;
} /* sinc_hex_vari_process */

static inline void
SR2_(calc_output_multi) (SR2_(SINC_FILTER) *filter, increment_t increment, increment_t start_filter_index, int channels, sr2_float_t scale, sr2_float_t * output)
{	sr2_float_t		fraction, icoeff ;
	/* The following line is 1999 ISO Standard C. If your compiler complains, get a better compiler. */
	sr2_float_t		*left, *right ;
	increment_t	filter_index, max_filter_index ;
	int			data_index, coeff_count, indx, ch ;

	left = filter->left_calc ;
	right = filter->right_calc ;

	/* Convert input parameters into fixed point. */
	max_filter_index = int_to_fp (filter->coeff_half_len) ;

	/* First apply the left half of the filter. */
	filter_index = start_filter_index ;
	coeff_count = (max_filter_index - filter_index) / increment ;
	filter_index = filter_index + coeff_count * increment ;
	data_index = filter->b_current - channels * coeff_count ;

	memset (left, 0, sizeof (left [0]) * channels) ;

	do
	{	fraction = fp_to_double (filter_index) ;
		indx = fp_to_int (filter_index) ;

		icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;

		/*
		**	Duff's Device.
		**	See : http://en.wikipedia.org/wiki/Duff's_device
		*/
		ch = channels ;
		do
		{
			switch (ch % 8)
			{	default :
					ch -- ;
					left [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 7 :
					ch -- ;
					left [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 6 :
					ch -- ;
					left [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 5 :
					ch -- ;
					left [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 4 :
					ch -- ;
					left [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 3 :
					ch -- ;
					left [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 2 :
					ch -- ;
					left [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 1 :
					ch -- ;
					left [ch] += icoeff * filter->buffer [data_index + ch] ;
				} ;
			}
		while (ch > 0) ;

		filter_index -= increment ;
		data_index = data_index + channels ;
		}
	while (filter_index >= MAKE_INCREMENT_T (0)) ;

	/* Now apply the right half of the filter. */
	filter_index = increment - start_filter_index ;
	coeff_count = (max_filter_index - filter_index) / increment ;
	filter_index = filter_index + coeff_count * increment ;
	data_index = filter->b_current + channels * (1 + coeff_count) ;

	memset (right, 0, sizeof (right [0]) * channels) ;
	do
	{	fraction = fp_to_double (filter_index) ;
		indx = fp_to_int (filter_index) ;

		icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;

		ch = channels ;
		do
		{
			switch (ch % 8)
			{	default :
					ch -- ;
					right [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 7 :
					ch -- ;
					right [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 6 :
					ch -- ;
					right [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 5 :
					ch -- ;
					right [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 4 :
					ch -- ;
					right [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 3 :
					ch -- ;
					right [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 2 :
					ch -- ;
					right [ch] += icoeff * filter->buffer [data_index + ch] ;
				case 1 :
					ch -- ;
					right [ch] += icoeff * filter->buffer [data_index + ch] ;
				} ;
			}
		while (ch > 0) ;

		filter_index -= increment ;
		data_index = data_index - channels ;
		}
	while (filter_index > MAKE_INCREMENT_T (0)) ;

	ch = channels ;
	do
	{
		switch (ch % 8)
		{	default :
				ch -- ;
				output [ch] = scale * (left [ch] + right [ch]) ;
			case 7 :
				ch -- ;
				output [ch] = scale * (left [ch] + right [ch]) ;
			case 6 :
				ch -- ;
				output [ch] = scale * (left [ch] + right [ch]) ;
			case 5 :
				ch -- ;
				output [ch] = scale * (left [ch] + right [ch]) ;
			case 4 :
				ch -- ;
				output [ch] = scale * (left [ch] + right [ch]) ;
			case 3 :
				ch -- ;
				output [ch] = scale * (left [ch] + right [ch]) ;
			case 2 :
				ch -- ;
				output [ch] = scale * (left [ch] + right [ch]) ;
			case 1 :
				ch -- ;
				output [ch] = scale * (left [ch] + right [ch]) ;
			} ;
		}
	while (ch > 0) ;

	return ;
} /* calc_output_multi */

static long
SR2_(sinc_multichan_vari_process) (SR2_(SRC_PRIVATE) *psrc, SR2_(SRC_DATA) *data)
{	SR2_(SINC_FILTER) *filter ;
	sr2_float_t	input_index, src_ratio, count, float_increment, terminate, rem ;
	increment_t	increment, start_filter_index ;
	int			half_filter_chan_len, samples_in_hand ;

	if (psrc->private_data == NULL)
		return SRC_ERR_NO_PRIVATE ;

	filter = (SR2_(SINC_FILTER)*) psrc->private_data ;

	/* If there is not a problem, this will be optimised out. */
	if (sizeof (filter->buffer [0]) != sizeof (data->data_in [0]))
		return SRC_ERR_SIZE_INCOMPATIBILITY ;

	filter->in_count = data->input_frames * filter->channels ;
	filter->out_count = data->output_frames * filter->channels ;
	filter->in_used = filter->out_gen = 0 ;

	src_ratio = psrc->last_ratio ;

	/* Check the sample rate ratio wrt the buffer len. */
	count = (filter->coeff_half_len + 2.0) / filter->index_inc ;
	if (MIN (psrc->last_ratio, data->src_ratio) < 1.0)
		count /= MIN (psrc->last_ratio, data->src_ratio) ;

	/* Maximum coefficientson either side of center point. */
	half_filter_chan_len = filter->channels * (lrint (count) + 1) ;

	input_index = psrc->last_position ;
	float_increment = filter->index_inc ;

	rem = SR2_(fmod_one) (input_index) ;
	filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
	input_index = rem ;

	terminate = 1.0 / src_ratio + 1e-20 ;

	/* Main processing loop. */
	while (filter->out_gen < filter->out_count)
	{
		/* Need to reload buffer? */
		samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;

		if (samples_in_hand <= half_filter_chan_len)
		  {	if ((psrc->error = SR2_(prepare_data) (filter, data, half_filter_chan_len)) != 0)
				return psrc->error ;

			samples_in_hand = (filter->b_end - filter->b_current + filter->b_len) % filter->b_len ;
			if (samples_in_hand <= half_filter_chan_len)
				break ;
			} ;

		/* This is the termination condition. */
		if (filter->b_real_end >= 0)
		{	if (filter->b_current + input_index + terminate >= filter->b_real_end)
				break ;
			} ;

		if (filter->out_count > 0 && fabs (psrc->last_ratio - data->src_ratio) > 1e-10)
			src_ratio = psrc->last_ratio + filter->out_gen * (data->src_ratio - psrc->last_ratio) / filter->out_count ;

		float_increment = filter->index_inc * 1.0 ;
		if (src_ratio < 1.0)
			float_increment = filter->index_inc * src_ratio ;

		increment = double_to_fp (float_increment) ;

		start_filter_index = double_to_fp (input_index * float_increment) ;

		SR2_(calc_output_multi) (filter, increment, start_filter_index, filter->channels, float_increment / filter->index_inc, data->data_out + filter->out_gen) ;
		filter->out_gen += psrc->channels ;

		/* Figure out the next index. */
		input_index += 1.0 / src_ratio ;
		rem = SR2_(fmod_one) (input_index) ;

		filter->b_current = (filter->b_current + filter->channels * lrint (input_index - rem)) % filter->b_len ;
		input_index = rem ;
		} ;

	psrc->last_position = input_index ;

	/* Save current ratio rather then target ratio. */
	psrc->last_ratio = src_ratio ;

	data->input_frames_used = filter->in_used / filter->channels ;
	data->output_frames_gen = filter->out_gen / filter->channels ;

	return SRC_ERR_NO_ERROR ;
} /* sinc_multichan_vari_process */

// init codes

static void SR2_(sinc_reset) (SR2_(SRC_PRIVATE) *psrc)
{
  SR2_(SINC_FILTER) *filter;
  
  filter = (SR2_(SINC_FILTER)*) psrc->private_data;
  if(filter == NULL) return;
  
  filter->b_current = filter->b_end = 0;
  filter->b_real_end = -1;
  
  filter->src_ratio = filter->input_index = 0.0;
  
  memset (filter->buffer, 0, filter->b_len * sizeof (filter->buffer [0]));
  
  /* Set this for a sanity check */
  memset (filter->buffer + filter->b_len, 0xAA, filter->channels * sizeof (filter->buffer [0]));
}

long SR2_(sinc_set_converter) (SR2_(SRC_PRIVATE) *psrc, long src_enum)
{
  SR2_(SINC_FILTER) *filter, temp_filter;
  increment_t count;
  long bits;
  
  /* Quick sanity check. */
  if(SHIFT_BITS >= sizeof (increment_t) * 8 - 1)
    return SRC_ERR_SHIFT_BITS;
  
  if(psrc->private_data != NULL)
    {
      free (psrc->private_data);
      psrc->private_data = NULL;
    }
  
  memset(&temp_filter, 0, sizeof (temp_filter));

  temp_filter.sinc_magic_marker = SINC_MAGIC_MARKER;
  temp_filter.channels = psrc->channels;
  
  if (psrc->channels > ARRAY_LEN (temp_filter.left_calc))
    return SRC_ERR_BAD_CHANNEL_COUNT;
  else if (psrc->channels == 1)
    {
      psrc->const_process = SR2_(sinc_mono_vari_process) ;
      psrc->vari_process = SR2_(sinc_mono_vari_process) ;
    }
  else if (psrc->channels == 2)
    {
      psrc->const_process = SR2_(sinc_stereo_vari_process) ;
      psrc->vari_process = SR2_(sinc_stereo_vari_process) ;
    }
  else if (psrc->channels == 4)
    {
      psrc->const_process = SR2_(sinc_quad_vari_process) ;
      psrc->vari_process = SR2_(sinc_quad_vari_process) ;
    }
  else if (psrc->channels == 6)
    {
      psrc->const_process = SR2_(sinc_hex_vari_process) ;
      psrc->vari_process = SR2_(sinc_hex_vari_process) ;
    }
  else
    {
      psrc->const_process = SR2_(sinc_multichan_vari_process) ;
      psrc->vari_process = SR2_(sinc_multichan_vari_process) ;
    }
  
  psrc->reset = SR2_(sinc_reset);
  
  switch (src_enum)
    {
    case SRC_SINC_FASTEST :
      temp_filter.coeffs = fastest_coeffs.coeffs;
      temp_filter.coeff_half_len = ARRAY_LEN (fastest_coeffs.coeffs) - 1;
      temp_filter.index_inc = fastest_coeffs.increment;
      break;
      
    case SRC_SINC_MEDIUM_QUALITY :
      temp_filter.coeffs = mid_qual_coeffs.coeffs;
      temp_filter.coeff_half_len = ARRAY_LEN (mid_qual_coeffs.coeffs) - 1;
      temp_filter.index_inc = mid_qual_coeffs.increment;
      break;
      
    case SRC_SINC_BEST_QUALITY :
      temp_filter.coeffs = high_qual_coeffs.coeffs;
      temp_filter.coeff_half_len = ARRAY_LEN (high_qual_coeffs.coeffs) - 1;
      temp_filter.index_inc = high_qual_coeffs.increment;
      break;

#ifdef SRCNEWCOEFFS
    case SRC_SINC_SLOW_MEDIUM_QUALITY :
      temp_filter.coeffs = slow_mid_qual_coeffs.coeffs;
      temp_filter.coeff_half_len = ARRAY_LEN (slow_mid_qual_coeffs.coeffs) - 1;
      temp_filter.index_inc = slow_mid_qual_coeffs.increment;
      break;
      
    case SRC_SINC_SLOW_BEST_QUALITY :
      temp_filter.coeffs = slow_high_qual_coeffs.coeffs;
      temp_filter.coeff_half_len = ARRAY_LEN (slow_high_qual_coeffs.coeffs) - 1;
      temp_filter.index_inc = slow_high_qual_coeffs.increment;
      break;
#endif
      
    default :
      return SRC_ERR_BAD_CONVERTER;
    };
  
  /*
  ** FIXME : This needs to be looked at more closely to see if there is
  ** a better way. Need to look at SR2_(prepare_data) () at the same time.
  */
  
  temp_filter.b_len = lrint (2.5 * temp_filter.coeff_half_len / (temp_filter.index_inc * 1.0) * SRC_MAX_RATIO);
  temp_filter.b_len = MAX (temp_filter.b_len, 4096);
  temp_filter.b_len *= temp_filter.channels;
  
  if((filter = calloc (1, sizeof (SR2_(SINC_FILTER)) +
		       sizeof (filter->buffer [0]) *
		       (temp_filter.b_len + temp_filter.channels))) == NULL)
    return SRC_ERR_MALLOC_FAILED;
  
  *filter = temp_filter;
  memset (&temp_filter, 0xEE, sizeof (temp_filter));
  
  psrc->private_data = filter;
  
  SR2_(sinc_reset) (psrc);
  
  count = filter->coeff_half_len;
  for (bits = 0; (MAKE_INCREMENT_T (1) << bits) < count; bits++)
    count |= (MAKE_INCREMENT_T (1) << bits);
  
  if(bits + SHIFT_BITS - 1 >= (long) (sizeof (increment_t) * 8))
    return SRC_ERR_FILTER_LEN;
  
  return SRC_ERR_NO_ERROR;
}
