/* -*- Mode: C ; c-basic-offset: 2 -*- */
/*****************************************************************************
 *
 *   Copyright (C) 2007 Nedko Arnaudov <nedko@arnaudov.name>
 *   Copyright (C) Josh Green <jgreen@users.sourceforge.net>
 *   Copyright (C) Achim Settelmeier <vocoder@m1.sirlab.de>
 *
 *   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; version 2 of the License
 *
 *   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; if not, write to the Free Software
 *   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 *****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include <lv2.h>

#define MAX_BANDS  16
#define AMPLIFIER 16.0

struct bandpass
{
  float c, f, att;

  float freq;
  float low1, low2;
  float mid1, mid2;
  float high1, high2;
  float y;
};

struct bands_out
{
  float decay;
  float oldval;
  float level;            /* 0.0 - 1.0 level of this output band */
};

const float decay_table[] =
{
  1/100.0,
  1/100.0, 1/100.0, 1/100.0,
  1/125.0, 1/125.0, 1/125.0,
  1/166.0, 1/166.0, 1/166.0,
  1/200.0, 1/200.0, 1/200.0,
  1/250.0, 1/250.0, 1/250.0
};

/* The port numbers for the plugin: */

#define PORT_FORMANT   0
#define PORT_CARRIER   1
#define PORT_OUTPUT    2
#define CTRL_BANDCOUNT 3
#define CTRL_BAND1LVL  4

#define PORT_COUNT     4 + MAX_BANDS


/* useful macros */
#undef CLAMP
#define CLAMP(x, low, high)  (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))

/* Instance data for the vocoder plugin */
struct vocoder
{
  float sample_rate;

  int num_bands;                /* current number of bands */
  float mainvol;                /* main volume */

  struct bandpass bands_formant[MAX_BANDS]; /* one structure per band */
  struct bandpass bands_carrier[MAX_BANDS]; /* one structure per band */
  struct bands_out bands_out[MAX_BANDS]; /* one structure per band */

  /* Ports */

  float * portFormant;    /* Formant signal port data location */
  float * portCarrier;    /* Carrier signal port data location */
  float * portOutput;     /* Output audio port data location */
  float * ctrlBandCount;  /* Band count control */
  float * ctrlBandLevels[MAX_BANDS]; /* level controls for each band */

};

void
vocoder_do_bandpasses(
  struct bandpass * bands,
  float sample,
  struct vocoder * vocoder_ptr)
{
  int i;
  for (i=0; i < vocoder_ptr->num_bands; i++)
  {
    bands[i].high1 = sample - bands[i].f * bands[i].mid1 - bands[i].low1;
    bands[i].mid1 += bands[i].high1 * bands[i].c;
    bands[i].low1 += bands[i].mid1;

    bands[i].high2 = bands[i].low1 - bands[i].f * bands[i].mid2 - bands[i].low2;
    bands[i].mid2 += bands[i].high2 * bands[i].c;
    bands[i].low2 += bands[i].mid2;
    bands[i].y = bands[i].high2 * bands[i].att;
  }
}

static
LV2_Handle
instantiate(
  const LV2_Descriptor * descriptor,
  double sample_rate,
  const char * bundle_path,
  const LV2_Feature * const * host_features)
{
  struct vocoder * vocoder_ptr;

  vocoder_ptr = malloc(sizeof(struct vocoder));
  if (vocoder_ptr == NULL)
  {
    return NULL;
  }

  vocoder_ptr->sample_rate = sample_rate;
  vocoder_ptr->num_bands = -1;

  return (LV2_Handle)vocoder_ptr;
}

#define vocoder_ptr ((struct vocoder *)lv2instance)

/* Initialise and activate a plugin instance. */
static
void
activate(
  LV2_Handle lv2instance)
{
  int i;

  vocoder_ptr->mainvol = 1.0 * AMPLIFIER;

  for (i = 0; i < MAX_BANDS; i++)
  {
    vocoder_ptr->bands_out[i].oldval = 0.0;
  }
}

static
void
deactivate(
  LV2_Handle lv2instance)
{
}

