/*
  !MidiPlay   A MIDI synthesiser and file player.

  filters.c - Filters and effects

  created  22/11/08
  1.0      14/06/14
*/

#include <math.h>
#include "main.h"
#include "kbd.h"
#include "filters.h"
#include "midisyn.h"

/*
 These filter effects are called at the sample rate when selected.
 They are based on delay lines.
 Each filter has 4 functions.
  1. Initialise
  2. Terminate
  3. Control
  4. The actual filter

 The filters were originally characterised for a sample rate of 44100.
 Although they cope with changes to this, various parameters require
 adjustment relative to 44100. This is why the number 44100 appears
 in various places in the code.
*/


/*
============================================================================
                               Flanger
                               -------
 Adds a swept delay to the current sample. The sweep rate can be set as follows,
 sweep rate in Hz = ph.rate * SAMPLE_RATE / (1<<(32-RATE_SCALE))

*/

#define PH_RATE_SCALE  9 // so that 0..1023 is a usable range
#define PH_BUFF_BITS   8 // log2 buffer length (256, 5.8ms, 172Hz)

static short int ph_buff[2][1<<PH_BUFF_BITS]; // stereo buffer (1k RAM)

typedef struct ph_s
{
  unsigned int
    wr,    // delay line index
    ptr,   // sweep oscillator pointer
    rate,  // sweep oscillator rate,  user control
    scaled_rate; // rate adjusted for sample_rate;
} ph_t;

static ph_t ph =
{
  0,  // wr,    buffer index
  0,  // ptr,   sweep oscillator pointer
  30, // rate,  sweep oscillator rate, fairly slow sweep 6.3seconds cycle 0.158Hz
  0
};


/*
 * flanger_init
 * ------------
 * Initialises and enables the Flanger filter
 */
static int flanger_init(void)
{
  memset(ph_buff, 0, sizeof(ph_buff));
  ph.wr = ph.ptr = 0;
  ph.scaled_rate = (ph.rate * syn.sample_rate) / 44100;
  syn.switches |= (1<<FLT_FLANGER);
  return NO_ERROR_;
}


/*
 * flanger_term
 * ------------
 * Terminates the Flanger filter
 */
static int flanger_term(void)
{
  syn.switches &= ~(1<<FLT_FLANGER);
  return NO_ERROR_;
}


/*
 * cmd_flanger
 * -----------
 * Controls the Flanger filter
 */
#define MAX_FLANGER_RATE 1023
int cmd_flanger(char *msg, int len)
{
  int data[2], update = FALSE;

  if(len == 0) // display status
    myprintf("Flanger %s, rate = %d\n", offon[(syn.switches >> FLT_FLANGER) & 1], ph.rate);

  else if(msg[0] == '?') // display help
    myprintf(" Controls the flanger filter\n"
           "  ,I       enable\n"
           "  ,D       disable\n"
           "  ,<rate>  set sweep rate, 0 to %d\n", MAX_FLANGER_RATE);

  else if(msg[0] == '!') // report for controller
    myprintf("FLANGER,%c\nFLANGER,%d\n",
             (syn.switches & (1<<FLT_FLANGER)) ? 'I' : 'D',
              ph.rate);

  else if((msg[1] == 'D')||(msg[1] == 'd'))
  {
    flanger_term();
    update = TRUE;
  }

  else if((msg[1] == 'I')||(msg[1] == 'i'))
  {
    flanger_init();
    update = TRUE;
  }

  else if(sscanf(msg, ",%d", &data[0]) != 1)
    return -PARAMETER_ERROR;

  else if((data[0] < 0)||(data[0] > 1023))
    return -INVALID_RATE;

  else
  {
    ph.rate = data[0];
    ph.scaled_rate = (ph.rate * 44100) / syn.sample_rate;
    update = TRUE;
  }

  if(update)
  {
    data[0] = (syn.switches >> FLT_FLANGER) & 1;
    data[1] = ph.rate;
    midiPlayer_display(ITEM_FLANGER, data);
  }

  return NO_ERROR_;
}


/*
 * flanger
 * -------
 * The Flanger filter
 */
