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

static float s[MAXWINDOW], y[MAXWINDOW], h[MAXWINDOW];
static float fa[6], u, u1, yp1, yp2;

inline static void
auto_correl(float *w, int n, int p, float *r)
{
      int i, k, nk;

      for (k = 0; k <= p; k++) {
            nk = n - k;
            r[k] = 0.0f;
            for (i = 0; i < nk; i++)
                  r[k] += w[i] * w[i + k];
      }
}

inline static void
durbin(float *r, int p, float *k, float *g)
{
      int i, j;
      float a[LPC_FILTORDER + 1], at[LPC_FILTORDER + 1], e;

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

      e = r[0];
      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];
            }
            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];
      }

      *g = (float)sqrt(e);
}

inline static void
inverse_filter(float *w, float *k)
{
      int i, j;
      float b[PITCHORDER + 1], bp[PITCHORDER + 1], f[PITCHORDER + 1];

      for (i = 0; i <= PITCHORDER; i++)
            b[i] = f[i] = bp[i] = 0.0;

      for (i = 0; i < BUFLEN / DOWN; i++) {
            f[0] = b[0] = w[i];
            for (j = 1; j <= PITCHORDER; j++) {
                  f[j] = f[j - 1] + k[j] * bp[j - 1];
                  b[j] = k[j] * f[j - 1] + bp[j - 1];
                  bp[j - 1] = b[j - 1];
            }
            w[i] = f[PITCHORDER];
      }
}

inline static void
calc_pitch(float *w, float *per)
{
      int i, j, rpos;
      float d[MAXWINDOW / DOWN], k[PITCHORDER + 1], r[MAXPER + 1], g, rmax;
      float rval, rm, rp;
      float a, b, c, x, y;
      static int vuv = 0;

      for (i = 0, j = 0; i < BUFLEN; i += DOWN)
            d[j++] = w[i];
      auto_correl(d, BUFLEN / DOWN, PITCHORDER, r);
      durbin(r, PITCHORDER, k, &g);
      inverse_filter(d, k);
      auto_correl(d, BUFLEN / DOWN, MAXPER + 1, r);
      rpos = 0;
      rmax = 0.0;
      for (i = MINPER; i <= MAXPER; i++) {
            if (r[i] > rmax) {
                  rmax = r[i];
                  rpos = i;
            }
      }

      rm = r[rpos - 1];
      rp = r[rpos + 1];
      rval = rmax / r[0];

      a = 0.5 * rm - rmax + 0.5 * rp;
      b = -0.5 * rm * (2.0 * rpos + 1.0) + 
          2.0 * rpos * rmax + 0.5 * rp * (1.0 - 2.0 * rpos);
      c = 0.5 * rm * (rpos * rpos + rpos) +
          rmax * (1.0 - rpos * rpos) + 0.5 * rp * (rpos * rpos - rpos);

      x = -b / (2.0 * a);
      y = a * x * x + b * x + c;
      x *= DOWN;

      rmax = y;
      rval = rmax / r[0];
      if (rval >= 0.4 || (vuv == 3 && rval >= 0.3)) {
            *per = x;
            vuv = (vuv & 1) * 2 + 1;
      } else {
            *per = 0.0;
            vuv = (vuv & 1) * 2;
      }
}

void
lpc_init(lpcstate_t* state)
{
      int     i;
      float   r, v, w, wcT;

      for (i = 0; i < BUFLEN; i++) {
            s[i] = 0.0;
            h[i] = WSCALE * (0.54 - 0.46 *
                   cos(2 * M_PI * i / (BUFLEN - 1.0)));
      }
      wcT = 2 * M_PI * FC / FS;
      r = 0.36891079f * wcT;
      v = 0.18445539f * wcT;
      w = 0.92307712f * wcT;
      fa[1] = -exp(-r);
      fa[2] = 1.0 + fa[1];
      fa[3] = -2.0 * exp(-v) * cos(w);
      fa[4] = exp(-2.0 * v);
      fa[5] = 1.0 + fa[3] + fa[4];

      u1 = 0.0f;
      yp1 = 0.0f;
      yp2 = 0.0f;

      state->Oldper = 0.0f;
      state->OldG = 0.0f;
      for (i = 0; i <= LPC_FILTORDER; i++) {
            state->Oldk[i] = 0.0f;
            state->bp[i] = 0.0f;
      }
      state->pitchctr = 0;
}

