close all;
clear all;
dir = './Batch4Results/';name='BuoyA5';Hs=2;dt=.05;tf=500;b=14000;Lc=0.5;Ls=20;

%results_filenm = create_filenm([dir,'FlatPlate_' name],Hs,Ls,Lc,dt,tf,b)
load(['Feb17-ValidationResults_b14000_720seconds.mat']);

t = get(Buoy,'t');

%Incident Wave profile
figure('Position', [100 100 1000 600])
set(gcf,'color','white');
plot(t,eta0(1:N));
legend('Incident wave profile (m)');

%Get Buoy motion results
x = get(Buoy,'x');
xdot = get(Buoy,'xdot');
xddot = get(Buoy,'xddot');
 
%Get Plate motion results
y = get(Plate,'x');
ydot = get(Plate,'xdot');
yddot = get(Plate,'xddot');

%plotting results

Bforce = get(Buoy,'Forces');
Fforce = get(Plate,'Forces');

%Cylinder kinematics

figure('Position', [100 100 1000 600])
set(gcf,'color','white');
subplot(3,1,1),plot(t,x(:,1),t,x(:,3),t,x(:,5));
title('Buoy Motions','FontName','times','FontSize', 12);
legend('surge(m)','heave(m)','pitch(rad)');

subplot(3,1,2),plot(t,xdot(:,1),t,xdot(:,3),t,xdot(:,5));
legend('surge vel (m/s)','heave vel (m/s)','pitch vel (m/s)');

subplot(3,1,3),plot(t,xddot(:,1),t,xddot(:,3),t,xddot(:,5));
legend('surge accel (m/s^2)','heave accel (m/s^2)','pitch accel (m/s^2)');
xlabel('time (s)');


%foil kinematics
figure('Position', [100 100 1000 600])
set(gcf,'color','white');

subplot(3,1,1), plot(t,y(:,1),t,y(:,3),t,y(:,5));
title('Plate Motions','FontName','times','FontSize', 12);
legend('surge','heave(m)','pitch(rad)');

subplot(3,1,2), plot(t,ydot(:,1),t,ydot(:,3),t,ydot(:,5));
legend('surge(m/s)','heave vel (m/s)','pitch vel (m/s)');

subplot(3,1,3), plot(t,yddot(:,1),t,yddot(:,3),t,yddot(:,5));
legend('surge(m/s^2)','heave accel (m/s^2)','pitch accel (m/s^2)');
xlabel('time (s)');


%p_aB = [0.0  0.0  -1.25];  %Tether attachment point on Buoy (local CoordSys).
%p_aF = [0.0  0.0  0.0];  %Tether attachment point on Plate (local CoordSys).

xoB = get(Buoy,'x0');
xoF = get(Plate,'x0');


Elongation = zeros(length(t),1);
ElongationRate = zeros(length(t),1);
SpringTension = zeros(length(t),1);
DampingTension = zeros(length(t),1);
StictionTension = zeros(length(t),1);

s = zeros(length(t),3);
for i = 1:length(t)
    [Elongation(i),ElongationRate(i),s(i,:)] = TetherElongations(xoB,x(i,:)',xdot(i,:)',p_aB,xoF,y(i,:)',ydot(i,:)',p_aF);
    if(Elongation(i) > 0)  %If ram is fully retracted, tether is slack and no force is exerted.
        SpringTension(i) = ppval(Spring_Force_Coeffs,Elongation(i));
        DampingTension(i) = ppval(Damping_Force_Coeffs,ElongationRate(i));
        StictionTension(i) = ppval(Stiction_Force_Coeffs,ElongationRate(i));
    else
        Elongation(i) = 0;
        ElongationRate(i) = 0;
    end
end






%Tether kinematics and Dynamics
figure('Position', [100 100 1000 600])
set(gcf,'color','white');

subplot(2,1,1), plot(t,Elongation)
ylabel('Tether Elongation (m)');

subplot(2,1,2), plot(t,SpringTension/4.45)
ylabel('SpringTension - lbs');
xlabel('time (s)');


figure('Position', [100 100 1000 600])
set(gcf,'color','white');

subplot(2,1,1), plot(t,ElongationRate)
ylabel('Tether Elongation Rate (m/s)');

subplot(2,1,2), plot(t,DampingTension/4.45)
ylabel('DampingTension - lbs');
xlabel('time (s)');

figure('Position', [100 100 1000 600])
set(gcf,'color','white');

subplot(2,1,1), plot(t,ElongationRate)
ylabel('Tether Elongation Rate (m/s)');

%subplot(2,1,2), plot(t,StictionTension/4.45)
%ylabel('StictionTension - lbs');
%xlabel('time (s)');




%Compute and display auxillary results
pto_power = ElongationRate.*DampingTension;
stiction_power = ElongationRate.*StictionTension;
propulsion_force = Bforce{end}(:,1);
propulsion_power = propulsion_force.*xdot(:,1);
disp_power = -ydot(:,3).*Fforce{3}(:,3);
display(['Mean Forward Velocity = ' num2str(mean(xdot(:,1))) ' m/s']);
display(['Mean Propulsio Force = ' num2str(mean(propulsion_force)/4.45) ' lbs']);
display(['Mean Propulsion Power = ' num2str(mean(propulsion_power)) ' Watts']);
display(['Mean PTO Power = ' num2str(mean(pto_power)) ' Watts']);
display(['Mean Stiction Power = ' num2str(mean(stiction_power)) ' Watts']);
display(['Mean Power Dissipated = ' num2str(mean(disp_power)) ' Watts']);



figure('Position', [100 100 1000 600])
set(gcf,'color','white');

subplot(3,1,1)
plot(t,y(:,3));
ylabel('eta(m)');

subplot(3,1,2)
plot(t,pto_power)
ylabel('PTO Power (W)');

subplot(3,1,3), plot(t,disp_power)
ylabel('Viscous Power Dissipation (W)');
xlabel('time (s)');



%M(:,1) = t;
%M(:,2) = y(:,1);
%M(:,3) = y(:,3); 
%M(:,4) = y(:,5);

%xlswrite([results_filenm '.xls'], M)
