% script file to create a plot of the crossover trajectories
%   for the collision avoidance example
%   (so swap x and y, scale by 5)
%
% a good viewing angle is 10, 15
%
% Ian Mitchell, 7/17/02

slices = [ -150; -90 ] * pi / 180;
%slices = [ -150 ] * pi / 180;
beta = 1;
scale = [ 5, 5, 180 / pi ];
aspect = scale .* [ 1 1 2 ];
dt = 0.05;

figure

for i = 1 : length(slices)

  subplot(1,length(slices),i)

  % plot the barrier (for comparison purposes)
  [ xp xm xc sp sm ] = barrier(slices(i), beta);
  xp = xp(:, [ 2 1 3]) .* repmat(scale, size(xp, 1), 1);
  xm = xm(:, [ 2 1 3]) .* repmat(scale, size(xm, 1), 1);
  xc = xc(:, [ 2 1 3]) .* repmat(scale, size(xc, 1), 1);

  % complete the capture circle
  xc = [ xc; -xc(:, 1:2), xc(:,3) ];

  lhTemp = plot3(xp(:,1), xp(:,2), xp(:,3) + 360, 'm:', ...
                 xm(:,1), xm(:,2), xm(:,3) + 360, 'm:', ...
                 xc(:,1), xc(:,2), xc(:,3) + 360, 'm:');
  lh(1) = lhTemp(1);
  hold on;

  % get the crossover stats
  [ bcase, p, pos ] = crossover(slices(i), beta);

  % left barrier
  sigmaE = -1;

  z0 = [ 0; -beta; 0 ];
  if(bcase(1) == 1)
    [ z2, z1, z12, z01 ] = optTraj2(3, 1, sigmaE, sigmaE, p.tau1m, p.tau2m, ...
                                    z0, dt);

  else
    % start with type 3 trajectory, then type 2
    [ z2, z1, z12, z01 ] = optTraj2(3, 2, sigmaE, sigmaE, p.tau1m, p.tau2m, ...
                                    z0, dt);
  end
  z = [ z01; z12 ];
  z = z(:, [ 2 1 3 ]) .* repmat(scale, size(z, 1), 1);
  z0 = z0([ 2 1 3 ]) .* scale';  
  z1 = z1([ 2 1 3 ]) .* scale';  
  z2 = z2([ 2 1 3 ]) .* scale';
  lhTemp = plot3(z(:,1), z(:,2), z(:,3) + 360, 'b-', ...
                 z0(1), z0(2), z0(3) + 360, 'bo', ...
                 z1(1), z1(2), z1(3) + 360, 'bs', ...
                 z2(1), z2(2), z2(3) + 360, 'b*');
  lh(2) = lhTemp(1);

  % add a capture circle at z0
  plot3(xc(:,1), xc(:,2), z0(3) * ones(size(xc(:,3))) + 360, 'm:');

  % right barrier
  sigmaE = +1;

  if(bcase(2) == 1)
    z0 = [ 0; -beta; 0 ];
    [ z2, z1, z12, z01 ] = optTraj2(3, 1, sigmaE, sigmaE, p.tau1p, p.tau2p, ...
                                    z0, dt);
    z = [ z01; z12 ];

  else
    z0 = [ -beta * sin(p.psi0p); -beta * cos(p.psi0p); 2 * p.psi0p ];
    z = optTraj(1, sigmaE, z0, [ 0:dt:p.tau2p, p.tau2p ]');
    z1 = z0;
    z2 = z(end,:)';
  end
  z = z(:, [ 2 1 3 ]) .* repmat(scale, size(z, 1), 1);
  z0 = z0([ 2 1 3 ]) .* scale';  
  z1 = z1([ 2 1 3 ]) .* scale';  
  z2 = z2([ 2 1 3 ]) .* scale';

  % to avoid jump in trajectory, need to separate z out
  zp = z(find(z(:,3) >= 0), :);
  zm = z(find(z(:,3) < 0), :);

  lhTemp = plot3(zp(:,1), zp(:,2), zp(:,3) + (360 * (zp(:,3) < 0)), 'r.-', ...
                 zm(:,1), zm(:,2), zm(:,3) + (360 * (zm(:,3) < 0)), 'r.-', ...
                 z0(1), z0(2), z0(3) + (360 * (z0(3) < 0)), 'ro', ...
                 z1(1), z1(2), z1(3) + (360 * (z1(3) < 0)), 'rs', ...
                 z2(1), z2(2), z2(3) + (360 * (z2(3) < 0)), 'r*');
  lh(3) = lhTemp(1);

  % add a capture circle at z0
  plot3(xc(:,1), xc(:,2), z0(3) * ones(size(xc(:,3))), 'm:');

end

for i = 1 : length(slices)
  subplot(1, length(slices), i);

  xlabel('x_r');
  ylabel('y_r');
  zlabel('\psi_r');
  title([ 'Slice at \psi_r = ' num2str(slices(i) * 180 / pi + 360) '^\circ' ]);
  axis([ -6 20 -6 12 0 360 ]);
  daspect(aspect);
  grid on;
  view(10, 15);
end

legend(lh, { 'barrier', 'left trajectory', 'right trajectory' });
