close all
clear all

filename = 'logs/2023.01.17T13.36.20';  %Dockside tests

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



for i = 1:length(idx)

fh = figure('Position', [143 564 1559 934],'PaperOrientation','landscape','PaperPosition',[.25 .25 10.5 8]);
ah1 = subplot(3,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(['Pitch Motions  -  ' Name{i}]);




ah2 = subplot(3,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(3,1,3)
plot(t(idx{i}),PitchCmd(idx{i}));
grid on;
hold all;

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

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

fh = figure('Position', [143 564 1559 934],'PaperOrientation','landscape','PaperPosition',[.25 .25 10.5 8]);
ah1 = subplot(3,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(['Roll Motions  -  ' Name{i}]);

ah2 = subplot(3,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(3,1,3)
plot(t(idx{i}),RollCmd(idx{i}));
grid on;
hold all;

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

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

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

end
