
index = find(dataTime~=0);
dateVector = zeros(size(quat,1),1);
if(exist('pressure'))
    pressVector = zeros(size(quat,1),1);
end

if (size(index,2)==1),  %if there is only one time stamp in the record
    %calculate the start time in epoch seconds
    FirstTimeStampEpochSec=(dataTime(index(1))-datenum(1970,1,1))*86400;
    %for the spaces between time stamps use the known sampling rate to
    %calculate the time in epoch seconds
    for j=1:index(1)-1,
        k= FirstTimeStampEpochSec-Ts*(index(1)-j);
        dateVector(j,1)=(k/86400)+datenum(1970,1,1);
    end
    LastTimeStampEpochSec=(dataTime(index(end))-datenum(1970,1,1))*86400;
     
    %calculate the last time stamp in the file
    for j=1:size(dateVector,1)-index(end)+1,
        k= LastTimeStampEpochSec+Ts*j;
        dateVector(index(end)+j-1,1)=(k/86400)+datenum(1970,1,1);
    end
else    %there are multiple time stamps in the file
    
    FirstTimeStampEpochSec=(dataTime(index(1))-datenum(1970,1,1))*86400;
    %using sampling rate create time stamps in epoch seconds for the
    %records before the first time stamp is recorded
    for j=1:index(1)-1,
        k= FirstTimeStampEpochSec-Ts*(index(1)-j);
        dateVector(j,1)=(k/86400)+datenum(1970,1,1);
    end
    %this linerarly interpolated between the two time stamps for the data
    %not at the start or tail of the file
    for i= 1:size(index,2)-1,
        
        dateVector(index(i):index(i+1)-1,1)=linspace(dataTime(index(i)),dataTime(index(i+1)),index(i+1)-index(i))';
        
    end
    %calculate epoch sec value of last time stamp
    LastTimeStampEpochSec=(dataTime(index(end))-datenum(1970,1,1))*86400;
    
    %this fills in time stamps for the data from the last written time
    %stamp through the end of the file.
    for j=1:size(dateVector,1)-index(end)+1,
        k= LastTimeStampEpochSec+Ts*j;
        dateVector(index(end)+j-1,1)=(k/86400)+datenum(1970,1,1);
    end
end
% Index pressure data here
if(exist('pressure'))
    index = find(pressure~=0);
    
    
    %     for i= 1:size(index,2),
    %         if i==1
    %             pressVector(i:index(i+1)-1,1)= zeros(size(index(i+1)))';
    %         elseif i==size(index,2)
    %             %   dateVector(index(i):end-1,1)=zeros(size(end-index(i)))';
    %         else
    %             pressVector(index(i):index(i+1)-1,1)=linspace(pressure(index(i)),pressure(index(i+1)),index(i+1)-index(i))';
    %         end
    %     end
    % else
    %     pressVector=zeros(size(dateVector,1),1);
    
    
    if (size(index,2)==1),
        for j=1:index(1)-1,
            pressVector(j,1)=NaN;
        end
        
        pressVector(index(1),1)=pressure(index(1));
        pressVector(index(1)+1:end,1)=NaN;
    else
        
        for j=1:size(index,2),
            if j==1,
                pressVector(1:index(j)-1,1)=NaN;
                pressVector(index(1),1)=pressure(index(1));
            else
                pressVector(index(j-1)+1:index(j)-1,1)=NaN;
                pressVector(index(j),1)=pressure(index(j));
            end
            pressVector(index(j)+1:end,1)=NaN;
        end
    end
    if size(pressure,2)>size(acc_wrld',2)-2,
        quat=[quat;(NaN*ones(size(pressure,2)-size(quat',2),4))];
        acc_wrld=[acc_wrld;(NaN*ones(size(pressure,2)-size(acc_wrld',2),3))];
        dateVector=[dateVector;(NaN*ones(size(pressure,2)-size(dateVector',2),1))];
    else
    end
end

if(exist('pressure','var'))

AllData=[dateVector,acc_wrld,quat,pressVector];
else
    AllData=[dateVector,acc_wrld,quat,zeros(size(quat,1),1)];
    
end

ZeroIndex=find((AllData(:,2)==0)&(AllData(:,3)==0)& (AllData(:,4)==0));
% AllData(ZeroIndex,:)=[];
% b=find(AllData(:,1)==0);
% AllData(b,:)=[];
AllData(ZeroIndex,2:8)=NaN;
b=find(AllData(:,1)==0);
AllData(b,2:8)=NaN;
c=find(AllData(:,9)==0);
AllData(c,9)=NaN;


% AllData =[time (matlab datenum),AccX,AccY,AccZ, Quat(1:4)]
%X is 1, Y is 2, Z is 3
%acc_wrld(i) = a'

figure('Name', 'Accelerations');
plot(AllData(:,1),AllData(:,2),'r',AllData(:,1),AllData(:,3),'g',AllData(:,1),AllData(:,4),'b');
legend('x','y','z');
xlabel('Time (mm:ss)');
ylabel('Acceleration (g)');
title([fileName ' Accelerations (g)  ' datestr(AllData(1,1))]);

datetick('x','HH:MM:SS','keepticks','keeplimits')

s=strtok(fileName,'.');
figureName=strcat('Acc','_',s);
saveas(gcf,strcat(figureName,'.tiff'));

euler=[];
euler=quatern2euler(quaternConj(AllData(:,5:8))); % use conjugate for sensor frame relative to Earth

figure('Name', 'Euler Angles');
hold on;
plot(AllData(:,1), euler(:,1).*180/pi, 'r'); %phi
plot(AllData(:,1), euler(:,2).*180/pi, 'g');  %theta  
plot(AllData(:,1), euler(:,3).*180/pi, 'b');    %psi
datetick('x','MM:SS','keepticks','keeplimits')

title('Euler angles');
xlabel('Time (mm:ss)');
ylabel('Angle (deg)');

% phi is a rotation around X, theta around Y and psi around Z
legend('phi', 'theta', 'psi');
title([fileName ' Euler Angles  ' datestr(AllData(1,1))]);
hold off;
figureName=strcat('Euler','_',s);
saveas(gcf,strcat(figureName,'.tiff'));



% figure('Name', 'Pressure');
% hold on;
% plot(AllData(:,1), AllData(:,9)*10, 'r');
% datetick('x','MM:SS','keepticks','keeplimits')
%
% title('Pressure (decibar)');
% xlabel('Time (mm:ss)');
% ylabel('Decibar');
% hold off;
%
% figureName=strcat('Press','_',s);
% saveas(gcf,strcat(figureName,'.tiff'));

if(exist('pressure'))

figure('Name', 'Depth');
hold on;
plot(AllData(:,1),((((AllData(:,9))*2-0.5)*70/4)-1)*10,'rx');
datetick('x','MM:SS','keepticks','keeplimits')

title([fileName ' Pressure (decibar)  ' datestr(AllData(1,1))]);
xlabel('Time (mm:ss)');
ylabel('Decibar');
hold off;
figureName=strcat('Depth','_',s);
saveas(gcf,strcat(figureName,'.tiff'));
end
[t,r]=strtok(fileName,'.');
fileType=strtok(r,'.');
a=strcat(t,'_',fileType,'.mat');
save(a,'AllData');
