close all
clear all

data = load('Nov10_2_machine');

unix_time = data(:,1);
PropGain = data(:,2);
IntGain = data(:,3);
DerGain = data(:,4);
FFGain = data(:,5);
PitchTarg = data(:,6);
Pitch = data(:,8);
Cmd = data(:,10);
Vout = data(:,11);
mode = data(:,12);
Amp = data(:,13);
Freq = data(:,14);
nav440Roll = data(:,15);
nav440Pitch = data(:,16);
nav440RollRate = data(:,17);
nav440PitchRate = data(:,18);
nav440FiltPitchRate = data(:,19);
nav440Latitude = data(:,20);
nav440Longitude = data(:,21);

t = unix_time-unix_time(1);  %Convert to seconds since start of program
dt = t(end)/length(t);
%time = dt*(0:length(t)-1)';  %Smooth out time bumps, it's not clear why there's a periodic time advancement/delay recorded in the data.
time = t;

PitchVel = [0 ; diff(Pitch)./diff(time)];

idx = 3:length(time)-3;  %Skip end few data points.
%idx = find((time > 1500) & (time < 1900));
%idx = find((time > 1000) & (time < 1900));
idx = find((time > 1420) & (time < 1920));

figure('Position',[  167         128         999        1325]);
ah1 = subplot(5,1,1)
plot(time(idx),PitchTarg(idx)-mean(PitchTarg(idx)));
hold all
plot(time(idx),nav440Pitch(idx)-mean(nav440Pitch(idx)))
plot(time(idx),Pitch(idx)-mean(PitchTarg(idx)),'-k');
legend('Pitch Target','Paragon Pitch','Pole Pitch');
ylabel('Degrees');
xlabel('time (sec)');
ylim([-10 10]);
grid on;
title('Fig 1a:  Paragon and Pole Pitch');

N = 8;

PitchVelFilt = filtfilt(ones(N,1),N,PitchVel);
PitchVelRelative = nav440PitchRate-PitchVelFilt;
ah2 = subplot(5,1,2)
plot(time(idx),nav440PitchRate(idx));
hold all;
plot(time(idx),PitchVelFilt(idx));
%plot(time(idx),PitchVelRelative(idx),'-k');

legend('Prgn Pitch Velocity','Pole Pitch Velocity');
xlabel('time (sec)');
ylabel('deg/sec');
ylim([-20 20]);
grid on;
title('Fig 1b:  Paragon and Pole Pitch Rates');

ah3 = subplot(5,1,3);
plot(time(idx),Vout(idx));
ylabel('Valve Command Voltage (V)');
xlabel('time (sec)');
grid on;
title('Fig 1c:  Valve Command Voltage to 5GPM Valve (+/- 10V max)');

ah4 = subplot(5,1,4);
plot(time(idx),PropGain(idx));
hold all;
plot(time(idx),IntGain(idx));
plot(time(idx),DerGain(idx));
plot(time(idx),FFGain(idx));
legend('Prop Gain','Int Gain','Der Gain','Vel FF Gain');
xlabel('time (sec)');
grid on;
title('Fig 1d:  Pitch Control Gain Settings');


PitchLever = 20;  %Inches
RamOD = 1.5;  %Inches
RodOD = 0.75;  %Inches
CubicInchPerGallon = 231;
ABarrel = 0.25*pi*RamOD^2;  %in^2
ARod = ABarrel-0.25*pi*RodOD^2;  %in^2
VolDotBarrel = -(PitchVelRelative*pi/180)*PitchLever*ABarrel*60/CubicInchPerGallon;  %Volume Rate Barrel side of ram (GPM)
VolDotRod = (PitchVelRelative*pi/180)*PitchLever*ARod*60/CubicInchPerGallon;  %Volume Rate for Rod side of ram (GPM)

ah5 = subplot(5,1,5);
plot(time(idx),VolDotBarrel(idx));
hold all;
plot(time(idx),VolDotRod(idx));

