clear all
close all

% Tests pour le suivi de courbe de Bézier
n_pts = 100;

% robot
b = 0.25;
v_max = 1;

% Params
%P = [0;0];
%S = [3;0];
%theta1 =  pi/4;   v1 = 1;
%theta2 = -pi/6;   v2 = 2;
P = [0;0];
S = [3;3];
theta1 =  0;
theta2 = 0;

% Détermination de la trajectoire

[bc_params] = gen_bezier(P, S, theta1, theta2, 2.0);

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)];

% Points de la courbe
u = linspace(0,1,n_pts);

pts = bc_params(:,1) * ((1-u).*(1-u).*(1-u)) + ...
      3*bc_params(:,2) * (u .* (1-u).*(1-u)) + ...
      3*bc_params(:,3) * (u .* u .*(1-u)) + ...
      bc_params(:,4) * (u .* u .* u);

pts2 = params(:,4) * (u.*u.*u) + params(:,3) * (u.*u) + params(:,2) * u + params(:,1) * ones(size(u));

% Trajectoire
cr = zeros(1,length(u));
for i=1:length(u)
    cr(i) = cr_bezier(params, u(i));
end

v_ratio = (-cr+b/2) ./ (-cr - b/2); % v_l / v_r

v_l = zeros(1,length(u));
v_r = zeros(1,length(u));
for i=1:length(u)
    if abs(v_ratio(i)) >= 1
        v_l(i) = v_max;
        v_r(i) = v_l(i) / v_ratio(i);
    else
        v_r(i) = v_max;
        v_l(i) = v_r(i) * v_ratio(i);
    end
end

v_lin = (v_l + v_r) / 2.0;
v_rot = (v_l - v_r) / b;

% simulation de la trajectoire
r_u(1) = 0;
r_x(1) = P(1); r_y(1) = P(2);
r_theta(1) = theta1;

i = 1;
dt = 0.01;
t_end = 5;
dparams = [params(:,2), 2*params(:,3), 3*params(:,4), [0;0]];
for t=0:dt:t_end
    r_cr(i) = cr_bezier(params, r_u(i));
    r_v_ratio(i) = (r_cr(i)+b/2) ./ (r_cr(i) - b/2);
    if r_cr(i) >= 0
        r_v_l(i) = v_max;
        r_v_r(i) = r_v_l(i) / r_v_ratio(i);
    else
        r_v_r(i) = v_max;
        r_v_l(i) = r_v_r(i) * r_v_ratio(i);
    end
    r_v_lin(i) = (r_v_l(i) + r_v_r(i)) / 2.0;
    r_v_rot(i) = (r_v_l(i) - r_v_r(i)) / b;
    dxu = dparams(1,:) * [1;r_u(i);r_u(i)^2;r_u(i)^3];
    dyu = dparams(2,:) * [1;r_u(i);r_u(i)^2;r_u(i)^3];
    dsu = sqrt(dxu^2+dyu^2);
    r_u(i+1) = r_u(i) +r_v_lin(i)*dt/dsu;
    if r_u(i+1) > 1
        break;
    end
    r_x(i+1) = r_x(i) + r_v_lin(i) * cos(r_theta(i)) * dt;
    r_y(i+1) = r_y(i) + r_v_lin(i) * sin(r_theta(i)) * dt;
    r_theta(i+1) = r_theta(i) + r_v_rot(i) * dt;
    i = i+1;
end

figure();
title('trajectoire');
plot(bc_params(1,:), bc_params(2,:), 'r--', pts(1,:), pts(2,:), 'r',...
     r_x, r_y, 'b.',...
     pts(1,linspace(1,n_pts,10)), pts(2,linspace(1,n_pts,10)), 'rx');
figure();
subplot(1,2,1);
plot(u, cr);
title('rayon de courbure');
subplot(1,2,2);
plot(u, v_ratio);
title('rapport des vitesses');
figure();
subplot(1,2,1);
plot(u, v_l);
title('v_l');
subplot(1,2,2);
plot(u, v_r);
title('v_r');
figure();
subplot(1,2,1);
plot(u, v_lin);
title('v_lin');
subplot(1,2,2);
plot(u, v_rot);
title('v_rot');
