disp('read_motion_AMMA_2008.m')

if hhh<10,
   hr=['0',num2str(hhh)];
else
   hr=num2str(hhh);
end;

if ddd<10,
   jd=['00',num2str(ddd)];
elseif ddd<100,
   jd=['0',num2str(ddd)];
else
   jd=num2str(ddd);
end;


dfx=[jd hr];
z=[];temp=[];
dfl=['C:\Data\AMMA\day',jd,'\mot008',jd, hr, '_raw.txt'];
disp(dfl)
flist=fopen(dfl,'r');         %disp(dfl)
% dmp2=fgets(flist);


if flist>0
   while feof(flist)==0,

        try    
            temp =textscan(flist,'%2f%2f%3f %f %f %f %f %f %f %*[^\n]','delimiter', ', ','headerlines',1,'emptyvalue',NaN);
            z=[z cell2mat(temp)'];
%             z(4:9,any(isnan(z),1))=NaN;      %put NaN into any rows containing NaNs (to remove bad lines, gap...)
        catch 
         for ll=1:9
             if length(temp{1,ll})~=length(temp{1,9})
                 temp{1,ll}(length(temp{1,ll}))=[];
             end;
         end;
         
         if isempty(temp{1,1});
             temp{1,9}=temp{1,9}';
         end;
         z=[z cell2mat(temp)'];
        end;     
    end;
else
    disp('file not found')
end;
% if flist<0
%     z=zeros(9,36000);
% end
z(:,any(isnan(z),1))=[]; %remove lines containing NaNs (bad lines, too long...)
motionlength=length(z)
tmotion=z(1,:)*60+z(2,:)+z(3,:)/1000; %real raw time in s
% if tmotion(1)==0
%     tmotion=0:0.1:3599.9;
% end;
tref=0:1/10:35999/10;

% tref=tmotion;
% tmotion=0:1/10:(length(z)-1)/10;  
% tmotion=tmotion';

%  As mounted:  motion-pak
%  Xm is toward port
%  Ym is toward bow
%  Zm is down

%  We desire the coordinate system such that
%  +X is toward bow
%  +Y is toward port
%  +Z is up
%  +rotation about the x-axis is port up (phi = roll) 
%  +rotation about the y-axis is bow down (theta = pitch)
%  +rotation about the z-axis is bow to port

accx=+z(8,:)./3.75.*9.81;               %forward is positive 
accy=+(z(7,:)-.1)./3.75.*9.81;          %to port is positive
accz=-z(9,:)./3.756.*9.81;              %up is positive

ratex=+z(5,:)/0.025/180*pi;          %port up is positive phi
ratey=+z(4,:)/0.025/180*pi;          %bow down is positive theta
ratez=-z(6,:)/0.025023/180*pi;       %bow to port is positive psi (right-handed)

try   
    accx=despike_gasexIII(accx);
    accy=despike_gasexIII(accy);
    accz=despike_gasexIII(accz);
    ratex=despike_gasexIII(ratex);
    ratey=despike_gasexIII(ratey);
    ratez=despike_gasexIII(ratez);
catch
    accx=accx;
    accy=accy;
    accz=accz;
    ratex=ratex;
    ratey=ratey;
    ratez=ratez;
end;
%%%%  fixe problems due to threshold issues in accz, and ratez... %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
accxi=accx;tmotioni=tmotion;
accyi=accy;
acczi=accz;
ratexi=ratex;
rateyi=ratey;
ratezi=ratez;



% ii=find(abs(diff(ratez))>0.01);
% ratez(ii)=ratez(ii-2);
% % aa=[];
% % for jj=1:length(ii)
% %     a=ii(jj)-1:ii(jj)+1;
% %     aa=[aa a];
% % end;
% % 
% % iil=unique(aa);
% % 
% % 
% % acczl=accz;tmotionl=tmotion;
% % acczl(iil)=[];tmotionl(iil)=[];
% % 
% % % accz(ii(1)-5:ii(1)+15)= spline(tmotion(ii(1)-5:ii(1)+15),accz(ii(1)-5:ii(1)+15),tmotioni(ii(1)-5:ii(1)+15));

saccx=spline(tmotion,accx,tref);
saccy=spline(tmotion,accy,tref);
saccz=spline(tmotion,accz,tref);
sratex=spline(tmotion,ratex,tref);
sratey=spline(tmotion,ratey,tref);
sratez=spline(tmotion,ratez,tref);


% saccz1=interp1(tmotioni,acczi,tref,'spline','extrap');


G=sqrt(mean(accx)^2+mean(accy)^2+mean(accz)^2);

fclose(flist);

Lenmotion=36000;

accx=saccx;
accy=saccy;
accz=saccz;
ratex=sratex;
ratey=sratey;
ratez=sratez;

%%%%%%%%%  file hhh=oo always starts late by 6-7s. Fix the interpolation issues at beginning of hhh 00 %%%%%%%%%%%%%%%%%%%%%%
if hhh==00
    t1=floor(tmotion(1)*10);
    accx(1:t1+5)=accx(t1+10);
    accy(1:t1+5)=accy(t1+10);
    accz(1:t1+5)=accz(t1+10);
    ratex(1:t1+5)=ratex(t1+10);
    ratey(1:t1+5)=ratey(t1+10);
    ratez(1:t1+5)=ratez(t1+10);
end;    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  

% 
% Cdref=load('C:\Data\CRUISES\GasExIII\data\CorrectPbsMotocrr.prn');
% ii=find(ddd==Cdref(:,1) & hhh==Cdref(:,2));
% for ll=1:length(ii)
%     accx(Cdref(ii(ll),3):Cdref(ii(ll),4))=nanmedian(setdiff(accx,accx(Cdref(ii(ll),3):Cdref(ii(ll),4))));
%     accy(Cdref(ii(ll),3):Cdref(ii(ll),4))=nanmedian(setdiff(accy,accy(Cdref(ii(ll),3):Cdref(ii(ll),4))));
%     accz(Cdref(ii(ll),3):Cdref(ii(ll),4))=nanmedian(setdiff(accz,accz(Cdref(ii(ll),3):Cdref(ii(ll),4))));
%     ratex(Cdref(ii(ll),3):Cdref(ii(ll),4))=nanmedian(setdiff(ratex,ratex(Cdref(ii(ll),3):Cdref(ii(ll),4))));
%     ratey(Cdref(ii(ll),3):Cdref(ii(ll),4))=nanmedian(setdiff(ratey,ratey(Cdref(ii(ll),3):Cdref(ii(ll),4))));
%     ratez(Cdref(ii(ll),3):Cdref(ii(ll),4))=nanmedian(setdiff(ratez,ratez(Cdref(ii(ll),3):Cdref(ii(ll),4))));
% end;

try   
    accx=despike_gasexIII(accx);
    accy=despike_gasexIII(accy);
    accz=despike_gasexIII(accz);
    ratex=despike_gasexIII(ratex);
    ratey=despike_gasexIII(ratey);
    ratez=despike_gasexIII(ratez);
catch
    accx=accx;
    accy=accy;
    accz=accz;
    ratex=ratex;
    ratey=ratey;
    ratez=ratez;
end;

tmotion=0:1/10:35999/10;
