libs/Music/Filter.cpp

   1 // Copyright 2007 "Gilles Degottex"
   2
   3 // This file is part of "Music"
   4
   5 // "Music" is free software; you can redistribute it and/or modify
   6 // it under the terms of the GNU Lesser General Public License as published by
   7 // the Free Software Foundation; either version 2.1 of the License, or
   8 // (at your option) any later version.
   9 //
  10 // "Music" is distributed in the hope that it will be useful,
  11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  13 // GNU Lesser General Public License for more details.
  14 //
  15 // You should have received a copy of the GNU Lesser General Public License
  16 // along with this program; if not, write to the Free Software
  17 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  18
  19 #include "Filter.h"
  20
  21 // #include <iostream>
  22 using namespace std;
  23 #include <CppAddons/CAMath.h>
  24 using namespace Math;
  25 #include "Music.h"
  26
  27 #include <math.h>
  28 #include <stdio.h>
  29 #include <stdlib.h>
  30
  31 // DOESN'T WORK
  32 // b = fir1(32,[0.00001 0.23]);
  33 vector<double> Music::fir1_lowpass(int n, double cutoff)
  34 {
  35         vector<double> b(n);
  36
  37         int d = (n-1)/2;
  38
  39         for(size_t i=0; i<b.size(); i++)
  40                 b[i] = cutoff*sinc(cutoff*(i-d));
  41
  42         return b;
  43 }
  44
  45 // DOESN'T WORK
  46 vector<double> Music::fir1_highpass(int n, double cutoff)
  47 {
  48         vector<double> b(n);
  49
  50         int d = (n-1)/2;
  51
  52         double s=0.0;
  53         for(size_t i=0; i<b.size(); i++)
  54         {
  55                 b[i] = cutoff*sinc(cutoff*(i-d));
  56                 s += b[i];
  57         }
  58
  59         for(size_t i=0; i<b.size(); i++)
  60                 b[i] = -b[i];
  61         b[d] = s+b[d];
  62
  63         return b;
  64 }
  65
  66 // cutoff in ]0;0.5[ where 0.5 is the Nyquist frequency
  67 vector<double> Music::fir1_bandpass(int n, double low_cutoff, double high_cutoff)
  68 {
  69     double *weights, *desired, *bands;
  70     double *h;
  71     int i;
  72
  73     bands = (double *)malloc(6 * sizeof(double));
  74     weights = (double *)malloc(3 * sizeof(double));
  75     desired = (double *)malloc(3 * sizeof(double));
  76     h = (double *)malloc(1000 * sizeof(double));
  77
  78     desired[0] = 1;
  79     desired[1] = 1;
  80     desired[2] = 0;
  81
  82     weights[0] = 10;
  83     weights[1] = 1;
  84     weights[2] = 10;
  85
  86     bands[0] = 0;
  87     bands[1] = 0.1;
  88     bands[2] = 0.2;
  89     bands[3] = 0.3;
  90     bands[4] = 0.35;
  91     bands[5] = 0.5;
  92 //     bands[0] = 0;
  93 //     bands[1] = low_cutoff/2;
  94 //     bands[2] = low_cutoff;
  95 //     bands[3] = high_cutoff;
  96 //     bands[4] = 0.5*(high_cutoff+0.5);
  97 //     bands[5] = 0.5;
  98
  99    remez(h, n, 3, bands, desired, weights, BANDPASS);
 100
 101    vector<double> hv(n);
 102
 103    for (i=0; i<n; i++)
 104    {
 105        hv[i] = h[i];
 106 //        printf("%23.20f %23.20f\n", h[i], hv[i]);
 107    }
 108
 109    free(bands);
 110    free(weights);
 111    free(desired);
 112    free(h);
 113
 114     return hv;
 115 }
 116
 117 Music::FIRRTFilter::FIRRTFilter(std::vector<double>& imp_res)
 118 {
 119         assert(imp_res.size()>0);
 120
 121         m_imp_res = imp_res;
 122 //                      m_to_filter.reserve(imp_res.size());
 123 }
 124
 125 double Music::FIRRTFilter::operator()(double v)
 126 {
 127         double value = 0.0;
 128
 129         m_to_filter.push_front(v);
 130
 131         if(m_to_filter.size()>=m_imp_res.size())
 132         {
 133                 // convolve
 134                 for(size_t i=0; i<m_imp_res.size(); i++)
 135                         value += m_imp_res[i]*m_to_filter[i];
 136
 137 //              if(decim++%4==0)
 138 //                      m_queue.push_front(value);
 139
 140                 // drop unused data
 141                 m_to_filter.pop_back();
 142         }
 143
 144         return value;
 145 }
 146
 147 Music::RectangularHighPassRTFilter::RectangularHighPassRTFilter(int N)
 148 {
 149         reset(N);
 150 }
 151 void Music::RectangularHighPassRTFilter::reset(int N)
 152 {
 153         if(N<1) N=1;
 154
 155         m_N = N;
 156         m_sum = 0.0;
 157         m_summed_values.clear();
 158 }
 159 double Music::RectangularHighPassRTFilter::operator()(double v)
 160 {
 161         m_summed_values.push_front(v);
 162         m_sum += v;
 163         while(int(m_summed_values.size())>m_N)
 164         {
 165                 m_sum -= m_summed_values.back();
 166
 167                 m_summed_values.pop_back();
 168         }
 169
 170     double fv;
 171     fv = m_summed_values[m_summed_values.size()/2] - m_sum/m_summed_values.size();
 172
 173         return fv;
 174 }
 175
 176 double Music::RectangularLowPassRTFilter::operator()(double v)
 177 {
 178         return v;
 179 }
 180
 181 double Music::RectangularBandPassRTFilter::operator()(double v)
 182 {
 183         return v;
 184 }
 185
 186 /*
 187 LP and HP filter
 188 Type : biquad, tweaked butterworthReferences : Posted by Patrice TarrabiaCode : www.musicdsp.org
 189 r  = rez amount, from sqrt(2) to ~ 0.1
 190 f  = cutoff frequency
 191 (from ~0 Hz to SampleRate/2 - though many
 192 synths seem to filter only  up to SampleRate/4)
 193
 194 The filter algo:
 195 out(n) = a1 * in + a2 * in(n-1) + a3 * in(n-2) - b1*out(n-1) - b2*out(n-2)
 196
 197 Lowpass:
 198       c = 1.0 / tan(pi * f / sample_rate);
 199
 200       a1 = 1.0 / ( 1.0 + r * c + c * c);
 201       a2 = 2* a1;
 202       a3 = a1;
 203       b1 = 2.0 * ( 1.0 - c*c) * a1;
 204       b2 = ( 1.0 - r * c + c * c) * a1;
 205
 206 Hipass:
 207       c = tan(pi * f / sample_rate);
 208
 209       a1 = 1.0 / ( 1.0 + r * c + c * c);
 210       a2 = -2*a1;
 211       a3 = a1;
 212       b1 = 2.0 * ( c*c - 1.0) * a1;
 213       b2 = ( 1.0 - r * c + c * c) * a1;
 214 */
 215
 216 /**************************************************************************
 217  * Parks-McClellan algorithm for FIR filter design (C version)
 218  *-------------------------------------------------
 219  *  Copyright (c) 1995,1998  Jake Janovetz (janovetz@uiuc.edu)
 220  *
 221  *  This library is free software; you can redistribute it and/or
 222  *  modify it under the terms of the GNU Library General Public
 223  *  License as published by the Free Software Foundation; either
 224  *  version 2 of the License, or (at your option) any later version.
 225  *
 226  *  This library is distributed in the hope that it will be useful,
 227  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 228  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 229  *  Library General Public License for more details.
 230  *
 231  *  You should have received a copy of the GNU Library General Public
 232  *  License along with this library; if not, write to the Free
 233  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 234  *
 235  *************************************************************************/
 236
 237
 238 /*******************
 239  * CreateDenseGrid
 240  *=================
 241  * Creates the dense grid of frequencies from the specified bands.
 242  * Also creates the Desired Frequency Response function (D[]) and
 243  * the Weight function (W[]) on that dense grid
 244  *
 245  *
 246  * INPUT:
 247  * ------
 248  * int      r        - 1/2 the number of filter coefficients
 249  * int      numtaps  - Number of taps in the resulting filter
 250  * int      numband  - Number of bands in user specification
 251  * double   bands[]  - User-specified band edges [2*numband]
 252  * double   des[]    - Desired response per band [numband]
 253  * double   weight[] - Weight per band [numband]
 254  * int      symmetry - Symmetry of filter - used for grid check
 255  *
 256  * OUTPUT:
 257  * -------
 258  * int    gridsize   - Number of elements in the dense frequency grid
 259  * double Grid[]     - Frequencies (0 to 0.5) on the dense grid [gridsize]
 260  * double D[]        - Desired response on the dense grid [gridsize]
 261  * double W[]        - Weight function on the dense grid [gridsize]
 262  *******************/
 263
 264 void CreateDenseGrid(int r, int numtaps, int numband, double bands[],
 265                      double des[], double weight[], int *gridsize,
 266                      double Grid[], double D[], double W[],
 267                      int symmetry)
 268 {
 269    int i, j, k, band;
 270    double delf, lowf, highf;
 271
 272    delf = 0.5/(GRIDDENSITY*r);
 273
 274 /*
 275  * For differentiator, hilbert,
 276  *   symmetry is odd and Grid[0] = max(delf, band[0])
 277  */
 278
 279    if ((symmetry == NEGATIVE) && (delf > bands[0]))
 280       bands[0] = delf;
 281
 282    j=0;
 283    for (band=0; band < numband; band++)
 284    {
 285       Grid[j] = bands[2*band];
 286       lowf = bands[2*band];
 287       highf = bands[2*band + 1];
 288       k = (int)((highf - lowf)/delf + 0.5);   /* .5 for rounding */
 289       for (i=0; i<k; i++)
 290       {
 291          D[j] = des[band];
 292          W[j] = weight[band];
 293          Grid[j] = lowf;
 294          lowf += delf;
 295          j++;
 296       }
 297       Grid[j-1] = highf;
 298    }
 299
 300 /*
 301  * Similar to above, if odd symmetry, last grid point can't be .5
 302  *  - but, if there are even taps, leave the last grid point at .5
 303  */
 304    if ((symmetry == NEGATIVE) &&
 305        (Grid[*gridsize-1] > (0.5 - delf)) &&
 306        (numtaps % 2))
 307    {
 308       Grid[*gridsize-1] = 0.5-delf;
 309    }
 310 }
 311
 312
 313 /********************
 314  * InitialGuess
 315  *==============
 316  * Places Extremal Frequencies evenly throughout the dense grid.
 317  *
 318  *
 319  * INPUT:
 320  * ------
 321  * int r        - 1/2 the number of filter coefficients
 322  * int gridsize - Number of elements in the dense frequency grid
 323  *
 324  * OUTPUT:
 325  * -------
 326  * int Ext[]    - Extremal indexes to dense frequency grid [r+1]
 327  ********************/
 328
 329 void InitialGuess(int r, int Ext[], int gridsize)
 330 {
 331    int i;
 332
 333    for (i=0; i<=r; i++)
 334       Ext[i] = i * (gridsize-1) / r;
 335 }
 336
 337
 338 /***********************
 339  * CalcParms
 340  *===========
 341  *
 342  *
 343  * INPUT:
 344  * ------
 345  * int    r      - 1/2 the number of filter coefficients
 346  * int    Ext[]  - Extremal indexes to dense frequency grid [r+1]
 347  * double Grid[] - Frequencies (0 to 0.5) on the dense grid [gridsize]
 348  * double D[]    - Desired response on the dense grid [gridsize]
 349  * double W[]    - Weight function on the dense grid [gridsize]
 350  *
 351  * OUTPUT:
 352  * -------
 353  * double ad[]   - 'b' in Oppenheim & Schafer [r+1]
 354  * double x[]    - [r+1]
 355  * double y[]    - 'C' in Oppenheim & Schafer [r+1]
 356  ***********************/
 357
 358 void CalcParms(int r, int Ext[], double Grid[], double D[], double W[],
 359                 double ad[], double x[], double y[])
 360 {
 361    int i, j, k, ld;
 362    double sign, xi, delta, denom, numer;
 363
 364 /*
 365  * Find x[]
 366  */
 367    for (i=0; i<=r; i++)
 368       x[i] = cos(Pi2 * Grid[Ext[i]]);
 369
 370 /*
 371  * Calculate ad[]  - Oppenheim & Schafer eq 7.132
 372  */
 373    ld = (r-1)/15 + 1;         /* Skips around to avoid round errors */
 374    for (i=0; i<=r; i++)
 375    {
 376        denom = 1.0;
 377        xi = x[i];
 378        for (j=0; j<ld; j++)
 379        {
 380           for (k=j; k<=r; k+=ld)
 381              if (k != i)
 382                 denom *= 2.0*(xi - x[k]);
 383        }
 384        if (fabs(denom)<0.00001)
 385           denom = 0.00001;
 386        ad[i] = 1.0/denom;
 387    }
 388
 389 /*
 390  * Calculate delta  - Oppenheim & Schafer eq 7.131
 391  */
 392    numer = denom = 0;
 393    sign = 1;
 394    for (i=0; i<=r; i++)
 395    {
 396       numer += ad[i] * D[Ext[i]];
 397       denom += sign * ad[i]/W[Ext[i]];
 398       sign = -sign;
 399    }
 400    delta = numer/denom;
 401    sign = 1;
 402
 403 /*
 404  * Calculate y[]  - Oppenheim & Schafer eq 7.133b
 405  */
 406    for (i=0; i<=r; i++)
 407    {
 408       y[i] = D[Ext[i]] - sign * delta/W[Ext[i]];
 409       sign = -sign;
 410    }
 411 }
 412
 413
 414 /*********************
 415  * ComputeA
 416  *==========
 417  * Using values calculated in CalcParms, ComputeA calculates the
 418  * actual filter response at a given frequency (freq).  Uses
 419  * eq 7.133a from Oppenheim & Schafer.
 420  *
 421  *
 422  * INPUT:
 423  * ------
 424  * double freq - Frequency (0 to 0.5) at which to calculate A
 425  * int    r    - 1/2 the number of filter coefficients
 426  * double ad[] - 'b' in Oppenheim & Schafer [r+1]
 427  * double x[]  - [r+1]
 428  * double y[]  - 'C' in Oppenheim & Schafer [r+1]
 429  *
 430  * OUTPUT:
 431  * -------
 432  * Returns double value of A[freq]
 433  *********************/
 434
 435 double ComputeA(double freq, int r, double ad[], double x[], double y[])
 436 {
 437    int i;
 438    double xc, c, denom, numer;
 439
 440    denom = numer = 0;
 441    xc = cos(Pi2 * freq);
 442    for (i=0; i<=r; i++)
 443    {
 444       c = xc - x[i];
 445       if (fabs(c) < 1.0e-7)
 446       {
 447          numer = y[i];
 448          denom = 1;
 449          break;
 450       }
 451       c = ad[i]/c;
 452       denom += c;
 453       numer += c*y[i];
 454    }
 455    return numer/denom;
 456 }
 457
 458
 459 /************************
 460  * CalcError
 461  *===========
 462  * Calculates the Error function from the desired frequency response
 463  * on the dense grid (D[]), the weight function on the dense grid (W[]),
 464  * and the present response calculation (A[])
 465  *
 466  *
 467  * INPUT:
 468  * ------
 469  * int    r      - 1/2 the number of filter coefficients
 470  * double ad[]   - [r+1]
 471  * double x[]    - [r+1]
 472  * double y[]    - [r+1]
 473  * int gridsize  - Number of elements in the dense frequency grid
 474  * double Grid[] - Frequencies on the dense grid [gridsize]
 475  * double D[]    - Desired response on the dense grid [gridsize]
 476  * double W[]    - Weight function on the desnse grid [gridsize]
 477  *
 478  * OUTPUT:
 479  * -------
 480  * double E[]    - Error function on dense grid [gridsize]
 481  ************************/
 482
 483 void CalcError(int r, double ad[], double x[], double y[],
 484                int gridsize, double Grid[],
 485                double D[], double W[], double E[])
 486 {
 487    int i;
 488    double A;
 489
 490    for (i=0; i<gridsize; i++)
 491    {
 492       A = ComputeA(Grid[i], r, ad, x, y);
 493       E[i] = W[i] * (D[i] - A);
 494    }
 495 }
 496
 497 /************************
 498  * Search
 499  *========
 500  * Searches for the maxima/minima of the error curve.  If more than
 501  * r+1 extrema are found, it uses the following heuristic (thanks
 502  * Chris Hanson):
 503  * 1) Adjacent non-alternating extrema deleted first.
 504  * 2) If there are more than one excess extrema, delete the
 505  *    one with the smallest error.  This will create a non-alternation
 506  *    condition that is fixed by 1).
 507  * 3) If there is exactly one excess extremum, delete the smaller
 508  *    of the first/last extremum
 509  *
 510  *
 511  * INPUT:
 512  * ------
 513  * int    r        - 1/2 the number of filter coefficients
 514  * int    Ext[]    - Indexes to Grid[] of extremal frequencies [r+1]
 515  * int    gridsize - Number of elements in the dense frequency grid
 516  * double E[]      - Array of error values.  [gridsize]
 517  * OUTPUT:
 518  * -------
 519  * int    Ext[]    - New indexes to extremal frequencies [r+1]
 520  ************************/
 521
 522 void Search(int r, int Ext[],
 523             int gridsize, double E[])
 524 {
 525    int i, j, k, l, extra;     /* Counters */
 526    int up, alt;
 527    int *foundExt;             /* Array of found extremals */
 528
 529 /*
 530  * Allocate enough space for found extremals.
 531  */
 532    foundExt = (int *)malloc((2*r) * sizeof(int));
 533    k = 0;
 534
 535 /*
 536  * Check for extremum at 0.
 537  */
 538    if (((E[0]>0.0) && (E[0]>E[1])) ||
 539        ((E[0]<0.0) && (E[0]<E[1])))
 540       foundExt[k++] = 0;
 541
 542 /*
 543  * Check for extrema inside dense grid
 544  */
 545    for (i=1; i<gridsize-1; i++)
 546    {
 547       if (((E[i]>=E[i-1]) && (E[i]>E[i+1]) && (E[i]>0.0)) ||
 548           ((E[i]<=E[i-1]) && (E[i]<E[i+1]) && (E[i]<0.0)))
 549          foundExt[k++] = i;
 550    }
 551
 552 /*
 553  * Check for extremum at 0.5
 554  */
 555    j = gridsize-1;
 556    if (((E[j]>0.0) && (E[j]>E[j-1])) ||
 557        ((E[j]<0.0) && (E[j]<E[j-1])))
 558       foundExt[k++] = j;
 559
 560
 561 /*
 562  * Remove extra extremals
 563  */
 564    extra = k - (r+1);
 565
 566    while (extra > 0)
 567    {
 568       if (E[foundExt[0]] > 0.0)
 569          up = 1;                /* first one is a maxima */
 570       else
 571          up = 0;                /* first one is a minima */
 572
 573       l=0;
 574       alt = 1;
 575       for (j=1; j<k; j++)
 576       {
 577          if (fabs(E[foundExt[j]]) < fabs(E[foundExt[l]]))
 578             l = j;               /* new smallest error. */
 579          if ((up) && (E[foundExt[j]] < 0.0))
 580             up = 0;             /* switch to a minima */
 581          else if ((!up) && (E[foundExt[j]] > 0.0))
 582             up = 1;             /* switch to a maxima */
 583          else
 584      {
 585             alt = 0;
 586             break;              /* Ooops, found two non-alternating */
 587          }                      /* extrema.  Delete smallest of them */
 588       }  /* if the loop finishes, all extrema are alternating */
 589
 590 /*
 591  * If there's only one extremal and all are alternating,
 592  * delete the smallest of the first/last extremals.
 593  */
 594       if ((alt) && (extra == 1))
 595       {
 596          if (fabs(E[foundExt[k-1]]) < fabs(E[foundExt[0]]))
 597             l = foundExt[k-1];   /* Delete last extremal */
 598          else
 599             l = foundExt[0];     /* Delete first extremal */
 600       }
 601
 602       for (j=l; j<k; j++)        /* Loop that does the deletion */
 603       {
 604          foundExt[j] = foundExt[j+1];
 605       }
 606       k--;
 607       extra--;
 608    }
 609
 610    for (i=0; i<=r; i++)
 611    {
 612       Ext[i] = foundExt[i];       /* Copy found extremals to Ext[] */
 613    }
 614
 615    free(foundExt);
 616 }
 617
 618
 619 /*********************
 620  * FreqSample
 621  *============
 622  * Simple frequency sampling algorithm to determine the impulse
 623  * response h[] from A's found in ComputeA
 624  *
 625  *
 626  * INPUT:
 627  * ------
 628  * int      N        - Number of filter coefficients
 629  * double   A[]      - Sample points of desired response [N/2]
 630  * int      symmetry - Symmetry of desired filter
 631  *
 632  * OUTPUT:
 633  * -------
 634  * double h[] - Impulse Response of final filter [N]
 635  *********************/
 636 void FreqSample(int N, double A[], double h[], int symm)
 637 {
 638    int n, k;
 639    double x, val, M;
 640
 641    M = (N-1.0)/2.0;
 642    if (symm == POSITIVE)
 643    {
 644       if (N%2)
 645       {
 646          for (n=0; n<N; n++)
 647          {
 648             val = A[0];
 649             x = Pi2 * (n - M)/N;
 650             for (k=1; k<=M; k++)
 651                val += 2.0 * A[k] * cos(x*k);
 652             h[n] = val/N;
 653          }
 654       }
 655       else
 656       {
 657          for (n=0; n<N; n++)
 658          {
 659             val = A[0];
 660             x = Pi2 * (n - M)/N;
 661             for (k=1; k<=(N/2-1); k++)
 662                val += 2.0 * A[k] * cos(x*k);
 663             h[n] = val/N;
 664          }
 665       }
 666    }
 667    else
 668    {
 669       if (N%2)
 670       {
 671          for (n=0; n<N; n++)
 672          {
 673             val = 0;
 674             x = Pi2 * (n - M)/N;
 675             for (k=1; k<=M; k++)
 676                val += 2.0 * A[k] * sin(x*k);
 677             h[n] = val/N;
 678          }
 679       }
 680       else
 681       {
 682           for (n=0; n<N; n++)
 683           {
 684              val = A[N/2] * sin(Pi * (n - M));
 685              x = Pi2 * (n - M)/N;
 686              for (k=1; k<=(N/2-1); k++)
 687                 val += 2.0 * A[k] * sin(x*k);
 688              h[n] = val/N;
 689           }
 690       }
 691    }
 692 }
 693
 694 /*******************
 695  * isDone
 696  *========
 697  * Checks to see if the error function is small enough to consider
 698  * the result to have converged.
 699  *
 700  * INPUT:
 701  * ------
 702  * int    r     - 1/2 the number of filter coeffiecients
 703  * int    Ext[] - Indexes to extremal frequencies [r+1]
 704  * double E[]   - Error function on the dense grid [gridsize]
 705  *
 706  * OUTPUT:
 707  * -------
 708  * Returns 1 if the result converged
 709  * Returns 0 if the result has not converged
 710  ********************/
 711
 712 short isDone(int r, int Ext[], double E[])
 713 {
 714    int i;
 715    double min, max, current;
 716
 717    min = max = fabs(E[Ext[0]]);
 718    for (i=1; i<=r; i++)
 719    {
 720       current = fabs(E[Ext[i]]);
 721       if (current < min)
 722          min = current;
 723       if (current > max)
 724          max = current;
 725    }
 726    if (((max-min)/max) < 0.0001)
 727       return 1;
 728    return 0;
 729 }
 730
 731 /********************
 732  * remez
 733  *=======
 734  * Calculates the optimal (in the Chebyshev/minimax sense)
 735  * FIR filter impulse response given a set of band edges,
 736  * the desired reponse on those bands, and the weight given to
 737  * the error in those bands.
 738  *
 739  * INPUT:
 740  * ------
 741  * int     numtaps     - Number of filter coefficients
 742  * int     numband     - Number of bands in filter specification
 743  * double  bands[]     - User-specified band edges [2 * numband]
 744  * double  des[]       - User-specified band responses [numband]
 745  * double  weight[]    - User-specified error weights [numband]
 746  * int     type        - Type of filter
 747  *
 748  * OUTPUT:
 749  * -------
 750  * double h[]      - Impulse response of final filter [numtaps]
 751  ********************/
 752
 753 void Music::remez(double h[], int numtaps,
 754            int numband, double bands[], double des[], double weight[],
 755            int type)
 756 {
 757    double *Grid, *W, *D, *E;
 758    int    i, iter, gridsize, r, *Ext;
 759    double *taps, c;
 760    double *x, *y, *ad;
 761    int    symmetry;
 762
 763    if (type == BANDPASS)
 764       symmetry = POSITIVE;
 765    else
 766       symmetry = NEGATIVE;
 767
 768    r = numtaps/2;                  /* number of extrema */
 769    if ((numtaps%2) && (symmetry == POSITIVE))
 770       r++;
 771
 772 /*
 773  * Predict dense grid size in advance for memory allocation
 774  *   .5 is so we round up, not truncate
 775  */
 776    gridsize = 0;
 777    for (i=0; i<numband; i++)
 778    {
 779       gridsize += (int)(2*r*GRIDDENSITY*(bands[2*i+1] - bands[2*i]) + .5);
 780    }
 781    if (symmetry == NEGATIVE)
 782    {
 783       gridsize--;
 784    }
 785
 786 /*
 787  * Dynamically allocate memory for arrays with proper sizes
 788  */
 789    Grid = (double *)malloc(gridsize * sizeof(double));
 790    D = (double *)malloc(gridsize * sizeof(double));
 791    W = (double *)malloc(gridsize * sizeof(double));
 792    E = (double *)malloc(gridsize * sizeof(double));
 793    Ext = (int *)malloc((r+1) * sizeof(int));
 794    taps = (double *)malloc((r+1) * sizeof(double));
 795    x = (double *)malloc((r+1) * sizeof(double));
 796    y = (double *)malloc((r+1) * sizeof(double));
 797    ad = (double *)malloc((r+1) * sizeof(double));
 798
 799 /*
 800  * Create dense frequency grid
 801  */
 802    CreateDenseGrid(r, numtaps, numband, bands, des, weight,
 803                    &gridsize, Grid, D, W, symmetry);
 804    InitialGuess(r, Ext, gridsize);
 805
 806 /*
 807  * For Differentiator: (fix grid)
 808  */
 809    if (type == DIFFERENTIATOR)
 810    {
 811       for (i=0; i<gridsize; i++)
 812       {
 813 /* D[i] = D[i]*Grid[i]; */
 814          if (D[i] > 0.0001)
 815             W[i] = W[i]/Grid[i];
 816       }
 817    }
 818
 819 /*
 820  * For odd or Negative symmetry filters, alter the
 821  * D[] and W[] according to Parks McClellan
 822  */
 823    if (symmetry == POSITIVE)
 824    {
 825       if (numtaps % 2 == 0)
 826       {
 827          for (i=0; i<gridsize; i++)
 828          {
 829             c = cos(Pi * Grid[i]);
 830             D[i] /= c;
 831             W[i] *= c;
 832          }
 833       }
 834    }
 835    else
 836    {
 837       if (numtaps % 2)
 838       {
 839          for (i=0; i<gridsize; i++)
 840          {
 841             c = sin(Pi2 * Grid[i]);
 842             D[i] /= c;
 843             W[i] *= c;
 844          }
 845       }
 846       else
 847       {
 848          for (i=0; i<gridsize; i++)
 849          {
 850             c = sin(Pi * Grid[i]);
 851             D[i] /= c;
 852             W[i] *= c;
 853          }
 854       }
 855    }
 856
 857 /*
 858  * Perform the Remez Exchange algorithm
 859  */
 860    for (iter=0; iter<MAXITERATIONS; iter++)
 861    {
 862       CalcParms(r, Ext, Grid, D, W, ad, x, y);
 863       CalcError(r, ad, x, y, gridsize, Grid, D, W, E);
 864       Search(r, Ext, gridsize, E);
 865       if (isDone(r, Ext, E))
 866          break;
 867    }
 868    if (iter == MAXITERATIONS)
 869    {
 870       printf("Reached maximum iteration count.\nResults may be bad.\n");
 871    }
 872
 873    CalcParms(r, Ext, Grid, D, W, ad, x, y);
 874
 875 /*
 876  * Find the 'taps' of the filter for use with Frequency
 877  * Sampling.  If odd or Negative symmetry, fix the taps
 878  * according to Parks McClellan
 879  */
 880    for (i=0; i<=numtaps/2; i++)
 881    {
 882       if (symmetry == POSITIVE)
 883       {
 884          if (numtaps%2)
 885             c = 1;
 886          else
 887             c = cos(Pi * (double)i/numtaps);
 888       }
 889       else
 890       {
 891          if (numtaps%2)
 892             c = sin(Pi2 * (double)i/numtaps);
 893          else
 894             c = sin(Pi * (double)i/numtaps);
 895       }
 896       taps[i] = ComputeA((double)i/numtaps, r, ad, x, y)*c;
 897    }
 898
 899 /*
 900  * Frequency sampling design with calculated taps
 901  */
 902    FreqSample(numtaps, taps, h, symmetry);
 903
 904 /*
 905  * Delete allocated memory
 906  */
 907    free(Grid);
 908    free(W);
 909    free(D);
 910    free(E);
 911    free(Ext);
 912    free(x);
 913    free(y);
 914    free(ad);
 915 }
 916