/*
 * vi:set ts=4 nowrap:
 * PD is Copyright (c) 1997-2002 Miller Puckette.
 *
 * $Id: roller.c,v 0.16 2002/09/25 05:49:11 roc Exp $
 *
 * Copyright 2002 Nicola Bernardini (nicb@centrotemporeale.it)
 *
 * 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 Matthias Rath (rath@sci.univr.it).
 *
 * Other authors (either writers or designers) are:
 *
 * Davide Rocchesso (rocchesso@sci.univr.it)
 * Federico Fontana (fontana@sci.univr.it)
 * Laura Ottaviani (ottavian@sci.univr.it)
 * Gianpaolo Borin (gianpaolo.borin@tin.it)
 * Federico Avanzini (avanzini@dei.unipd.it)
 * Nicola Bernardini (nicb@centrotemporeale.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 <string.h>
#include <math.h>
#include "roller.h"

#define ROLLER_DEFAULT_DAMPING (0.4)
#define ROLLER_MAX_DAMPING (1000.0)
#define ROLLER_MIN_DAMPING (0.0)
#define ROLLER_MIN_X (-1.0)
#define ROLLER_MAX_X (1.0)
#define ROLLER_MIN_Y (-1.0)
#define ROLLER_MAX_Y (1.0)
#define ROLLER_MIN_Z (-1.0)
#define ROLLER_MAX_Z (1.0)

static t_class *roller_class;

static t_int pushed;

static void
roller_output(t_roller *x)
{
	outlet_float(x->pos_outlet, x->state.p);
	outlet_float(x->vel_outlet, x->state.v_rad);
	outlet_float(x->x_pos_outlet, 
		     (x->state.x_pos - x->state.x_push) * x->flag +  x->state.x_push);
	outlet_float(x->y_pos_outlet, 
		     (x->state.y_pos - x->state.y_push) * x->flag +  x->state.y_push);
	outlet_float(x->tan_vel_outlet, x->state.v_tan);
}


t_float
distance(t_float xa, t_float ya, t_float xb, t_float yb)
{
  return (sqrt((xb - xa)*(xb - xa) + (yb - ya)*(yb - ya)));
}

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

/*
 * Binary search of an element of roller_ptilde[] close to x->state.ptilde.
 * It relies on a monotonic increasing shape of the table roller_ptilde[].
 * The search returns the index of the element closest to x->state.ptilde.
 */

static t_int
find_ptilde(t_roller *x)
{
	t_int high = x->tables.size - 1, low = 0, mid;

	while (low < high)
	  {
	    mid = floor((low + high)/2);
	    if ((x->tables.ptilde[mid] <= x->state.ptilde) && 
		    (x->state.ptilde <= x->tables.ptilde[mid+1]))
	      if ((x->state.ptilde - x->tables.ptilde[mid]) <= 
		      (x->tables.ptilde[mid+1] - x->state.ptilde))
		    return mid;
	      else
		    return (mid + 1);
	    else if (x->state.ptilde < x->tables.ptilde[mid])
	      high = mid;
	    else low = mid + 1; 
	  }

	if (low == high) return(low);
	if (low > high) return (x->tables.size - 1);
	if (high < low) return (0);
	
	return (-1);
}

