/*

	TiMidity -- Experimental MIDI to WAVE converter
	Copyright (C) 1995 Tuukka Toivonen <toivonen@clinet.fi>

	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., 675 Mass Ave, Cambridge, MA 02139, USA.

   filter.c: written by Vincent Pagel ( pagel@loria.fr )

   implements fir antialiasing filter : should help when setting sample
   rates as low as 8Khz.

   April 95
	  - first draft

   22/5/95
	  - modify "filter" so that it simulate leading and trailing 0 in the buffer
   */

#include "pent_include.h"

#ifdef USE_TIMIDITY_MIDI

#	include "timidity.h"
#	include "timidity_common.h"
#	include "timidity_controls.h"
#	include "timidity_filter.h"
#	include "timidity_instrum.h"

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

#	ifdef NS_TIMIDITY
namespace NS_TIMIDITY {
#	endif

	/*  bessel  function   */
	static float ino(float x) {
		float y;
		float de;
		float e;
		float sde;
		int   i;

		y  = x / 2;
		e  = 1.0;
		de = 1.0;
		i  = 1;
		do {
			de  = de * y / static_cast<float>(i);
			sde = de * de;
			e += sde;
		} while (!((e * 1.0e-08 - sde > 0) || (i++ > 25)));
		return e;
	}

	/* Kaiser Window (symetric) */
	static void kaiser(float* w, int n, float beta) {
		auto xind = static_cast<float>((2 * n - 1) * (2 * n - 1));
		for (int i = 0; i < n; i++) {
			auto xi = static_cast<float>(i + 0.5);
			w[i] = ino(static_cast<float>(beta * sqrt(1. - 4 * xi * xi / xind)))
				   / ino(beta);
		}
	}

	/*
	 * fir coef in g, cuttoff frequency in fc
	 */
	static void designfir(float* g, float fc) {
		for (int i = 0; i < ORDER2; i++) {
			auto xi    = static_cast<float>(i + 0.5);
			auto omega = static_cast<float>(PI * xi);
			g[i]       = static_cast<float>(
                    sin(static_cast<double>(omega * fc)) / omega);
		}

		auto att  = 40.; /* attenuation  in  db */
		auto beta = static_cast<float>(
				exp(log(0.58417 * (att - 20.96)) * 0.4)
				+ 0.07886 * (att - 20.96));
		float w[ORDER2];
		kaiser(w, ORDER2, beta);

		/* Matrix product */
		for (int i = 0; i < ORDER2; i++) {
			g[i] = g[i] * w[i];
		}
	}

	/*
	 * FIR filtering -> apply the filter given by coef[] to the data buffer
	 * Note that we simulate leading and trailing 0 at the border of the
	 * data buffer
	 */
	static void filter(
			sample_t* result, const sample_t* data, sint32 length,
			const float coef[]) {
		sint16 peak = 0;

		/* Simulate leading 0 at the begining of the buffer */
		for (sint32 sample = 0; sample < ORDER2; sample++) {
			float  sum           = 0.0;
			sint32 sample_window = sample - ORDER2;

			for (sint32 i = 0; i < ORDER; i++) {
				sum += static_cast<float>(
						coef[i]
						* ((sample_window < 0) ? 0.0 : data[sample_window++]));
			}

			/* Saturation ??? */
			if (sum > 32767.) {
				sum = 32767.;
				peak++;
			}
			if (sum < -32768.) {
				sum = -32768;
				peak++;
			}
			result[sample] = static_cast<sample_t>(sum);
		}

		/* The core of the buffer  */
		for (sint32 sample = ORDER2; sample < length - ORDER + ORDER2;
			 sample++) {
			float  sum           = 0.0;
			sint32 sample_window = sample - ORDER2;

			for (sint32 i = 0; i < ORDER; i++) {
				sum += data[sample_window++] * coef[i];
			}

			/* Saturation ??? */
			if (sum > 32767.) {
				sum = 32767.;
				peak++;
			}
			if (sum < -32768.) {
				sum = -32768;
				peak++;
			}
			result[sample] = static_cast<sample_t>(sum);
		}

		/* Simulate 0 at the end of the buffer */
		for (sint32 sample = length - ORDER + ORDER2; sample < length;
			 sample++) {
			float  sum           = 0.0;
			sint32 sample_window = sample - ORDER2;

			for (sint32 i = 0; i < ORDER; i++) {
				sum += static_cast<float>(
						coef[i]
						* ((sample_window >= length) ? 0.0
													 : data[sample_window++]));
			}

			/* Saturation ??? */
			if (sum > 32767.) {
				sum = 32767.;
				peak++;
			}
			if (sum < -32768.) {
				sum = -32768;
				peak++;
			}
			result[sample] = static_cast<sample_t>(sum);
		}

		if (peak) {
			ctl->cmsg(
					CMSG_ERROR, VERB_NORMAL, "Saturation %2.3f %%.",
					100.0 * peak / static_cast<float>(length));
		}
	}

	/***********************************************************************/
	/* Prevent aliasing by filtering any freq above the output_rate        */
	/*                                                                     */
	/* I don't worry about looping point -> they will remain soft if they  */
	/* were already                                                        */
	/***********************************************************************/
	void antialiasing(Sample* sp, sint32 output_rate) {
		float fir_symetric[ORDER];
		float fir_coef[ORDER2];

		ctl->cmsg(
				CMSG_INFO, VERB_NOISY, "Antialiasing: Fsample=%iKHz",
				sp->sample_rate);

		/* No oversampling  */
		if (output_rate >= sp->sample_rate) {
			return;
		}

		/* cutoff frequency [0..1.0] FREQ_CUT/SAMP_FREQ*/
		float freq_cut = static_cast<float>(output_rate) / sp->sample_rate;
		ctl->cmsg(
				CMSG_INFO, VERB_NOISY, "Antialiasing: cutoff=%f%%",
				freq_cut * 100.);

		designfir(fir_coef, freq_cut);

		/* Make the filter symetric */
		for (int i = 0; i < ORDER2; i++) {
			fir_symetric[ORDER - 1 - i] = fir_symetric[i]
					= fir_coef[ORDER2 - 1 - i];
		}

		/* We apply the filter we have designed on a copy of the patch */
		auto* temp = safe_Malloc<sample_t>(sp->data_length);
		memcpy(temp, sp->data, sp->data_length);

		filter(sp->data, temp, sp->data_length / sizeof(sample_t),
			   fir_symetric);

		free(temp);
	}

#	ifdef NS_TIMIDITY
}
#	endif

#endif    // USE_TIMIDITY_MIDI
