/*************************************************************************\
  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 = 1.0, sum = 0;

  /* find an appropriate extremal frequency */
  if (nfcns > 2) {   /* have we calculated extremal frequencies at all? */
    for (j = 0; j<=nfcns; j++)
      if (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, 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) */
}