/* * remez.c - Parks-McClellan algorithm for FIR filter design (C version) * * Copyright (C) 1995,1998 Jake Janovetz * Copyright (C) 1998-2005 Atari800 development team (see DOC/CREDITS) * * This file is part of the Atari800 emulator project which emulates * the Atari 400, 800, 800XL, 130XE, and 5200 8-bit computers. * * Atari800 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. * * Atari800 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 Atari800; if not, write to the Free Software *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA*/#include <stdio.h>#include <stdlib.h>#include <math.h>#include "remez.h"#ifdef ASAP /* external project, see http://asap.sf.net */#include "asap_internal.h"#else#include "log.h"#include "util.h"#endif/******************* * CreateDenseGrid *================= * Creates the dense grid of frequencies from the specified bands. * Also creates the Desired Frequency Response function (D[]) and * the Weight function (W[]) on that dense grid * * * INPUT: * ------ * int      r        - 1/2 the number of filter coefficients * int      numtaps  - Number of taps in the resulting filter * int      numband  - Number of bands in user specification * double   bands[]  - User-specified band edges [2*numband] * double   des[]    - Desired response per band [numband] * double   weight[] - Weight per band [numband] * int      symmetry - Symmetry of filter - used for grid check * * OUTPUT: * ------- * int    gridsize   - Number of elements in the dense frequency grid * double Grid[]     - Frequencies (0 to 0.5) on the dense grid [gridsize] * double D[]        - Desired response on the dense grid [gridsize] * double W[]        - Weight function on the dense grid [gridsize] *******************/static void CreateDenseGrid(int r, int numtaps, int numband, double bands[],                            const double des[], const double weight[],                            int *gridsize, double Grid[],                            double D[], double W[], int symmetry){	int i, j, k, band;	double delf, lowf, highf;	delf = 0.5 / (GRIDDENSITY * r);	/* For differentiator, hilbert,	 *   symmetry is odd and Grid[0] = max(delf, band[0]) */	if (symmetry == NEGATIVE && delf > bands[0])		bands[0] = delf;	j = 0;	for (band = 0; band < numband; band++) {		Grid[j] = bands[2 * band];		lowf = bands[2 * band];		highf = bands[2 * band + 1];		k = (int) ((highf - lowf) / delf + 0.5); /* .5 for rounding */		for (i = 0; i < k; i++) {			D[j] = des[band];			W[j] = weight[band];			Grid[j] = lowf;			lowf += delf;			j++;		}		Grid[j - 1] = highf;	}	/* Similar to above, if odd symmetry, last grid point can't be .5	 *  - but, if there are even taps, leave the last grid point at .5 */	if ((symmetry == NEGATIVE) &&	   (Grid[*gridsize - 1] > (0.5 - delf)) &&	   (numtaps % 2))	{		Grid[*gridsize - 1] = 0.5 - delf;	}}/******************** * InitialGuess *============== * Places Extremal Frequencies evenly throughout the dense grid. * * * INPUT: * ------ * int r        - 1/2 the number of filter coefficients * int gridsize - Number of elements in the dense frequency grid * * OUTPUT: * ------- * int Ext[]    - Extremal indexes to dense frequency grid [r+1] ********************/static void InitialGuess(int r, int Ext[], int gridsize){	int i;	for (i = 0; i <= r; i++)		Ext[i] = i * (gridsize - 1) / r;}/*********************** * CalcParms *=========== * * * INPUT: * ------ * int    r      - 1/2 the number of filter coefficients * int    Ext[]  - Extremal indexes to dense frequency grid [r+1] * double Grid[] - Frequencies (0 to 0.5) on the dense grid [gridsize] * double D[]    - Desired response on the dense grid [gridsize] * double W[]    - Weight function on the dense grid [gridsize] * * OUTPUT: * ------- * double ad[]   - 'b' in Oppenheim & Schafer [r+1] * double x[]    - [r+1] * double y[]    - 'C' in Oppenheim & Schafer [r+1] ***********************/static void CalcParms(int r, const int Ext[], const double Grid[],                      const double D[], const double W[],                      double ad[], double x[], double y[]){	int i, j, k, ld;	double sign, xi, delta, denom, numer;	/* Find x[] */	for (i = 0; i <= r; i++)		x[i] = cos(Pi2 * Grid[Ext[i]]);	/* Calculate ad[]  - Oppenheim & Schafer eq 7.132 */	ld = (r - 1) / 15 + 1; /* Skips around to avoid round errors */	for (i = 0; i <= r; i++) {		denom = 1.0;		xi = x[i];		for (j = 0; j < ld; j++) {			for (k = j; k <= r; k += ld)				if (k != i)					denom *= 2.0 * (xi - x[k]);		}		if (fabs(denom) < 0.00001)			denom = 0.00001;		ad[i] = 1.0 / denom;	}	/* Calculate delta  - Oppenheim & Schafer eq 7.131 */	numer = denom = 0;	sign = 1;	for (i = 0; i <= r; i++) {		numer += ad[i] * D[Ext[i]];		denom += sign * ad[i] / W[Ext[i]];		sign = -sign;	}	delta = numer / denom;	sign = 1;	/* Calculate y[]  - Oppenheim & Schafer eq 7.133b */	for (i = 0; i <= r; i++) {		y[i] = D[Ext[i]] - sign * delta / W[Ext[i]];		sign = -sign;	}}/********************* * ComputeA *========== * Using values calculated in CalcParms, ComputeA calculates the * actual filter response at a given frequency (freq).  Uses * eq 7.133a from Oppenheim & Schafer. * * * INPUT: * ------ * double freq - Frequency (0 to 0.5) at which to calculate A * int    r    - 1/2 the number of filter coefficients * double ad[] - 'b' in Oppenheim & Schafer [r+1] * double x[]  - [r+1] * double y[]  - 'C' in Oppenheim & Schafer [r+1] * * OUTPUT: * ------- * Returns double value of A[freq] *********************/static double ComputeA(double freq, int r, const double ad[],                       const double x[], const double y[]){	int i;	double xc, c, denom, numer;	denom = numer = 0;	xc = cos(Pi2 * freq);	for (i = 0; i <= r; i++) {		c = xc - x[i];		if (fabs(c) < 1.0e-7) {			numer = y[i];			denom = 1;			break;		}		c = ad[i] / c;		denom += c;		numer += c * y[i];	}	return numer / denom;}/************************ * CalcError *=========== * Calculates the Error function from the desired frequency response * on the dense grid (D[]), the weight function on the dense grid (W[]), * and the present response calculation (A[]) * * * INPUT: * ------ * int    r      - 1/2 the number of filter coefficients * double ad[]   - [r+1] * double x[]    - [r+1] * double y[]    - [r+1] * int gridsize  - Number of elements in the dense frequency grid * double Grid[] - Frequencies on the dense grid [gridsize] * double D[]    - Desired response on the dense grid [gridsize] * double W[]    - Weight function on the desnse grid [gridsize] * * OUTPUT: * ------- * double E[]    - Error function on dense grid [gridsize] ************************/static void CalcError(int r, const double ad[],                      const double x[], const double y[],                      int gridsize, const double Grid[],                      const double D[], const double W[], double E[]){	int i;	double A;	for (i = 0; i < gridsize; i++) {		A = ComputeA(Grid[i], r, ad, x, y);		E[i] = W[i] * (D[i] - A);	}}/************************ * Search *======== * Searches for the maxima/minima of the error curve.  If more than * r+1 extrema are found, it uses the following heuristic (thanks * Chris Hanson): * 1) Adjacent non-alternating extrema deleted first. * 2) If there are more than one excess extrema, delete the *    one with the smallest error.  This will create a non-alternation *    condition that is fixed by 1). * 3) If there is exactly one excess extremum, delete the smaller *    of the first/last extremum * * * INPUT: * ------ * int    r        - 1/2 the number of filter coefficients * int    Ext[]    - Indexes to Grid[] of extremal frequencies [r+1] * int    gridsize - Number of elements in the dense frequency grid * double E[]      - Array of error values.  [gridsize] * OUTPUT: * ------- * int    Ext[]    - New indexes to extremal frequencies [r+1] ************************/static void Search(int r, int Ext[], int gridsize, const double E[]){	int i, j, k, l, extra;     /* Counters */	int up, alt;	int *foundExt;             /* Array of found extremals */	/* Allocate enough space for found extremals. */	foundExt = (int *) Util_malloc((2 * r) * sizeof(int));	k = 0;	/* Check for extremum at 0. */	if (((E[0] > 0.0) && (E[0] > E[1])) ||	   ((E[0] < 0.0) && (E[0] < E[1])))		foundExt[k++] = 0;	/* Check for extrema inside dense grid */