static void flanger(int sample[])
{
  int i;

  // store current sample
  ph_buff[0][ph.wr] = sample[0];
  ph_buff[1][ph.wr] = sample[1];

  // get current triangle wave sweep oscillator value
  i = ph.ptr >> (32-PH_BUFF_BITS-1);
  ph.ptr += ph.scaled_rate << PH_RATE_SCALE; // point to next sweep sample
  if(i >= (1<<PH_BUFF_BITS))
    i = (2<<PH_BUFF_BITS) - 1 - i;

  // get current delay index
  i = ph.wr - i;
  if(i < 0)
    i += (1<<PH_BUFF_BITS);

  // point to next delay sample
  if(++ph.wr >= (1<<PH_BUFF_BITS))
    ph.wr = 0;

  // mix delay with current
  sample[0] += ph_buff[0][i];
  sample[1] += ph_buff[1][i];

  // saturate to signed 16 bit
//  sample[0] = __SSAT(sample[0], 16);
//  sample[1] = __SSAT(sample[1], 16);
  LIMIT(-0x8000, sample[0], 0x7fff);
  LIMIT(-0x8000, sample[1], 0x7fff);
}


/*
============================================================================
                            Reverb
                            ------
 Uses a 3rd order Filter Delay Network.

*/

#define RV_ORDER      3
#define RV_SCALE     16
#define RV_BUFF_BITS 11 // log2 buffer length (2k, therefore maximum delay lengths = 2048)
#define RV_COEF(x) ((int)((x)*(1<<RV_SCALE)+0.5)) // rounded

static short int rv_buff[RV_ORDER][1<<RV_BUFF_BITS]; // delay line buffers (12k RAM)
static short int *rv_r[RV_ORDER], *rv_w[RV_ORDER];          // buffer pointers

typedef struct rv_s
{
  int a[RV_ORDER][RV_ORDER], b[RV_ORDER], c[RV_ORDER], d; // co-efficients
  int e[RV_ORDER]; // delay line lengths
  short int g; // gain before scaling
} rv_t;

static rv_t rv =
{
  {
    {+RV_COEF(0.3), +RV_COEF(0.3), -RV_COEF(0.3)}, // a, feedback matrix gains
    {+RV_COEF(0.3), -RV_COEF(0.3), -RV_COEF(0.3)},
    {-RV_COEF(0.3), +RV_COEF(0.3), -RV_COEF(0.3)}
  },
  { RV_COEF(0.8), RV_COEF(0.8), RV_COEF(0.8)},     // b, input gains
  { RV_COEF(0.8), RV_COEF(0.8), RV_COEF(0.8)},     // c, wet gains
  RV_COEF(0.8),                                    // d, dry gain
  { 1310, 1636, 1813 },                            // e, delay lengths
  150 // gain value before scaling => RV_COEF(0.3)
};


/*
 * reverb_init
 * -----------
 * Initialises and enables the Reverb filter
 */
static int reverb_init(void)
{
  // flush buffers
  memset(rv_buff, 0, sizeof(rv_buff));
  rv_w[0] = rv_buff[0];
  rv_r[0] = rv_buff[0] + 1;
  rv_w[1] = rv_buff[1];
  rv_r[1] = rv_buff[1] + 1;
  rv_w[2] = rv_buff[2];
  rv_r[2] = rv_buff[2] + 1;

  syn.switches |= (1<<FLT_REVERB);
  return NO_ERROR_;
}


/*
 * reverb_gain
 * -----------
 */
static int reverb_gain(int gain)
{
  rv.g = gain;
  int g  = (gain * (1<<RV_SCALE)) / 1000;
  // feedback matrix
  rv.a[0][0] =  g;
  rv.a[0][1] =  g;
  rv.a[0][2] = -g;
  rv.a[1][0] =  g;
  rv.a[1][1] = -g;
  rv.a[1][2] = -g;
  rv.a[2][0] = -g;
  rv.a[2][1] =  g;
  rv.a[2][2] = -g;
  return NO_ERROR_;
}


/*
 * reverb_term
 * -----------
 * Terminates the Reverb filter
 */
static int reverb_term(void)
{
  syn.switches &= ~(1<<FLT_REVERB);
  return NO_ERROR_;
}


/*
 * cmd_reverb
 * ----------
 * Controls the Reverb filter
 */
