% Chapter 3 Section 1 Demos
close all; clear s sd sdd;  

%%%%%%%%%%%% Section 3.1.1 Smooth One-Dimensional Trajectories
% Test quintic spline function
s = tpoly(0, 1, 50);
hndlp = figure;
plot(s, 'LineWidth', 3);
title('Position vs Time');

% Overshoot problem
s = tpoly(0, 3, 50, 0.5, 0);
figure;
plot(s, 'LineWidth', 2);

% Velocities and accelerations
[s, sd, sdd] = tpoly(0, 1, 50);
hndlv = figure; 
plot(sd, 'LineWidth', 3);
title('Velocity vs Time');
hndla = figure;
plot(sdd, 'LineWidth', 3);
title('Acceleration vs Time');

% Compare to linear segments with parabolic blends
[s, sd, sdd] = lspb(0, 1, 50);
figure(hndlp);
hold on;
grid;
plot(s, 'r', 'LineWidth', 2);
legend('quintic pos', 'lspb pos');
figure(hndlv); hold on; grid;
plot(sd, 'r', 'LineWidth', 2); legend('quintic vel', 'lspb vel');
figure(hndla); hold on; grid;
plot(sdd, 'r', 'LineWidth', 2); legend('quintic acc', 'lspb acc');

% lspb with same max acceleration takes less time than tpoly
[s, sd, sdd] = lspb(0, 1, 44);
figure(hndlp);
plot(s, 'k', 'LineWidth', 2);
legend('quintic pos', 'lspb pos', 'same vel max', 'same acc max');
figure(hndlv);
plot(sd, 'k', 'LineWidth', 2);
legend('quintic vel', 'lspb vel', 'same vel max', 'same acc max');
figure(hndla);
plot(sdd, 'k', 'LineWidth', 3);
legend('quintic acc', 'lspb acc', 'same vel max', 'same acc max');

% lspb with same max velocity takes still less time than tpoly
[s, sd, sdd] = lspb(0, 1, 40);
figure(hndlp);
plot(s, 'g', 'LineWidth', 2);
legend('quintic pos', 'lspb pos', 'same vel max');
figure(hndlv);
plot(sd, 'g', 'LineWidth', 3);
legend('quintic vel', 'lspb vel', 'same vel max');
figure(hndla);
plot(sdd, 'g', 'LineWidth', 2);
legend('quintic acc', 'lspb acc', 'same vel max');


%%%%%%%%%%%% Section 3.1.2 Multi-dimensional cases
xbegin = [0 1 2];
xend = [-1 3 0];
x = mtraj(@tpoly, xbegin, xend, 100);
hndlmtraj = figure; hold on;
plot(x);
figure; hold on;
plot3(x(:,1), x(:,2), x(:,3), 'linewidth', 2);
plot3(x(1,1), x(1,2), x(1,3), 'marker', 'd');
plot3(x(end,1), x(end,2), x(end,3), 'marker', 's');

% Suppose the y coordinate is a known function of x
y = x(:,1) .* x(:,1);
figure; hold on;
plot3(x(:,1), y, x(:,3), 'linewidth', 2);
plot3(x(1,1), y(1), x(1,3), 'marker', 'd');
plot3(x(end,1), y(end), x(end,3), 'marker', 's');

% Now with lspb
xbegin = [0 1 2];
xend = [-1 3 0];
x = mtraj(@lspb, xbegin, xend, 100);
figure(hndlmtraj);
plot(x);
figure; hold on;
plot3(x(:,1), x(:,2), x(:,3), 'linewidth', 2);
plot3(x(1,1), x(1,2), x(1,3), 'marker', 'd');
plot3(x(end,1), x(end,2), x(end,3), 'marker', 's');

% Suppose the y coordinate is a known function of x
y = x(:,1) .* x(:,1);
figure; hold on;
plot3(x(:,1), y, x(:,3), 'linewidth', 2);
plot3(x(1,1), y(1), x(1,3), 'marker', 'd');
plot3(x(end,1), y(end), x(end,3), 'marker', 's');


%%%%%%%%%%%% Section 3.1.3 Multi-Segment Trajectories
via = [4 1; 4 4; 5 2; 2 5];
q = mstraj(via, [2 1], [], [4 1], 0.05, 0);
figure; hold on;
plot(q);
q = mstraj(via, [2 1], [], [4 1], 0.05, 1);
plot(q);
q = mstraj(via, [2 1], [], [4 1], 0.05, 3);
plot(q);


%%%%%%%%%%%% Section 3.1.4 Interpolation of Orientation
close all
R1 = rotz(1) * roty(0.000) * rotz(1); % Near-singular EUL
R0 = rotx(-.8) * roty(-pi/2+0.000) * rotz(1); % Near-singular RPY
rpy0 = tr2rpy(R0);
rpy1 = tr2rpy(R1);
rpy = mtraj(@tpoly, rpy0, rpy1, 100);
about(rpy);
figure;
plot(rpy);
hndl_anim = figure;
trplot(R0, 'color', 'r'); hold on;
trplot(R1, 'color', 'k');
tranimate(rpy2tr(rpy));

eul0 = tr2eul(R0);
R0eul = eul2tr(eul0)
eul1 = tr2eul(R1);
R1eul = eul2tr(eul1)
eul = mtraj(@tpoly, eul0, eul1, 100);
figure;
plot(eul);
figure(hndl_anim);
tranimate(eul2tr(eul));

quat0 = Quaternion(R0);
R0q = quat0.R
quat1 = Quaternion(R1);
R1q = quat1.R
quat = interp(quat0, quat1, [0:50]'/50);
figure(hndl_anim);
tranimate(quat);
load handel;
sound(y,Fs);


%%%%%%%%%%%% Section 3.1.5 Cartesian Motion
T0 = transl(2, 1, 0) * r2t(R0);
T1 = transl(-4, -2, 1) * r2t(R1);
Ts = trinterp(T0, T1, [0:49]'/49);
figure;
limits = [-5 3 -3 2 -1 2];
trplot(Ts(:,:,1), 'color', 'r', 'axis', limits);
hold on;
trplot(Ts(:,:,end), 'color', 'k', 'axis', limits);
tranimate(Ts);
Ts = trinterp(T0, T1, lspb(0,1,100));
tranimate(Ts);
P = transl(Ts);
about(P);