% steering_twodof - trajectory generation for the steering example
% RMM, 15 Jul 07
% 
% This file uses a set of basis function to compute trajectories between
% one vehicle pose and another.  The code is lifted from DGC trajutils.

aminit;
global nBasisFunctions;
nBasisFunctions = 12;
wheelbase = 2;

% Initial and final condition (x, y, theta)
q0 = [0, -2, 0];
qf = [30, 2, 0];
Tf = 4;				% time to reach destination

% Rescale the problem to live in the interval 0 to 1
scale = sqrt( (qf(1) - q0(1))^2 + (qf(2) - q0(2))^2);
vscale = Tf / 1;		% library solves for 1 sec traj
pose1 = [(q0(1) - q0(1))/scale, (q0(2) - q0(2))/scale, q0(3)];
pose2 = [(qf(1) - q0(1))/scale, (qf(2) - q0(2))/scale, qf(3)];

% Solve for the path
alpha = steering_twopoint(pose1, pose2, 1);
path = steering_makepath(alpha, [0:0.01:1]);

% Now scale it back to our original coordinates
traj(1,:) = path(1,:) * scale + q0(1);
traj(2,:) = path(2,:) * scale + q0(2);

% Now compute out the full desired trajectory for the system
time = 0:0.01:1;
for tind = 1:length(time)
  % Figure compute out the values of the raw basis functions at this time
  x_t = 0; y_t = 0; xd_t = 0; yd_t = 0; xdd_t = 0; ydd_t = 0;
  for index=1:nBasisFunctions
    x_t = x_t + alpha(index) * steering_basis(index-1, 0, time(tind));
    y_t = y_t + alpha(index+nBasisFunctions) * steering_basis(index-1, 0, time(tind));
      
    xd_t = xd_t + alpha(index) * steering_basis(index-1, 1, time(tind));
    yd_t = yd_t + alpha(index+nBasisFunctions) * steering_basis(index-1, 1, time(tind));
      
    xdd_t = xdd_t + alpha(index) * steering_basis(index-1, 2, time(tind));
    ydd_t = ydd_t + alpha(index+nBasisFunctions) * steering_basis(index-1, 2, time(tind));
  end
  
  % Now scale these to the proper units
  x(tind) = x_t*scale + q0(1); 
  y(tind) = y_t*scale + q0(2);

  xd(tind) = xd_t*scale / vscale; 
  yd(tind) = yd_t*scale / vscale;

  xdd(tind) = xdd_t*scale / vscale^2; 
  ydd(tind) = ydd_t*scale / vscale^2;
end

% Finally compute the trajectory in terms of the states and inputs
thetad = atan2(yd, xd);
veld = xd .* cos(thetad) + yd .* sin(thetad);
phid = atan((ydd .* cos(thetad) - xdd .* sin(thetad)) ./ veld * wheelbase);
time = time * Tf;

% Plot the trajectory in the plane, along with some lines
figure(1); clf; subplot(221);
plot(x, y, 'LineWidth', AM_data_linewidth); hold on;
amaxis([-3 33 -10 10]);
set(gca, 'DataAspectRatio', [1.5 1.5 1]);

plot([-5 35], [-4 -4], 'k-', 'LineWidth', AM_ref_linewidth);
plot([-5 35], [0 0], 'k--', 'LineWidth', AM_ref_linewidth);
plot([-5 35], [4 4], 'k-', 'LineWidth', AM_ref_linewidth);

% Plot a small picture of the car (crude for now)
bodyx = [-0.5 2.5 2.5 -0.5 -0.5];
bodyy = [-1, -1, 1, 1, -1];
plot(bodyx + q0(1), bodyy + q0(2), 'r', 'LineWidth', AM_ref_linewidth);
plot(bodyx + qf(1), bodyy + qf(2), 'r', 'LineWidth', AM_ref_linewidth);
axis off; axis image;

% Add labels to the car position
amtext(q0(1)-1, q0(2)-3.8, 'x0, y0');
amtext(qf(1)-1, qf(2)+4.2, 'xf, yf');

amprint('steering-lanechange.eps');

% Plot the resulting trajectories versus time
figure(2); clf;

% Lateral position
subplot(421); plot(time, y, 'LineWidth', AM_data_linewidth);
amaxis; ylabel('y');

% Replace the phi calculation with a direct computation
dtheta = (thetad(2:length(thetad)) - thetad(1:length(thetad)-1));
dtime = (time(2:length(time)) - time(1:length(time)-1));
thvel = dtheta ./ dtime * -5;
phic = [0 -atan(wheelbase * thvel ./ veld(1:length(thetad)-1))];

% Nominal inputs
subplot(423); plot(time, phic, 'LineWidth', AM_data_linewidth);
amaxis; xlabel('time'); ylabel('steer');

amprint('steering-lanetraj.eps');

return;

% Plot the full state just to check on things
figure(3); clf;
subplot(421); plot(time, x); ylabel('x');
subplot(422);
  plot(time, xd, '-', time, cos(thetad).*veld, '--');
  ylabel('xdot');
  lgh = legend('dx', 'vd cos(thd)', 'Location', 'northwest', 'orientation', 'horizontal');
  legend(lgh, 'boxoff');

subplot(423); plot(time, y); ylabel('y');
subplot(424);
  plot(time, yd, '-', time, sin(thetad).*veld, '--');
  ylabel('ydot');
  lgh = legend('dy', 'vd sin(thd)', 'Location', 'southwest', 'orientation', 'horizontal');
  legend(lgh, 'boxoff');

subplot(425); plot(time, thetad); ylabel('theta');

subplot(426);
  dtheta = (thetad(2:length(thetad)) - thetad(1:length(thetad)-1));
  dtime = (time(2:length(time)) - time(1:length(time)-1));
  thvel = dtheta ./ dtime * -5;
  plot(time(2:length(time)), thvel, '-', time, veld .* tan(phid) / wheelbase, '--');
  lgh = legend('thvel', 'v/l tan(phid)', 'Location', 'southwest', 'orientation', 'horizontal');
  legend(lgh, 'boxoff');

subplot(427);
  plot(time, phid, ...
    time, [0 -atan(wheelbase * thvel ./ veld(1:length(thetad)-1))]);
  ylabel('phi'); 
  lgh = legend('phid', 'computed', 'Location', 'southwest', 'orientation', 'horizontal');
  legend(lgh, 'boxoff');
