%  Mouse-driven motion planning
%      for  inverted  heavy chain system
%  march 99
%  Ecole des Mines de Paris, 
%  Centre Automatique et Systemes
%  60 Bd St Michel 
%  75272 Paris
%    Nicolas Petit: petit@cas.ensmp.fr
%    Pierre Rouchon: rouchon@cas.ensmp.fr
% 
% Linear approximation (Cf the model studied by Daniel Bernoulli 1738 )
%     D_tt H  =  D_x (  g x D_x H ) = 0  pour -L < x < 0
%     H(x=-L,t)= D (the control, horizontal position of the trolley) 
%
% flat output: y(t)=H(0,t) the position of the free end 
% motion planing formulae 
% 
%  H(x,t) = 1/(2 pi) int_0^pi [ y(t - 2 i sqrt(x/g) sin s) + y(t + 2 i sqrt(x/g) sin s) ] ds
%   derived via  symbolic calculus  (Poisson integral representation of the 
%   Bessel function J_0)  (i=sqrt(-1))
% 
%     
clear all;  
L=1.;  % rope  length in  m
A_max= 1.5*L ; % amplitude of horizontal motion  depth  in  m
g=9.81; % garvity in  m/s^2
T_max=2*sqrt(A_max/g);  % interval of motion (s)
eps=1.e-8;
sig=1.7*sqrt(L/g);

DT=sqrt(-log(eps))*sig ;

scale=min(11., 11/max(A_max*14/19,L)); 

% space mesh 
dx=L/40.;
x=[0:dx:L];
nx=length(x);
H=zeros(1,nx);
% time step 
dt0=DT/50;
dt=dt0/3; 
dtmin=DT/1000;
dtmax=DT/3.;




TT=[-DT:dt0:DT]';
YY=0*TT;
mt=length(YY);
m0=(mt+1)/2.;
mm=[1:mt-1];

Y=0*[-5*DT:dt:0]; D=Y;t=Y;
nt=length(Y);
nn=[1:nt-1];





figure(1),clf;
set(gca,'position',[0 0 1 1],'visible','off','Xlim',[0 19],'Ylim',[0 14]....
   ,'nextplot','add');
          

   
%  trolley

Xt=1+[0   1  1  0  ]+scale*H(nx);
Yt=1.5+[0   0  1/2  1/2  ];
trol=fill(Xt,Yt,'g','EraseMode','background');


% cable

Xc=1.5 + scale*H;
Yc=12- scale*x;
chain=line(Xc,Yc,'EraseMode','background','LineWidth', 4, 'Color', [0 0 1 ]);

%  ref
Xr=1+[0   1  0.5  0  ] + scale*H(nx);
Yr=0.3+[0   0  1/2  0  ];
ref=fill(Xr,Yr,'y','EraseMode','background');


set(gcf,'WindowButtonMotionFcn','1;');


Hplay=uicontrol('style','push',...
                'units','normalized','position',[15/19 12.6/14 2/19 .5/14], ....
                'string','Plot','callback', ...
                ['figure(2);clf;' ...
                 'subplot(211);' ...
                 'plot(t,Y);'...
                 'title(''flat output  (m) versus time'');'...
                 'subplot(212);'...
                 'plot(t,D);'...
                 'title(''troley position (m/s)versus time''); pause;figure(1);'...
                ]);
             
set(Hplay,'visible','off');
             
Hrun=uicontrol('style','push',...
                'units','normalized','position',[1/19 12.7/14 2/19 .5/14], ....
                'string','stop','callback', ...
                ' ; ' ,'visible','off');
             
             
             
             
  % title
text(9.5,13.5,'HEAVY CHAIN: MOUSE-DRIVEN MOTION PLANNING','color',[0 0 0],...
   'EraseMode','background','HorizontalAlignment','center');
text(9.5,13,'display speed slider','color',[0 0 0],'EraseMode','background','HorizontalAlignment','center');
p1=[7.5/19 12.6/14 4/19 0.2/14];
Hslider=uicontrol(figure(1),'units','normalized','style','slider','position',p1,...
			'min',log(dtmin),'max',log(dtmax),'value',log(dt),...
         'callback',['buf=get(Hslider,''value'');dt=exp(buf);']); 
           
             
             
             

figure(1);
pause(1)
set(Hrun,'string','Stop','callback','i=2;');
set(Hplay,'visible','on')
set(Hrun,'visible','on')



t1=T_max/100;
t2=t1;
seuil=A_max;
y1=0;
y2=0;

i=0;

dth=pi/100;
th=[0:dth:pi-dth];
Sth=sin(th);

tt=0.;
y1_old=y1;

% the matrix (nx,mt)
EE=zeros(nx,mt);
for ix=1:nx;
    tau=2*sqrt(x(ix)/g);
    s=sqrt(-1)*tau*Sth;
    for it=1:mt;
       z= (TT(it)+s)/sig;
       EE(ix,it)=dt0*(dth/pi)*real(sum(exp(-z.*z)));
    end
end  

EE=EE/sig/sqrt(pi);





while (i < 1)
    buf=get(gca,'CurrentPoint');
    A=(buf(1,1)-1.5)/scale;
    A=max(0,A);
    A=min(A_max,A); 
    
   y1=(y1+dt*y2/t1)/(1+dt/t1);
   t2b=t2*max(1,abs(A-y2)/seuil);
   y2=(y2+dt*A/t2b)/(1+dt/t2b);
   
   tt=tt+dt;
   if (tt >= dt0);
      ii=floor(tt/dt0);
      dy1=(y1-y1_old)/tt;
      for iii=1:ii;
         YY(mm)=YY(mm+1);YY(mt)=y1_old+iii*dt0*dy1;
      end;
      y1_old=y1-dy1*(tt-ii*dt0);
      tt=0.;
   end;
    
   
   
   H=EE*YY;

    
    
    Y(nn)=Y(nn+1); Y(nt)=H(1);
    D(nn)=D(nn+1); D(nt)=H(nx);
    t(nn)=t(nn+1); t(nt)=t(nt)+dt;
 
    
    
    Xt=1+[0   1  1  0  ] + scale*H(nx);
    Xr=1+[0   1  0.5  0  ] + scale*A;
    Xc=1.5 + scale*H;
    set(trol, 'XData',Xt); 
    set(chain,'XData',Xc); 
    set(ref,'XData',Xr); 
    drawnow;
   
end;


close(figure(1));
close(figure(2));

return