%function [ output_args ] = PlotFFD(data)

C = 12.22*.0254;  %Wing Chord
nu = 1e-6;  %Viscosity, for Re number calculation

t = (data.Time_DateNum-data.Time_DateNum(1))*24*60*60;

%Plot InputData
figure('Position',[ 1000          44         974        1454]);
ah1= subplot(3,1,1);
plot(t,data.EA(:,2))
title('PlotFFD Input Data');
ylabel('pitch');
ah2= subplot(3,1,2);
plot(t,data.RC_AngRate(:,2)*180/pi)
ylabel('AngRate');
legend('pitch');
ah3 = subplot(3,1,3);
plot(t,data.RC_NortekVel);
ylabel('Vel (m/s)');
legend('1','2','3');
linkaxes([ah1 ah2 ah3],'x');

%Compute Velocities
v0(1,:) = data.RC_NortekVel(:,3)';  %Velocities of rigid body at sample point, x (1) is to the front, y (2) is up.
v0(2,:) = data.RC_NortekVel(:,1)';
th_dot = data.RC_AngRate(:,2)';

r0 = 0.0254*[90.63 ; 4.08];
r1 = 0.0254*[78.45 ; 8.28];
rm = 0.0254*[0 ; 8.28];
r12 = 0.0254*[-75.55 ; 8.28];

r = cell(12,1);
r{1} = 0.0254*[78.45 ; 8.28];
for j = 2:12
   r{j} = r{j-1}-0.0254*[14 ; 0]; 
end

for i = 1:length(th_dot)
   v(1,i) = v0(1,i)-th_dot(i)*(-r0(2));
   v(2,i) = v0(2,i)-th_dot(i)*(r0(1));
end

%Compute Water Velocity relative to foil at front, mid, and back foils.
v_w = cell(12,1);
alpha = cell(12,1);
F = cell(12,1);
for j = 1:length(r)    
  leg_str{j} = ['Wing ' num2str(j)];
  v_w{j} = zeros(2,length(th_dot));
  theta{j} = zeros(1,length(th_dot));
  Mag{j} = zeros(1,length(th_dot));
    for i = 1:length(th_dot)
      v_w{j}(1,i) = -(v(1,i)+th_dot(i)*(-r{j}(2)));
      v_w{j}(2,i) = -(v(2,i)+th_dot(i)*(r{j}(1)));
      theta{j}(i) = (180/pi)*atan2(-v_w{j}(2,i),-v_w{j}(1,i));
      Mag{j}(i) = (v_w{j}(2,i).^2+v_w{j}(1,i).^2).^.5;
    end
    Re{j} = Mag{j}*C/nu;
end

%Ganged Wings
for i = 1:length(th_dot)
   [alpha{6}(i)  F{6}(:,i)] = ActualWingAngle(  theta{6}(i), norm(v_w{6}(:,i)));
   % [alpha{6}(i)  F{6}(:,i)] = OptimumWingAngle(  theta{6}(i), norm(v_w{6}(:,i)));
end
alpha{6} = pi*alpha{6}/180;
for j = 1:length(r)
    alpha{j} = alpha{6}; 
    F{j} = F{6};
end


%Independent Wings
%for j = 1:length(r)
%disp(['Computing Angles Wing ' num2str(j)]);
%  for i = 1:length(th_dot)
%    [alpha{j}(i)  F{j}(:,i) ] = ActualWingAngle(  theta{j}(i), norm(v_w{j}(:,i)));
%    [alpha{j}(i)  F{j}(:,i)] = OptimumWingAngle(  theta{j}(i), norm(v_w{j}(:,i)));
%  end
%alpha{j} = pi*alpha{j}/180;
%end


F_total = F{1};
for j = 2:length(r)
  F_total = F{j}+F_total;    
end

F_mean = mean(F_total,2);


accel_x = data.RC_Accel(:,1).*cos(pi*data.EA(:,1)/180)+data.RC_Accel(:,2).*sin(pi*data.EA(:,1)/180);
accel_y = -data.RC_Accel(:,1).*sin(pi*data.EA(:,1)/180)+data.RC_Accel(:,2).*cos(pi*data.EA(:,1)/180);

