/*
 * 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 */