#define MAX_REVERB 500
int cmd_reverb(char *msg, int len)
{
  int data[2], update = FALSE;

  if(len == 0) // display status
    myprintf("Reverb  %s, gain = %d\n", offon[(syn.switches >> FLT_REVERB) & 1], rv.g);

  else if(msg[0] == '?') // display help
    myprintf(" Controls the Reverb filter\n"
           "  ,I       enable\n"
           "  ,D       disable\n"
           "  ,<gain>  set feedback gain, 0 to %d\n", MAX_REVERB);

  else if(msg[0] == '!') // report for controller
    myprintf("REVERB,%c\nREVERB,%d\n",
             (syn.switches & (1<<FLT_REVERB)) ? 'I' : 'D',
              rv.g);

  else if((msg[1] == 'D')||(msg[1] == 'd'))
  {
    reverb_term();
    update = TRUE;
  }

  else if((msg[1] == 'I')||(msg[1] == 'i'))
  {
    reverb_init();
    update = TRUE;
  }

  else if(sscanf(msg, ",%d", &data[0]) != 1)
    return -PARAMETER_ERROR;

  else if((data[0] < 0)||(data[0] > MAX_REVERB))
    return -INVALID_GAIN;

  else
  {
    reverb_gain(data[0]);
    update = TRUE;
  }

  if(update)
  {
    data[0] = (syn.switches >> FLT_REVERB) & 1;
    data[1] = rv.g;
    midiPlayer_display(ITEM_REVERB, data);
  }

  return NO_ERROR_;
}


/*
 * reverb
 * ------
 * The Reverb filter
 */
static void reverb(int sample[])
{
  int ai[3], ao[3];

  ai[0] = *rv_r[0];
  ai[1] = *rv_r[1];
  ai[2] = *rv_r[2];

  int in = (sample[0] + sample[1]) / 2;
  int out = (ai[0] * rv.c[0]) + (ai[1] * rv.c[1]) + (ai[2] * rv.c[2]);
  sample[0] = ((sample[0] * rv.d) + out) >> RV_SCALE;
  sample[1] = ((sample[1] * rv.d) + out) >> RV_SCALE;

  // saturate to signed 16 bit
//  sample[0] = __SSAT(sample[0], 16);
//  sample[1] = __SSAT(sample[1], 16);
  LIMIT(-0x8000, sample[0], 0x7fff);
  LIMIT(-0x8000, sample[1], 0x7fff);

  ao[0] = ((rv.a[0][0] * ai[0]) + (rv.a[0][1] * ai[1]) + (rv.a[0][2] * ai[2])) >> RV_SCALE;
  ao[1] = ((rv.a[1][0] * ai[0]) + (rv.a[1][1] * ai[1]) + (rv.a[1][2] * ai[2])) >> RV_SCALE;
  ao[2] = ((rv.a[2][0] * ai[0]) + (rv.a[2][1] * ai[1]) + (rv.a[2][2] * ai[2])) >> RV_SCALE;

  *rv_w[0] = ((in * rv.b[0]) >> RV_SCALE) + ao[0];
  *rv_w[1] = ((in * rv.b[1]) >> RV_SCALE) + ao[1];
  *rv_w[2] = ((in * rv.b[2]) >> RV_SCALE) + ao[2];

  rv_w[0] = rv_r[0];
  if(++rv_r[0] >= (rv_buff[0] + rv.e[0]))
    rv_r[0] = rv_buff[0];

  rv_w[1] = rv_r[1];
  if(++rv_r[1] >= (rv_buff[1] + rv.e[1]))
    rv_r[1] = rv_buff[1];

  rv_w[2] = rv_r[2];
  if(++rv_r[2] >= (rv_buff[2] + rv.e[2]))
    rv_r[2] = rv_buff[2];
}



/*
============================================================================
                                 Chorus
                                 ------
A fast swept delay using a quadrature sweep for left and right channels.

*/
#define CHR_BUFF_BITS  10  // log2 buffer length (1k)
#define CHR_SWEEP_Q    20  // Q point for sweep oscillator
static short int chr_buff[2][1<<CHR_BUFF_BITS]; // stereo buffer (4k RAM)
#define SWEEP_MARGIN   10  // log2 buffer margin so delays never get close to zero or maximum

