path: root/src/lingot-core.c

//-*- C++ -*-
/*
 * lingot, a musical instrument tuner.
 *
 * Copyright (C) 2004-2007  Ibán Cereijo Graña, Jairo Chapela Martínez.
 *
 * This file is part of lingot.
 *
 * lingot 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.
 *
 * lingot 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 lingot; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

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

#ifndef LIB_FFTW
# include "lingot-complex.h"
#endif

#include "lingot-fft.h"
#include "lingot-signal.h"
#include "lingot-core.h"
#include "lingot-config.h"
#include "lingot-yin.h"

LingotCore* lingot_core_new(LingotConfig* conf) {

	LingotCore* core = malloc(sizeof(LingotCore));

	core->conf = conf;
	core->running = 1;

# ifdef LIBSNDOBJ
#  ifdef OSS
	core->audio = new SndRTIO(1, SND_INPUT, 512, 512, SHORTSAM_LE, NULL,
			core->conf->read_buffer_size*core->conf->oversampling, core->conf->sample_rate, core->conf->audio_dev);
#  endif		  
#  ifdef ALSA
	core->audio = new SndRTIO(1, SND_INPUT, 512, 4, SHORTSAM_LE, NULL,
			core->conf->read_buffer_size*core->conf->oversampling, core->conf->sample_rate, "plughw:0,0");
#  endif		  
# else
	// creates an audio handler.
	core->audio = lingot_audio_new(1, core->conf->sample_rate,
			core->conf->audio_dev);
# endif

	// since the SPD is simmetrical, we only store the 1st half.
	if (core->conf->fft_size > 256) {
		core->spd_fft = malloc((core->conf->fft_size >> 1)*sizeof(FLT));
		core->X = malloc((core->conf->fft_size >> 1)*sizeof(FLT));
		memset(core->spd_fft, 0, (core->conf->fft_size >> 1)*sizeof(FLT));
		memset(core->X, 0, (core->conf->fft_size >> 1)*sizeof(FLT));
	} else { // if the fft size is 256, we store the whole signal for representation.
		core->spd_fft = malloc((core->conf->fft_size)*sizeof(FLT));
		core->X = malloc((core->conf->fft_size)*sizeof(FLT));
		memset(core->spd_fft, 0, core->conf->fft_size*sizeof(FLT));
		memset(core->X, 0, (core->conf->fft_size)*sizeof(FLT));
	}

	core->spd_dft = malloc((core->conf->dft_size)*sizeof(FLT));
	memset(core->spd_dft, 0, core->conf->dft_size*sizeof(FLT));

	core->diff2_spd_fft = malloc(core->conf->fft_size*sizeof(FLT)); // 2nd derivative from SPD.
	memset(core->diff2_spd_fft, 0, core->conf->fft_size*sizeof(FLT));

	memset(core->spd_dft, 0, core->conf->dft_size*sizeof(FLT));

#ifndef LIB_FFTW  
	lingot_fft_create_phase_factors(conf); // creates the phase factors for FFT.
	core->fft_out = malloc((core->conf->fft_size)*sizeof(LingotComplex)); // complex signal in freq domain.
	memset(core->fft_out, 0, core->conf->fft_size*sizeof(LingotComplex));
#else
	core->fftw_in = new fftw_complex[core->conf->fft_size];
	memset(core->fftw_in, 0, core->conf->fft_size*sizeof(fftw_complex));
	core->fftw_out = new fftw_complex[core->conf->fft_size];
	memset(core->fftw_out, 0, core->conf->fft_size*sizeof(fftw_complex));
	core->fftwplan = fftw_create_plan(core->conf->fft_size, FFTW_FORWARD, FFTW_ESTIMATE);
#endif

	// read buffer from soundcard.
	core->read_buffer= malloc((core->conf->read_buffer_size
			*core->conf->oversampling)*sizeof(SAMPLE_TYPE));
	memset(core->read_buffer, 0, (core->conf->read_buffer_size
			*core->conf->oversampling)*sizeof(SAMPLE_TYPE));

	// read buffer from soundcard in floating point format.
	core->flt_read_buffer= malloc((core->conf->read_buffer_size
			*core->conf->oversampling)*sizeof(FLT));
	memset(core->flt_read_buffer, 0, (core->conf->read_buffer_size
			*core->conf->oversampling)*sizeof(FLT));

	// stored samples.
	core->temporal_buffer = malloc((core->conf->temporal_buffer_size)
			*sizeof(FLT));
	memset(core->temporal_buffer, 0, core->conf->temporal_buffer_size
			*sizeof(FLT));

	core->hamming_window_temporal = malloc((core->conf->temporal_buffer_size)
			*sizeof(FLT));
	lingot_signal_hamming_window(core->conf->temporal_buffer_size,
			core->hamming_window_temporal);
	core->hamming_window_fft = malloc((core->conf->fft_size) *sizeof(FLT));
	lingot_signal_hamming_window(core->conf->fft_size, core->hamming_window_fft);

	core->windowed_temporal_buffer = malloc((core->conf->temporal_buffer_size)
			*sizeof(FLT));
	memset(core->windowed_temporal_buffer, 0, core->conf->temporal_buffer_size
			*sizeof(FLT));
	core->windowed_fft_buffer = malloc((core->conf->fft_size) *sizeof(FLT));
	memset(core->windowed_fft_buffer, 0, core->conf->fft_size *sizeof(FLT));

	/*
	 * 8 order Chebyshev filters, with wc=0.9/i (normalized respect to
	 * Pi). We take 0.9 instead of 1 to leave a 10% of safety margin,
	 * in order to avoid aliased frequencies near to w=Pi, due to non
	 * ideality of the filter.
	 *
	 * The cut frequencies wc=Pi/i, with i=1..20, correspond with the
	 * oversampling factor, avoiding aliasing at decimation.
	 * 
	 * Why Chebyshev filters?, for a given order, those filters yield
	 * abrupt falls than other ones as Butterworth, making the most of
	 * the order. Although Chebyshev filters affects more to the phase,
	 * it doesn't matter due to the analysis is made on the signal
	 * power distribution (only module).
	 */
	core->antialiasing_filter= lingot_filter_cheby_design(8, 0.5, 0.9
			/core->conf->oversampling);

	// ------------------------------------------------------------

	core->freq = 0.0;
	return core;
}

