/** \file kviolin~.c
 *
 * PD plugin implementing a waveguide string bowed by a (non-linear)
 * elasto-plastic friction model.
 *
 * \author Stefania Serafin, Federico Avanzini, and Davide Rocchesso
 *
 * Implemented during the SOHO Research Atelier in Mestre (Venice,
 * Italy) on june 10 - 21, 2002.  The friction model is explained in
 * Avanzini, Rocchesso and Serafin "Modeling interactions between
 * rubbed dry surfaces using an elasto-plastic friction model". To
 * appear in Proc. DAFX 2002, Hamburg, Germany (www.dafx.de)
 *
 * Copyright 2002 Stefania Serafin (serafin@ccrma.stanford.edu)
 *
 * The  pd  modules  contained herein are the result of a joint effort from
 * several authors in the context of the IST-FET Disappearing Computer pro-
 * active    project    called    SOb   (Sound   Object)   (IST-2000-25287,
 * http://www.soundobject.org)  at  the   University   of   Verona,   Italy
 * (http://www.univr.it).
 *
 * The main author of the modules is Stefania Serafin (serafin@ccrma.stanford.edu)
 *
 * Other authors (either writers or designers) are:
 *
 * Federico Avanzini (avanzini@dei.unipd.it)
 * Davide Rocchesso (rocchesso@sci.univr.it)
 *
 * This file is part of the SOb PD modules.
 *
 * The  SOb  PD modules are free software; you can redistribute them and/or
 * modify them 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.
 *
 * The SOb PD modules are 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 the SOb PD modules; if not, write to the Free  Software  Foundation,
 * Inc.,  59 Temple Place, Suite 330, Boston, MA 02111-1307  USA
 * */

#include "m_pd.h"
#include "math.h"

#define BUFSIZE 10000  
#define R_TWOPI		6.28318530717958647692

static t_class *kviolin_class;

t_float min(t_float x, t_float y);
t_float sign(t_float x);


t_float nlfriction(t_float p1, t_float p2, t_float y1fn, t_float fc, t_float fs, t_float strv, t_float z_ba, t_float sigma0, t_float sigma1, t_float sigma2, t_float K1, t_float K2);

typedef struct _kviolin
{
  t_object x_obj;

  t_float v_fb;	//force of the bow
  t_float v_vb;	//velocity of the bow
  int v_posright;
  int v_posleft;

  t_float *vinut;
  t_float *vinbridge;

  t_float bpos; //bow position
  t_float lenght;
  long string; 
  
  t_float del_nut;
  t_float del_bridge;

  t_float position;
  t_float temp;
  t_float ndelay;
  t_float freq;
  t_float ynb;
  t_float ynn;
	
  t_float b_ym1b;
  t_float b_ym2b;
  t_float b_xm1b;
  t_float b_xm2b;
  t_float b_ym1n;
  t_float b_ym2n;
  t_float b_xm1n;
  t_float b_xm2n;
  

  t_float y1nb;
  t_float y1fn;
  t_float vs;
  t_float zs;
  t_float f_tot_r;
  t_float x_bs;
  t_float x_b;
  t_float dotx_bs;  
  t_float dotx_b;
  t_float K1;
  t_float K2;
  t_float f_fr;
  t_float f_tot_b;
  t_float y;
  t_float z;
  t_float v;
  t_float dotx_b1;
  t_float fe_b;
  t_float z1;
  t_float x_b1;
  t_float f_tot_b1;
  t_float f_fr1;

} t_kviolin;

static t_int *kviolin_perform(t_int *w)
{
  t_float *freq = (t_float *)(w[1]);
  t_float *fb = (t_float *)(w[2]);
  t_float *vb = (t_float *)(w[3]);
  t_float *bpos = (t_float *)(w[4]);
  t_float *sigma0 = (t_float *)(w[5]);
  t_float *sigma1 = (t_float *)(w[6]);
  t_float *sigma2 = (t_float *)(w[7]);
  t_float *out = (t_float *)(w[8]);

  t_kviolin *x = (t_kviolin *)(w[9]);
  
  int n = (int)(w[10]);
  t_float ynn,xnn;
  t_float ynb,xnb;
  t_float del_left;
  t_float del_right;
  t_float *vinut;
  t_float *vinbridge;
  
  int posr=x->v_posright;
  int posl=x->v_posleft;
  
  t_float slope=0.1;
  t_float newvel=0;
  t_float vrel=0;
  t_float den, f;
  t_float vin,vib, vh;
  
  t_float viin, viib;
  t_float srate;
  t_float length;
  int samp_rperiod;
  int samp_lperiod;
   
  t_float vob,von; 
  int np, nnp, ltp;
  t_float a_b;
  t_float b_b,b,past_b1,past_b2,past_b3;
  t_float a_z1, a_z2;
  t_float dotx_bs;

  t_float v;
  t_float stringLength;
 t_float stringImpedance;
 t_float zslope;
 long i;
 t_float delta, ds, delnut, delbridge, omega,omegaleft,omegaright;

 t_float K1, K2;
 t_float  f_fr;

 t_float mus, mud, strv, fc, fs;

 t_float zba;
 t_float z_ba;
 t_float fe_b;
 t_float z1, y1fn;
 t_float f_fr1;
 t_float f_tot_b1;
 t_float f_tot_r1;

 t_float f_tot_r;
 t_float f_tot_b;
 t_float past_z2;
  t_float errmax = 1e-7;
  t_float y = y1fn; //first estimate of the contact force
  t_float espon;
  t_float alpha;
  t_float zss,z;
  t_float cosarg;
  t_float dfnl_v;
  t_float dfnl_z;
  t_float fnl;
  t_float dg;
  t_float output;
  t_float sinarg;
  t_float g, dz_ss;
  t_float dalpha_v, dalpha_z;

  
  t_float err=1;
  t_float count=1;           //counter for iterations


  t_float  vtemp;
  t_float m,q;
  t_float vfar;
 
  t_float mass_b; //the mass of the bow
  t_float qfact_b;
  t_float k_b;
  t_float r_b;
  t_float partials_b; 
  t_float past_z1;

  //new filter's coefficients

  t_float b0b,a1b,a2b,b1b,b2b;
  t_float b0n,a1n,a2n,b1n,b2n;

  t_float h;
  
  t_float vs, zs;
  t_float q_b, den_b,m_b;
  t_float x_bs;
  t_float x_b, dotx_b;

  t_float v1, v2, ssparams;

  t_float a_b1, a_b2,a_b3, b_b1,b_b2,b_b3;
  t_float den_b1;

  t_float  ym1b=x->b_ym1b, ym2b= x->b_ym2b, xm1b= x->b_xm1b,  xm2b= x->b_xm2b;
  t_float  ym1n=x->b_ym1n, ym2n= x->b_ym2n, xm1n= x->b_xm1n,  xm2n= x->b_xm2n;
 

  t_float x_b1, dotx_b1;
 
  vinbridge=x->vinbridge;
  vinut=x->vinut;
  zs = x->zs;
  vs = x->vs;
  x_bs = x->x_bs;
  x_b = x->x_b;
  dotx_b = x->dotx_b;
  dotx_bs = x->dotx_bs;
  f_fr = x->f_fr;
  f_tot_r = x-> f_tot_r;
  f_tot_b = x-> f_tot_b;
  dotx_b1 = x->dotx_b1;
  f_fr1 = x->f_fr1;
  f_tot_b1 = x->f_tot_b1;

  //BRIDGE SIDE, TRANSVERSAL WAVES;

  xnn=xnb=0;
  b0b=0.859210;
  b1b=-0.704922;
  b2b=0.022502;
  a1b=-0.943639;
  a2b=  0.120665;


  //FINGER SIDE, TRANSVERSAL WAVES

  b0n= 7.0580050e-001;
  b1n=-5.3168461e-001 ;
  b2n= 1.4579750e-002;
  a1n= -9.9142489e-001;
  a2n=  1.8012052e-001;

  stringLength = 0.69; // m 

  srate=44100;
  h = 2*srate;
  i=0;

  //constant values


  mus=.8;  //.8;              //static friction coeff
  mud=.35;                //dynamic friction coeff (must be < mus!!)
  strv=0.8e-1;             // "stribeck" velocity
  m_b=1;    // mass of each 2nd order oscillator
  stringImpedance = 1/0.55;   //impedance for trasversal waves


  fe_b=x->fe_b;  
  z1 = x->z1; //initial mean bristle displacement
  y1fn = x->y1fn;


  // this is the external force on the bow such that the bow
  // steady-state velocity (after an initial transient) is Vb

  x->f_fr1 = f_fr1;                // initial friction force 

  x->f_tot_b1=f_fr1;        // initial total force on bow


  den_b = 1/(h*h*m_b);
 
  a_b1 = 1;
  a_b2= 2/h;
  a_b3=den_b;
  b_b1 = 0;
  b_b2=1;
  b_b3 = h*den_b;
    
  b=2/(stringImpedance) +h*den_b;
  K1=-b/(1+sigma2[i]*b)*(sigma0[i]/h +sigma1[i]);
  K2 = 1/h;  // K matrix

  a_z1 = 1;
  a_z2 = 1/h;
  x_b1 = x->x_b1; //initial bow velocity
  y=x->y;
  z=x->z;
  v=x->v;

  for(i=0;i<n;i++) //main DSP loop
    {
      
      fc=mud*fb[i];             // coulomb force
      fs=mus*fb[i];             // stiction force
	  
      length=(srate/(freq[i])-2); // trasversal waves, delay line's length

      del_right=length*bpos[i];   //length in the right side
      del_left=length*(1-bpos[i]) ;  //length in the left side
      samp_rperiod=(int)del_right;  //integer part of the delay line
      samp_lperiod=(int)del_left;
      if(samp_rperiod<0) samp_rperiod = 0;
      if(samp_rperiod>BUFSIZE-1) samp_rperiod = BUFSIZE-1;
      if(samp_lperiod<0) samp_lperiod = 0;
      if(samp_lperiod>BUFSIZE-1) samp_lperiod = BUFSIZE-1;

      //get the incoming velocity (using non-fractional delay lines...)
     
      vib= (vinbridge[(posl+i + BUFSIZE- samp_lperiod)% BUFSIZE]);
      vin = (vinut[(posr+i+ BUFSIZE- samp_rperiod)% BUFSIZE]);
     
      //here filter the velocity at the bridge.....
     
      ynb =(((b0b * vib) + (b1b * xm1b) + (b2b * xm2b) - (a1b * ym1b) - (a2b * ym2b)));
     
      xm2b = xm1b;
      xm1b =  vib;
      ym2b = ym1b;
      ym1b = ynb;
      
      //...and at the nut

      ynn=((((b0n * vin) + (b1n * xm1n) + (b2n * xm2n) - (a1n * ym1n) - (a2n * ym2n))));
		
      xm2n = xm1n;
      xm1n = vin;
      ym2n = ym1n;
      ym1n = ynn;

				
      ynb= -ynb;
      ynn= -ynn;
     
      //calculate the incoming velocity at the bow point

      vh=(ynn+ynb);

       z_ba=0.7*fc/sigma0[i];    // break-away displacement 
       past_b1=x_b1;
       past_b2 = dotx_b1;
       past_b3 = f_tot_b1; //  vector of past values (bow dynamics)
   
       x_bs = a_b1*past_b1 + a_b2*past_b2+a_b3*past_b3;          // computable part
       dotx_bs=b_b1*past_b1 + b_b2*past_b2+b_b3*past_b3;       // of bow dynamics
      
       //    bristles 
       
       past_z1 = z1;
       past_z2 = y1fn;             // vector of past values (bristle dynamics)
       zs=a_z1*past_z1+a_z2*past_z2;              // computable part of bristle dynamics
       vs= 1/(1 +sigma2[i]*b)*(dotx_bs-vh-b*sigma0[i]*zs+h*den_b*fe_b); 
   
       if (fc>0)
	 {
	   y = nlfriction(vs,zs,y1fn,fc,fs,strv,z_ba,sigma0[i], sigma1[i], sigma2[i],K1,K2);
	   
	   v=vs+K1*y;                   // relative velocity
	   z=zs+K2*y;                   // mean bristle displacement
	   f_fr=sigma0[i]*z +sigma1[i]*y +sigma2[i]*v;   // the friction force!
	 }

       else
	 {                              //negative normal force => no contact
	   y=0; err=0;
	   z=0; f_fr=0; v=vs;
	   alpha=0;     
	 }
 
       f_tot_b=fe_b -f_fr;                  // total force on bow
       f_tot_r = f_fr;
   
   
       // update displacements and velocities 
 
       x_b=x_bs +den_b*f_tot_b;
       dotx_b=dotx_bs +h*den_b*f_tot_b;
  
       // %%%% state update %%%%%%%%%%%%

       y1fn=y;  
       f_tot_b1=f_tot_b;  

       x_b1=x_b;  
       dotx_b1=vb[i];
       z1=z;
   
       xnn = ynb+ (2*f_tot_r/(stringImpedance));       //new outgoing nut velocity
       xnb = ynn +  (2*f_tot_r/(stringImpedance));  //new outgoing bridge velocity
	 
   
     //write back in the buffer

      vinbridge[(posl+i+BUFSIZE)%BUFSIZE ] =xnb;
      vinut[(posr+i+BUFSIZE)%BUFSIZE] = xnn;
      out[i]=xnb;    //outgoing velocity at the bridge
   }

  posr=(posr+n)%BUFSIZE;
  posl=(posl+n)%BUFSIZE ;
  x->v_posright=posr;
  x->v_posleft=posl;
  x->b_ym1b = ym1b;
  x->b_ym2b = ym2b;
  x->b_xm1b = xm1b;
  x->b_xm2b = xm2b;
  x->b_ym1n = ym1n;
  x->b_ym2n = ym2n;
  x->b_xm1n = xm1n;
  x->b_xm2n = xm2n;
  x->zs =zs;
  x->vs=vs;
  x->x_bs = x_bs;
  x->x_b=x_b;
  x->dotx_b = dotx_b;
  dotx_bs = dotx_bs;
  x->K1 = K1;
  x->K2= K2;
  x->f_fr= f_fr;
  x->f_tot_b= f_tot_b;
  x-> y =y;
  x->z= z;
  x->v= v;
  x->x_b1 = x_b1;
  x->dotx_b1 = dotx_b1;
  x->z1 = z1;
  x->y1fn = y1fn;
  return (w+11);
}

//dsp function

static void kviolin_dsp(t_kviolin *x, t_signal **sp)
{

  dsp_add(kviolin_perform, 10, sp[0]->s_vec, sp[1]->s_vec,sp[2]->s_vec,sp[3]->s_vec,sp[4]->s_vec,sp[5]->s_vec,sp[6]->s_vec,sp[7]->s_vec,x, sp[0]->s_n);

}

static void *kviolin_new(void)
{
  int i;
  t_kviolin *x = (t_kviolin *)pd_new(kviolin_class);

  //create the inlets

  inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_signal, &s_signal);
  inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_signal, &s_signal); 
  inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_signal, &s_signal);
  inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_signal, &s_signal);
  inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_signal, &s_signal); 
  inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_signal, &s_signal);
  outlet_new(&x->x_obj, gensym("signal"));

  //initialize variables
  
  x->v_fb = 0;
  x->v_vb = 0;
  x->v_posright=0;
  x->v_posleft=0;
  x->freq = 147;
  x->bpos = 0.127236;
  x->position=0.2;
  x->ynb=0;
  x->ynn=0.;
  x->ynb=0.;
  x->zs = 0;
  x->vs=0;
  x->f_tot_r =0;
  x-> x_bs =0;
  x-> x_b=0;
  x->dotx_bs=0;  
  x-> dotx_b=0;
  x->K1 = 0;
  x->K2= 0;
  x->f_fr= 0;
  x->f_tot_b= 0;
  x-> y =0;
  x->z= 0;
  x->v= 0;
  x->z1 =0;
  x->y1fn=0;
  x->fe_b =0;
  x->x_b1 = 0;
  x->f_fr1=0;
 
  //initialize buffers

  x->vinut =  (t_float *) t_getbytes(BUFSIZE*sizeof(t_float)) ; /*allocates space in memory for bridge buffer*/
  x->vinbridge =  (t_float *) t_getbytes(BUFSIZE*sizeof(t_float)) ; /*allocates space in memory for bridge buffer*/

  for(i=0 ; i< BUFSIZE;  i++)
   {
     x->vinut[i] = 0. ; /*initialises nut buffer*/
     x->vinbridge[i] = 0. ; /*initialises nut buffer*/
   
   }
  return (x);
}

