/* http://www.slack.net/~ant/ */

#include "upsampler.h"

#include <string.h>
#include <limits.h>
#include <math.h>

/* Copyright (C) 2004-2007 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 */

#if INT_MAX < 0x7FFFFFFF
	#error "Requires that int type have at least 32 bits"
#endif

enum { stereo = 2 };
enum { write_offset = 2 * stereo - stereo };

typedef short sample_t;

sample_t upsampler_buf [RESAMPLER_BUF_SIZE];
sample_t* upsampler_write_pos;

enum { shift = 15 };
enum { unit = 1 << shift };

static int s_step;
static int s_pos;
static int s_prev [2];

void upsampler_set_rate( int in_rate, int out_rate )
{
	s_step = ((unsigned long) in_rate * unit + (unsigned) out_rate / 2) / out_rate;
	if ( !upsampler_write_pos )
		upsampler_clear();
}

void upsampler_clear( void )
{
	s_pos      = 0;
	s_prev [0] = 0;
	s_prev [1] = 0;
	upsampler_write_pos = upsampler_buf + write_offset;
	memset( upsampler_buf, 0, write_offset * sizeof upsampler_buf [0] );
}

int upsampler_read( sample_t* out_begin, int count )
{
	/* restrict helps optimizer by telling it that input and output don't overlap,
	allowing unrestricted reordering of memory loads and stores */
	sample_t* restrict out     = out_begin;
	sample_t* const    out_end = out + count;
	
	sample_t const* restrict in     = upsampler_buf;
	sample_t* const          in_end = upsampler_write_pos - write_offset;

	if ( in < in_end )
	{
		/* local copies help optimizer */
		int const step = s_step;
		int prev0 = s_prev [0];
		int prev1 = s_prev [1];
		int pos = s_pos;
		
		do
		{
			/* linear interpolation followed by two-point average */
			#define INTERP( i, out )\
			{\
				int t = in [0 + i] * (unit - pos) + in [2 + i] * pos;\
				out = (prev##i + t) >> (shift + 1);\
				prev##i = t;\
			}
			
			/* interpolate left and right */
			int out_0;
			INTERP( 0,                  out_0 )
			INTERP( 1, out [0] = out_0; out [1] )
			out += stereo;
			
			/* increment fractional position and separate whole part */
			pos += step;
			in += ((unsigned) pos >> shift) * stereo;
			pos &= unit - 1;
		}
		while ( in < in_end && out < out_end );
		
		s_prev [1] = prev1;
		s_prev [0] = prev0;
		s_pos = pos;
	}
	
	/* move unused samples to beginning of input buffer */
	{
		int result = out - out_begin;
		int remain = upsampler_write_pos - in;
		if ( remain < 0 )
			remain = 0;
		upsampler_write_pos = &upsampler_buf [remain];
		memmove( upsampler_buf, in, remain * sizeof *in );
		return result;
	}
}