/*************************************************************************\
  The following implementation of the Remez Exchange Algorithm (for design
  of FIR linear phase filters) was converted from a Fortran program by
  James H. McClellan,  Thomas W. Parks  and  Lawrence R. Rabiner,
  published in:

  Lawrence R. Rabiner/Bernard Gold:  Theory and Application of Digital
  Signal Processing.  London: Prentice-Hall, 1975.

  Equation numbers (where given) refer to that book.
\*************************************************************************/

#include <math.h>
#include <stdio.h>
#include "remez.h"

#define NCOEFF  (NFMAX/2 + 2)   /* number of coefficient values */
#define MAXGRID (8*NFMAX + 32)  /* frequency discretization */

double des[ MAXGRID ], grid[ MAXGRID ], wt[ MAXGRID ];
int    iext[ NCOEFF ], iext0[ NCOEFF ];
double eext[ NCOEFF ];
double ad[ NCOEFF ], alpha[ NCOEFF ], x[ NCOEFF ], y[ NCOEFF ];
float  h[ NFMAX ];

double pi, pi2;

int nfcns;          /* number of cosine functions */
int ngrid;          /* frequency discretization points */
int lgrid = 16;     /* grid density, <= 16 */
int nfilt;          /* filter length */
int remez_talk = 0; /* debugging output? */
int remez_dot = 0;  /* print one '.' for each iteration? */

int remez();
double d(int k, int n, int m);
double gee(double f);

int makefilter( int length )
    {
    int nodd, j, nm1, nz, rval;
    double delf, fr;

    /* Important initialization: */
    pi = 4*atan(1);
    pi2 = 2*pi;

    if( (nfilt = length) > NFMAX )
        {
        if (remez_talk)
            printf("illegal filter length: %d\n", length);
        return FILTER_TOO_LONG;
        }
    if (nfilt < 3)
        {   /* You call that a filter? */
        for (j = 0; j<nfilt; j++)
            h[j] = 1 / (float)nfilt;
        return REMEZ_OK;
        }
    nodd = nfilt % 2;
    nfcns = nfilt / 2 + nodd;
    /* Set up the frequency grid.
     * Note that it isn't "complete", i. e. frequencies from transition
     * bands (where the weight function returns 0) are simply left out.
     */
    delf = 0.5 / (nfcns * lgrid);
    ngrid = 0; fr = 0;
    while (fr <= (0.5 - delf))
        {
        if ((wt[ngrid] = weight(fr)) != 0)
            {
            des[ngrid] = desire(fr);
            grid[ngrid] = fr;
            ngrid++;
            }
        fr += delf;
        }
    /* For even <nfilt>: Set up a new approximation problem which is
     * equivalent to the original problem.
     */
    if (nodd == 0)
        for (j = 0; j<ngrid; j++)
            {
            des[j] /= cos(pi * grid[j]);
            wt[j] *= cos(pi * grid[j]);
            }
    /* Initial guess for the extremal frequencies - equally spaced along
     * the grid:
     */
    for (j = 0; j<nfcns; j++)
        iext[j] = ((ngrid-1) * j) / nfcns;
    iext[ nfcns ] = ngrid-1;

    rval = remez();   /* OK, let's go !!! */
    if (rval != 0)
        {
        /* Algorithm failed to converge. There seem to be two conditions under
         * which this happens:
         * (1) The initial guess for the extremal frequencies is so poor that
         *     the exchange iteration cannot get started, or
         * (2) Near the termination of a correct design, the deviation decreases
         *     due to rounding errors and the program stops. In this latter case
         *     the filter design is probably acceptable, but should be checked
         *     by computing a frequency response.
         */
        if (remez_talk)
            {
            printf("*** NO CONVERGENCE AFTER %d ITERATIONS ***\n", rval);
            printf("Probable cause is machine rounding error.\n");
            if (rval>3)
                printf("Design may be correct, but should be verified.\n");
            }
        if (rval>3)
            rval = REMEZ_UNSURE;
        else
            rval = REMEZ_FAIL;
        }

    /* Calculate the impulse response */
    nm1 = nfcns - 1;
    nz = nfcns + 1;
    if (nodd)
        {
        for (j = 0; j <= nm1; j++)
            h[j] = alpha[nm1-j];
        }
    else
        {
        h[0] = alpha[nm1];
        for( j = 1; j <= nm1; j++ )
            h[j] = 0.5 * (alpha[nm1-j] + alpha[nfcns-j]);
        }
    for( j = 0; j < nfcns; j++ )
        h[ nfilt-1-j ] = h[ j ];

    return rval;
    }


float extreme( float freq )
/* Returns one extremal value of the frequency response that is
 * closest to the specified frequency.
 */
    {
    int i, j;
    float err, sum = 0;

    /* find an appropriate extremal frequency */
    if( nfcns > 2 )  /* have we calculated extremal frequencies at all? */
        {
        for( j = 0; j <= nfcns; j++ )
            if( j == 0 || fabs(freq - grid[iext[j]]) < err )
                {
                i = j;
                err = fabs( freq - grid[ iext[ j ] ] );
                }
        freq = grid[ iext[ i ] ];
        }
    else
        freq = 0.25;    /* not necessarily true, but who cares */
    /* calculate the response */
    for( j = 0; j<nfilt; j++ )
        sum += h[j] * cos(pi*freq * (nfilt-1-2*j));
    return sum;
    }


int remez()
/* This function implements the Remez Exchange Algorithm for the weighted
 * Chebyshev approximation of a continuous function with a sum of cosines.
 * Inputs to the function are a dense grid which represents the frequency
 * axis (grid[ngrid]), the desired function on the grid (des[], wt[]),
 * the number of cosines (nfcns), and an initial guess of the extremal
 * frequencies (iext[nfcns+1]).
 * The program minimizes the Chebyshev error by determining the best
 * location of the extremal frequencies (points of maximum error) and then
 * calculates the coefficients of the best approximation.
 */