typedef struct chr_s
{
  int w;         // buffer pointer
  int cos, sin;  // sweep oscillator
  int rate;
  int scaled_rate;
} chr_t;

static chr_t chr =
{
  0,  // buffer pointer
  (1<<CHR_SWEEP_Q)-(1<<SWEEP_MARGIN), // cos sweep
  0,  // sin sweep
  14, // rate
  0   // rate scaled for sample rate
};

/*
 * chorus_init
 * -----------
 * Initialises and enables the Chorus filter
 */
static int chorus_init(void)
{
  memset(chr_buff, 0, sizeof(chr_buff));
  chr.w = 0;
  chr.cos = (1<<CHR_SWEEP_Q)-(1<<SWEEP_MARGIN);
  chr.sin = 0;
  chr.scaled_rate = (chr.rate * syn.sample_rate) / 44100;
  syn.switches |= (1<<FLT_CHORUS);
  return NO_ERROR_;
}


/*
 * chorus_term
 * -----------
 * Terminates the Chorus filter
 */
static int chorus_term(void)
{
  syn.switches &= ~(1<<FLT_CHORUS);
  return NO_ERROR_;
}


/*
 * cmd_chorus
 * ----------
 * Controls the Chorus filter
 */
#define MAX_CHORUS 1000
int cmd_chorus(char *msg, int len)
{
  int data[2], update = FALSE;

  if(len == 0) // display status
    myprintf("Chorus  %s, rate = %d\n", offon[(syn.switches >> FLT_CHORUS) & 1], chr.rate);

  else if(msg[0] == '?') // display help
    myprintf(" Controls the Chorus filter\n"
           "  ,I       enable\n"
           "  ,D       disable\n"
           "  ,<rate>  set sweep rate, 0 to %d\n", MAX_CHORUS);

  else if(msg[0] == '!') // report for controller
    myprintf("CHORUS,%d\nCHORUS,%c\n", chr.rate, (syn.switches & (1<<FLT_CHORUS)) ? 'I' : 'D');

  else if((msg[1] == 'D')||(msg[1] == 'd'))
  {
    chorus_term();
    update = TRUE;
  }

  else if((msg[1] == 'I')||(msg[1] == 'i'))
  {
    chorus_init();
    update = TRUE;
  }

  else if(sscanf(msg, ",%d", &data[0]) != 1)
    return -PARAMETER_ERROR;

  else if((data[0] < 0)||(data[0] > MAX_CHORUS))
    return -INVALID_RATE;

  else
  {
    chr.rate = data[0];
    chorus_init();
    update = TRUE;
  }

  if(update)
  {
    data[0] = (syn.switches >> FLT_CHORUS) & 1;
    data[1] = chr.rate;
    midiPlayer_display(ITEM_CHORUS, data);
  }

  return NO_ERROR_;
}


/*
 * chorus
 * ------
 * The Chorus filter
 */
static void chorus(int sample[])
{
  // store current sample
  chr_buff[0][chr.w] = sample[0];
  chr_buff[1][chr.w] = sample[1];

  // quadrature sweep generator
  chr.cos -= (chr.scaled_rate * chr.sin) >> CHR_SWEEP_Q;
  chr.sin += (chr.scaled_rate * chr.cos) >> CHR_SWEEP_Q;

  // unsign and scale left buffer pointer
  int i = (chr.cos + (1<<CHR_SWEEP_Q)) >> (CHR_SWEEP_Q + 1 - CHR_BUFF_BITS);
  // as a delay from the write position
  i = chr.w - i;
  if(i < 0)
    i += (1<<CHR_BUFF_BITS);

  // unsign and scale right buffer pointer
  int j = (chr.sin + (1<<CHR_SWEEP_Q)) >> (CHR_SWEEP_Q + 1 - CHR_BUFF_BITS);
  // as a delay from the write position
  j = chr.w - j;
  if(j < 0)
    j += (1<<CHR_BUFF_BITS);

  // point to next write location
  if(++chr.w >= (1<<CHR_BUFF_BITS))
    chr.w = 0;

  // mix delay with current
  sample[0] += chr_buff[0][i];
  sample[1] += chr_buff[1][j];

  // saturate to signed 16 bit
//  sample[0] = __SSAT(sample[0], 16);
//  sample[1] = __SSAT(sample[1], 16);
  LIMIT(-0x8000, sample[0], 0x7fff);
  LIMIT(-0x8000, sample[1], 0x7fff);
}


