%
% 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
%
%
Fs = 100; % in samples per second: try a very small Fs to see the difference between p and ptilde
          % A very small Fs may produce ringing near equilibrium (non-passivity)
h = 2 * Fs; % constant for bilinear transformation (i.e., Adams-Moulton 1 step integration)
simulationlength = 100; % in seconds
inct = 0.01; % linear increment to express the curve in parametric form
          % too few points may produce ringing near equilibrium (non-passivity)
tmax = 0.5; % maximum value of the parameter (for parametric curve), in meters
tdipmax = 0.05; % half width of the bottom tip
gravity = 9.8; % vertical acceleration in m/s
gamma = 0.4; % damping over mass [1/seconds]
t = [-tmax:inct:tmax];

% Parametric curve: (x(t), y(t))
x = t;
y = log(abs(t.*(abs(t)>=tdipmax))+1);
y = y + (y==0).*log(tdipmax+1); % curve with a flat tip
y=y.^2;
figure(1); xlabel('m'); ylabel('m'); plot(x,y,';y(x);'); 

% Array of spatial increments between adjacent point of the parametric curve
pinc = sqrt((x(2:length(x)) - x(1:length(x)-1)).^2 + (y(2:length(y)) - y(1:length(y)-1)).^2);

% pslope = dy/dx (local slope at each point computed by finite difference)
pslope = zeros(1, length(pinc)+1);
pslope(2:length(pslope)) = diff(y) ./ diff(x);

% p = linear coordinate along the curve (for each parameter value)
p = zeros(1, length(pinc)+1);
for i=1:length(pinc)
  p(i+1) = p(i) + pinc(i);
end;
pslope(1) = pslope(2);

% Precalculation of sigma = f(p)
sigma = -gravity*sin(atan(pslope));

% Sheared variable ptilde, to access the sheared function sigma = f1(ptilde)
ptilde = p - 1/(h * (h + gamma)) * sigma;

figure(2);
xlabel('m'); ylabel('m/s^2'); plot(p,sigma,'r;sigma=f(p);'); hold on;
plot(ptilde, sigma,'g;sigma=f1(ptilde);'); hold off;

figure(3); 
xlabel('m'); ylabel('m');
plot(t,p,';p(t);'); hold on; plot(t,ptilde,';ptilde(t);'); hold off;

instants = simulationlength * Fs;
pn = zeros(1,instants); vn = zeros(1,instants); sigman = zeros(1,instants);
ptilden = zeros(1,instants); 

% Simulation cycles
for n = 2:instants
  ptilden(n) = pn(n-1) + (2*h + gamma)/(h * (h + gamma)) * vn(n-1) + 1/(h * (h + gamma)) * sigman(n-1);

  % find the index of vector ptilde corresponding to value ptilden(n)
  [minimo, indminimo] = min(abs(ptilde - ptilden(n)));

  % sigman(n) = f1(ptilden(n))
  sigman(n) = sigma(indminimo);

  % update tangential velocity
  vn(n) = h / (h + gamma) * vn(n-1) + 1/(h + gamma) * (sigman(n) + sigman(n-1));

  % update position along the curve
  pn(n) = pn(n-1) + 1/h * (vn(n) + vn(n-1));

%  printf("ptilden=%f, indminimo=%d, sigman=%f, vn=%f, pn=%f\n", ptilden(n), indminimo, sigman(n), vn(n), pn(n));
%  pause;

end;

figure(4);
ylabel('m'); xlabel('s'); 
plot([1:instants]/Fs, pn, ';curvilinear position in time;'); 
hold on; 
ylabel('m/s'); xlabel('s');
plot([1:instants]/Fs, vn, ';curvilinear velocity in time;'); 
hold off;
