close all
clear all

deploydata_path = '.\JAN25_2011\SurfaceData\';
%filename = 'D000004_110125_130458';
%filename = 'D000000_110127_094926';
%filename = 'FullDataSet';
filename = '1930-2215';

load([deploydata_path filename '.mat']);

%Damping behavior of PTO only (from test machine measurments, not including spring and other drag effects)
vel = (0:5:60)/39.4;  %m/s
force = [0 554 1180 2280 3837 5871 8393 11335 14877 18713 22671 25434 26579];  %Newtons
pp_PTO = interp1(vel,force,'linear','pp');

figure('Position',[196    79   928   661],'PaperOrientation', 'landscape','PaperPositionMode','manual','PaperType','usletter','PaperPosition',[0 0 11 8.5]); 
xx = (0:60)/39.4;
plot(vel,force/4.45,'x-',xx,14000*xx.*abs(xx)/4.45)
set(gca,'FontSize',12);
legend('Measured PTO Force','PTO damping model (14000 N/(m/2)^2  * V^2','Location','NorthWest');
xlabel('PTO Speed (m/s)');
ylabel('Force (lbs)')

print('-dpdf','PTO_Damping');
print('-dpng','PTO_Damping');
print('-dill','PTO_Damping');

PistonDistance = PistonDistance/100;  %Convert to meters
meanPistonDistance = mean(PistonDistance)

z = filtfilt(ones(8,1)/8,1,PistonDistance);

%Compute piston postion based on pressure and temperature

A = ones(length(PistonDistance),3);
A(:,2) = -1./UpperSpringPressure;
A(:,3) = -UpperSpringTemperature./UpperSpringPressure;

coeffs = A\PistonDistance';

zz_upper = A*coeffs;

A = ones(length(PistonDistance),3);
A(:,2) = -1./LowerSpringPressure;
A(:,3) = -LowerSpringTemperature./LowerSpringPressure;

coeffs = A\PistonDistance';

zz_lower = A*coeffs;

zz = filtfilt(ones(3,1)/3,1,(zz_upper+zz_lower)/2);

z = zz';

zdot = [0 diff(z)./diff(time)];



pow = ppval(pp_PTO,abs(zdot)).*abs(zdot);
mean(pow)

figure('Position',[196  79  1004  598],'PaperOrientation', 'landscape','PaperPositionMode','manual','PaperType','usletter','PaperPosition',[0 0 11 8.5]); 
ah1 = subplot(3,1,1);
plot(time-time(1),PistonDistance,time-time(1),z);
set(gca,'FontSize',14);
ylabel('Piston Position (m)');
ylim([.5 1.5]);
ah2 = subplot(3,1,2);
plot(time-time(1),zdot);
set(gca,'FontSize',14);
ylabel('Piston Velocity (m/s)');
ylim([-.5 .5]);
ah3 = subplot(3,1,3);
plot(time-time(1),pow)
set(gca,'FontSize',14);
ylim([0 1000]);

legend(['PTO Power (mean = ' num2str(mean(pow),3) 'W']);
ylabel('PTO Power (W)');
xlabel('Time (s)');

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

print('-dpdf',[deploydata_path 'PTO_Behavior_zFromPressureData']);
print('-dpng',[deploydata_path 'PTO_Behavior_zFromPressureData']);
print('-dill',[deploydata_path 'PTO_Behavior_zFromPressureData']);


xlim([0 120]);
print('-dpdf',[deploydata_path 'PTO_Behavior_zoomed_zFromPressureData']);
print('-dpng',[deploydata_path 'PTO_Behavior_zoomed_zFromPressureData']);
print('-dill',[deploydata_path 'PTO_Behavior_zoomed_zFromPressureData']);