/* Freebirth
 * Copyright (C) 1999 topher lafata <topher@topher.com>,
 *		      Jake Donham <jake@bitmechanic.com>
 *
 * This program 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.
 *
 * This program 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 this program (see COPYING); if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */

#include "filter.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#define FILTER_SECTIONS        2 /* 2 filter sections for 24 db/oct filter */
#define MIN_GAIN              .4
#define MAX_GAIN              .7

#define SAMPLE_RATE       44100


void filter_next_buffer(filter *this);

typedef struct {
        double a0, a1, a2;       /* numerator coefficients */
        double b0, b1, b2;       /* denominator coefficients */
} BIQUAD;

BIQUAD ProtoCoef[FILTER_SECTIONS] = {
  { 1.0, 0, 0, 1.0, 0.765367, 1.0 },
  { 1.0, 0, 0, 1.0, 1.847759, 1.0 }
};


static float **coef_lookup_table;
static int looked_up = 0;



/*coeficient calculation function */
void szxform(
    double *a0, double *a1, double *a2,     /* numerator coefficients */
    double *b0, double *b1, double *b2,   /* denominator coefficients */
    double fc,           /* Filter cutoff frequency */
    double fs,           /* sampling rate */
    double *k,           /* overall gain factor */
    float *coef);         /* pointer to 4 iir coefficients */

float *filter_get_coef(int sections,double cutoff,double q, double gain)
{
  
  int nInd;

  double k    = gain;
  float *Coef = (float *)calloc(4 * sections + 1, sizeof(float));
  float *coef = Coef + 1;
  double Q    = q;
  double fc   = cutoff;
  double fs   = SAMPLE_RATE;
  
  double   a0, a1, a2, b0, b1, b2;


  /* printf("cutoff is %f\n",fc); */
  /* printf("Q is %f\n",Q);  */

 for(nInd = 0; nInd < sections; nInd++)
    {
      a0 = ProtoCoef[nInd].a0;
      a1 = ProtoCoef[nInd].a1;
      a2 = ProtoCoef[nInd].a2;
      
      b0 = ProtoCoef[nInd].b0;
      b1 = ProtoCoef[nInd].b1 / Q;      /* Divide by resonance or Q */
      b2 = ProtoCoef[nInd].b2;
      szxform(&a0, &a1, &a2, &b0, &b1, &b2, fc, fs, &k, coef);
      coef += 4;                       /* Point to next filter section */      

    }
  
  Coef[0] = k;
  return Coef;
}


void filter_init_lookup_table()
{
  int index;
  double q,fc;
  int freq_range, q_range, table_length;
  float q_scale ;
  float gain_scale;

  q_scale = MAX_Q/MAX_FC;
  gain_scale   = - (MAX_GAIN - MIN_GAIN) / MAX_FC; 
  freq_range   = MAX_FC - MIN_FC;
  q_range      = (MAX_Q - MIN_Q) / (double)Q_INCREMENT;

  table_length = freq_range * q_range;

  printf("Calculating coefficient table.");

  index = 0;
  for(fc = MIN_FC; fc < MAX_FC;fc++)
    for(q = MIN_Q;q < MAX_Q; q += (double)Q_INCREMENT){
      /*PATCH BY: Harold Gutch <logix@foobar.franken.de>
       FreeBSD can't handle devide by zero and it was possible
       for scaled_q to go to zero*/
      double scaled_q =  (((q_scale * (fc))  / MAX_Q)* q) + MIN_Q;
      coef_lookup_table[index] = filter_get_coef(FILTER_SECTIONS,
						 fc,
						 scaled_q,
						 gain_scale * fc + MAX_GAIN);
      
      index++;
      if(index % 1000 == 0){
	printf(".");
	fflush(0);
	}
    }
  looked_up = 1;
  printf("\nDone\n");
}

void filter_set_coeffs(filter *this, int fc, float q)
{
  int index;
  index  = ((fc - MIN_FC) * (MAX_Q - MIN_Q) / Q_INCREMENT) 
    + (q / Q_INCREMENT);
 
  this->coef = coef_lookup_table[index];
  
}