void
lpc_analyze(short *buf, lpcparams_t *params)
{
      int     i, j;
      float   w[MAXWINDOW], r[LPC_FILTORDER + 1];
      float   per, G, k[LPC_FILTORDER + 1];

      for (i = 0, j = BUFLEN - FRAMESIZE; i < FRAMESIZE; i++, j++) {
            s[j] = (float)(*buf++)/32768.0;
            u = fa[2] * s[j] - fa[1] * u1;
            y[j] = fa[5] * u1 - fa[3] * yp1 - fa[4] * yp2;
            u1 = u;
            yp2 = yp1;
            yp1 = y[j];
      }

      calc_pitch(y, &per);

      for (i = 0; i < BUFLEN; i++)
            w[i] = s[i] * h[i];
      auto_correl(w, BUFLEN, LPC_FILTORDER, r);
      durbin(r, LPC_FILTORDER, k, &G);

      params->period = (unsigned short)(per * 256.0f);
      params->gain = (unsigned char)(G * 256.0f);
      for (i = 0; i < LPC_FILTORDER; i++)
            params->k[i] = (char)(k[i + 1] * 128.f);

      memcpy(s, s + FRAMESIZE, (BUFLEN - FRAMESIZE) * sizeof(s[0]));
      memcpy(y, y + FRAMESIZE, (BUFLEN - FRAMESIZE) * sizeof(y[0]));
}

int
lpc_synthesize(short *buf, lpcparams_t *params, lpcstate_t* state, double speedup)
{
      int i, j;
      int frameSize = FRAMESIZE/speedup;
      register double u, f, per, G, NewG, Ginc, Newper, perinc;
      double k[LPC_FILTORDER + 1], Newk[LPC_FILTORDER + 1],
             kinc[LPC_FILTORDER + 1];

      per = (double) params->period / 256.;
      G = (double) params->gain / 256.;
      k[0] = 0.0;
      for (i = 0; i < LPC_FILTORDER; i++)
            k[i + 1] = (double) (params->k[i]) / 128.;

      G /= sqrt(BUFLEN / (per == 0.0? 3.0 : per));
      Newper = state->Oldper;
      NewG = state->OldG;
      for (i = 1; i <= LPC_FILTORDER; i++)
            Newk[i] = state->Oldk[i];

      if (state->Oldper != 0 && per != 0) {
            perinc = (per - state->Oldper) / (double)frameSize;
            Ginc = (G - state->OldG) / (double)frameSize;
            for (i = 1; i <= LPC_FILTORDER; i++)
                  kinc[i] = (k[i] - state->Oldk[i]) / (double)frameSize;
      } else {
            perinc = 0.0;
            Ginc = 0.0;
            for (i = 1; i <= LPC_FILTORDER; i++)
                  kinc[i] = 0.0;
      }

      if (Newper == 0)
            state->pitchctr = 0;

      for (i = 0; i < frameSize; i++) {
            if (Newper == 0) {
                  u = ((double)rand() / RAND_MAX) * NewG;
            } else {
                  if (state->pitchctr == 0) {
                        u = NewG;
                        state->pitchctr = (int) Newper;
                  } else {
                        u = 0.0;
                        state->pitchctr--;
                  }
            }

            f = u;
            for (j = LPC_FILTORDER; j >= 1; j--) {
                  register double b = state->bp[j - 1];
                  register double kj = Newk[j];
                  Newk[j] = kj + kinc[j];
                  f -= b * kj;
                  b += f * kj;
                  state->bp[j] = b;
            }
            state->bp[0] = f;

            *buf++ = (int)(f*32768.0*GAIN_REDUCTION) & 0xffff;

            Newper += perinc;
            NewG += Ginc;
      }

      state->Oldper = per;
      state->OldG = G;
      for (i = 1; i <= LPC_FILTORDER; i++)
            state->Oldk[i] = k[i];
      return frameSize;
}