/*****************************************************************************

        GRAOUMF TRACKER 2
        Author: Laurent de Soras, 1996-2016

--- Legal stuff ---

This program is free software. It comes without any warranty, to
the extent permitted by applicable law. You can redistribute it
and/or modify it under the terms of the Do What The Fuck You Want
To Public License, Version 2, as published by Sam Hocevar. See
http://sam.zoy.org/wtfpl/COPYING for more details.

*****************************************************************************/



/*\\\ FICHIERS INCLUDE \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/

#include	"base.h"
#include	"base.hpp"
#include	"base_ct.h"
#include	"log.h"
#include	"memory.h"
#include	"ReconstructionFilter.h"
#include	"splhandl.h"

#include	<cassert>
#include	<cmath>
#include	<cstdio>
#include	<cstdlib>
#include	<cstring>



/*\\\ CONSTANTES PUBLIQUES \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/



/*\\\ CONSTANTES PRIVEES \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/

#define	ReconstructionFilter_MAX_RESAMPLE_RATE	256



/*\\\ TYPES & STRUCTURES PRIVEES \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/



/*\\\ DEFINITION DES VARIABLES DE CLASSE PUBLIQUES \\\\\\\\\\\\\\\\\\\\\\\\\*/



/*\\\ DEFINITION DES VARIABLES DE CLASSE PRIVEES \\\\\\\\\\\\\\\\\\\\\\\\\\\*/



/*\\\ METHODES PUBLIQUES \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/



/*==========================================================================*/
/*      Nom: (constructeur)                                                 */
/*      Description: Initialise                                             */
/*      Parametres en entree:                                               */
/*        - width: largeur du sinus cardinal utilise par le filtre (en      */
/*                 samples)                                                 */
/*==========================================================================*/

ReconstructionFilter::ReconstructionFilter (int width)
{
	_buffer_ptr = NULL;

	if (width <= 0)
	{
		LOG_printf ("ReconstructionFilter::ReconstructionFilter: Error: width parameter (%d) must be > 0.\n",
		            width);
		return;
	}
	_width = width;

	_buf_len = 1;
	while (_buf_len < _width + ReconstructionFilter_MAX_RESAMPLE_RATE)
	{
		_buf_len <<= 1;
	}

	_buffer_ptr = (double (*)[2]) MALLOC (_buf_len * sizeof (**_buffer_ptr) * 2);
	if (_buffer_ptr == NULL)
	{
		LOG_printf ("ReconstructionFilter::ReconstructionFilter: Error: couldn't allocate memory for buffer.\n");
		return;
	}

	clear_buffer ();
}



/*==========================================================================*/
/*      Nom: (destructeur)                                                  */
/*      Description: Detruit                                                */
/*==========================================================================*/

ReconstructionFilter::~ReconstructionFilter ()
{
	if (_buffer_ptr != NULL)
	{
		FREE (_buffer_ptr);
		_buffer_ptr = NULL;
	}
}



/*==========================================================================*/
/*      Nom:                                                                */
/*      Description:                                                        */
/*      Parametres en entree:                                               */
/*      Parametres en sortie:                                               */
/*      Parametres en entree/sortie:                                        */
/*      Retour:                                                             */
/*==========================================================================*/