/*
============================================================================
                                  Echo
                                  ----
The main echo buffer uses all 64K of the Core Coupled RAM. This gives a maximum delay of
 65536/4/44100 = 371.5ms
 65536/4/48000 = 341.3ms
Each sample is 4 bytes (stereo shorts)
*/

#define EKO_BUFF_LEN 16384 // buffer length
#define EKO_GAIN_Q      10 // feedback gain Q point
static /*__attribute__ ((section (".ccm")))*/ short int eko_buff[2][EKO_BUFF_LEN]; // stereo buffer

typedef struct eko_s
{
  int w;  // buffer pointer
  short int sg; // straight feedback gain
  short int cg; // cross channel feedback gain
  short int d;  // delay
} eko_t;

static eko_t eko =
{
  0,
  500,
  0,
  EKO_BUFF_LEN-1
};


/*
 * echo_init
 * ---------
 * Initialises and enables the Echo filter
 */
static int echo_init(void)
{
  memset(eko_buff, 0, sizeof(eko_buff));
  eko.w = 0;
  syn.switches |= (1<<FLT_ECHO);
  return NO_ERROR_;
}


/*
 * echo_term
 * ---------
 * Terminates the Echo filter
 */
static int echo_term(void)
{
  syn.switches &= ~(1<<FLT_ECHO);
  return NO_ERROR_;
}


/*
 * cmd_echo
 * --------
 * Controls the Echo filter
 */
int cmd_echo(char *msg, int len)
{
  int data[4], update = FALSE;

  if(len == 0) // display status
    myprintf("Echo    %s, delay = %d, straight feedback = %d, cross feedback = %d\n",
           offon[(syn.switches >> FLT_ECHO) & 1], eko.d, eko.sg, eko.cg);

  else if(msg[0] == '?') // display help
    myprintf(" Controls the Echo filter\n"
           "  .I  enable\n"
           "  ,D  disable\n"
           "  ,E,<delay>          1 to %d\n"
           "  ,S,<straight gain>  0 to %d\n"
           "  ,C,<cross gain>     0 to %d\n"
           , EKO_BUFF_LEN-1, (1<<EKO_GAIN_Q)-1, (1<<EKO_GAIN_Q)-1);

  else if(msg[0] == '!') // report for controller
    myprintf("ECHO,%c\nECHO,E,%d\nECHO,S,%d\nECHO,C,%d\n",
             (syn.switches & (1<<FLT_ECHO)) ? 'I' : 'D', eko.d, eko.sg, eko.cg);

  else if((msg[1] == 'D')||(msg[1] == 'd'))
  {
    echo_term();
    update = TRUE;
  }

  else if((msg[1] == 'I')||(msg[1] == 'i'))
  {
    echo_init();
    update = TRUE;
  }

  else if((msg[1] == 'E')||(msg[1] == 'e'))
  {
    if(sscanf(msg+2, ",%d", &data[0]) != 1)
      return -PARAMETER_ERROR;
    if((data[0] < 1)||(data[0] >= EKO_BUFF_LEN))
      return -INVALID_DELAY;
    eko.d = data[0];
    update = TRUE;
  }

  else if((msg[1] == 'S')||(msg[1] == 's'))
  {
    if(sscanf(msg+2, ",%d", &data[0]) != 1)
      return -PARAMETER_ERROR;
    if((data[0] < 0)||(data[0] >= (1<<EKO_GAIN_Q)))
      return -INVALID_GAIN;
    eko.sg = data[0];
    update = TRUE;
  }

  else if((msg[1] == 'C')||(msg[1] == 'c'))
  {
    if(sscanf(msg+2, ",%d", &data[0]) != 1)
      return -PARAMETER_ERROR;
    if((data[0] < 0)||(data[0] >= (1<<EKO_GAIN_Q)))
      return -INVALID_GAIN;
    eko.cg = data[0];
    update = TRUE;
  }

  else
    return -PARAMETER_ERROR;

  if(update)
  {
    data[0] = (syn.switches >> FLT_ECHO) & 1;
    data[1] = eko.d;
    data[2] = eko.sg;
    data[3] = eko.cg;
    midiPlayer_display(ITEM_ECHO, data);
  }

  return NO_ERROR_;
}


