close all
clear all

deploydata_path = '.\SurfaceData\';
%filename = 'D000004_110125_130458';
%filename = 'D000000_110127_094926';zdot
%filename = 'FullDataSet';
filename = 'InWater';

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

start_idx = 50000;

PistonDistance = PistonDistance(start_idx:end);
UpperSpringPressure = UpperSpringPressure(start_idx:end);
LowerSpringPressure = LowerSpringPressure(start_idx:end);
UpperSpringTemperature = UpperSpringTemperature(start_idx:end);
LowerSpringTemperature = LowerSpringTemperature(start_idx:end);
LoadCellForce = LoadCellForce(start_idx:end);
eastVelocity = eastVelocity(start_idx:end);
downVelocity = downVelocity(start_idx:end);
northVelocity = northVelocity(start_idx:end);

time = time(start_idx:end);



%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,xx,ppval(pp_PTO,xx)/4.45);
set(gca,'FontSize',12);
legend('Measured PTO Force','b = (14000 N/(m/s)^2  * V^2','PTO Damping Model','Location','NorthWest');
xlabel('PTO Speed (m/s)');
ylabel('Force (lbs)')

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

idx = find(PistonDistance > 218)
PistonDistance(idx) = 185;

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';


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


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


zddot = [0 diff(zdot)./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),4) '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']);

%Filter bad values out of LowerSpringPressure

Pl = LowerSpringPressure;
idx = find(LowerSpringPressure < 153);
i = 1;
while(i < length(idx))
    i
    j = 0;
    idx(i+j)
    idx(i+j+1)-1
    while (idx(i+j) == idx(i+j+1)-1)
      idx(i+j)
      idx(i+j+1)-1
      j = j + 1;
    end
    j
    for k = i:i+j
      k
      LowerSpringPressure(idx(k)) = LowerSpringPressure(idx(i)-1); 
    end
    i = i+j+1;
end

SpringForce = LowerSpringPressure*0.25*pi*(5^2-2^2)-UpperSpringPressure*0.25*pi*5^2;

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),LowerSpringPressure,time-time(1),UpperSpringPressure);
ylabel('Spring Pressures (lbs)');

ah2 = subplot(3,1,2);
plot(time-time(1),SpringForce);
ylabel('Spring Force (lbs)');

ah3 = subplot(3,1,3);
plot(time-time(1),z);
ylabel('Piston Position (m)');


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






DampingForce = SpringForce-LoadCellForce;

pow = 4.45*DampingForce.*zdot;


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),DampingForce);
set(gca,'FontSize',14);
ylabel('Damping Force (lbs)');
%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),4) 'W']);
ylabel('PTO Power (W)');
xlabel('Time (s)');

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




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),z);
set(gca,'FontSize',14);
ylabel('Piston Position');
%ylim([.5 1.5]);
ah2 = subplot(3,1,2);
plot(time-time(1),zdot);
set(gca,'FontSize',14);
ylabel('Piston Velocity (m/s)');

ah3 = subplot(3,1,3);
plot(time-time(1),zddot);
set(gca,'FontSize',14);
ylabel('Piston Accel (m/s^2)');

xlabel('Time (s)');

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