void	ReconstructionFilter::resample_and_mix (const SWORD *s_buffer_ptr, float *d_buffer_ptr,
		                                        LWORD length, int s_stereo, int d_stereo,
		                                        SLWORD volume, ULWORD pos_frac, double inv_resample_factor)
{
	double	sinc_step;
	double	sinc_pos;
	double	channel_amp;
	double	multiplier;
	double	val [2];
	int		chn_cnt;
	int		buf_len;
	int		buf_pos;
	int		buf_cnt;
	double	sinc_val;
	double	new_val;
	double	d_pos;
	double	old_d_pos;
	double	temp_dbl;
	int		nbr_spl;
	int		sample_cnt;
	int		buf_mask;
	double	resample_factor;
	double	window_scale;
	double	window_pos;

	buf_mask = _buf_len - 1;
	resample_factor = 1.0 / inv_resample_factor;
	multiplier = 1.0 / 0x100;
	if (pos_frac == 0)
	{
		d_pos = 0;
	}
	else
	{
		d_pos = 1.0 - (double)pos_frac / 4294967296.0;
		d_pos *= resample_factor;
		s_buffer_ptr += s_stereo;
	}

	channel_amp = multiplier * volume;
	if (resample_factor > 1)
	{
		sinc_step =	PI * inv_resample_factor;
	}
	else
	{
		sinc_step = PI;
		channel_amp *= resample_factor;
	}

	window_scale = PI / ((_width >> 1) * sinc_step);

/*______________________________________________
 *
 * Reechantillonnage
 *______________________________________________
 */

	old_d_pos = 0;
	for ( ; ; )
	{
		/* Ecrit les samples destination (les nouveaux de la boucle precedente,
		   ou les vieux de la frame d'avant qui n'avaient pas encore ete ecrits
		   parce qu'on n'en voulait pas. */
		nbr_spl = (int)(floor (d_pos) - floor (old_d_pos));
		for (sample_cnt = nbr_spl; sample_cnt > 0; sample_cnt --)
		{
			for (chn_cnt = 0; chn_cnt < d_stereo; chn_cnt ++)
			{
				*d_buffer_ptr = float (_buffer_ptr [_buffer_pos] [chn_cnt]);
				d_buffer_ptr ++;
			}

			/* Efface du buffer le sample qu'on vient de prendre car on n'en a
			   plus besoin. Cette case memoire du buffer sera utilisee par le
			   sample suivant donc doit etre intialisee a 0 maintenant. */
			for (chn_cnt = 0; chn_cnt < 2; chn_cnt ++)
			{
				_buffer_ptr [_buffer_pos] [chn_cnt] = 0;
			}

			/* Sample destination suivant */
			_buffer_pos = (_buffer_pos + 1) & buf_mask;
			length --;
			if (length <= 0)
			{
				return;
			}
		}

		/* Preleve le(s) sample(s) source(s) */
		for (chn_cnt = 0; chn_cnt < s_stereo; chn_cnt ++)
		{
			val [chn_cnt] = *s_buffer_ptr * channel_amp;
			++ s_buffer_ptr;
		}

		/* En cas de source mono, on repartit le sample sur les autres canaux */
		if (s_stereo == 1)
		{
			val [1] = val [0];
		}

		/* En cas de destination mono, on mixe tous les canaux sur le premier */
		if (d_stereo == 1)
		{
			val [0] += val [1];
		}

		/* Reconstruction */
		sinc_pos =   (modf (d_pos, &temp_dbl) + (_width >> 1))
		           * (-sinc_step);
		buf_pos = _buffer_pos;
		buf_len = _width;
		do
		{
			buf_cnt = buf_len;
			if (buf_cnt + buf_pos >= _buf_len)
			{
				buf_cnt = _buf_len - buf_pos;
			}
			buf_len -= buf_cnt;
			do
			{
				/* Calcul de la valeur de la fonction du filtre (qui en est la
				   reponse impulsionnelle). Comme le filtre est un passe-bas, sa
				   reponse est un sinc () sur lequel on va appliquer une fenetre
				   pour le tronquer. */
				if (BASE_is_null (sinc_pos))
				{
					sinc_val = 1;
				}
				else
				{
					/* Sinus cardinal et fenetre de Blackman */
					window_pos = sinc_pos * window_scale;
					sinc_val =   (sin (sinc_pos) / sinc_pos)
					           * (  0.42
					              + 0.5 * cos (window_pos)
					              + 0.08 * cos (window_pos * 2));
				}

				for (chn_cnt = 0; chn_cnt < d_stereo; chn_cnt ++)
				{
					new_val = val [chn_cnt] * sinc_val;
					_buffer_ptr [buf_pos] [chn_cnt] += new_val;
					if (d_stereo == 1)
					{
						_buffer_ptr [buf_pos] [1] += new_val;
					}
				}

				sinc_pos += sinc_step;
				buf_pos ++;
				buf_cnt --;
			}
			while (buf_cnt > 0);
			buf_pos &= buf_mask;
		}
		while (buf_len > 0);

		/* Sample(s) destination suivant(s) */
		old_d_pos = d_pos;
		d_pos += resample_factor;
	}
}



/*==========================================================================*/
/*      Nom:                                                                */
/*      Description:                                                        */
/*      Parametres en entree:                                               */
/*      Parametres en sortie:                                               */
/*      Parametres en entree/sortie:                                        */
/*      Retour:                                                             */
/*==========================================================================*/

void	ReconstructionFilter::clear_buffer (void)
{
	if (_buffer_ptr != NULL)
	{
		memset (_buffer_ptr, 0, _buf_len * sizeof (**_buffer_ptr) * 2);
	}

	_buffer_pos = 0;
}



void	ReconstructionFilter::copy_state (const ReconstructionFilter &other)
{
	assert (&other != 0);

	if (&other != this)
	{
		assert (other._width   == _width);
		assert (other._buf_len == _buf_len);

		memcpy (
			_buffer_ptr,
			other._buffer_ptr,
			_buf_len * sizeof (**_buffer_ptr) * 2
		);
		_buffer_pos = other._buffer_pos;
	}
}



/*\\\ METHODES PRIVEES \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\*/



/****************************************************************************/
/*                                                                          */
/*                                                                          */
/*                                                                          */
/****************************************************************************/



/*==========================================================================*/
/*      Nom:                                                                */
/*      Description:                                                        */
/*      Parametres en entree:                                               */
/*      Parametres en sortie:                                               */
/*      Parametres en entree/sortie:                                        */
/*      Retour:                                                             */
/*==========================================================================*/