/* Connect a port to a data location. */
static
void
connect_port(
  LV2_Handle lv2instance,
  uint32_t port,
  void * data)
{
  switch (port)
  {
  case PORT_FORMANT:            /* formant port */
    vocoder_ptr->portFormant = data;
    return;
  case PORT_CARRIER:            /* carrier port */
    vocoder_ptr->portCarrier = data;
    return;
  case PORT_OUTPUT:             /* output port */
    vocoder_ptr->portOutput = data;
    return;
  case CTRL_BANDCOUNT:          /* band count control */
    vocoder_ptr->ctrlBandCount = data;
    return;
  }

  /* a band level control? */
  if (port >= CTRL_BAND1LVL && port < CTRL_BAND1LVL + MAX_BANDS)
  {
    vocoder_ptr->ctrlBandLevels[port - CTRL_BAND1LVL] = data;
    return;
  }

  assert(0);
}

/* Run a vocoder instance for a block of SampleCount samples. */
static
void
run(
  LV2_Handle lv2instance,
  uint32_t sample_count)
{
  int i, j, numbands;
  float a;
  float x, c;

  numbands = (int)(*vocoder_ptr->ctrlBandCount);
  if (numbands < 1 || numbands > MAX_BANDS) numbands = MAX_BANDS;

  /* initialize bandpass information if num_bands control has changed,
     or on first run */
  if (vocoder_ptr->num_bands != numbands)
  {
    vocoder_ptr->num_bands = numbands;

    for(i=0; i < numbands; i++)
    {
      memset(&vocoder_ptr->bands_formant[i], 0, sizeof(struct bandpass));

      a = 16.0 * i/(double)numbands;  // stretch existing bands

      if (a < 4.0)
        vocoder_ptr->bands_formant[i].freq = 150 + 420 * a / 4.0;
      else
        vocoder_ptr->bands_formant[i].freq = 600 * pow (1.23, a - 4.0);

      c = vocoder_ptr->bands_formant[i].freq * 2 * M_PI / vocoder_ptr->sample_rate;
      vocoder_ptr->bands_formant[i].c = c * c;

      vocoder_ptr->bands_formant[i].f = 0.4/c;
      vocoder_ptr->bands_formant[i].att =
        1/(6.0 + ((exp (vocoder_ptr->bands_formant[i].freq
                        / vocoder_ptr->sample_rate) - 1) * 10));

      memcpy(&vocoder_ptr->bands_carrier[i],
             &vocoder_ptr->bands_formant[i], sizeof(struct bandpass));

      vocoder_ptr->bands_out[i].decay = decay_table[(int)a];
      vocoder_ptr->bands_out[i].level =
        CLAMP (*vocoder_ptr->ctrlBandLevels[i], 0.0, 1.0);
    }
  }
  else                 /* get current values of band level controls */
  {
    for (i = 0; i < numbands; i++)
      vocoder_ptr->bands_out[i].level = CLAMP (*vocoder_ptr->ctrlBandLevels[i],
                                           0.0, 1.0);
  }

  for (i=0; i < sample_count; i++)
  {
    vocoder_do_bandpasses(vocoder_ptr->bands_carrier, vocoder_ptr->portCarrier[i], vocoder_ptr);
    vocoder_do_bandpasses(vocoder_ptr->bands_formant, vocoder_ptr->portFormant[i], vocoder_ptr);

    vocoder_ptr->portOutput[i] = 0.0;
    for (j=0; j < numbands; j++)
    {
      vocoder_ptr->bands_out[j].oldval = vocoder_ptr->bands_out[j].oldval
        + (fabs (vocoder_ptr->bands_formant[j].y)
           - vocoder_ptr->bands_out[j].oldval)
        * vocoder_ptr->bands_out[j].decay;
      x = vocoder_ptr->bands_carrier[j].y * vocoder_ptr->bands_out[j].oldval;
      vocoder_ptr->portOutput[i] += x * vocoder_ptr->bands_out[j].level;
    }
    vocoder_ptr->portOutput[i] *= vocoder_ptr->mainvol;
  }
}

static
void
cleanup(LV2_Handle lv2instance)
{
  free(vocoder_ptr);
}

static LV2_Descriptor g_lv2descriptor =
{
  .URI = "http://home.gna.org/lv2vocoder/1",
  .instantiate = instantiate,
  .connect_port = connect_port,
  .activate = activate,
  .deactivate = deactivate,
  .run = run,
  .cleanup = cleanup,
};

const LV2_Descriptor *
lv2_descriptor(uint32_t index)
{
  if (index == 0)
  {
    return &g_lv2descriptor;
  }

  return NULL;
}