sample filter_process_sample(filter *this, sample input)
{
  
  unsigned int i;
  float *hist1_ptr,*hist2_ptr,*coef_ptr;
  float output,new_hist,history1,history2;


/* allocate history array if different size than last call */
  if(!this->history) 
    this->history = (float *) calloc(2*this->length,sizeof(float));
  
  coef_ptr = this->coef;                /* coefficient pointer */
  
  hist1_ptr = this->history;            /* first history */
  hist2_ptr = hist1_ptr + 1;           /* next history */

  
  output = (float)input * (*coef_ptr++);
  
  for (i = 0 ; i < this->length; i++)
    {
      history1 = *hist1_ptr;           /* history values */
      history2 = *hist2_ptr;
      
      output = output - history1 * (*coef_ptr++);
      new_hist = output - history2 * (*coef_ptr++);    /* poles */
      
      output = new_hist + history1 * (*coef_ptr++);
      output = output + history2 * (*coef_ptr++);      /* zeros */
      
      *hist2_ptr++ = *hist1_ptr;
      *hist1_ptr++ = new_hist;
      hist1_ptr++;
      hist2_ptr++;
    } 
  
  return (sample)output;
}

void filter_next_buffer(filter *this) {
  this->next = 1;
}

sample *filter_get_buffer(filter *this)
{
  int i;
  sample *filt_e_buffer;
  sample *buffer;

  if (this->next) {
    this->next = 0;
    buffer = this->sp->get_buffer(this->sp);
    filt_e_buffer = this->filt_e->get_buffer(this->filt_e);
    for(i = 0; i < TBASS_BUFF_SIZE; i++)
      {
	float s;

	/*
	  we're scaling the 0-SAMPLE_MAX range of the envelope into
          a LIMIT-SAMPLE_MAX range 'cause the filter doesn't like the
	  cutoff to be 0
	*/
	int fe = filt_e_buffer[i] * (SAMPLE_MAX - LIMIT) / SAMPLE_MAX + LIMIT;
	int fc = this->fc * fe / SAMPLE_MAX;
	filter_set_coeffs(this,
				fc,
				this->Q);
	s = filter_process_sample(this,buffer[i]);
      
	/*
	  PATCH: Benno Senoner <benno@gardena.net>
	         clip on underflow as well as overflow
	*/
	if(s > SAMPLE_MAX)
	  s = SAMPLE_MAX;
	else if (s < SAMPLE_MIN)
	  s = SAMPLE_MIN;

	this->buffer[i] = s;

      }
  }
  return this->buffer;
}


void filter_set_cutoff(filter *this,double cutoff)
{
  this->fc = cutoff;
  filter_set_coeffs(this,this->fc,this->Q);

}

void filter_set_gain(filter *this,double gain)
{
  this->gain = gain;

}

void filter_set_q(filter *this,double q)
{
  this->Q = q;
  filter_set_coeffs(this,this->fc,this->Q);
}


void filter_delete(filter *this)
{
  free(this->history);
  free(this->coef);
  free(this);

}



