close all
clear all

%filenames{1} = 'logs/2023.05.25T10.47.37';  
%filenames{1} = 'logs/2023.05.25T12.54.12';

%filenames{1} = 'logs/2023.05.25T13.29.35';
filenames{1} = 'logs/2023.05.25T13.34.56';

%filenames{1} = 'logs/2023.05.25T13.43.57';
%filenames{1} = 'logs/2023.05.25T15.35.05';
%filenames{2} = 'logs/2023.05.25T13.54.29';


for i = 1:length(filenames)
filename = filenames{i};

data = load(filename);
j = 1;
t = data(:,j); j = j+1;
status = data(:,j); j = j+1;
lat = data(:,j); j = j+1;
lon = data(:,j); j = j+1;
pitchCosFreq = data(:,j); j = j+1;
rollCosFreq = data(:,j); j = j+1;

pP = data(:,j); j = j+1;
pI = data(:,j); j = j+1;
pD = data(:,j); j = j+1;
pFF = data(:,j); j = j+1;
PrgnPitchAngle = data(:,j); j = j+1;
PrgnPitchAngleRate = data(:,j); j = j+1;
PrgnFiltPitchAngleRate = data(:,j); j = j+1;
PitchTarget = data(:,j); j = j+1;
Pitch = data(:,j); j = j+1;
PitchRate = data(:,j); j = j+1;
PitchCmd = data(:,j); j = j+1;

rP = data(:,j); j = j+1;
rI = data(:,j); j = j+1;
rD = data(:,j); j = j+1;
rFF = data(:,j); j = j+1;
PrgnRollAngle = data(:,j); j = j+1;
PrgnRollAngleRate = data(:,j); j = j+1;
PrgnFiltRollAngleRate = data(:,j); j = j+1;
RollTarget = data(:,j); j = j+1;
Roll = data(:,j); j = j+1;
RollRate = data(:,j); j = j+1;
RollCmd = data(:,j); j = j+1;

PitchDitherSignal = data(:,j); j = j+1;
RollDitherSignal = data(:,j); j = j+1;

t = t-t(1);

idx{1} = 1:length(t);


Name{1} = 'All';
Name{2} = 'Cross-Swell';
Name{3} = 'Up-Swell';
Name{4} = 'Down-Swell';

Name{5} = 'Cross-Swell Close Up View';
Name{6} = 'Up-Swell Close Up View';
Name{7} = 'Down-Swell Close Up View';


figure
latlon_idx = find((lat(idx{1}) > 30) & (lon(idx{1}) < -90));

plot(lon(latlon_idx),lat(latlon_idx));


xlabel('Longitude');
ylabel('Latitude');
grid on;
title(filename);


for i = 1:length(idx)

fh = figure('Position', [    363         263        1721         934],'PaperOrientation','landscape','PaperPosition',[.25 .25 10.5 8]);
ah1 = subplot(5,1,1)
plot(t(idx{i}),PitchTarget(idx{i}))
hold all
plot(t(idx{i}),Pitch(idx{i}))
plot(t(idx{i}),PrgnPitchAngle(idx{i}));
legend('Pitch Target',['Pitch: min/max = ' num2str(min(Pitch(idx{i})),"%.2f") '/' num2str(max(Pitch(idx{i})),"%.2f")],'Prgn Pitch');
xlabel('seconds');
ylabel('Degrees');
grid on

title([filename ' : Pitch Motions  -  ' Name{i}]);




ah2 = subplot(5,1,2)
plot(t(idx{i}),pP(idx{i}));
hold all
plot(t(idx{i}),pI(idx{i}));
plot(t(idx{i}),pD(idx{i}));
legend('Prop','Int','Der');
grid on
xlabel('seconds');


ah3 = subplot(5,1,3)
plot(t(idx{i}),PitchCmd(idx{i}));
grid on;
hold all;

xlabel('seconds');
ylabel('Valve Percentage')

ah4 = subplot(5,1,4)
GPM = abs(PitchCmd(idx{i})*4.0);
plot(t(idx{i}),GPM);
grid on;
hold all;
jdx = find((t(idx{i}) > 90) & (t(idx{i}) < 400));
legend(['Mean = ' num2str(mean(GPM(jdx))) ' GPM']);

xlabel('seconds');
ylabel('GPM')


ah5 = subplot(5,1,5)
plot(t(idx{i}),status(idx{i}) & 4);
grid on;
hold all;
plot(t(idx{i}),status(idx{i}) & 8);

legend('Deploy Request','Deploy Status');

xlabel('seconds');


linkaxes([ah1 ah2 ah3 ah4 ah5],'x');
print(['PitchMotions' Name{i}],'-dpdf');

fh = figure('Position', [    363         263        1721         934],'PaperOrientation','landscape','PaperPosition',[.25 .25 10.5 8]);
ah1 = subplot(4,1,1)
plot(t(idx{i}),RollTarget(idx{i}))
hold all
plot(t(idx{i}),Roll(idx{i}));
plot(t(idx{i}),PrgnRollAngle(idx{i}));
legend('Roll Target',['Roll: min/max = ' num2str(min(Roll(idx{i})),"%.2f") '/' num2str(max(Roll(idx{i})),"%.2f")],'Prgn Roll');
xlabel('seconds');
ylabel('Degrees');
grid on
title([filename ' : Roll Motions  -  ' Name{i}]);

ah2 = subplot(4,1,2)
plot(t(idx{i}),rP(idx{i}));
hold all
plot(t(idx{i}),rI(idx{i}));
plot(t(idx{i}),rD(idx{i}));
legend('Prop','Int','Der');
grid on
xlabel('seconds');


ah3 = subplot(4,1,3)
plot(t(idx{i}),RollCmd(idx{i}));
grid on;
hold all;

xlabel('seconds');
ylabel('Valve Percentage')


ah4 = subplot(4,1,4)
GPM = abs(RollCmd(idx{i})*2.0);
plot(t(idx{i}),GPM);
grid on;
hold all;
jdx = find((t(idx{i}) > 90) & (t(idx{i}) < 400));
legend(['Mean = ' num2str(mean(GPM(jdx))) ' GPM']);

xlabel('seconds');
ylabel('GPM')

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

print(['RollMotions' Name{i}],'-dpdf');

end


end