/*  Linear Predictive Coding - PARCOR and residual generation
 *  Copyright (C) 2005 Nicolas Chetry <okin@altern.org>
 *  and Edward Kelly <morph_2016@yahoo.co.uk>
 *
 *  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; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "m_pd.h"
#include <math.h>
#define MAXPOLES 100

static t_class *lpc_tilde_class;

typedef struct _lpc_schur
{
  t_float *c_input, *c_residual;
  /* just so we don't get clicks */
  t_atom x_last_parcors[MAXPOLES];
  t_atom x_parcors[MAXPOLES];
} t_lpc_schur;

typedef struct _lpc_tilde
{
  t_object x_obj;
  t_float f_dummy;
  t_float x_order, x_lastorder;
  t_outlet *parcor_list;
  t_lpc_schur x_schur;
} t_lpc_tilde;

t_int *lpc_tilde_perform(t_int *w)
{
  t_lpc_tilde     *x =     (t_lpc_tilde *)(w[1]);
  t_lpc_schur *schur =     (t_lpc_schur *)(w[2]);
  int              n =               (int)(w[3]);
  t_float        *in = schur->c_input;
  t_float       *res = schur->c_residual;
//  t_float   *parcors = schur->PARCORS;
//  t_float       *acf = schur->ACF;
//  t_float         *k = schur->K;
//  t_float         *p = schur->P;
//  t_float         *r = schur->r;
  t_int          ord = (int)x->x_order;
  t_int        l_ord = (int)x->x_lastorder;
//  t_float parcors[ord];
  float acf[ord+1];
  float k[ord+1];
  float p[ord+1];
  float r[ord+1];
  float mem[ord];
  int i, j, bid, y, z;
  int g, h;
  float tmp, temp, sav, di;
  float parcor_1;  
  for (i=0; i<ord; i++) 
  {
    SETFLOAT (&schur->x_parcors[i],0);
    mem[i] = 0.0;
  }
  for (j=0; j<=ord; j++) 
  {
    acf[j] = 0;
    for (i=j; i<n; i++) 
	{
	  acf[j] += in[i]*in[i-j];
	}
  }
  if (acf[0] == 0) 
  {
    for (i=0; i<ord; i++)
	{
    SETFLOAT (&schur->x_parcors[i],0);
	}
  }
  for (i=0; i<=ord; i++)
  {
    p[i]=acf[i];
	if (i > 0 && i < ord)
	{
	  k[ord+1-i] = acf[i];
	}
  }
  /* Schurr recursion */
  for (y=1; y<=ord; y++)
  {
    if (p[0] < fabs (p[1]))
	{
	  for (i=y; i<=ord; i++)
	  {
	    r[i] = 0;
	  }
	  for (bid=1; bid <=ord; bid++)
	  {
            SETFLOAT (&schur->x_parcors[bid-1],r[bid]);
	    //	    x->x_parcors[bid-1] = r[bid];
	  }
	}
	r[y] = fabs(p[1])/p[0];
	
	if (p[1] >0)
	{
	  r[y] = -1.*r[y];
	}
	if (y==ord)
	{
	  for (bid=1; bid <=ord; bid++)
	  {
            SETFLOAT (&schur->x_parcors[bid-1],r[bid]);
	    //	    x->x_parcors[bid-1] = r[bid];
	  }
	}
	p[0] += p[1]*r[y];
	
	for (z=1; z <=ord-y; z++)
	{
	  p[z]        = p[z+1] + k[ord+1-z]*r[y];
	  k[ord+1-z] += p[z+1] * r[y];
	}
  }
  for (bid=1; bid <=ord; bid++)
  {
    SETFLOAT (&schur->x_parcors[bid-1],r[bid]);
  }
  parcor_1 = atom_getfloatarg(0,ord,schur->x_parcors); /* in order to avoid nil coefficients */
  if (parcor_1 > 1e-5 || parcor_1 < -1e-5)
    {
      outlet_list(x->parcor_list,gensym("list"),ord,schur->x_parcors);

  /* Analysis FIR lattice filtering */
      for (g=0; g<n; g++)
	{
        
    /* Analysis - Lattice structure */
	  sav = di = in[g];
	  for (i=0; i<ord; i++)
	    {
	      t_float parcor = atom_getfloatarg (i,ord,schur->x_parcors);
	      SETFLOAT (&schur->x_last_parcors[i],parcor);
	      x->x_lastorder = ord;
	      temp = mem[i] + parcor*di;
	      di += parcor*mem[i];
	      mem[i] = sav;
	      sav = temp;
	    }
	  res[g] = di;
            
	  }  /* next g */
    }
  else 
    {
      outlet_list(x->parcor_list,gensym("list"),l_ord,schur->x_last_parcors);
      for (g=0; g<n; g++)
	{
	  res[g] = 0;
	}
    }
  return(w+4);
}

void *lpc_tilde_dsp(t_lpc_tilde *x, t_signal **sp)
{
  x->x_schur.c_input = sp[0]->s_vec;
  x->x_schur.c_residual = sp[1]->s_vec;
  dsp_add(lpc_tilde_perform, 3, x, &x->x_schur, sp[0]->s_n);
  return (void *)x;
}

void *lpc_tilde_new(t_floatarg f)
{
  t_lpc_tilde *x = (t_lpc_tilde *)pd_new(lpc_tilde_class);
  x->x_order = f >= 1 ? (int)f : 5;
  
  floatinlet_new(&x->x_obj,&x->x_order);
  outlet_new(&x->x_obj, &s_signal);
  x->parcor_list = outlet_new(&x->x_obj, &s_list);
  return (void *)x;
}

void lpc_tilde_setup(void)
{
  lpc_tilde_class = class_new(gensym("lpc~"), (t_newmethod)lpc_tilde_new, 0, sizeof(t_lpc_tilde), CLASS_DEFAULT, A_DEFFLOAT, 0);

  post("\n. . Linear Predictive Coding. . . . . . . .");
  post(". . PARCOR coefficients from input. . . . .");
  post(". . by Nicolas Chetry <okin@altern.org> . .");
  post(". & Edward Kelly <morph_2016@yahoo.co.uk> .");
  
  class_addmethod(lpc_tilde_class, (t_method)lpc_tilde_dsp, gensym("dsp"), 0);


  CLASS_MAINSIGNALIN(lpc_tilde_class, t_lpc_tilde, f_dummy);
}