// -----------------------------------------------------------------------

/* Deallocate resources */
void lingot_core_destroy(LingotCore* core) {
	pthread_attr_destroy(&core->attr);

#ifdef LIB_FFTW  
	fftw_destroy_plan(core->fftwplan);
	free(core->fftw_in);
	free(core->fftw_out);
#else
	lingot_fft_destroy_phase_factors(); // destroy phase factors.
	free(core->fft_out);
#endif

	lingot_audio_destroy(core->audio);

	free(core->spd_fft);
	free(core->X);
	free(core->spd_dft);
	free(core->diff2_spd_fft);
	free(core->read_buffer);
	free(core->flt_read_buffer);
	free(core->temporal_buffer);

	free(core->windowed_temporal_buffer);
	free(core->windowed_fft_buffer);

	free(core->hamming_window_temporal);
	free(core->hamming_window_fft);

	lingot_filter_destroy(core->antialiasing_filter);

	free(core);
}

// -----------------------------------------------------------------------

// signal decimation with antialiasing, in & out overlapables.
void lingot_core_decimate(LingotCore* core, FLT* in, FLT* out) {
	register unsigned int i, j;

	// low pass filter to avoid aliasing.
	lingot_filter_filter(core->antialiasing_filter,
			core->conf->read_buffer_size*core->conf->oversampling, in, in);

	// compression.
	for (i = j = 0; i < core->conf->read_buffer_size; i++, j
			+= core->conf->oversampling)
		out[i] = in[j];
}

// -----------------------------------------------------------------------

