; /*
gcc -O2 -noixemul notchf.c -o notchf -lm
quit 0 ; */
/*************************************************************************\
  notchf.c:
    A cascade of notch filters to remove a narrow frequency range, e. g. 
    mains hum, from 8SVX samples.
  arguments and their meanings:
    <infile>
    <outfile>
    -f<requency>      0..0.5, one or more may be given
    -r                pole radius 0..1, determines the sharpness
    -v[erbose]
  <outfile> defaults to <infile>.out, sharpness defaults to -r0.98
\*************************************************************************/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <signal.h>
#include <math.h>

char version[] = "$VER: notchf 1.1 (11.08.96)";

#define ID_FORM 0x464F524D
#define ID_8SVX 0x38535658
#define ID_VHDR 0x56484452
#define ID_BODY 0x424F4459
#define SHIFT 14            /* for fixed-point filter coefficients */
#define UNIT (1<<SHIFT)
#define NMAX 100            /* max. number of filters to cascade */

typedef unsigned long ULONG;
typedef unsigned short UWORD;
typedef unsigned char UBYTE;

ULONG getlong();
void handle_iff();
float fresponse(float f);
float whereis(float h, float f0, float f1);

FILE *infile, *outfile;
int verbose = 0, n = 0;
float r = 0.98;
struct filter {
  float freq;
  short int a0, a1;      /* filter parameters ... */
  short int x, x1, x2, y, y1, y2;    /* ... and state memory */
};
struct filter filters[NMAX];

main (int argc, char* argv[])
{
  ULONG l;
  int i,rawmode;
  float f,f1,f2, pi = 4*atan(1);
  long rawsize;
  char c, *s;
  char name1[80], name2[80];
  
  name1[0] = name2[0] = '\0';
  while (--argc) {
    s = *++argv;
    if (*s != '-') {
      if (name1[0] == '\0')
        strcpy(name1, s);
      else
        strcpy(name2, s);
    } else {
      switch (*++s) {
        case 'f': f = atof(++s); 
          if (n==NMAX) { printf("too many frequencies\n"); return 5; }
          i = n++;   /* sort frequencies by inserting: */
          while (i>0 && f<filters[i-1].freq) {
            filters[i].freq = filters[i-1].freq; i--;
          }
          filters[i].freq = f; break;
        case 'r': r = atof(++s); break;
        case 'v': verbose = 1; break;
        default:
          printf("Illegal parameter '%s', please use -f/-r/-v!\n", argv[0]);
      }
    }
  }
  if (n<1) {
    printf("Please specify a frequency to cancel (-f)!\n");
    return 5;
  }
  if (name1[0] == '\0') {
    printf("Please specify an input file!\n");
    return 5;
  }
  if (name2[0] == '\0') {
    strcpy(name2, name1);
    strcat(name2, ".out");
  }
  if (!(infile = fopen(name1,"r"))) {
    printf("Can't open %s!\n", name1);
    return 10;
  }
  if (getlong() != ID_FORM) {
    printf("Not an IFF file: %s!\n", name1);
    fclose(infile);
    return 5;
  } 
  l = getlong();  /* skip file size */
  if (getlong() != ID_8SVX) {
    printf("Not an 8SVX file: %s!\n", name1);
    fclose(infile);
    return 5;
  }
  if (!(outfile = fopen(name2,"w"))) {
    printf("Can't open %s for output!\n", name2);
    fclose(infile);
    return 10;
  }
  /* calculate the filter parameters and note the effect of quantization */
  /* on pole radius and frequencies: */
  for (i=0; i<n; i++)
    filters[i].a0 = UNIT * r*r;
  r = sqrt(filters[0].a0 / (float)UNIT);
  for (i=0; i<n; i++) {
    filters[i].a1 = -UNIT*(1 + r*r) * cos(2*pi * filters[i].freq);
    filters[i].freq = acos(-filters[i].a1 / (UNIT*(1 + r*r))) / (2*pi);
  }
  /* tell the user what his parameters will do */
  printf("filtering \"%s\" as \"%s\"\n", name1, name2);
  if (n == 1) {
    printf("  filter sharpness is -r%f, notch shape:\n", r);
    f1 = whereis(0.01,0,f); f2 = whereis(0.01,f,0.5);
    printf("  -40dB : %.6f ± %.6f\n", (f1+f2)/2, (f2-f1)/2);
    f1 = whereis(0.7,0,f); f2 = whereis(0.7,f,0.5);
    printf("   -3dB :          ± %.6f\n", (f2-f1)/2);
  } else {
    printf("  filter sharpness is -r%f, frequencies:\n", r);
    for (i=0; i<n; i++) {
      if (i>0) {
        f = (filters[i-1].freq + filters[i].freq) / 2;
        printf("  %.6f: %6.1f dB\n", f, 20*log10(fresponse(f)));
      }
      f = filters[i].freq;
      printf("  %.6f: zero (%.1g)\n", f, fresponse(f));
    }
  }
  rewind(infile);
  handle_iff();
  fclose(infile);
  fclose(outfile);

  return 0;
}

