/*
 * Talkbox
 *
 * This module is ported from the mdaTalkbox plugin by Paul Kellet
 * (maxim digital audio).
 *
 * More information on his plugins and the original code can be found here:
 *
 * http://mda.smartelectronix.com/
 *
 */

#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "soundpipe.h"

#ifndef TWO_PI
#define TWO_PI 6.28318530717958647692528676655901
#endif

#define ORD_MAX 50

static void lpc_durbin(SPFLOAT *r, int p, float *k, float *g)
{
  int i, j;
  SPFLOAT a[ORD_MAX], at[ORD_MAX], e=r[0];

  for(i=0; i<=p; i++) a[i] = at[i] = 0.0f;

  for(i=1; i<=p; i++)
  {
    k[i] = -r[i];

    for(j=1; j<i; j++)
    {
      at[j] = a[j];
      k[i] -= a[j] * r[i-j];
    }
    if(fabs(e) < 1.0e-20f) { e = 0.0f;  break; }
    k[i] /= e;

    a[i] = k[i];
    for(j=1; j<i; j++) a[j] = at[j] + k[i] * at[i-j];

    e *= 1.0f - k[i] * k[i];
  }

  if(e < 1.0e-20f) e = 0.0f;
  *g = (float)sqrt(e);
}

static void lpc(float *buf, float *car, uint32_t n, uint32_t o)
{
    SPFLOAT z[ORD_MAX], r[ORD_MAX], k[ORD_MAX], G, x;
    uint32_t i, j, nn=n;
    SPFLOAT min;

    /* buf[] is already emphasized and windowed */
    for(j=0; j<=o; j++, nn--) {
        z[j] = r[j] = 0.0f;
        for(i=0; i<nn; i++) r[j] += buf[i] * buf[i+j]; /* autocorrelation */
    }

    r[0] *= 1.001f;  /* stability fix */

    min = 0.00001f;
    if(r[0] < min) { for(i=0; i<n; i++) buf[i] = 0.0f; return; }

    lpc_durbin(r, o, k, &G);  /* calc reflection coeffs */

    for(i=1; i<=o; i++) {
        if(k[i] > 0.995f) k[i] = 0.995f; else if(k[i] < -0.995f) k[i] = -.995f;
    }

    for(i=0; i<n; i++) {
        x = G * car[i];
        /* lattice filter */
        for(j=o; j>0; j--) {
            x -= k[j] * z[j-1];
            z[j] = z[j-1] + k[j] * x;
        }
        buf[i] = z[0] = x;  /* output buf[] will be windowed elsewhere */
    }
}

int sp_talkbox_create(sp_talkbox **p)
{
    *p = malloc(sizeof(sp_talkbox));
    return SP_OK;
}

int sp_talkbox_destroy(sp_talkbox **p)
{
    free(*p);
    return SP_OK;
}

int sp_talkbox_init(sp_data *sp, sp_talkbox *p)
{
    uint32_t n;
    p->quality = 1.f;
    p->N = 1;
    p->K = 0;

    n = (uint32_t)(0.01633f * sp->sr);
    if(n > SP_TALKBOX_BUFMAX) n = SP_TALKBOX_BUFMAX;

    /* calculate hanning window */
    if(n != p->N) {
        SPFLOAT dp;
        SPFLOAT pos;
        p->N = n;
        dp = TWO_PI / (SPFLOAT)p->N;
        pos = 0.0f;
        for(n=0; n < p->N; n++) {
            p->window[n] = 0.5f - 0.5f * (SPFLOAT)cos(pos);
            pos += dp;
        }
    }

    /* zero out variables and buffers */
    p->pos = p->K = 0;
    p->emphasis = 0.0f;
    p->FX = 0;

    p->u0 = p->u1 = p->u2 = p->u3 = p->u4 = 0.0f;
    p->d0 = p->d1 = p->d2 = p->d3 = p->d4 = 0.0f;

    memset(p->buf0, 0, SP_TALKBOX_BUFMAX * sizeof(SPFLOAT));
    memset(p->buf1, 0, SP_TALKBOX_BUFMAX * sizeof(SPFLOAT));
    memset(p->car0, 0, SP_TALKBOX_BUFMAX * sizeof(SPFLOAT));
    memset(p->car1, 0, SP_TALKBOX_BUFMAX * sizeof(SPFLOAT));
    return SP_OK;
}

int sp_talkbox_compute(sp_data *sp, sp_talkbox *t, SPFLOAT *src, SPFLOAT *exc, SPFLOAT *out)
{
    int32_t p0=t->pos, p1 = (t->pos + t->N/2) % t->N;
    SPFLOAT e=t->emphasis, w, o, x, fx=t->FX;
    SPFLOAT p, q, h0=0.3f, h1=0.77f;
    SPFLOAT den;

    t->O = (uint32_t)((0.0001f + 0.0004f * t->quality) * sp->sr);

    o = *src;
    x = *exc;

    p = t->d0 + h0 * x;
    t->d0 = t->d1;
    t->d1 = x - h0 * p;

    q = t->d2 + h1 * t->d4;
    t->d2 = t->d3;
    t->d3 = t->d4 - h1 * q;

    t->d4 = x;

    x = p + q;

    if(t->K++) {
        t->K = 0;

        /* carrier input */
        t->car0[p0] = t->car1[p1] = x;

        /* 6dB/oct pre-emphasis */
        x = o - e;  e = o;

        /* 50% overlapping hanning windows */
        w = t->window[p0]; fx = t->buf0[p0] * w;  t->buf0[p0] = x * w;
        if(++p0 >= t->N) { lpc(t->buf0, t->car0, t->N, t->O);  p0 = 0; }

        w = 1.0f - w;  fx += t->buf1[p1] * w;  t->buf1[p1] = x * w;
        if(++p1 >= t->N) { lpc(t->buf1, t->car1, t->N, t->O); }
    }

    p = t->u0 + h0 * fx;
    t->u0 = t->u1;
    t->u1 = fx - h0 * p;

    q = t->u2 + h1 * t->u4;
    t->u2 = t->u3;
    t->u3 = t->u4 - h1 * q;

    t->u4 = fx;
    x = p + q;

    o = x * 0.5;
    *out = o;
    t->emphasis = e;
    t->pos = p0;
    t->FX = fx;

    den = 1.0e-10f;
    /* anti-denormal */
    if(fabs(t->d0) < den) t->d0 = 0.0f;
    if(fabs(t->d1) < den) t->d1 = 0.0f;
    if(fabs(t->d2) < den) t->d2 = 0.0f;
    if(fabs(t->d3) < den) t->d3 = 0.0f;
    if(fabs(t->u0) < den) t->u0 = 0.0f;
    if(fabs(t->u1) < den) t->u1 = 0.0f;
    if(fabs(t->u2) < den) t->u2 = 0.0f;
    if(fabs(t->u3) < den) t->u3 = 0.0f;
    return SP_OK;
}