xoa = get(ESP,'x0');
xob = get(Buoy,'x0');


buoy_pts = [-34 -34  34  34  -34; 
          18  -10 -10  18  18];  %inches
buoy_pts = 0.0254*buoy_pts;  %meters


max_buoy_rng = max(max(abs(buoy_pts)));
current_buoy_center = [xob(1) xob(3)];

ESP_pts = [-15 -15  15  15  -15; 
          20  -20 -20  20  20];  %inches
ESP_pts = 0.0254*ESP_pts;  %meters

max_ESP_rng = max(max(abs(ESP_pts)));


xinc = current_buoy_center(1)-10:10/100:current_buoy_center(1)+10;
eta0 = eta(wave,xinc,0);


fig = figure('Position',[360 34 633 919]);

set(fig,'DoubleBuffer','on');

h_buoy = plot(buoy_pts(1,:)+xob(1),buoy_pts(2,:)+xob(3),'k');
hold on;
axis equal;
foo(1) = current_buoy_center(1)-10;
foo(2) = current_buoy_center(1)+10;
foo(3) = current_buoy_center(2)+min(TargetDepth)-5;
foo(4) = current_buoy_center(2)+5;
axis(foo);
xlabel('m');
ylabel('m');
h_incwave = plot(xinc,eta0,'b');



h_ESP = plot(ESP_pts(1,:)+xoa(1),ESP_pts(2,:)+xoa(3),'k');

Buoy_attachment = [p_aB(1) p_aB(3)]';
ESP_attachment = [p_aE(1) p_aE(3)]';

h_tether = plot([Buoy_attachment(1)+xob(1) ESP_attachment(1)+xoa(1)],[Buoy_attachment(2)+xob(3) ESP_attachment(2)+xoa(3)],'-g');

h_time=text(-7,4,sprintf('t = %4.1f s',i*dt));
h_target = text(-7,3,sprintf('target_depth = %4.1f m',TargetDepth(i)));
h_tension=text(7,4,sprintf('Tension = %6.1f lbs',Tension(i)/4.45));
h_payout = text(7,3,sprintf('Payout Rate = %4.1f m/s',TargetDepth(i)));

set(h_ESP,'EraseMode','xor');
set(gca,'nextplot','replace');



NN = t(end)/dt;
%Frames = moviein(NN); % initialize the matrix 'Frames'

for i = 1:NN;  %length(t)
  
  pause(dt);
  
  R = [cos(x(i,5)) sin(x(i,5)); -sin(x(i,5)) cos(x(i,5))];
  ppts = R*buoy_pts;  
  for j = 1:length(ppts)
     ppts(1,j) = ppts(1,j)+x(i,1)+xob(1);
     ppts(2,j) = ppts(2,j)+x(i,3)+xob(3);
  end
  set(h_buoy,'XData',ppts(1,:),'YData',ppts(2,:));
  eta0 = eta(wave,xinc-x(i,1),i*dt);
  set(h_incwave,'Xdata',xinc,'YData',eta0);
  set(h_time,'string',sprintf('t = %4.1f',i*dt));
  set(h_target,'string',sprintf('target_depth = %4.1f',TargetDepth(i)));
  set(h_tension,sprintf('Tension = %6.1f lbs',Tension(i)/4.45));
  set(h_payout,
h_payout = text(7,3,sprintf('Payout Rate = %4.1f m/s',TargetDepth(i)));
    
  Buoy_a = R*Buoy_attachment;
  
  R = [cos(y(i,5)) sin(y(i,5)); -sin(y(i,5)) cos(y(i,5))];
  ppts = R*ESP_pts;       
   
  for j = 1:length(ppts)
     ppts(1,j) = ppts(1,j)+y(i,1)+xoa(1);
     ppts(2,j) = ppts(2,j)+y(i,3)+xoa(3);
  end
  set(h_ESP,'XData',ppts(1,:),'YData',ppts(2,:));
    
  
  ESP_a = R*ESP_attachment;
  
  set(h_tether,'XData',[Buoy_a(1)+xob(1)+x(i,1) ESP_a(1)+xoa(1)+y(i,1)],'YData',[Buoy_a(2)+xob(3)+x(i,3) ESP_a(2)+xoa(3)+y(i,3)]);
  
  %Frames(i) = getframe(gcf);
end

%map = colormap;
%mpgwrite(Frames,map,'example.mpg')
%clear Frames;