/*
 * echo
 * ----
 * The Echo filter
 */
static void echo(int sample[])
{
  int rd = eko.w - eko.d;
  if(rd < 0)
    rd += EKO_BUFF_LEN;

  // get delayed sample
  int r[2];
  r[0] = eko_buff[0][rd];
  r[1] = eko_buff[1][rd];

  // store and output current sample plus feedback components
  eko_buff[0][eko.w] = sample[0] += ((eko.sg * r[0]) + (eko.cg * r[1])) >> EKO_GAIN_Q;
  eko_buff[1][eko.w] = sample[1] += ((eko.sg * r[1]) + (eko.cg * r[0])) >> EKO_GAIN_Q;

  // point to next
  if(++eko.w >= EKO_BUFF_LEN)
    eko.w = 0;

  // saturate to signed 16 bit
//  sample[0] = __SSAT(sample[0], 16);
//  sample[1] = __SSAT(sample[1], 16);
  LIMIT(-0x8000, sample[0], 0x7fff);
  LIMIT(-0x8000, sample[1], 0x7fff);
}


/*
============================================================================
                        Tone Control
                        ------------
       First order bass and treble shelving filters. Fixed Point
*/

static int tone_gain[2] = {0,0};
static int tone_coef[2][3]; // filter coefficients
static int tone_dly[2][2]; // filter delay registers
#define Q 15

/*
 * set_tone_coef
 * -------------
 * Calculates the coefficients for a single filter
 * gain = boost/cut in dB
 * n = number of filter or band (0 = bass, 1 = treble)
 */
void set_tone_coef(int gain, int n)
{
  tone_gain[n] = gain;

  double c[3];
  double g = exp((double)gain/20*LN10);
  double Fs = (double)syn.sample_rate;
  double Fc, a;

  if(gain == 0) // flat
  {
    c[0] = 0;
    c[1] = 0;
    c[2] = 1;
  }
  else if(gain > 0)
    if(n == 0) // bass boost, low pass filter
    {
      Fc = 400 / (g - 1);
      a = 2*PI*Fc/Fs;
      c[0] = a * (g - 1);
      c[1] = 1 - a;
      c[2] = 1;
    }
    else // treble boost, high pass filter
    {
      Fc = 800 * (g - 1);
      a = 2*PI*Fc/Fs;
      c[0] = a * (1 - g);
      c[1] = 1 - a;
      c[2] = g;
    }
  else // (gain < 0)
    if(n == 0) // bass cut, high pass filter
    {
      Fc = 400;
      a = 2*PI*Fc/Fs;
      c[0] = a * (g - 1);
      c[1] = 1 - a;
      c[2] = 1;
    }
    else // treble cut, low pass filter
    {
      Fc = 800;
      a = 2*PI*Fc/Fs;
      c[0] = a * (1 - g);
      c[1] = 1 - a;
      c[2] = g;
    }
  int *ci = (n == 0) ? tone_coef[0] : tone_coef[1];
  ci[0] = (int)(c[0] * (1<<Q));
  ci[1] = (int)(c[1] * (1<<Q));
  ci[2] = (int)(c[2] * (1<<Q));
}


/*
 * tone_init
 * ---------
 */
void tone_init(void)
{
  memset(tone_dly, 0, sizeof(tone_dly));
  set_tone_coef(tone_gain[0], 0);
  set_tone_coef(tone_gain[1], 1);
  syn.switches |= (1<<FLT_TONE);
  midiPlayer_display(ITEM_BASS, &tone_gain[0]);
  midiPlayer_display(ITEM_TREBLE, &tone_gain[1]);
}


/*
 * tone_term
 * ---------
 */
void tone_term(void)
{
  syn.switches &= ~(1<<FLT_TONE);
}


/*
 * cmd_tone
 */
