close all
clear all

addpath('SpinCalc');
addpath('quaternions');

MatlabFilename = 'AllData.mat';
if(~exist('AllData'))
    AllData = load(MatlabFilename);
end


SegmentNames{1} = 'Monday';

for k = 1:length(SegmentNames)
    cmd = [SegmentNames{k} ' = load(''' SegmentNames{k} '.mat'');'];
    eval(cmd);
    
    cmd = ['Today = ' SegmentNames{k} ';'];
    eval(cmd);
    
    Today.RC_Qtn =qmult(Today.RC_Qtn,[0 0 1 0]);  %Rotate sensor 180 degrees to make the x-axis point forward.
    Today.RC_AngRate(:,1) = -Today.RC_AngRate(:,1); %Rotate sensor 180 degrees to make the x-axis point forward.
    Today.RC_AngRate(:,2) = -Today.RC_AngRate(:,2); %Rotate sensor 180 degrees to make the x-axis point forward.
    Today.EA = SpinCalc('QtoEA321',Today.RC_Qtn,.001,1);
    Today.t = (Today.Time_DateNum-Today.Time_DateNum(1))*24*60*60;
    
    idx = find(Today.EA(:,1) >180);
    Today.EA(idx,1) = Today.EA(idx,1)-360;
    
    idx = find(Today.EA(:,2) >180);
    Today.EA(idx,2) = Today.EA(idx,2)-360;
    
    idx = find(Today.EA(:,3) > 180);
    Today.EA(idx,3) = Today.EA(idx,3)-360; 
    
    figure('Position',[ 1000          44         974+20*k       1454+20*k]);
    ah1= subplot(9,1,1)
    plot(Today.t,Today.RC_Heading,Today.t,Today.RC_TargetHeading)
    legend('Actual Heading','Target Heading');
    ylabel('Heading (deg)');
    title(SegmentNames{k});
    ah2= subplot(9,1,2)
    plot(Today.t,Today.EA(:,1))
    ylabel('yaw');
    ah3= subplot(9,1,3)
    plot(Today.t,Today.EA(:,2))
    ylabel('pitch');
    ah4= subplot(9,1,4)
    plot(Today.t,Today.EA(:,3))
    ylabel('roll');
    ah5= subplot(9,1,5)
    plot(Today.t,Today.RC_AngRate*180/pi)
    ylabel('AngRate');
    legend('1','2','3');
    ah6 = subplot(9,1,6)
    plot(Today.t,-Today.RC_Pressure/1.5);
    ylabel('Depth');
    ah7 = subplot(9,1,7)
    plot(Today.t,Today.RC_Accel);
    ylabel('Accel (m/s)');
    legend('N','E','D');
    ah8 = subplot(9,1,8)
    plot(Today.t,Today.RC_NortekVel);
    ylabel('Vel (m/s)');
    legend('1','2','3');
    ah9 = subplot(9,1,9)
    Today.RamPosition = Today.RC_RamPosition-nanmean(Today.RC_RamPosition);
    plot(Today.t,Today.RC_RamPosition);
    ylabel('Flap Angle(deg)');
    linkaxes([ah1 ah2 ah3 ah4 ah5 ah6 ah7 ah8 ah9],'x');
    
    figure
    plot(Today.RC_TargetHeading,Today.RC_Heading,'x');
    xlabel('Target Heading');
    ylabel('Actual Heading');
    corrcoef(Today.RC_TargetHeading,Today.RC_Heading)
    
    
    cmd = [SegmentNames{k} ' = Today;'];
    eval(cmd);
    clear Today;
end

idx = 1:length(Monday.t);

data.RC_AngRate = Monday.RC_AngRate(idx,:);
data.Time_DateNum = Monday.Time_DateNum(idx);
data.RC_Accel = Monday.RC_Accel(idx,:);
data.EA = Monday.EA(idx,:);
data.t = Monday.t(idx);
data.FlapAngle = -(Monday.RC_RamPosition(idx)-nanmean(Monday.RC_RamPosition(idx)));

MaxVal = .95;
data.InValidVel_idx = find((abs(Monday.RC_NortekVel(:,1)) > MaxVal) ...
        | (abs(Monday.RC_NortekVel(:,2)) > MaxVal) ...
        | (abs(Monday.RC_NortekVel(:,3)) > MaxVal) );


data.RC_NortekVel = Monday.RC_NortekVel;    
data.RC_NortekVel(data.InValidVel_idx,:) = NaN;


data.RC_NortekVel = fillgaps(data.RC_NortekVel,500);

PlotVelocities(data);   
return;

PlotFFD; %(data);

return

idx = 1:1*60*10:length(Monday.Time_DateNum);
Monday.deltaLat = [0 ; diff(Monday.XB_Lat(idx))./(24*3600*diff(Monday.Time_DateNum(idx)))];  %Deg/s
Monday.deltaLon = [0 ; diff(Monday.XB_Lon(idx))./(24*3600*diff(Monday.Time_DateNum(idx)))];  %Deg/s