int ctrl_c = 0;

void oops()
/* break-handler during filter operation */
{
  ctrl_c = 1;
}

long filter_body(long len)
/* Return value is the length of the filtered sample. */
{
  long outlen = 0, warnings = 0;
  int i, modth, min = 0, max = 0;
  struct filter *f;
  signed char c;
  short int sample;

  signal(SIGINT, *oops);  /* intercept user break (Ctrl-C) */
  for (i=0; i<n; i++) {
    f = &filters[i];
    f->x1 = f->x2 = f->y1 = f->y2 = 0;
  }
  modth = (len-1) % 1000;       /* start modulo counter */
  while (len--) {
    sample = c = fgetc(infile);
    sample <<= 6;                    /* internal 14-bit resolution */
    if (modth-- == 0 ) {
      modth = 999;
      printf("\r%d input bytes to go. ",len); fflush(stdout);
    }
    for (i=0; i<n; i++) {   /* here come the filters */
      f = &filters[i];
      f->x = sample;
      f->y = (f->a0*(f->x - f->y2) + f->a1*(f->x1 - f->y1)) / UNIT + f->x2;
      f->x2 = f->x1; f->x1 = f->x; f->y2 = f->y1; f->y1 = f->y;
      sample = (f->y + f->x) / 2;
    }
    sample /= 64;     /* >>=6 only works on unsigned */
    if (sample>127) {
      if (sample>max) max = sample;
      sample = 127; warnings++;
    } else if (sample<-128) {
      if (sample<min) min = sample;
      sample = -128; warnings++;
    }
    fputc(sample, outfile); outlen++;
    if (ctrl_c) {
      printf("*** BREAK - output sample is incomplete!\n");
      break;
    }
  }
  if (warnings)
    printf("\r%ld warnings out of %ld bytes, range %d..%d\n",
      warnings, outlen, min, max);
  else
    printf("\r%ld bytes filtered, no warnings\n", outlen);
  signal(SIGINT, SIG_DFL);  /* restore normal handling */
  
  return outlen;
}

ULONG getlong()
/* returns 0 at EOF, not so brilliant, but sufficient for our purpose */
{
  ULONG l=0;
  int i, c;
  for (i=0; i<4; i++)
    l = (l<<8) | (c = fgetc(infile));
  if (c != EOF)
    return l;
  else
    return 0L;
}

void putlong(ULONG l)
{
  int i, byte[4];
  for (i=0; i<4; i++) {
    byte[3-i] = l & 0xff; l >>= 8;
  }
  for (i=0; i<4; i++)
    fputc(byte[i], outfile);
}

UWORD getword()
{
  UWORD w;
  w = fgetc(infile) << 8;
  return w | fgetc(infile);
}

void putword(UWORD w)
{
  fputc(w >> 8, outfile);
  fputc(w & 0xff, outfile);
}

void copy_chunk(int verbose)
/* copy an IFF chunk to the output file (its ID has already been copied) */
{
  ULONG l;
  int i=0;
  char c;
  
  putlong(l = getlong());
  if (verbose>0)
    while (l--) {
      fputc(c = fgetc(infile), outfile);
      if (i++ < verbose)
        if ((c & 0x7f)<' ') putchar('.'); else putchar(c);
    }
  else if (verbose<0)
    while (l--) {
      fputc(c = fgetc(infile), outfile);
      if (i++ < -verbose) {
        printf("%02x",c); if ((i & 3) == 0) putchar(' ');
      }
    }
  else
    while (l--)
      fputc(fgetc(infile), outfile);
}