alpha_dot = [0 diff(alpha{6})./diff(t')];
figure

ah(1) = subplot(6,1,1)
plot(t,alpha{6}*180/pi);
xlabel('time');
ylabel('Wing Angle (deg)');

ah(2) = subplot(6,1,2)
plot(t,alpha_dot*180/pi);
xlabel('time');
ylabel('Wing Angle Rate (deg/sec)');

ah(3) = subplot(6,1,3)
plot(t,F_total(3,:)*39.4/4.45);
xlabel('time');
ylabel('(in/lbs)');
legend('Pivot Torque - All Wings');

ah(4) = subplot(6,1,4)
PivotPower = alpha_dot.*F_total(3,:);
plot(t,alpha_dot.*F_total(3,:));
xlabel('time');
ylabel('Wing Pivot Power (W)');
legend(['RMS Power = ' num2str(rms(PivotPower)) ' W']);

ah(5) = subplot(6,1,5)
plot(t,theta{6});
xlabel('time');
ylabel('Water Angle (deg)');

ah(6) = subplot(6,1,6)
plot(t,alpha{6}*180/pi-theta{6});
xlabel('time');
ylabel('Wing AOA (deg)');

linkaxes(ah,'x');




figure
hist(Mag{6},100)
title('Histogram of all water velocity magnitudes');

idx = find(abs(theta{6}) < 45);
figure
hist(Mag{6}(idx),100);
title('Histogram of water velocity magnitudes for which |theta| < 45');


figure
ah1 = subplot(2,1,1);
plot(t,v0(1,:),t,v(1,:));
xlabel('Frame x vel');
legend('v0','v');
ah2 = subplot(2,1,2);
plot(t,[v0(2,:) ; v(2,:)]);
xlabel('Frame y vel');
legend('v0','v');

jdx = [1 6 12];
figure
ah1 = subplot(5,1,1);
for j = jdx
  plot(t,v_w{j}(1,:));
  hold all
end
legend(leg_str{jdx});
ylabel('Water x vel w.r.t. spine');
ah2 = subplot(5,1,2);
for j = jdx
  plot(t,v_w{j}(2,:));
  hold all
end
legend(leg_str{jdx});
ylabel('Water y vel w.r.t. spine');
ah3 = subplot(5,1,3);
for j = jdx
  plot(t,theta{j});
  hold all
end
legend(leg_str{jdx});
ylabel('theta (deg)');
ah4 = subplot(5,1,4);
plot(t,[theta{1}-theta{6} ; theta{12}-theta{6}]);
ylabel('delta theta (deg)');
legend('1-6','12-6');

ah5 = subplot(5,1,5);
plot(t,accel_x);
ylabel('Accel (x)');


linkaxes([ah1 ah2 ah3 ah4 ah5],'x');












fh = figure('Position',[       100         481        2336        1017]);

ah1 = axes('OuterPosition',[0 0 1 1]);

spine_pts = [-88.55       85.45         85.45          -88.55        -88.55 ;
             11.28        11.28         5.28           5.28         11.28]; %inches

wing_ctr = [78.45:-14:-76];
wing_ctr(2,:) = 8.28*ones(1,length(wing_ctr(1,:)));

spine_pts = .0254*spine_pts;  %meters
wing_ctr = 0.0254*wing_ctr;  %meters

Chord = .0254*12.34;
ctr = .0254*2.875;

set(fh, 'renderer', 'zbuffer');
set(fh,'DoubleBuffer','on');
set(ah1,'nextplot','replace');
set(ah1,'FontSize',18);

hold(ah1,'on');
axis(ah1,'equal');
h_spine = fill(spine_pts(1,:),spine_pts(2,:),[.9 .9 .9]);
ArrowScale = 2.0;
for i = 1:length(wing_ctr)
  v_w{i} = ArrowScale*v_w{i};  
  h_wingctr(i) = plot(wing_ctr(1,i),wing_ctr(2,i),'ro');
  h_Vw(i) = quiver(wing_ctr(1,i),wing_ctr(2,i),v_w{i}(1,1),v_w{i}(1,2),'b');
  h_Vwm(i) = quiver(wing_ctr(1,i),wing_ctr(2,i),v_w{1}(1,1),v_w{1}(1,2),'g');
  [pts] = WingPoints([wing_ctr(1,i) wing_ctr(2,i)],-alpha{i}(1),Chord,ctr);
  h_wings(i) = plot(pts(1,:),pts(2,:),'k');
end
%h_force = quiver(0,-.75,F_total(1),0);
%set(h_force,'MaxHeadSize',1);
title(['Mean Horizontal Force = ' num2str(F_mean(1)/4.45) ' lbs']);
xlim([-3 3]);
ylim([-1 1]);
xlabel('m');
ylabel('m');
time_str = (['Time = ' num2str(t(1),3) ' s']);
th = text(2.25,.4,time_str,'FontSize',14);

for k = 2:length(t)
    disp(['timestep = ' num2str(k)]);
  for i = 1:length(wing_ctr);
    set(h_Vw(i),'Udata',v_w{i}(1,k));
    set(h_Vw(i),'Vdata',v_w{i}(2,k));
    set(h_Vwm(i),'Udata',v_w{1}(1,k));
    set(h_Vwm(i),'Vdata',v_w{1}(2,k));    
    [pts] = WingPoints([wing_ctr(1,i) wing_ctr(2,i)],-alpha{i}(k),Chord,ctr);
    set(h_wings(i),'Xdata',pts(1,:));
    set(h_wings(i),'Ydata',pts(2,:));
  end
 % set(h_force,'Udata',F_total(k)/100);
  time_str = (['Time = ' num2str(t(k),3) ' s']);
  set(th,'String',time_str);
  drawnow;
end

%end

