% stabilisation traj d'un point contraint

clear all, close all;
global b params dparams u_tab v_lin_tab t_ret v_ret goal
Tfin=90;
dt=0.05;
TSPAN=linspace(0,Tfin,Tfin/dt);

% le robot
a=00; bb=0.300; l=0.250; b=0.240; % longueur, largeur, position de C
% les roues
r=0.050; e=0.010; % rayon et �paisseur
%la roue folle 
d=0;
% Limites pour le profile de vitesse
v_max = 1.0;
at_max = 1.0;
ar_max = 2.0;

% Initialisation tableaux pour récupération données
t_ret = 0:0.05:15;
v_ret = zeros(3,length(t_ret));

% courbe de Bezier
marche = 1; % 0 avant, 1 arrière
P = [0;-2];
S = [3;-2];
theta1 = pi/4+marche*pi;
theta2 = -pi/4;
bc_params = gen_bezier(P, S, theta1, theta2, (1-2*marche)*6, 6);
params = [bc_params(:,1),...
          -3*bc_params(:,1)+3*bc_params(:,2),...
          3*bc_params(:,1)-6*bc_params(:,2)+3*bc_params(:,3),...
          -bc_params(:,1)+3*bc_params(:,2)-3*bc_params(:,3)+bc_params(:,4)];
dparams = [params(:,2), 2*params(:,3), 3*params(:,4), [0;0]];
[u_tab,v_lin_tab] = speed_profile_u(params, 0.01, v_max, at_max, ar_max, 0.01, 0);
goal = S;

% Conditions Initiales
XPOS=0;
YPOS=-2;
alpha=pi/4+marche*pi;

% cond initiales robot r�f�rence
XR=P(1);
YR=P(2);
alphaR=theta1+marche*pi;

ETATinit=[XPOS,YPOS,alpha,0,XR,YR,alphaR];

%integrer param�tres d'�tat
options = odeset('Events',@event_bezier);
[T,ETAT,te,ye,ie]=ode23(@PointContraintStateEquation,TSPAN,ETATinit,options);


%%% affichage 

%qques r�glages graphiques
x=zeros(1,5);
y=zeros(1,5);
h2=plot(x,y,'-');
axis([-1 5 -3 3]);
axis square;
grid on;

set(h2,'EraseMode','xor','LineWidth',2);

for i=1:1:size(T) 

    XPOS=ETAT(i,1);
    YPOS=ETAT(i,2);
    alpha=ETAT(i,3);
    psi = 0;

    %% Calcul des points graphiques du robot
    %la roue folle 
    
    auxX=[0,d-r,d-r,d+r,d+r,d-r,d-r,0];
    auxY=[0,0,-e/2,-e/2,e/2,e/2,0,0];
    mat=[cos(psi),-sin(psi);sin(psi),cos(psi)];
    
    auxi=mat*[auxX;auxY]; auxX0=auxi(1,:); auxY0=auxi(2,:);
    
    %La roue
    RX1=[0,r,r,-r,-r,0];
    RY1=[0,0,-e,-e,0,0];
    RX2=[0,r,r,-r,-r,0];
    RY2=[0,0,e,e,0,0];
    
    auxX=[RX1-a,bb+auxX0,RX2-a,-a];
    auxY=[RY1-b/2+e/2,auxY0,RY2+b/2-e/2,-b/2+e/2];
    
    ROT = [cos(alpha),-sin(alpha);sin(alpha),cos(alpha)];
    auxi=ROT*[auxX;auxY]+[XPOS*ones(1,size(auxX,2));YPOS*ones(1,size(auxX,2))]; 
    PRX=auxi(1,:); 
    PRY=auxi(2,:);
    
   
    
    set(h2,'XData',PRX,'YData',PRY); hold on;
    
    %la traj du robot
    

        plot(ETAT(1:i,1),ETAT(1:i,2),'.k')
        plot(ETAT(1:i,5),ETAT(1:i,6),'.r')
        
    drawnow;
    
    if (i==1) pause(0.1); else pause(0.1*(T(i)-T(i-1))); end
    %pause(0.05);
end

% Tracé de paramètres de la courbe
u = linspace(0,1,100);
cr = zeros(1,length(u));
for i=1:length(u)
    cr(i) = cr_bezier(params, u(i));
end

figure();
subplot(1,2,1);
plot(T, ETAT(:,4));
title('u(t)');
subplot(1,2,2);
plot(u,cr);
title('rayon de courbure');

% Calcul de la vitesse
dx = (ETAT(2:end,1)-ETAT(1:(end-1),1))./(T(2:end)-T(1:(end-1)));
dy = (ETAT(2:end,2)-ETAT(1:(end-1),2))./(T(2:end)-T(1:(end-1)));
v_lin_r = sqrt(dx.^2 + dy.^2);
dx = (ETAT(2:end,5)-ETAT(1:(end-1),5))./(T(2:end)-T(1:(end-1)));
dy = (ETAT(2:end,6)-ETAT(1:(end-1),6))./(T(2:end)-T(1:(end-1)));
v_lin_g = sqrt(dx.^2 + dy.^2);
figure();
plot(T(1:end-1),v_lin_r, T(1:end-1),v_lin_g);
title('Vitesse linéaire');