void handle_iff()
/* The input file has been recognized as valid IFF 8SVX, so copy it */
{
  ULONG l, ifflen, vhdlen, bodylen;
  UWORD spersec;
  fpos_t pos1, pos2, pos3;  /* markers for values calculated later */
  int i, done = 0;
  long delta;
  char s1[5];

  putlong(getlong());  /* "FORM" */
  fgetpos(outfile, &pos1);  /* pos1: for ifflen */
  putlong(ifflen = getlong());
  putlong(getlong());  /* "8SVX" */
  while (!done && (l = getlong())) {
    putlong(l);
    if (verbose) {
      strncpy(s1, (char *)&l, 4); s1[4] = '\0'; printf("%s: ",s1);
    }
    if (l == ID_VHDR) {
      fgetpos(infile, &pos2);
      fseek(infile, 16, SEEK_CUR);
      spersec = getword();
      fgetc(infile);
      if (fgetc(infile)) {    /* sCompression byte */
        printf("sorry, can't handle compressed samples\n");
        exit(10);
      }
      fsetpos(infile, &pos2);
      copy_chunk(-24*verbose); if (verbose) putchar('\n');
    } else if (l == ID_BODY) {
      if (verbose) putchar('\n');
      fgetpos(outfile, &pos3);  /* pos3: for bodylen */
      putlong(bodylen = getlong());
      delta = filter_body(bodylen) - bodylen;  /* -> delta<0 */
      if ((bodylen + delta) & 1L) {  /* odd chunk size, fix it */
        fputc(0, outfile); delta++;
      }
      done = 1;   /* process no chunks after BODY */
    } else {
      copy_chunk(60*verbose); if (verbose) putchar('\n');
    }
  }
  /* now adjust the sizes: */
  fsetpos(outfile, &pos1); putlong(ifflen + delta);
  fsetpos(outfile, &pos3); putlong(bodylen + delta);
}


/*************************************************************************\
      Here come some routines for complex arithmetic to calculate the
               frequency response of our filter arrangement.
\*************************************************************************/

typedef struct {
  float re, im;
} complex;


float cmag(complex c)
/* magnitude of c */
{
  return sqrt(c.re*c.re + c.im*c.im);
}

float arg(complex c)
/* angle of complex c */
{
  return atan2(c.im, c.re);
}

void cmult(complex a, complex b, complex *c)
/* well, multiply complex numbers (c = a*b) */
{
  c->re = a.re*b.re - a.im*b.im;
  c->im = a.re*b.im + a.im*b.re;
}

void cdiv(complex a, complex b, complex *c)
/* divide complex numbers (c = a/b) */
{
  float r2 = b.re*b.re + b.im*b.im;
  if (r2 != 0) {
    c->re = (a.re*b.re + a.im*b.im)/r2;
    c->im = (a.im*b.re - a.re*b.im)/r2;
  }
}

void cadd(complex a, complex b, complex *c)
/* complex addition */
{
  c->re = a.re + b.re;
  c->im = a.im + b.im;
}

void all_pass(float a0, float a1, complex z, complex *h)
/*                    a0*z^2 + a1*z + 1
 * calculate  h(z) = -------------------
 *                     z^2 + a1*z + a0
 */
{
  complex c1, c2;
  c1.re = a0; c1.im = 0;  /* numerator */
  cmult(c1, z, &c1);
  c1.re += a1;
  cmult(c1, z, &c1);
  c1.re += 1;
  c2.re = 1; c2.im = 0;   /* denominator */
  cmult(c2, z, &c2);
  c2.re += a1;
  cmult(c2, z, &c2);
  c2.re += a0;
  cdiv(c1, c2, h);
}

float fresponse(float f)
/* returns the magnitude of the (complex) frequency response */
{
  float a0, a1, pi = 4*atan(1);
  complex z, hi, h;
  int i;

  z.re = cos(2*pi*f); z.im = sin(2*pi*f);
  h.re = 1; h.im = 0;
  for (i=0; i<n; i++) {
    a0 = filters[i].a0 / (float)UNIT;
    a1 = filters[i].a1 / (float)UNIT;
    all_pass(a0, a1, z, &hi);
    /* create a notch filter from the all-pass filter: */
    hi.re += 1; hi.re /= 2; hi.im /= 2;
    cmult(h, hi, &h);
  }
  return cmag(h);
}

float whereis(float h, float f0, float f1)
/* iterates (by binary search) to find an <f> where fresponse(f) == h */
{
  float f, e, e0, e1;

  e1 = fresponse(f1) - h;
  e0 = fresponse(f0) - h;
  if (e0*e1>0) return 0;
  while (f1 - f0 > 1e-7) {
    f = (f0 + f1)/2; e = fresponse(f) - h;
    if (e*e1<0) {
      f0 = f; e0 = e;
    } else {
      f1 = f; e1 = e;
    }
  }
  return f;
}