void lingot_core_process(LingotCore* core) {
	register unsigned int i, k; // loop variables.
	FLT delta_w_FFT = 2.0*M_PI/core->conf->fft_size;
	// FFT resolution in rads.  

# ifdef LIBSNDOBJ
	audio->Read();
	for (i = 0; i < core->conf->oversampling*core->conf->read_buffer_size; i++)
	flt_read_buffer[i] = audio->Output(i);
# else
	if ((lingot_audio_read(core->audio, core->read_buffer,
			core->conf->oversampling*core->conf->read_buffer_size
					*sizeof(SAMPLE_TYPE)))< 0) {
		//perror("Error reading samples");
		return;
	}

	for (i = 0; i < core->conf->oversampling*core->conf->read_buffer_size; i++)
		core->flt_read_buffer[i] = core->read_buffer[i];
# endif

	//
	// just readed:
	//
	//  ----------------------------
	// |bxxxbxxxbxxxbxxxbxxxbxxxbxxx|
	//  ----------------------------
	//
	// <----------------------------> read_buffer_size*oversampling
	//

	/* we shift the temporal window to leave a hollow where place the new piece
	 of data read. The buffer is actually a queue. */
	if (core->conf->temporal_buffer_size > core->conf->read_buffer_size)
		memcpy(core->temporal_buffer,
				&core->temporal_buffer[core->conf->read_buffer_size],
				(core->conf->temporal_buffer_size
						- core->conf->read_buffer_size) *sizeof(FLT));

	//
	// previous buffer situation:
	//
	//  ------------------------------------------
	// | xxxxxxxxxxxxxxxxxxxxxx | yyyyy | aaaaaaa |
	//  ------------------------------------------
	//                                    <------> read_buffer_size
	//                           <---------------> fft_size
	//  <----------------------------------------> temporal_buffer_size
	//
	// new situation:
	//
	//  ------------------------------------------
	// | xxxxxxxxxxxxxxxxyyyyaa | aaaaa |         |
	//  ------------------------------------------   
	//

	/* we decimate the read signal and put it at the end of the buffer. */
	if (core->conf->oversampling > 1)
		lingot_core_decimate(
				core,
				core->flt_read_buffer,
				&core->temporal_buffer[core->conf->temporal_buffer_size - core->conf->read_buffer_size]);
	else
		memcpy(
				&core->temporal_buffer[core->conf->temporal_buffer_size - core->conf->read_buffer_size],
				core->flt_read_buffer, core->conf->read_buffer_size*sizeof(FLT));
	//
	//  ------------------------------------------
	// | xxxxxxxxxxxxxxxxyyyyaa | aaaaa | bbbbbbb |
	//  ------------------------------------------ 
	//

	if (core->conf->estimator == 1) core->freq = core->conf->sample_rate / (FLT)core->conf->oversampling / yin(core);

	// ----------------- TRANSFORMATION TO FREQUENCY DOMAIN ----------------

	FLT _1_N2 = 1.0/(core->conf->fft_size*core->conf->fft_size);
	// SPD normalization constant

	// windowing
	for (i = 0; i < core->conf->fft_size; i++) {
		core->windowed_fft_buffer[i]
				= core->temporal_buffer[core->conf->temporal_buffer_size - core->conf->fft_size + i]
						*core->hamming_window_fft[i];
	}

# ifdef LIB_FFTW
	for (i = 0; i < core->conf->fft_size; i++)
	fftw_in[i].re = core->windowed_fft_buffer[i];

	// transformation.
	fftw_one(core->fftwplan, core->fftw_in, core->fftw_out);

	// esteem of SPD from FFT. (normalized squared module)
	for (i = 0; i < ((core->conf->fft_size > 256) ? (core->conf->fft_size >> 1) : 256); i++)
	core->spd_fft[i] = (core->fftw_out[i].re*core->fftw_out[i].re +
			core->fftw_out[i].im*core->fftw_out[i].im)*_1_N2;
# else

	// transformation.
	lingot_fft_fft(core->windowed_fft_buffer, core->fft_out,
			core->conf->fft_size);

	// esteem of SPD from FFT. (normalized squared module)
	for (i = 0; i < ((core->conf->fft_size > 256) ? (core->conf->fft_size >> 1)
			: 256); i++)
		core->spd_fft[i] = (core->fft_out[i].r*core->fft_out[i].r
				+ core->fft_out[i].i*core->fft_out[i].i)*_1_N2;
# endif


	// representable piece
	memcpy(core->X, core->spd_fft,
			((core->conf->fft_size > 256) ? (core->conf->fft_size >> 1) : 256)
					*sizeof(FLT));

	if (core->conf->estimator == 1 ) return;

	// truncated 2nd derivative esteem, to enhance peaks
	core->diff2_spd_fft[0] = 0.0;
	for (i = 1; i < (core->conf->fft_size >> 1) - 1; i++) {
		core->diff2_spd_fft[i] = 2.0*core->spd_fft[i]- core->spd_fft[i - 1]
				- core->spd_fft[i + 1]; // centred 2nd order derivative, to avoid group delay.
		if (core->diff2_spd_fft[i] < 0.0)
			core->diff2_spd_fft[i] = 0.0; // truncation
	}

	// peaks searching in that signal.
	int Mi = lingot_signal_get_fundamental_peak(core->conf, core->spd_fft,
			core->diff2_spd_fft, (core->conf->fft_size >> 1)); // take the fundamental peak.

	if (Mi == (signed) (core->conf->fft_size >> 1)) {
		core->freq = 0.0;
		return;
	}

	FLT w = (Mi - 1)*delta_w_FFT;
	// frecuencia de la muestra anterior al pico.

	//  Approximation to fundamental frequency by selective DFTs
	// ---------------------------------------------------------

	FLT d_w = delta_w_FFT;
	for (k = 0; k < core->conf->dft_number; k++) {

		d_w = 2.0*d_w/(core->conf->dft_size - 1); // resolution in rads.

		if (k == 0) {
			lingot_fft_spd(core->windowed_fft_buffer, core->conf->fft_size, w
					+ d_w, d_w, &core->spd_dft[1], core->conf->dft_size- 2);
			core->spd_dft[0] = core->spd_fft[Mi - 1];
			core->spd_dft[core->conf->dft_size - 1] = core->spd_fft[Mi + 1]; // 2 samples known.
		} else
			lingot_fft_spd(core->windowed_fft_buffer, core->conf->fft_size, w,
					d_w, core->spd_dft, core->conf->dft_size);

		lingot_signal_get_max(core->spd_dft, core->conf->dft_size, &Mi); // search the maximum.

		w += (Mi - 1)*d_w; // previous sample to the peak.
	}

	w += d_w; // approximation by DFTs.

	// windowing
	for (i = 0; i < core->conf->temporal_buffer_size; i++) {
		core->windowed_temporal_buffer[i] = core->temporal_buffer[i]
				*core->hamming_window_temporal[i];
	}

	//  Maximum finding by Newton-Raphson
	// -----------------------------------

	FLT wk = -1.0e5;
	FLT wkm1 = w;
	// first iterator set to the current approximation.
	FLT d1_SPD, d2_SPD;

	for (k = 0; (k < core->conf->max_nr_iter) && (fabs(wk - wkm1) > 1.0e-8); k++) {
		wk = wkm1;

		// ! we use the WHOLE temporal window for greater precission.
		lingot_fft_spd_diffs(core->windowed_temporal_buffer,
				core->conf->temporal_buffer_size, wk, &d1_SPD, &d2_SPD);
		//printf("%f %f %f\n", wk, d1_SPD, d2_SPD);
		wkm1 = wk - d1_SPD/d2_SPD;
	}

	w = wkm1; // frequency in rads.
	core->freq = (w*core->conf->sample_rate) /(2.0*M_PI
			*core->conf->oversampling); // analog frequency.

}

/* start running the core in another thread */
void lingot_core_start(LingotCore* core) {
	pthread_attr_init(&core->attr);

	// detached thread.
	//  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
	pthread_create(&core->thread, &core->attr, (void* (*)(void*)) lingot_core_run, core);
}

/* stop running the core */
void lingot_core_stop(LingotCore* core) {
	void* thread_result;

	core->running = 0;

	// wait for the thread exit
	pthread_join(core->thread, &thread_result);
}

/* run the core */
void lingot_core_run(LingotCore* core) {
	while (core->running) {
		lingot_core_process(core); // process new data block.
	}
}