close all
clear all

data = load('Nov3_5GPM_2_machine.txt');

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

figure('Position',[    54         240        1278        1262]);
ah1 = subplot(5,1,1)
plot(time,PitchTarg);
hold all
plot(time,Pitch);
legend('Pitch Target','Pitch');
ylabel('Degrees');
xlabel('time (sec)');
grid on;
ylim([-10 10]);

ah2 = subplot(5,1,2)

plot(time,PitchVel);
hold all
plot(time,nav440PitchRate);
plot(time,nav440FiltPitchRate);

legend('Pole Pitch Velocity','Prgn Pitch Velocity','Prgn  Pitch Velocity (filtered)');
xlabel('time (sec)');
ylabel('deg/sec');
grid on;

ah3 = subplot(5,1,3);
plot(time,Vout);
ylabel('Valve Command Voltage (V)');
xlabel('time (sec)');
grid on;

ah4 = subplot(5,1,4);
plot(time,PropGain);
hold all;
plot(time,IntGain);
plot(time,DerGain);
plot(time,FFGain);
legend('Prop Gain','Int Gain','Der Gain','Vel FF Gain');
xlabel('time (sec)');
grid on;

ah5 = subplot(5,1,5);
plot(time,Freq);
hold all;
xlabel('time (sec)');
ylabel('Freq (Hz)');
grid on;

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


epsilon = .0001;

A= [5 1.79];
for ii = 1:length(A);

 if(ii == 1)
    f = 0.05:.01:.25;
 end
    
 if(ii == 2)
    f = 0.1:.01:.49;
 end
 
 jdx = find((Amp > A(ii)-epsilon) & (Amp < A(ii)+epsilon) & (Freq > f(1)-epsilon) & (Freq < f(end)+epsilon));
 [pxx,ff] = pwelch(PitchTarg(jdx),1/dt);
figure
plot(ff,pxx)
xlabel('Freq (hz)')
ylabel('S  [sqrt(deg^2/Hz)]');
xlim([0 .5]);
title('Pitch PSD');
 
 
idx = [];
jdx = [];
for i = 1:length(f)
   jdx = find((Amp > A(ii)-epsilon) & (Amp < A(ii)+epsilon) & (Freq > f(i)-epsilon) & (Freq < f(i)+epsilon));
   m = 1;
   crossing = [];
   for k = jdx(1):jdx(end)-1
      if((PitchTarg(k) <= 0) & (PitchTarg(k+1) > 0))  %Upcrossing
          crossing(m) = k;
          m = m+1;
      end
   end
   idx{i} = crossing(1):crossing(end);
end
    
    


figure
ah1 = subplot(2,1,1);
plot(time,PitchTarg)
hold all
for i = 1:length(idx)
    plot(time(idx{i}),PitchTarg(idx{i}),'kx');
    plot(time(idx{i}),Pitch(idx{i}),'r-');
end
grid on
ah2 = subplot(2,1,2);
plot(time,Freq);
linkaxes([ah1 ah2],'x');


for i = 1:length(idx)
   MeasuredRMS(i) = rms(Pitch(idx{i}));
   MeasuredAmp(i) = sqrt(2)*MeasuredRMS(i);
   [Xa,Ya,Phase_D(i)] = alignsignals(Pitch(idx{i}),PitchTarg(idx{i}));
   Phase_t(i) = Phase_D(i)*dt;
   Phase_deg(i) = 360*(Phase_t(i)*f(i));
end

figure
ah1 = subplot(2,1,1);
plot(f,MeasuredAmp/A(ii),'-x');
xlabel('Frequency (Hz)');
ylabel('Amplitude Ratio (meas/targ)');
grid on;
title(['Target Amplitude = ' num2str(A(ii)) ' deg']);
ah2 = subplot(2,1,2);
plot(f,Phase_deg,'-x');
xlabel('Frequency (Hz)');
ylabel('Phase (deg)');
grid on;

end