static void
next_sample(t_roller *x)
{
  t_int idx = 0;
  t_float last_sigma = x->state.sigma;
  t_float last_v = x->state.v_rad;
  t_float angle;
  t_float denom;
  t_float dist;
  t_float v_xu, v_yu, v_xt, v_yt; /* radial and tangential velocities */
  t_float vrad;
  extern t_float roller_default_pmax;

  /* radial position update */
  x->state.ptilde = x->state.p +
	(2*x->h + x->damping)/(x->h * (x->h + x->damping)) *
	x->state.v_rad + 1/(x->h * (x->h + x->damping)) * x->state.sigma;
  idx = find_ptilde(x);  
  x->state.sigma = x->state.z_push * x->tables.sigma[idx];
  x->state.v_rad = x->h / (x->h + x->damping) * x->state.v_rad +
	1/(x->h + x->damping) * (x->state.sigma + last_sigma);
  x->state.p = x->state.p +
    1 / x->h * (x->state.v_rad + last_v);
  x->flag = (x->state.p < roller_default_pmax) ? -1 : 1; /* left or right of tip */

  /* unit vector pos->push */
  dist =  distance(x->state.x_pos, x->state.y_pos,
		   x->state.x_push, x->state.y_push);
  if (dist > 0)
    {
      x->state.u_x = (x->state.x_push - x->state.x_pos) / dist * x->flag;
      x->state.u_y = (x->state.y_push - x->state.y_pos) / dist * x->flag;
    }

  /* projection of current velocity onto unit vector (radial velocity) */
  v_xu = x->state.x_vel * x->state.u_x; /* radial velocity proj.: x component */
  v_yu = x->state.y_vel * x->state.u_y; /* radial velocity proj.: y component */

  vrad = v_xu + v_yu; /* projection of velocity onto radius */
  v_xu = vrad * x->state.u_x;  /* radial velocity x component */
  v_yu = vrad * x->state.u_y;  /* radial velocity y component */

  /* tangential velocity */
  v_xt = (x->state.x_vel - v_xu); /* tangential velocity x component */
  v_yt = (x->state.y_vel - v_yu); /* tangential velocity y component */
  x->state.v_tan = sqrt(v_xt * v_xt + v_yt * v_yt);

  /* position update along radius */  
  x->state.x_pos = (roller_default_pmax - x->state.p) * x->state.u_x + x->state.x_push;
  x->state.y_pos = (roller_default_pmax - x->state.p) * x->state.u_y + x->state.y_push;

  /* position adjustment along tangent */
  x->state.x_pos = x->state.x_pos + v_xt * 2 / x->h;
  x->state.y_pos = x->state.y_pos + v_yt * 2 / x->h;

  /* velocity update */
  x->state.x_vel = x->state.v_rad * x->state.u_x + v_xt;
  x->state.y_vel = x->state.v_rad * x->state.u_y + v_yt;

/*   post("Next: x_push=%f p=%f x_pos=%f. v_rad=%f, vquad=%f, vquad2=%f, xd=%f", */
/*        x->state.x_push, x->state.p, x->state.x_pos, x->state.v_rad, */
/*        x->state.x_vel *  x->state.x_vel +  x->state.y_vel *  x->state.y_vel, */
/*        x->state.v_tan * x->state.v_tan  + x->state.v_rad  * x->state.v_rad, */
/*        (x->state.x_pos - x->state.x_push) * x->flag +  x->state.x_push); */

  roller_output(x);
}

static void
roller_run(t_roller *x)
{
	if (x->on_off)
	{
		next_sample(x);
		clock_delay(x->x_clock, x->rate);
	}
	else
		clock_unset(x->x_clock);
}

static void
roller_on_off(t_roller *x, t_float f)
{
	x->on_off = (t_int) f;
	roller_run(x);	/* start the ball rolling */
}

static void
roller_damping(t_roller *x, t_float f)
{
	x->damping = (f > ROLLER_MAX_DAMPING) ? ROLLER_MAX_DAMPING :
		((f < ROLLER_MIN_DAMPING) ? ROLLER_MIN_DAMPING : f);
}

static void
roller_x_push(t_roller *x, t_float f)
{
  extern t_float roller_default_pmax;
  t_float dist;
  t_float v_xu, v_yu, v_xt, v_yt; /* radial and tangential velocities */
  t_float xd;

  xd = (x->state.x_pos - x->state.x_push) * x->flag +  x->state.x_push;
	x->state.x_push = (f > ROLLER_MAX_X) ? ROLLER_MAX_X :
		((f < ROLLER_MIN_X) ? ROLLER_MIN_X : f);
/*  post("before: x_push=%f p=%f x_pos=%f. v_rad=%f, vquad=%f, x_posd=%f",  */
/*        x->state.x_push, x->state.p, x->state.x_pos, x->state.v_rad, */
/*        x->state.x_vel *  x->state.x_vel +  x->state.y_vel *  x->state.y_vel, */
/*        xd); */
 	x->state.p = distance(x->state.x_pos, x->state.y_pos,
			      x->state.x_push, x->state.y_push);
	x->state.p = roller_default_pmax - x->state.p;

	x->flag = (x->state.p < roller_default_pmax) ? -1 : 1; /* left or right of tip */

	/* unit vector pos->push */
	dist =  distance(x->state.x_pos, x->state.y_pos,
		   x->state.x_push, x->state.y_push);
	if (dist > 0)
	  {
	    x->state.u_x = (x->state.x_push - x->state.x_pos) / dist; // * x->flag;;
	    x->state.u_y = (x->state.y_push - x->state.y_pos) / dist; // * x->flag;;
	  }
	x->state.v_rad = (x->state.x_vel * x->state.u_x + x->state.y_vel * x->state.u_y) ;
	v_xu = x->state.v_rad * x->state.u_x;  /* radial velocity x component */
	v_yu = x->state.v_rad * x->state.u_y;  /* radial velocity y component */

	/* tangential velocity */
	v_xt = (x->state.x_vel - v_xu); /* tangential velocity x component */
	v_yt = (x->state.y_vel - v_yu); /* tangential velocity y component */
	x->state.v_tan = sqrt(v_xt * v_xt + v_yt * v_yt);
	x->state.sigma = 0;

	x->state.x_pos = (xd + x->state.x_push*(x->flag - 1)) / x->flag;
/*   post("after: x_push=%f p=%f x_pos=%f. v_rad=%f, v_quad2=%f, x_posd=%f",  */
/*        x->state.x_push, x->state.p, x->state.x_pos, x->state.v_rad,  */
/*         x->state.v_tan * x->state.v_tan  + x->state.v_rad  * x->state.v_rad, */
/*        (x->state.x_pos - x->state.x_push) * x->flag +  x->state.x_push); */
}

static void
roller_y_push(t_roller *x, t_float f)
{
  extern t_float roller_default_pmax;
  t_float dist;
  t_float v_xu, v_yu, v_xt, v_yt; /* radial and tangential velocities */
  t_float yd;

  yd = (x->state.y_pos - x->state.y_push) * x->flag +  x->state.y_push;
	x->state.y_push = (f > ROLLER_MAX_Y) ? ROLLER_MAX_Y :
		((f < ROLLER_MIN_Y) ? ROLLER_MIN_Y : f);
	x->state.p = distance(x->state.x_pos, x->state.y_pos,
			      x->state.x_push, x->state.y_push);
	x->state.p = roller_default_pmax - x->state.p;
	
	x->flag = (x->state.p < roller_default_pmax) ? -1 : 1; /* left or right of tip */

	/* unit vector pos->push */
	dist =  distance(x->state.x_pos, x->state.y_pos,
		   x->state.x_push, x->state.y_push);
	if (dist > 0)
	  {
	    x->state.u_x = (x->state.x_push - x->state.x_pos) / dist; //* x->flag;
	    x->state.u_y = (x->state.y_push - x->state.y_pos) / dist; //* x->flag;
	  }
	x->state.v_rad = (x->state.x_vel * x->state.u_x + x->state.y_vel * x->state.u_y) ;
	v_xu = x->state.v_rad * x->state.u_x;  /* radial velocity x component */
	v_yu = x->state.v_rad * x->state.u_y;  /* radial velocity y component */

	/* tangential velocity */
	v_xt = (x->state.x_vel - v_xu); /* tangential velocity x component */
	v_yt = (x->state.y_vel - v_yu); /* tangential velocity y component */
	x->state.v_tan = sqrt(v_xt * v_xt + v_yt * v_yt);
	x->state.sigma = 0;

	x->state.y_pos = (yd + x->state.y_push*(x->flag - 1)) / x->flag;
}