xlabel('time (sec)');
ylabel('GPM');
ylim([-4 4]);
grid on;
leg_str{1} = ['Mean Barrel Side Flow Rate = ' num2str(mean(abs(VolDotBarrel(idx))),"%.2f") ' GPM' newline ...
        'Max Barrel Side Flow Rate = ' num2str(max(abs(VolDotBarrel(idx))),"%.2f") ' GPM'];
leg_str{2} = ['Mean Rod Side Flow Rate = ' num2str(mean(abs(VolDotRod(idx))),"%.2f") ' GPM' newline ...
        'Max Rod Side Flow Rate = ' num2str(max(abs(VolDotRod(idx))),"%.2f") ' GPM'];
legend(leg_str);
title('Fig 1e: Flow Rates for Pitch Ram at 20", 1.5" Cyl Diam, .75" Rod Diam)');

print('Fig1','-dpdf','-bestfit');
print('Fig1','-dpng');

PosPitch_idx = find(PitchVelRelative > 0);
NegPitch_idx = find(PitchVelRelative <= 0);
ValveSupplyFlowRate(PosPitch_idx) = VolDotRod(PosPitch_idx);
ValveSupplyFlowRate(NegPitch_idx) = VolDotBarrel(NegPitch_idx);
ReturnToTankFlowRate(PosPitch_idx) = -VolDotBarrel(PosPitch_idx);
ReturnToTankFlowRate(NegPitch_idx) = -VolDotRod(NegPitch_idx);

%Find direction change points
j = 0;
clear DirChange_idx;
for i = 1:length(time(idx))-1
    if(PitchVelRelative(idx(i))*PitchVelRelative(idx(i+1)) < 0)  %Sign change
        j = j+1;
        DirChange_idx(j) = idx(i);
    end
end

%Find volume per stroke (defined as time between Pitch Velocity direction changes)
for i = 1:length(DirChange_idx)-1
   Volume(i) = trapz(time(DirChange_idx(i):DirChange_idx(i+1)),CubicInchPerGallon*ValveSupplyFlowRate(DirChange_idx(i):DirChange_idx(i+1))/60);
   PeakFlowRate(i) = max(ValveSupplyFlowRate(DirChange_idx(i):DirChange_idx(i+1)));
end


figure('Position',[  245         178        1020        1325]);
ah21 = subplot(4,1,1);
plot(time(idx),ValveSupplyFlowRate(idx));
%hold all;
%plot(time(DirChange_idx),ValveSupplyFlowRate(DirChange_idx),'x');
%plot(time(idx),ReturnToTankFlowRate(idx));

xlabel('time (sec)');
ylabel('GPM');
%ylim([-4 4]);
grid on;
leg_str{1} = ['Mean Supply Flow Rate = ' num2str(mean(ValveSupplyFlowRate(idx)),"%.2f") ' GPM' char(10) ...
        'Max Supply Flow Rate = ' num2str(max(ValveSupplyFlowRate(idx)),"%.2f") ' GPM'];
%leg_str{2} = ['Mean Return Flow Rate = ' num2str(mean(ReturnToTankFlowRate(idx)),"%.2f") ' GPM' char(10) ...
%        'Max Return Flow Rate = ' num2str(max(ReturnToTankFlowRate(idx)),"%.2f") ' GPM'];
legend(leg_str);
title('Fig 2a: Supply Flow Rates for Pitch Ram at 20", 1.5" Cyl Diam, .75" Rod Diam)');


SupplyPressure = 1500;  %Psi
WattsPerHP = 745.7;
WattsPerGPMPSI = WattsPerHP/1714;  %HP = GPM*PSI/1714, 

ValveSupplyPower = WattsPerGPMPSI*ValveSupplyFlowRate*SupplyPressure;
ah22 = subplot(4,1,2);
plot(time(idx),ValveSupplyPower(idx));