/*
 * ----------------------------------------------------------
 *      bilinear.c
 *
 *      Perform bilinear transformation on s-domain coefficients
 *      of 2nd order biquad section.
 *      First design an analog filter and use s-domain coefficients
 *      as input to szxform() to convert them to z-domain.
 *
 * Here's the butterworth polinomials for 2nd, 4th and 6th order sections.
 *      When we construct a 24 db/oct filter, we take to 2nd order
 *      sections and compute the coefficients separately for each section.
 *
 *      n       Polinomials
 * --------------------------------------------------------------------
 *      2       s^2 + 1.4142s +1
 *      4       (s^2 + 0.765367s + 1) (s^2 + 1.847759s + 1)
 *      6       (s^2 + 0.5176387s + 1) (s^2 + 1.414214 + 1) (s^2 + 1.931852s + 1)
 *
 *      Where n is a filter order.
 *      For n=4, or two second order sections, we have following equasions for each
 *      2nd order stage:
 *
 *      (1 / (s^2 + (1/Q) * 0.765367s + 1)) * (1 / (s^2 + (1/Q) * 1.847759s + 1))
 *
 *      Where Q is filter quality factor in the range of
 *      1 to 1000. The overall filter Q is a product of all
 *      2nd order stages. For example, the 6th order filter
 *      (3 stages, or biquads) with individual Q of 2 will
 *      have filter Q = 2 * 2 * 2 = 8.
 *
 *      The nominator part is just 1.
 *      The denominator coefficients for stage 1 of filter are:
 *      b2 = 1; b1 = 0.765367; b0 = 1;
 *      numerator is
 *      a2 = 0; a1 = 0; a0 = 1;
 *
 *      The denominator coefficients for stage 1 of filter are:
 *      b2 = 1; b1 = 1.847759; b0 = 1;
 *      numerator is
 *      a2 = 0; a1 = 0; a0 = 1;
 *
 *      These coefficients are used directly by the szxform()
 *      and bilinear() functions. For all stages the numerator
 *      is the same and the only thing that is different between
 *      different stages is 1st order coefficient. The rest of
 *      coefficients are the same for any stage and equal to 1.
 *
 *      Any filter could be constructed using this approach.
 *
 *      References:
 *             Van Valkenburg, "Analog Filter Design"
 *             Oxford University Press 1982
 *             ISBN 0-19-510734-9
 *
 *             C Language Algorithms for Digital Signal Processing
 *             Paul Embree, Bruce Kimble
 *             Prentice Hall, 1991
 *             ISBN 0-13-133406-9
 *
 *             Digital Filter Designer's Handbook
 *             With C++ Algorithms
 *             Britton Rorabaugh
 *             McGraw Hill, 1997
 *             ISBN 0-07-053806-9
 * ----------------------------------------------------------
 */


void prewarp(double *a0, double *a1, double *a2, double fc, double fs);
void bilinear(
    double a0, double a1, double a2,    /* numerator coefficients */
    double b0, double b1, double b2,    /* denominator coefficients */
    double *k,                                   /* overall gain factor */
    double fs,                                   /* sampling rate */
    float *coef);                         /* pointer to 4 iir coefficients */


/*
 * ----------------------------------------------------------
 *      Pre-warp the coefficients of a numerator or denominator.
 *      Note that a0 is assumed to be 1, so there is no wrapping
 *      of it.
 * ----------------------------------------------------------
 */
void prewarp(
    double *a0, double *a1, double *a2,
    double fc, double fs)
{
    double wp, pi;

    pi = 4.0 * atan(1.0);
    wp = 2.0 * fs * tan(pi * fc / fs);

    if(wp == 0)   /*PATCH BY:  free bsd from Hans Huebner<hans@huebner.org> */
      wp = 0.0001;
    *a2 = (*a2) / (wp * wp);
    *a1 = (*a1) / wp;
}






/*
 * ----------------------------------------------------------
 * bilinear()
 *
 * Transform the numerator and denominator coefficients
 * of s-domain biquad section into corresponding
 * z-domain coefficients.
 *
 *      Store the 4 IIR coefficients in array pointed by coef
 *      in following order:
 *             beta1, beta2    (denominator)
 *             alpha1, alpha2  (numerator)
 *
 * Arguments:
 *             a0-a2   - s-domain numerator coefficients
 *             b0-b2   - s-domain denominator coefficients
 *             k               - filter gain factor. initially set to 1
 *                                and modified by each biquad section in such
 *                                a way, as to make it the coefficient by
 *                                which to multiply the overall filter gain
 *                                in order to achieve a desired overall filter gain,
 *                                specified in initial value of k.
 *             fs             - sampling rate (Hz)
 *             coef    - array of z-domain coefficients to be filled in.
 *
 * Return:
 *             On return, set coef z-domain coefficients
 * ----------------------------------------------------------
 */
void bilinear(
    double a0, double a1, double a2,    /* numerator coefficients */
    double b0, double b1, double b2,    /* denominator coefficients */
    double *k,           /* overall gain factor */
    double fs,           /* sampling rate */
    float *coef         /* pointer to 4 iir coefficients */
)
{
    double ad, bd;

                 /* alpha (Numerator in s-domain) */
    ad = 4. * a2 * fs * fs + 2. * a1 * fs + a0;
                 /* beta (Denominator in s-domain) */
    bd = 4. * b2 * fs * fs + 2. * b1* fs + b0;

                 /* update gain constant for this section */
    *k *= ad/bd;

                 /* Denominator */
    *coef++ = (2. * b0 - 8. * b2 * fs * fs)
                           / bd;         /* beta1 */
    *coef++ = (4. * b2 * fs * fs - 2. * b1 * fs + b0)
                           / bd; /* beta2 */

                 /* Nominator */
    *coef++ = (2. * a0 - 8. * a2 * fs * fs)
                           / ad;         /* alpha1 */
    *coef = (4. * a2 * fs * fs - 2. * a1 * fs + a0)
                           / ad;   /* alpha2 */
}