void kviolin_tilde_setup(void)
{

  kviolin_class = class_new(gensym("kviolin~"), (t_newmethod)kviolin_new, 0,
			   sizeof(t_kviolin), 0, A_DEFFLOAT, 0);
  class_addmethod(kviolin_class, nullfn, gensym("signal"), 0);
  class_addmethod(kviolin_class, (t_method)kviolin_dsp, gensym("dsp"), 0);
}



t_float nlfriction(t_float vs, t_float zs, t_float y1fn, t_float fc, t_float fs, t_float strv, t_float z_ba, t_float sigma0,t_float sigma1,t_float sigma2, t_float K1, t_float K2)
{
  t_float v;
  t_float errmax = 1e-7;
  t_float y = y1fn; //first estimate of the contact force
  t_float espon;
  t_float alpha;
  t_float zss,z;
  t_float cosarg;
  t_float dfnl_v;
  t_float dfnl_z;
  t_float fnl;
  t_float dg;
  t_float output;
  t_float sinarg;
  t_float argsinus;

  t_float g, dz_ss;
  t_float dalpha_v, dalpha_z;

   
t_float err=1;
 t_float count=1;           //counter for iterations
 argsinus = 0;
 v=0;
 z=0;

 while ((err > errmax) && (count < 50)) 
 {  
   v=vs+K1*y;      // est. for rel. velocity (according to K-method)
   z=zs+K2*y;      // est. for bristle displac. (according to K-method)
   
   // compute functions %%
   
   espon=exp(-(v/strv)*(v/strv));         //exponential function
   zss=sign(v)*(fc +(fs-fc)*espon)/sigma0;   //steady state curve: z_ss(v)
   					      
   if (v==0)
     zss=fs/sigma0;
   
      
   alpha=0;                  //elasto-plastic function \alpha (v,z)
   if (sign(z)==sign(v))
     {
       if ( (fabs(z)>z_ba)&& (fabs(z)<zss) )
	 {
         
	   argsinus=(R_TWOPI/2)*(z-0.5*(zss+z_ba))/(zss-z_ba);
	   sinarg=sin(argsinus);
	   alpha=0.5*(1+sinarg);
	 }
       else if (fabs(z)>zss)
         alpha=1;
     
     }
   
   fnl=v*(1-alpha*z/zss);     //non-linear function estimate
   g=fnl-y;                //Newton-Raphson function estimate
   
   //   %% compute derivatives %%
   
   dz_ss=-sign(v)*2*v/(sigma0*strv*strv) *(fs-fc)*espon;    //d(z_ss)/dv
   dalpha_v=0;                     //d(alpha)/dv 
   dalpha_z=0;                     
   if ((sign(z)==sign(v)) && (fabs(z)>z_ba) && (fabs(z)<zss))
     {
      cosarg=cos(argsinus);
      dalpha_v=0.5*(R_TWOPI/2)*cosarg*dz_ss*(z_ba-z)/((zss-z_ba)*(zss-z_ba)); 
      dalpha_z=0.5*(R_TWOPI/2)*cosarg/(zss-z_ba);
     }
   
   dfnl_v=1 -z*((alpha +v*dalpha_v)*zss -dz_ss*alpha*v)/(zss*zss); //d(f_nl)/dv
   dfnl_z=-v/zss*(z*dalpha_z +alpha);               //d(f_nl)/dz
   
  dg=dfnl_v*K1 + dfnl_z*K2 -1;                  //dg/dy
   
  output = y - g/dg;           // Newton-Raphson iteration
  err = fabs(output-y);
  y = output;
  count=count+1;

 }
 return(y);
}


//sign function

t_float sign(t_float x)
{
  t_float y;
  if (x>=0) y=1;
  else y=-1;
  return(y);
}

