cog/Frameworks/GME/gme/Fir_Resampler.cpp

// Game_Music_Emu https://bitbucket.org/mpyne/game-music-emu/

#include "Fir_Resampler.h"

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

/* Copyright (C) 2004-2006 Shay Green. This module is free software; you
can redistribute it and/or modify it under the terms of the GNU Lesser
General Public License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. This
module 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 Lesser General Public License for more
details. You should have received a copy of the GNU Lesser General Public
License along with this module; if not, write to the Free Software Foundation,
Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */

#include "blargg_source.h"

#undef PI
#define PI 3.1415926535897932384626433832795029

static void gen_sinc( double rolloff, int width, double offset, double spacing, double scale,
		int count, short* out )
{
	double const maxh = 256;
	double const step = PI / maxh * spacing;
	double const to_w = maxh * 2 / width;
	double const pow_a_n = pow( rolloff, maxh );
	scale /= maxh * 2;
	
	double angle = (count / 2 - 1 + offset) * -step;
	while ( count-- )
	{
		*out++ = 0;
		double w = angle * to_w;
		if ( fabs( w ) < PI )
		{
			double rolloff_cos_a = rolloff * cos( angle );
			double num = 1 - rolloff_cos_a -
					pow_a_n * cos( maxh * angle ) +
					pow_a_n * rolloff * cos( (maxh - 1) * angle );
			double den = 1 - rolloff_cos_a - rolloff_cos_a + rolloff * rolloff;
			double sinc = scale * num / den - scale;
			
			out [-1] = (short) (cos( w ) * sinc + sinc);
		}
		angle += step;
	}
}

Fir_Resampler_::Fir_Resampler_( int width, sample_t* impulses_ ) :
	width_( width ),
	write_offset( width * stereo - stereo ),
	impulses( impulses_ )
{
	write_pos = 0;
	res       = 1;
	imp_phase = 0;
	skip_bits = 0;
	step      = stereo;
	ratio_    = 1.0;
}

Fir_Resampler_::~Fir_Resampler_() { }

void Fir_Resampler_::clear()
{
	imp_phase = 0;
	if ( buf.size() )
	{
		write_pos = &buf [write_offset];
		memset( buf.begin(), 0, write_offset * sizeof buf [0] );
	}
}

blargg_err_t Fir_Resampler_::buffer_size( int new_size )
{
	RETURN_ERR( buf.resize( new_size + write_offset ) );
	clear();
	return 0;
}
	
double Fir_Resampler_::time_ratio( double new_factor, double rolloff, double gain )
{
	ratio_ = new_factor;
	
	double fstep = 0.0;
	{
		double least_error = 2;
		double pos = 0;
		res = -1;
		for ( int r = 1; r <= max_res; r++ )
		{
			pos += ratio_;
			double nearest = floor( pos + 0.5 );
			double error = fabs( pos - nearest );
			if ( error < least_error )
			{
				res = r;
				fstep = nearest / res;
				least_error = error;
			}
		}
	}
	
	skip_bits = 0;
	
	step = stereo * (int) floor( fstep );
	
	ratio_ = fstep;
	fstep = fmod( fstep, 1.0 );
	
	double filter = (ratio_ < 1.0) ? 1.0 : 1.0 / ratio_;
	double pos = 0.0;
	input_per_cycle = 0;
	for ( int i = 0; i < res; i++ )
	{
		gen_sinc( rolloff, int (width_ * filter + 1) & ~1, pos, filter,
				double (0x7FFF * gain * filter),
				(int) width_, impulses + i * width_ );
		
		pos += fstep;
		input_per_cycle += step;
		if ( pos >= 0.9999999 )
		{
			pos -= 1.0;
			skip_bits |= 1 << i;
			input_per_cycle++;
		}
	}
	
	clear();
	
	return ratio_;
}

int Fir_Resampler_::input_needed( blargg_long output_count ) const
{
	blargg_long input_count = 0;
	
	unsigned long skip = skip_bits >> imp_phase;
	int remain = res - imp_phase;
	while ( (output_count -= 2) > 0 )
	{
		input_count += step + (skip & 1) * stereo;
		skip >>= 1;
		if ( !--remain )
		{
			skip = skip_bits;
			remain = res;
		}
		output_count -= 2;
	}
	
	long input_extra = input_count - (write_pos - &buf [(width_ - 1) * stereo]);
	if ( input_extra < 0 )
		input_extra = 0;
	return input_extra;
}

int Fir_Resampler_::avail_( blargg_long input_count ) const
{
	int cycle_count = input_count / input_per_cycle;
	int output_count = cycle_count * res * stereo;
	input_count -= cycle_count * input_per_cycle;
	
	blargg_ulong skip = skip_bits >> imp_phase;
	int remain = res - imp_phase;
	while ( input_count >= 0 )
	{
		input_count -= step + (skip & 1) * stereo;
		skip >>= 1;
		if ( !--remain )
		{
			skip = skip_bits;
			remain = res;
		}
		output_count += 2;
	}
	return output_count;
}

int Fir_Resampler_::skip_input( long count )
{
	int remain = write_pos - buf.begin();
	int max_count = remain - width_ * stereo;
	if ( count > max_count )
		count = max_count;
	
	remain -= count;
	write_pos = &buf [remain];
	memmove( buf.begin(), &buf [count], remain * sizeof buf [0] );
	
	return count;
}