/*
 * ----------------------------------------------------------
 * Transform from s to z domain using bilinear transform
 * with prewarp.
 *
 * Arguments:
 *      For argument description look at bilinear()
 *
 *      coef - pointer to array of floating point coefficients,
 *                     corresponding to output of bilinear transofrm
 *                     (z domain).
 *
 * Note: frequencies are in Hz.
 * ----------------------------------------------------------
 */
void szxform(
    double *a0, double *a1, double *a2, /* numerator coefficients */
    double *b0, double *b1, double *b2, /* denominator coefficients */
    double fc,         /* Filter cutoff frequency */
    double fs,         /* sampling rate */
    double *k,         /* overall gain factor */
    float *coef)         /* pointer to 4 iir coefficients */
{
                 /* Calculate a1 and a2 and overwrite the original values */
        prewarp(a0, a1, a2, fc, fs);
        prewarp(b0, b1, b2, fc, fs);
        bilinear(*a0, *a1, *a2, *b0, *b1, *b2, k, fs, coef);
}



static sample_producer **get_children(filter *this)
{
  return &(this->sp);
}

static char **get_header(filter *this)
{
  static char *header[] = {
    "int $n_fc = ((filter *)$t)->fc;",
    "float $n_Q = ((filter *)$t)->Q;",
    "float $n_s;",
    NULL
  };
  return header;
}

static char **get_code(filter *this)
{
  static char *code[] = {
    "filter_set_coeffs((filter *)$t, $n_fc *",
    "                  ($i1 * (SAMPLE_MAX - LIMIT) / SAMPLE_MAX + LIMIT) /",
    "                  SAMPLE_MAX, $n_Q);",
    "$n_s = filter_process_sample((filter *)$t, $i0);",
    "if ($n_s > SAMPLE_MAX) $n_s = SAMPLE_MAX;",
    "else if ($n_s < SAMPLE_MIN) $n_s = SAMPLE_MIN;",
    "$o = $n_s;",
    NULL
  };
  return code;
}

static char **get_footer(filter *this)
{
  static char *footer[] = { NULL };
  return footer;
}

filter *filter_new(double cutoff, double q, sample_producer *filt_e,
		   sample_producer* sp)
{
  int i;
  filter *out = malloc(sizeof(filter)); 
  out->get_buffer = filter_get_buffer;
  out->next_buffer = filter_next_buffer;
  out->get_children	= get_children;
  out->get_header	= get_header;
  out->get_code		= get_code;
  out->get_footer	= get_footer;
  out->next = 0;
  out->schedule   = sp_schedule_event;
  out->trigger    = NULL; /*what does it mean to trigger the filter? */

  /*we aren't responding to events yet in the filter */
  out->events     = (event **)malloc(sizeof(event *) 
			 	 * TBASS_BUFF_SIZE);

  for(i = 0; i < TBASS_BUFF_SIZE; i++)
    out->events[i] = NULL; 


  out->length     = FILTER_SECTIONS;
  out->gain       = MAX_GAIN;
  out->coef       = (float *) calloc(4 * out->length + 1, sizeof(float));
  out->fc         = cutoff;
  out->Q          = q;
  out->sp         = sp;
  out->filt_e     = filt_e;
  out->unused     = NULL;
  out->buffer      = (sample *)malloc(sizeof(sample) * TBASS_BUFF_SIZE);

  /* we're doing this here because we don't call filter_init_lookup_table
     in fusebirth, and we need to create the filter but not have it
     blow up in filter_set_coeffs because the table doesn't exist. */
  coef_lookup_table = (float **)
    malloc(sizeof(float*) *
	   (MAX_FC - MIN_FC) * (MAX_Q - MIN_Q) / (double)Q_INCREMENT);

  filter_set_coeffs(out,out->fc,out->Q);
  return out;
  
}

/*
  Local Variables:
  mode: font-lock
  End:
*/
