close all
clear all

filename = '2023.04.19T08.48.05';
%filename = 'logs/2022.12.13T09.24.40';
%filename = '2023.04.18T13.39.40';
%filename = '2023.04.19T06.18.54';


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;

pDither = data(:,j); j = j+1;
rDither = data(:,j); j = j+1;


t = t-t(1);


figure
plot(lon,lat);
% 
% Vnorth = [0 ; diff(lat)*60*3600./diff(t)];
% Veast = [0 ;cos(36*pi/180)*diff(lon)*60*3600./diff(t)];
% Vel = (Vnorth.^2+Veast.^2)^.5;
% 
% fh = figure('Position', [143 564 2104 934]);
% plot(t,Vel);
% xlabel('knots');


fh = figure('Position', [143 564 2104 934]);
ah1 = subplot(3,1,1)
plot(t,PitchTarget)
hold all
plot(t,Pitch)
plot(t,PrgnPitchAngle);
legend('Pitch Target','Pitch','Prgn Pitch');
xlabel('seconds');
ylabel('Degrees');
grid on
title("Pitch");


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


ah3 = subplot(3,1,3)
plot(t,PitchCmd);
hold all

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

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


fh = figure('Position', [143 564 2104 934]);
ah1 = subplot(3,1,1)
plot(t,RollTarget)
hold all
plot(t,Roll);
plot(t,PrgnRollAngle);
legend('Roll Target','Roll','Prgn Roll');
xlabel('seconds');
ylabel('Degrees');
grid on
title("Roll");

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


ah3 = subplot(3,1,3)
plot(t,RollCmd);
hold all

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

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

idx = find((t>2030) & (t<2360));
PitchAngularRate = [0; diff(Pitch(idx))./diff(t(idx))];

jdx = find((t>2660) & (t<2860));
RollAngularRate = [0; diff(Roll(jdx))./diff(t(jdx))];

figure
ah1 = subplot(2,1,1)
plot(PitchAngularRate(idx),PitchCmd(idx),'x')
ylabel('Pitch Command');
xlabel('Pitch Rate (deg/sec)');
grid on;

ah2 = subplot(2,1,2)
plot(RollAngularRate(idx),RollCmd(jdx),'x')
ylabel('Roll Command');
xlabel('Roll Rate (deg/sec)');
grid on;