Monday.SOG_N = 1609.604*60*Monday.deltaLat;  %m/s
Monday.SOG_W = cos(39*pi/180)*1609.604*60*Monday.deltaLon;  %m/s
Monday.SOG = (Monday.SOG_N.^2+Monday.SOG_W.^2).^.5;
Monday.COG = (180/pi)*atan2(Monday.deltaLat,cos(39*pi/180)*Monday.deltaLon);
%idx = find(~isnan(Monday.SOG));
%N = 3*60*10;  %Average over 3 minutes.
%Monday.SOG = filtfilt(ones(1,N)/N,1,Monday.SOG(idx));
%Monday.COG = filtfilt(ones(1,N)/N,1,Monday.COG(idx));
figure
subplot(2,1,1)
plot(Monday.Time_DateNum(idx),Monday.SOG);
ylabel('GPS SOG (m/s)');
datetick('x');
subplot(2,1,2)
plot(Monday.Time_DateNum(idx),Monday.COG);
hold all
plot(Monday.Time_DateNum,Monday.RC_Heading,Monday.Time_DateNum,Monday.RC_TargetHeading);
legend('GPS COG','Plate Heading','Plate Target Heading');
ylabel('(deg)');
datetick('x');


figure
plot(AllData.XB_Lon,AllData.XB_Lat);
hold all
plot(Monday.XB_Lon,Monday.XB_Lat,'ro');
plot(Monday.XB_Lon,Monday.XB_Lat,'go');
plot(Thursday.XB_Lon,Thursday.XB_Lat,'ko');
plot(-121.9,36.783,'rx');
axis([-121.92 -121.8 36.74 36.88]);


figure
plot(AllData.Time_DateNum,AllData.PC_Power);
hold all
plot(Monday.Time_DateNum,Monday.PC_Power,'ro');
plot(Monday.Time_DateNum,Monday.PC_Power,'go');
plot(Thursday.Time_DateNum,Thursday.PC_Power,'ko');
datetick('x');
legend('All',['Monday: mean = ' num2str(nanmean(Monday.PC_Power)) ' W'],['Monday: mean = ' num2str(nanmean(Monday.PC_Power)) ' W'],['Thursday: mean = ' num2str(nanmean(Thursday.PC_Power)) ' W']);

figure
ah1 = subplot(4,1,1)
plot(Monday.RC_Pressure);
title('Monday');
ylabel('Pressure (psi)');
ah2 = subplot(4,1,2)
plot(Monday.RC_NortekVel);
ylabel('Nortek Vel (m/s)');
legend('1','2','3');
ah3 = subplot(4,1,3)
plot(Monday.RC_Accel);
ylabel('Accel (m/s^2)');
legend('1','2','3');
ah4 = subplot(4,1,4)
plot(Monday.RC_AngRate*180/pi);
ylabel('Ang Rate (deg/s)');
legend('1','2','3');
linkaxes([ah1 ah2 ah3 ah4],'x');


figure
ah1 = subplot(4,1,1)
plot(Monday.RC_Pressure);
title('Monday');
ylabel('Pressure (psi)');
ah2 = subplot(4,1,2)
plot(Monday.RC_NortekVel);
ylabel('Nortek Vel (m/s)');
legend('1','2','3');
ah3 = subplot(4,1,3)
plot(Monday.RC_Accel);
ylabel('Accel (m/s^2)');
legend('1','2','3');
ah4 = subplot(4,1,4)
plot(Monday.RC_AngRate*180/pi);
ylabel('Ang Rate (deg/s)');
legend('1','2','3');
linkaxes([ah1 ah2 ah3 ah4],'x');


figure
ah1 = subplot(4,1,1)
plot(Thursday.RC_Pressure);
title('Thursday');
ylabel('Pressure (psi)');
ah2 = subplot(4,1,2)
plot(Thursday.RC_NortekVel);
ylabel('Nortek Vel (m/s)');
legend('1','2','3');
ah3 = subplot(4,1,3)
plot(Thursday.RC_Accel);
ylabel('Accel (m/s^2)');
legend('1','2','3');
ah4 = subplot(4,1,4)
plot(Thursday.RC_AngRate*180/pi);
ylabel('Ang Rate (deg/s)');
legend('1','2','3');
linkaxes([ah1 ah2 ah3 ah4],'x');

figure
ah1 = subplot(3,1,1)
plot(Monday.Time_DateNum,Monday.RC_TargetHeading)
hold all
plot(Monday.Time_DateNum,Monday.RC_Heading);
ah2 = subplot(3,1,2)
plot(Monday.Time_DateNum,Monday.RC_TargetHeading)
hold all
plot(Monday.Time_DateNum,Monday.RC_Heading);
ah3 = subplot(3,1,3)
plot(Thursday.Time_DateNum,Thursday.RC_TargetHeading);
hold all
plot(Thursday.Time_DateNum,Thursday.RC_Heading);