xlabel('time (sec)');
ylabel('Hydraulic Power (W)');
grid on;
leg_str{1} = ['Mean Supply Power = ' num2str(mean(ValveSupplyPower(idx)),"%.1f") ' W' '/' num2str(mean(ValveSupplyPower(idx)/WattsPerHP),"%.2f") ' HP' char(10) ...
        'Max Supply Power = ' num2str(max(ValveSupplyPower(idx)),"%.1f") ' W' '/' num2str(max(ValveSupplyPower(idx)/WattsPerHP),"%.2f") ' HP' ];
legend(leg_str);
title("Fig 2b: Hydraulic Power supplied to valve manifold (supply pressure times flow rate)");

12/14
hyd_pump_eff = 0.5;
RPM = 2400;
ShaftTorque = (ValveSupplyPower/hyd_pump_eff)/(RPM*2*pi/60);
ah23 = subplot(4,1,3)
plot(time(idx),ShaftTorque(idx));
xlabel('Time (s)');
ylabel('N-m');
title(['Fig 2c: Shaft Torque required of electric motor at ' num2str(RPM) 'RPM and ' num2str(hyd_pump_eff*100) '% eff Hyd Pump']);
clear leg_str;
leg_str{1} = ['Mean Torque = ' num2str(mean(ShaftTorque(idx)),"%.1f") 'N-m' char(10) ...
              'Max Torque = ' num2str(max(ShaftTorque(idx)),"%.1f") ' N-m'];
legend(leg_str);
grid on;

elec_motor_eff = 0.6;
RPM = 2400;
SupplyVoltage = 36;
MotorCurrent = ShaftTorque*(RPM*2*pi/60)/elec_motor_eff/SupplyVoltage;
ah24 = subplot(4,1,4)
plot(time(idx),MotorCurrent(idx));
xlabel('Time (s)');
ylabel('A');
title(['Fig 2d: Motor Current required by electric motor at ' num2str(RPM) 'RPM, ' num2str(SupplyVoltage) 'Volts,  and ' num2str(elec_motor_eff*100) '% eff Elec Motor']);
clear leg_str;
leg_str{1} = ['Mean Current = ' num2str(mean(MotorCurrent(idx)),"%.1f") 'A' char(10) ...
              'Max Current = ' num2str(max(MotorCurrent(idx)),"%.1f") ' A'];
legend(leg_str);
grid on;






linkaxes([ah1 ah2 ah3 ah4 ah5 ah21 ah22 ah23 ah24],'x');
print('Fig2','-dpdf','-bestfit');
print('Fig2','-dpng');

figure('Position',[796   516   847   989]);
plot(PeakFlowRate,Volume,'x')
xlabel("Peak Flow Rate During Stoke (GPM)");
ylabel("Fluid Volume per stroke (in^3)");
grid on;
title("Fig 3: Volume versus Rate per stroke for Accumulator Sizing");

print('-dpdf','Fig3');
print('-dpng','Fig3');









% 
% 
% 
% 
% 
% 
% 
% 
% 
% OrigPitchPSD = load('Nov2019_PitchPSD.mat');
% 
% tt = OrigPitchPSD.dt*(0:length(OrigPitchPSD.Nav440Data.Pitch)-1);
% figure
% plot(time(idx),nav440Pitch(idx)-mean(nav440Pitch(idx)));
% hold all;
% plot(tt,OrigPitchPSD.Nav440Data.Pitch);
% xlim([2000 2200]);
% xlabel('time (s)');
% ylabel('Paragon Pitch (deg)');
% legend('Nov10\_2021','OriginalTest');
% 
% 
% dt = 1/50;
% %idx = 27000:length(nav440Pitch);
% [pxx,f] = pwelch(nav440Pitch(idx)-mean(nav440Pitch(idx)),1/dt)
% figure
% plot(f,pxx)
% hold all;
% plot(OrigPitchPSD.f,OrigPitchPSD.pxx);
% xlabel('Freq (hz)')
% ylabel('S  [sqrt(deg^2/Hz)]');
% xlim([0 .5]);
% title('Paragon Pitch PSD');
% legend('Nov10\_2021','OriginalTest');
% grid on;
