close all
clear all

hard_copy = 0;

ppath = 'SurfaceData/';

validation_path = 'ValidationResults/';

dirlisting = dir([ppath '*.mat']);
 
i = 12;
   
filename = dirlisting(i).name

load([ppath filename]);

PistonDistance = RangeFinderCalibration(PistonDistance/100);
LoadDumpCurrent = LoadDumpCurrent/10;

for j = 1:length(PistonDistance)-6
  PistonDistance(j) = PistonDistance(j+6);
end


%Find datapoints corrupted by delay.
idx2 = find(abs(diff(time)-.1)<.05);

%Remove those points entirely.
PistonDistance = PistonDistance(idx2);
time = 


windowSize = 8;
PistonDistance_x = filtfilt(ones(1,windowSize)/windowSize,1,PistonDistance);
windowSize = 8;
PistonDistance_xdot = filtfilt(ones(1,windowSize)/windowSize,1,[0 diff(PistonDistance_x)./diff(time)]);




%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)];

title_txt = regexprep(filename, '[_]', ':')


Au = 0.25*pi*(5^2);  %in^2
Al = 0.25*pi*(5^2-1.5^2);  %in^2

SpringForce = Al*LowerSpringPressure-Au*UpperSpringPressure;

figure('Position',[520   304   744   794],'PaperPositionMode','manual','PaperType','usletter','PaperPosition',[0 0 8.5 11]); 
subplot(4,1,1)
plot(39.4*PistonDistance_x+4.1,SpringForce,'-x');
legend(['Average Spring Force = ' num2str(mean(SpringForce),4) ' lbs']);
%axis([0 80 1200 3000]);
title(title_txt);
xlabel('Ram Position (in)');
ylabel('Spring Force (lbs)');

subplot(4,1,2)
plot(39.4*PistonDistance_x+4.1,LoadDumpCurrent.*LoadDumpVoltage);
legend(['Average Power = ' num2str(mean(LoadDumpCurrent.*LoadDumpVoltage),4) ' W']);
xlabel('Ram Position (in)');
ylabel('Load Dump Power (W)');

subplot(4,1,3)
plot(39.4*PistonDistance_x+4.1,LoadCellForce);
legend(['Average Force = ' num2str(mean(LoadCellForce),4) ' lbs']);
xlabel('Ram Position (in)');
ylabel('Load Cell Force (lbs)');

subplot(4,1,4)
plot(39.4*PistonDistance_x+4.1,LoadCellForce-SpringForce);
legend(['Average Force = ' num2str(mean(LoadCellForce-SpringForce),4) ' lbs']);
xlabel('Ram Position (in)');
ylabel('LoadCellForce minus SpringForce (lbs)');
%print('-dpdf',[ppath filename '_forces']);




idx = 1:36000
figure('Position',[520   304   744   794],'PaperPositionMode','manual','PaperType','usletter','PaperPosition',[0 0 8.5 11]); 
ah1 = subplot(5,1,1)
plot(time(idx)-time(idx(1)),PistonDistance(idx));
title(title_txt);
ylabel('Piston Position (m)');
ah2 = subplot(5,1,2)
plot(time(idx)-time(idx(1)),LoadDumpVoltage(idx));
ylabel('Load Dump Voltage (V)');
ah3 = subplot(5,1,3)
plot(time(idx)-time(idx(1)),LoadDumpCurrent(idx));
ylabel('Load Dump Current (A)');
ah4 = subplot(5,1,4)
plot(time(idx)-time(idx(1)),LoadDumpVoltage(idx).*LoadDumpCurrent(idx));
legend(['mean power = ' num2str(mean(LoadDumpVoltage(idx).*LoadDumpCurrent(idx)),3) ' W']);
ylabel('Load Dump Power (W)');
ah5 = subplot(5,1,5)
plot(time(idx)-time(idx(1)),LoadDumpTemp(idx));
xlabel('time (seconds)');
ylabel('Load Dump Temperature (deg C)');
linkaxes([ah1 ah2 ah3 ah4 ah5],'x');

%print('-dpdf',[ppath filename]);



idx = find(SafetyRelayPos > 0);
ModLoadDumpVoltage = LoadDumpVoltage;
ModLoadDumpVoltage(idx) = -ModLoadDumpVoltage(idx);
ModLoadDumpCurrent = LoadDumpCurrent;
ModLoadDumpCurrent(idx) = -LoadDumpCurrent(idx);

figure('Position',[520   304   744   794],'PaperPositionMode','manual','PaperType','usletter','PaperPosition',[0 0 8.5 11]); 
plot(ModLoadDumpVoltage,ModLoadDumpCurrent);


load([validation_path 'Jan5_1100PDT_ValidationResults']);



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


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



%Work Done on Buoy by Water (positive equals energy into buoy)
%Bforce{1,1} = Hydrostatic Force
%Bforce{1,2} = Weight
%Bforce{1,3} = Radiation Force
%Bforce{1,4} = Exciting Force
%Bforce{1,5} = Drag Force
%Bforce{1,6} = Tether Force
mean_hydrostatic = mean(Bforce{1,1});

%WaterForceOnBuoy = (Bforce{1,1}-mean_hydrostatic);







n_window_M = 48;
n_window_S = 4;
[M_f, M_PxxElon] = tseries2spectrum(time-time(1),PistonDistance_x-mean(PistonDistance_x),n_window_M);
[S_f, S_PxxElon] = tseries2spectrum(t,Elongation-mean(Elongation),n_window_S);
n_window = 48;
[M_f, M_PxxElonRate] = tseries2spectrum(time-time(1),PistonDistance_xdot-mean(PistonDistance_xdot),n_window_M);
[S_f, S_PxxElonRate] = tseries2spectrum(t,ElongationRate-mean(ElongationRate),n_window_S);

figure('Position',[520   304   744   794],'PaperPositionMode','manual','PaperType','usletter','PaperPosition',[0 0 8.5 11]); 
subplot(2,1,1)
plot(M_f,M_PxxElon,S_f,S_PxxElon);
legend('Measured Results','Modeled Results');
foo = axis; foo(1) = 0;foo(2) = .6; axis(foo);
xlabel('Hz');
ylabel('m^2/Hz');
title('Spectrum of PTO Motions, Measured Versus Modeled  Jan52012_1100PDT');
subplot(2,1,2)
plot(M_f,M_PxxElonRate,S_f,S_PxxElonRate);
legend('Measured Results','Modeled Results');
foo = axis; foo(1) = 0;foo(2) = .6; axis(foo);
xlabel('Hz');
ylabel('(m/s)^2/Hz');