#define MAXITER 25
/* The return value is 0 if OK, else the number of iterations attempted */
    {
    int iter = MAXITER;
    int nz = nfcns + 1;
    int j, jchnge = 0, k, l, minpos;
    double dtemp, dnum, dden, dev, minval;
    double devl = -1;
    double err0, err1, errm1;
    double cn, ft;

    while( --iter )
        {
        /* Calculate the Lagrange coefficients for use with funtion gee() */
        for( j = 0; j<nz; j++ )
            x[ j ] = cos( pi2 * grid[ iext[ j ] ] );    /* eq. (3.123) */
        for (j = 0; j<nz; j++)
            {
            dtemp = 1;
            for( l = 0; l < nz; l++ )
                if( l != j )
                    dtemp *= 2 * (x[j] - x[l]);
            ad[j] = 1/dtemp;    /* eq. (3.122) */
            }
        dnum = 0;
        dden = 0;
        k = 1;
        for (j = 0; j<nz; j++)
            {
            dnum += ad[j] * des[iext[j]];
            dden += k * ad[j] / wt[iext[j]];
            k = -k;
            }
        dev = dnum / dden;      /* eq. (3.121) */
        if( remez_talk )
            printf("deviation = %f\n", dev);
        k = 1;
        if( dev>0 )
            k = -1;
        dev = -k*dev;
        /* set new interpolation points: */
        for (j = 0; j<nz; j++)
            {
            y[j] = des[iext[j]] + k * dev / wt[iext[j]];   /* eq. (3.124) */
            k = -k;
            }
        if( (devl-dev)/(fabs(devl)+fabs(dev)) > 0.001 )
            break;  /* convergence failure !!! */
        devl = dev;

        if (remez_dot)
            {
            printf( "." );
            fflush( stdout );
            }

        /* Search for the extremal frequencies of the best approximation */
        /* remember the old frequencies first: */
        for (j = 0; j<nz; j++)
            iext0[j] = iext[j];
        j = 0;
        err0 = 0;
        err1 = (gee( grid[ 0 ] ) - des[ 0 ]) * wt[0];
        for( l = 1; l<ngrid; l++ )
            {
            errm1 = err0;
            err0 = err1;
            err1 = (gee(grid[l]) - des[l]) * wt[l];
            if ((err0 - errm1)*(err0 - err1) > 0 || (err0 == err1))
                {
                if( j == nz )
                    {   /* do we have to kick out less important extremes? */
                    minval = fabs(err0 - eext[j-1]);
                    minpos = 0;
                    for( k = 1; k<j; k++ )
                        if (fabs(eext[k] - eext[k-1]) < minval)
                            {
                            minpos = k;
                            minval = fabs( eext[k] - eext[k-1] );
                            }
                    if( minpos > 0 )
                        {       /* yes, we can */
                        if( remez_talk )
                        printf( "%d & %d dropped\n", iext[minpos-1], iext[minpos] );
                        for (k = minpos; k<j-1; k++)
                            {
                            iext[k-1] = iext[k+1];
                            eext[k-1] = eext[k+1];
                            }
                        j -= 2;
                        }
                    }
                if( j < nz )
                    {
                    iext[ j++ ] = l-1;
                    eext[ j ] = err0;
                    if( remez_talk )
                        printf( "%3d: %10.5f\n", l-1, err0 );
                    }
                }
            }
        /* Add the upper edge, if we need one more extreme, OR if it may */
        /* come in instead of the lower edge */
        if( j < nz )
            {
            iext[ j++ ] = ngrid-1;
            if( remez_talk )
                printf("%3d: %10.5f <\n", ngrid-1, err1);
            }
        else if( iext[0] == 0 && fabs(err1-eext[j-1]) > fabs( eext[1]-eext[0] ) )
            {
            for( k = 1; k<j; k++ )
                iext[ k-1 ] = iext[ k ];
            iext[ j-1 ] = ngrid-1;
            if( remez_talk )
                printf("%3d: %10.5f forced\n", ngrid-1, err1);
            }
        /* if still (j<nz), we've got a problem ;-) */

        /* Have there been any changes? */
        jchnge = 0;
        for (j = 0; j<nz; j++)
            if (iext0[j] != iext[j])
                jchnge++;
        if( jchnge == 0 )
            break;      /* normal way of termination */
        }

    /* Calculation of the coefficients of the best approximation using the
     * inverse Fourier transform. This will work, because a polynom of
     * cosines like gee(f) may as well be represented as a sum of cosines,
     * cos(0·f) to cos((nfcns-1)·f).
     */
    cn = 2*nfcns;
    for( j = 0; j<nfcns; j++ )
        alpha[ j ] = 0;
    for( k = 0; k<2*nfcns; k++ )
        {
        ft = k/cn;
        dtemp = gee(ft)/cn;
        for( j = 0; j<nfcns; j++ )
            alpha[j] += dtemp * cos(pi2*ft*j);
        }

    if (jchnge == 0)
        return 0;
    else
        return MAXITER - iter;
    }


double gee( double f )
/* Evaluates the frequency response using the Lagrange interpolation
 * formula in the barycentric form, from ad[], x[] and y[].
 */
    {
    double p = 0, d = 0;
    double c, xf;
    double fsh = 1e-6;
    int j;

    xf = cos(pi2 * f);
    for (j=0; j<=nfcns; j++)
        {
        if (fabs(xf-x[j]) < fsh)
            return y[j];
        c = ad[j] / (xf-x[j]);
        p += c * y[j];
        d += c;
        }
    return p/d;     /* eq. (3.125) */
    }