Chris@0: /*
Chris@0: ** Copyright (C) 2002-2011 Erik de Castro Lopo <erikd@mega-nerd.com>
Chris@0: **
Chris@0: ** This program is free software; you can redistribute it and/or modify
Chris@0: ** it under the terms of the GNU General Public License as published by
Chris@0: ** the Free Software Foundation; either version 2 of the License, or
Chris@0: ** (at your option) any later version.
Chris@0: **
Chris@0: ** This program is distributed in the hope that it will be useful,
Chris@0: ** but WITHOUT ANY WARRANTY; without even the implied warranty of
Chris@0: ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
Chris@0: ** GNU General Public License for more details.
Chris@0: **
Chris@0: ** You should have received a copy of the GNU General Public License
Chris@0: ** along with this program; if not, write to the Free Software
Chris@0: ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
Chris@0: */
Chris@0: 
Chris@0: #include "config.h"
Chris@0: 
Chris@0: #include "util.h"
Chris@0: 
Chris@0: #if (HAVE_FFTW3 == 1)
Chris@0: 
Chris@0: #include <stdio.h>
Chris@0: #include <stdlib.h>
Chris@0: #include <string.h>
Chris@0: #include <math.h>
Chris@0: 
Chris@0: #include <fftw3.h>
Chris@0: 
Chris@0: #define	MAX_SPEC_LEN	(1<<18)
Chris@0: #define	MAX_PEAKS		10
Chris@0: 
Chris@0: static void log_mag_spectrum (double *input, int len, double *magnitude) ;
Chris@0: static void smooth_mag_spectrum (double *magnitude, int len) ;
Chris@0: static double find_snr (const double *magnitude, int len, int expected_peaks) ;
Chris@0: 
Chris@0: typedef struct
Chris@0: {	double	peak ;
Chris@0: 	int		index ;
Chris@0: } PEAK_DATA ;
Chris@0: 
Chris@0: double
Chris@0: calculate_snr (float *data, int len, int expected_peaks)
Chris@0: {	static double magnitude [MAX_SPEC_LEN] ;
Chris@0: 	static double datacopy [MAX_SPEC_LEN] ;
Chris@0: 
Chris@0: 	double snr = 200.0 ;
Chris@0: 	int k ;
Chris@0: 
Chris@0: 	if (len > MAX_SPEC_LEN)
Chris@0: 	{	printf ("%s : line %d : data length too large.\n", __FILE__, __LINE__) ;
Chris@0: 		exit (1) ;
Chris@0: 		} ;
Chris@0: 
Chris@0: 	for (k = 0 ; k < len ; k++)
Chris@0: 		datacopy [k] = data [k] ;
Chris@0: 
Chris@0: 	/* Pad the data just a little to speed up the FFT. */
Chris@0: 	while ((len & 0x1F) && len < MAX_SPEC_LEN)
Chris@0: 	{	datacopy [len] = 0.0 ;
Chris@0: 		len ++ ;
Chris@0: 		} ;
Chris@0: 
Chris@0: 	log_mag_spectrum (datacopy, len, magnitude) ;
Chris@0: 	smooth_mag_spectrum (magnitude, len / 2) ;
Chris@0: 
Chris@0: 	snr = find_snr (magnitude, len, expected_peaks) ;
Chris@0: 
Chris@0: 	return snr ;
Chris@0: } /* calculate_snr */
Chris@0: 
Chris@0: /*==============================================================================
Chris@0: ** There is a slight problem with trying to measure SNR with the method used
Chris@0: ** here; the side lobes of the windowed FFT can look like a noise/aliasing peak.
Chris@0: ** The solution is to smooth the magnitude spectrum by wiping out troughs
Chris@0: ** between adjacent peaks as done here.
Chris@0: ** This removes side lobe peaks without affecting noise/aliasing peaks.
Chris@0: */
Chris@0: 
Chris@0: static void linear_smooth (double *mag, PEAK_DATA *larger, PEAK_DATA *smaller) ;
Chris@0: 
Chris@0: static void
Chris@0: smooth_mag_spectrum (double *mag, int len)
Chris@0: {	PEAK_DATA peaks [2] ;
Chris@0: 
Chris@0: 	int k ;
Chris@0: 
Chris@0: 	memset (peaks, 0, sizeof (peaks)) ;
Chris@0: 
Chris@0: 	/* Find first peak. */
Chris@0: 	for (k = 1 ; k < len - 1 ; k++)
Chris@0: 	{	if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1])
Chris@0: 		{	peaks [0].peak = mag [k] ;
Chris@0: 			peaks [0].index = k ;
Chris@0: 			break ;
Chris@0: 			} ;
Chris@0: 		} ;
Chris@0: 
Chris@0: 	/* Find subsequent peaks ans smooth between peaks. */
Chris@0: 	for (k = peaks [0].index + 1 ; k < len - 1 ; k++)
Chris@0: 	{	if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1])
Chris@0: 		{	peaks [1].peak = mag [k] ;
Chris@0: 			peaks [1].index = k ;
Chris@0: 
Chris@0: 			if (peaks [1].peak > peaks [0].peak)
Chris@0: 				linear_smooth (mag, &peaks [1], &peaks [0]) ;
Chris@0: 			else
Chris@0: 				linear_smooth (mag, &peaks [0], &peaks [1]) ;
Chris@0: 			peaks [0] = peaks [1] ;
Chris@0: 			} ;
Chris@0: 		} ;
Chris@0: 
Chris@0: } /* smooth_mag_spectrum */
Chris@0: 
Chris@0: static void
Chris@0: linear_smooth (double *mag, PEAK_DATA *larger, PEAK_DATA *smaller)
Chris@0: {	int k ;
Chris@0: 
Chris@0: 	if (smaller->index < larger->index)
Chris@0: 	{	for (k = smaller->index + 1 ; k < larger->index ; k++)
Chris@0: 			mag [k] = (mag [k] < mag [k - 1]) ? 0.999 * mag [k - 1] : mag [k] ;
Chris@0: 		}
Chris@0: 	else
Chris@0: 	{	for (k = smaller->index - 1 ; k >= larger->index ; k--)
Chris@0: 			mag [k] = (mag [k] < mag [k + 1]) ? 0.999 * mag [k + 1] : mag [k] ;
Chris@0: 		} ;
Chris@0: 
Chris@0: } /* linear_smooth */
Chris@0: 
Chris@0: /*==============================================================================
Chris@0: */
Chris@0: 
Chris@0: static int
Chris@0: peak_compare (const void *vp1, const void *vp2)
Chris@0: {	const PEAK_DATA *peak1, *peak2 ;
Chris@0: 
Chris@0: 	peak1 = (const PEAK_DATA*) vp1 ;
Chris@0: 	peak2 = (const PEAK_DATA*) vp2 ;
Chris@0: 
Chris@0: 	return (peak1->peak < peak2->peak) ? 1 : -1 ;
Chris@0: } /* peak_compare */
Chris@0: 
Chris@0: static double
Chris@0: find_snr (const double *magnitude, int len, int expected_peaks)
Chris@0: {	PEAK_DATA peaks [MAX_PEAKS] ;
Chris@0: 
Chris@0: 	int		k, peak_count = 0 ;
Chris@0: 	double	snr ;
Chris@0: 
Chris@0: 	memset (peaks, 0, sizeof (peaks)) ;
Chris@0: 
Chris@0: 	/* Find the MAX_PEAKS largest peaks. */
Chris@0: 	for (k = 1 ; k < len - 1 ; k++)
Chris@0: 	{	if (magnitude [k - 1] < magnitude [k] && magnitude [k] >= magnitude [k + 1])
Chris@0: 		{	if (peak_count < MAX_PEAKS)
Chris@0: 			{	peaks [peak_count].peak = magnitude [k] ;
Chris@0: 				peaks [peak_count].index = k ;
Chris@0: 				peak_count ++ ;
Chris@0: 				qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ;
Chris@0: 				}
Chris@0: 			else if (magnitude [k] > peaks [MAX_PEAKS - 1].peak)
Chris@0: 			{	peaks [MAX_PEAKS - 1].peak = magnitude [k] ;
Chris@0: 				peaks [MAX_PEAKS - 1].index = k ;
Chris@0: 				qsort (peaks, MAX_PEAKS, sizeof (PEAK_DATA), peak_compare) ;
Chris@0: 				} ;
Chris@0: 			} ;
Chris@0: 		} ;
Chris@0: 
Chris@0: 	if (peak_count < expected_peaks)
Chris@0: 	{	printf ("\n%s : line %d : bad peak_count (%d), expected %d.\n\n", __FILE__, __LINE__, peak_count, expected_peaks) ;
Chris@0: 		return -1.0 ;
Chris@0: 		} ;
Chris@0: 
Chris@0: 	/* Sort the peaks. */
Chris@0: 	qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ;
Chris@0: 
Chris@0: 	snr = peaks [0].peak ;
Chris@0: 	for (k = 1 ; k < peak_count ; k++)
Chris@0: 		if (fabs (snr - peaks [k].peak) > 10.0)
Chris@0: 			return fabs (peaks [k].peak) ;
Chris@0: 
Chris@0: 	return snr ;
Chris@0: } /* find_snr */
Chris@0: 
Chris@0: static void
Chris@0: log_mag_spectrum (double *input, int len, double *magnitude)
Chris@0: {	fftw_plan plan = NULL ;
Chris@0: 
Chris@0: 	double	maxval ;
Chris@0: 	int		k ;
Chris@0: 
Chris@0: 	if (input == NULL || magnitude == NULL)
Chris@0: 		return ;
Chris@0: 
Chris@0: 	plan = fftw_plan_r2r_1d (len, input, magnitude, FFTW_R2HC, FFTW_ESTIMATE | FFTW_PRESERVE_INPUT) ;
Chris@0: 	if (plan == NULL)
Chris@0: 	{	printf ("%s : line %d : create plan failed.\n", __FILE__, __LINE__) ;
Chris@0: 		exit (1) ;
Chris@0: 		} ;
Chris@0: 
Chris@0: 	fftw_execute (plan) ;
Chris@0: 
Chris@0: 	fftw_destroy_plan (plan) ;
Chris@0: 
Chris@0: 	/* (k < N/2 rounded up) */
Chris@0: 	maxval = 0.0 ;
Chris@0: 	for (k = 1 ; k < len / 2 ; k++)
Chris@0: 	{	magnitude [k] = sqrt (magnitude [k] * magnitude [k] + magnitude [len - k - 1] * magnitude [len - k - 1]) ;
Chris@0: 		maxval = (maxval < magnitude [k]) ? magnitude [k] : maxval ;
Chris@0: 		} ;
Chris@0: 
Chris@0: 	memset (magnitude + len / 2, 0, len / 2 * sizeof (magnitude [0])) ;
Chris@0: 
Chris@0: 	/* Don't care about DC component. Make it zero. */
Chris@0: 	magnitude [0] = 0.0 ;
Chris@0: 
Chris@0: 	/* log magnitude. */
Chris@0: 	for (k = 0 ; k < len ; k++)
Chris@0: 	{	magnitude [k] = magnitude [k] / maxval ;
Chris@0: 		magnitude [k] = (magnitude [k] < 1e-15) ? -200.0 : 20.0 * log10 (magnitude [k]) ;
Chris@0: 		} ;
Chris@0: 
Chris@0: 	return ;
Chris@0: } /* log_mag_spectrum */
Chris@0: 
Chris@0: #else /* ! (HAVE_LIBFFTW && HAVE_LIBRFFTW) */
Chris@0: 
Chris@0: double
Chris@0: calculate_snr (float *data, int len, int expected_peaks)
Chris@0: {	double snr = 200.0 ;
Chris@0: 
Chris@0: 	data = data ;
Chris@0: 	len = len ;
Chris@0: 	expected_peaks = expected_peaks ;
Chris@0: 
Chris@0: 	return snr ;
Chris@0: } /* calculate_snr */
Chris@0: 
Chris@0: #endif
Chris@0: