/* (C) Guenter Geiger <geiger@epy.co.at> */


#include "math.h"
#include <m_pd.h>

/* ----------------------------- moog ----------------------------- */
static t_class *moog_class;


typedef struct _moog
{
     t_object x_obj;
  t_pd   in2;
     t_float x_1,x_2,x_3,x_4;
     t_float y_1,y_2,y_3,y_4;
} t_moog;

static void moog_reset(t_moog *x)
{
	x->x_1 = x->x_2 = x->x_3 = x->x_4 = 0.0;
	x->y_1 = x->y_2 = x->y_3 = x->y_4 = 0.0;

}



static void *moog_new(t_symbol *s, int argc, t_atom *argv)
{
    if (argc > 1) post("moog~: extra arguments ignored");
    {
	t_moog *x = (t_moog *)pd_new(moog_class);
	outlet_new(&x->x_obj, &s_signal);
	inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_signal, &s_signal);
	inlet_new(&x->x_obj, &x->in2, &s_signal, &s_signal);
	moog_reset(x);
	return (x);
    }


}



static t_float calc_k(t_float f,t_float k) {
  if (k>4.) k =4.;
  if (k < 0.) k = 0.;
  if (f <= 3800) return k;
  k = k - 0.5*((f-3800)/4300);
  return k;
}

t_int *moog_perform(t_int *w)
{
    t_moog* x = (t_moog*) (w[1]); 
    t_float *in1 = (t_float *)(w[2]);
    t_float *p = (t_float *)(w[3]);
    t_float *k = (t_float *)(w[4]);

    t_float *out = (t_float *)(w[5]);
    int n = (int)(w[6]);
    float in;
    float pt,pt1;
    
    float x1 = x->x_1;
    float x2 = x->x_2;
    float x3 = x->x_3;
    float x4 = x->x_4;
    float ys1 = x->y_1;
    float ys2 = x->y_2;
    float ys3 = x->y_3;
    float ys4 = x->y_4;


   while (n--) {
     if (*p > 8140) *p = 8140.;
     *k = calc_k(*p,*k);
     pt =*p;
     pt1=(pt+1)*0.76923077;
     in = *in1++ - *k*ys4;
     ys1 = (pt1)*in + 0.3*x1 - pt*ys1;
     x1 = in;
     ys2 = (pt1)*ys1 + 0.3*x2 - pt*ys2;
     x2 = ys1;
     ys3 = (pt1)*ys2 + 0.3 *x3 - pt*ys3;
     x3 = ys2;
     ys4 = (pt1)*ys3 + 0.3*x4 - pt*ys4;
     x4 = ys3;
     *out++ = ys4;
   }

   
    x->y_1 = ys1;
    x->y_2 = ys2;
    x->y_3 = ys3;
    x->y_4 = ys4;
    x->x_1 = x1;
    x->x_2 = x2;
    x->x_3 = x3;
    x->x_4 = x4;

    return (w+7);
}


#define CLIP(x)  x = ((x) > 1.0 ? (1.0) : (x))

t_int *moog_perf8(t_int *w)
{
    t_moog* x = (t_moog*) (w[1]); 
    t_float *in1 = (t_float *)(w[2]);
    t_float *p = (t_float *)(w[3]);
    t_float *k = (t_float *)(w[4]);
    t_float *out = (t_float *)(w[5]);
    int n = (int)(w[6]);

    t_float x1 = x->x_1;
    t_float x2 = x->x_2;
    t_float x3 = x->x_3;
    t_float x4 = x->x_4;
    t_float ys1 = x->y_1;
    t_float ys2 = x->y_2;
    t_float ys3 = x->y_3;
    t_float ys4 = x->y_4;
    t_float temp,temp2;
    t_float pt,pt1;
    t_float in;

    while (n--) {
      if (*p > 8140.) *p = 8140.;
      *k = calc_k(*p,*k);

     pt =*p* 0.01*0.0140845 - 0.9999999f;
     pt1=(pt+1.0)*0.76923077;
     in = *in1++ - *k*ys4;
     ys1 = pt1*(in + 0.3*x1) - pt*ys1;
     x1 = in;
     ys2 = pt1*(ys1 + 0.3*x2) - pt*ys2;
     x2 = ys1;
     ys3 = pt1*(ys2 + 0.3*x3) - pt*ys3;
     x3 = ys2;
     ys4 = pt1*(ys3 + 0.3*x4) - pt*ys4;
     x4 = ys3;
     *out++ = ys4;

      p++;k++;
    }

    x->y_1 = ys1;
    x->y_2 = ys2;
    x->y_3 = ys3;
    x->y_4 = ys4;
    x->x_1 = x1;
    x->x_2 = x2;
    x->x_3 = x3;
    x->x_4 = x4;
    
    return (w+7);
}

void dsp_add_moog(t_moog *x, t_sample *in1, t_sample *in2, t_sample *in3, t_sample *out, int n)
{
    if (n&7)
    	dsp_add(moog_perform, 6,(t_int)x, in1,in2,in3, out, n);
    else	
    	dsp_add(moog_perf8, 6,(t_int) x, in1, in2, in3, out, n);
}

static void moog_dsp(t_moog *x, t_signal **sp)
{
    dsp_add_moog(x,sp[0]->s_vec, sp[1]->s_vec, sp[2]->s_vec, sp[3]->s_vec,sp[0]->s_n);
}


void moog_tilde_setup(void)
{
    moog_class = class_new(gensym("moog~"), (t_newmethod)moog_new, 0,
    	sizeof(t_moog), 0, A_GIMME, 0);
    class_addmethod(moog_class, nullfn, gensym("signal"), 0);
    class_addmethod(moog_class, (t_method)moog_reset, gensym("reset"), 0);
    class_addmethod(moog_class, (t_method)moog_dsp, gensym("dsp"), A_NULL);
}