int cmd_tone(char *msg, int len)
{
  int gain, band = -1;

  if(len == 0) // display status
    myprintf("Tone controls %s, Bass %ddB, Treble %ddB\n",
           offon[(syn.switches >> FLT_TONE) & 1], tone_gain[0], tone_gain[1]);

  else if(msg[0] == '?') // display help
    myprintf(" Bass and Treble tone controls\n"
             "  ,B,<gain>  set bass to gain in dB -15 to +15\n"
             "  ,T,<gain>  set treble to gain in dB -15 to +15\n"
             "  ,D  disable\n"
             "  ,I  enable and set a flat response\n");

  else if(msg[0] == '!') // report for controller
    myprintf("TONE,%c\nTONE,B,%d\nTONE,T,%d\n",
      (syn.switches & (1<<FLT_TONE)) ? 'I' : 'D',
       tone_gain[0], tone_gain[1]);

  else if((msg[1] == 'I')||(msg[1] == 'i'))
    tone_init();

  else if((msg[1] == 'D')||(msg[1] == 'd'))
    tone_term();

  else if((msg[1] == 'B')||(msg[1] == 'b'))
    band = 0;
  else if((msg[1] == 'T')||(msg[1] == 't'))
    band = 1;
  else
    return -PARAMETER_ERROR;

  if(band != -1)
  {
    if(sscanf(msg+2, ",%d", &gain) != 1)
      return -PARAMETER_ERROR;
    if(gain < -15)
      gain = -15;
    else if(gain > 15)
      gain = 15;
    set_tone_coef(gain, band);
    if(band == 0)
      midiPlayer_display(ITEM_BASS, &gain);
    else
      midiPlayer_display(ITEM_TREBLE, &gain);
  }

  return NO_ERROR_;
}


/*
 * tone
 * ----
 * The bass/treble filter code,
 */
static void tone(int *sample)
{
  int i, j;
  for(i=0; i<2; i++) // channels
  {
    int x = sample[i];
    for(j=0; j<2; j++) // bass/treble
    {
      int *c = tone_coef[j];
      int *z = &tone_dly[i][j];

      int y1 = c[0]*x + c[1]*(*z);
      x = (c[2]*x + y1) >> Q;
      *z = y1 >> Q;
    }
    sample[i] = x;
  }

  // saturate to signed 16 bit
//  sample[0] = __SSAT(sample[0], 16);
//  sample[1] = __SSAT(sample[1], 16);
  LIMIT(-0x8000, sample[0], 0x7fff);
  LIMIT(-0x8000, sample[1], 0x7fff);
}


/*
============================================================================
*/

/*
 * init_filters
 * ------------
 * Initialises filters if they are enabled.
 * Required if the sample rate is changed.
 */
void init_filters(void)
{
  if(syn.switches & (1<<FLT_TONE))
    tone_init();
  if(syn.switches & (1<<FLT_CHORUS))
    chorus_init();
  if(syn.switches & (1<<FLT_ECHO))
    echo_init();
  if(syn.switches & (1<<FLT_REVERB))
    reverb_init();
  if(syn.switches & (1<<FLT_FLANGER))
    flanger_init();
}


/*
 * cmd_filters
 * -----------
 */
int cmd_filters(char *msg, int len)
{
  if(msg[0] == '?') // display help
    myprintf(" Reports the status of all the filters\n");

  else if(len == 0)
  {
    myprintf("Filter status:\n");
    cmd_tone(NULL, 0);
    cmd_chorus(NULL, 0);
    cmd_echo(NULL, 0);
    cmd_reverb(NULL, 0);
    cmd_flanger(NULL, 0);
  }

  return NO_ERROR_;
}


/*
 * effects
 * -------
 * main selection and execution function
 */
void effects(int sample[])
{
  if(syn.switches & (1<<FLT_TONE))
    tone(sample);
  if(syn.switches & (1<<FLT_CHORUS))
    chorus(sample);
  if(syn.switches & (1<<FLT_ECHO))
    echo(sample);
  if(syn.switches & (1<<FLT_REVERB))
    reverb(sample);
  if(syn.switches & (1<<FLT_FLANGER))
    flanger(sample);
}


/*
============================================================================
*/