static void
roller_z_push(t_roller *x, t_float f)
{
  extern t_float roller_default_pmax;
  t_float dist;

	x->state.z_push = (f > ROLLER_MAX_Z) ? ROLLER_MAX_Z :
		((f < ROLLER_MIN_Z) ? ROLLER_MIN_Z : f);
}

static void
roller_rate(t_roller *x, t_float hz)
{
	extern const double roller_rate_default;

	x->rate = hz ? (1000/hz) : roller_rate_default;
	x->h = 2000/x->rate; /*
			      * bilinear transformation constant
			      * (i.e., Adams-Moulton 1 step integration)
			      */
	post("roller rate set to %8.3f milliseconds", x->rate);
}

static void
roller_bang(t_roller *x)
{
	roller_on_off(x, 1);
}

static void
roller_start(t_roller *x)
{
	roller_on_off(x, 1);
}

static void
roller_stop(t_roller *x)
{
	roller_on_off(x, 0);
}

static void
roller_free(t_roller *x)
{
	clock_free(x->x_clock);
	roller_free_table_memory(x);
}

static void
roller_reset(t_roller *x)
{
	memset(&x->state, 0, sizeof(roller_state));
	x->state.x_push = 0;
	x->state.y_push = 0;
	x->state.x_pos = -0.1;
	x->state.z_push = 1.0;
	post("x_pos = %f, z_push = %f", x->state.x_pos, x->state.z_push);
}

static void *
roller_new(t_symbol *s, int ac, t_atom *av)
{
    t_roller *x;
	extern const double roller_rate_default;

	x = (t_roller *)pd_new(roller_class);
	x->x_clock = clock_new(x, (t_method) roller_run);
	x->pos_outlet = outlet_new(&x->x_obj, &s_float);
	x->vel_outlet = outlet_new(&x->x_obj, &s_float);
	x->x_pos_outlet = outlet_new(&x->x_obj, &s_float);
	x->y_pos_outlet = outlet_new(&x->x_obj, &s_float);
	x->tan_vel_outlet = outlet_new(&x->x_obj, &s_float);
	(void *) inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("float"),
		gensym("damper"));
	(void *) inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("float"),
		gensym("x_push"));
	(void *) inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("float"),
		gensym("y_push"));
	(void *) inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("float"),
		gensym("z_push"));
	
	roller_reset(x);

	x->damping = ROLLER_DEFAULT_DAMPING;
	roller_rate(x, 0);

	roller_tables_reset(x);

    return (x);
}

void
roller_setup(void)
{
    roller_class = class_new(gensym("roller"),
			    (t_newmethod)roller_new,
			    (t_method)roller_free,
			    sizeof(t_roller), 0, 0);

	class_addbang(roller_class, roller_bang);
    class_addfloat(roller_class, roller_on_off);
	class_addmethod(roller_class, (t_method) roller_start, gensym("start"), 0);
	class_addmethod(roller_class, (t_method) roller_stop, gensym("stop"), 0);
	class_addmethod(roller_class, (t_method) roller_reset, gensym("reset"), 0);
	class_addmethod(roller_class, (t_method) roller_rate, gensym("rate"),
		A_DEFFLOAT, 0);
	class_addmethod(roller_class, (t_method) roller_damping, gensym("damper"),
		A_DEFFLOAT, 0);
	class_addmethod(roller_class, (t_method) roller_x_push, gensym("x_push"),
		A_DEFFLOAT, 0);
	class_addmethod(roller_class, (t_method) roller_y_push, gensym("y_push"),
		A_DEFFLOAT, 0);
	class_addmethod(roller_class, (t_method) roller_z_push, gensym("z_push"),
		A_DEFFLOAT, 0);
	class_addmethod(roller_class, (t_method) roller_tables_set, gensym("table"),
		A_DEFSYM, 0);
	class_addmethod(roller_class, (t_method) roller_plot, gensym("plot"), 0);
}
