close all;                                                                  % Ferme l'espace de travail
clear all;

Amax=1.2;                                                                    % Accélération maximale


t0=0;                                                                       % Temps de départ
t1=20;                                                                      % Fin de l'accélération
t2=80;                                                                      % Fin de la phase à vitesse de croisière
t3=100;                                                                     % Fin du mouvement
t=[t0:t3-1];
Vmax=t1*Amax;  

A=[ Amax*ones(t1-t0,1) ; zeros(t2-t1,1) ; -Amax*ones(t3-t2,1) ];            % Crée le vecteur accélération
V(1)=0;                                                                     % Initialise la première vitesse
P(1)=0;                                                                     % Initialise la première position
for i=2:t3
    V(i)=V(i-1)+A(i-1);
    P(i)=P(i-1)+V(i-1);
end

subplot (3,1,3);
plot (t,A);
grid on;
xlabel ('Temps');
ylabel ('Accéleration');

subplot (3,1,2);
plot (t,V);
grid on;
ylabel ('Vitesse');

subplot (3,1,1);
plot (t,P);
grid on;
ylabel ('Position');


figure;
plot (P,V);
grid on;





%%%% Modèle inverse
clear V;
%clear A;
P1=((Vmax*Vmax)/(2*Amax))+P(1);
P2=P(t3)-(Vmax*Vmax)/(2*Amax);
i=1;



while (P(i)<P1)
    V(i)=sqrt (2*Amax*(P(i)-P(1)));
    i=i+1;
end



while (P(i)<P2)
    V(i)=Vmax;
    i=i+1;
end



while (P(i)<P(t3))
    
    a=-0.5*Amax;
    b=Amax*t3;
    c=-0.5*Amax*t3*t3+P(t3)-P(i);
    
    Delta=b*b-4*a*c;
    
%    Delta = Amax *( t3*t3-t3*t3
    
    (-b+sqrt (Delta))/(2*a)
    
    
%    Delta=2*Amax*P(t3)

%    (Amax*t3+sqrt(Delta))/(-Amax)
    
%    V(i)=-200+Vmax-sqrt (2*Amax*(P(t2)-P(i)));
    P(i)=-0.5*Amax*i*i+Amax*t3*i-0.5*Amax*t3*t3+P(t3);
    V(i)=-Amax*i+Amax*t3;
    i=i+1;
end
V(i)=0;







figure;

% subplot (3,1,3);
% plot (t,A);
% grid on;
% xlabel ('Temps');
% ylabel ('Accéleration');

subplot (3,1,2);
plot (t,V);
grid on;
ylabel ('Vitesse');

subplot (3,1,1);
plot (t,P);
grid on;
ylabel ('Position');