disp('read_motion_neaqs_GOMECC.m')


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

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

dfx=[dfx1 dfx2];

dfl=[way_rawdata ,'\GOMECC\day',dfx(1:3),'\P5_',dfx]; 
disp(dfl)
flist=fopen(dfl,'r');         %disp(dfl)
headr=fgetl(flist);           %disp(headr)
headr=fgetl(flist);
z=fscanf(flist,'%g,%g,%g,%g,%g,%g,%g',[7,inf]);

tm1=z(1,:);
tmotion=tm1;
clear tm1;
tmotion=tmotion-tmotion(1);
tmotion=0:1/10:(length(tmotion)-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(6,:)./3.75.*9.81;              %forward is positive 
accy=+z(5,:)./3.75.*9.81;              %to port is positive
accz=-z(7,:)./3.756.*9.81;              %up is positive
%ratex=+z(4,:)/0.049888/180*pi;          %port up is positive phi
%ratey=+z(3,:)/0.050343/180*pi;          %bow down is positive theta
%ratez=+z(5,:)/0.050084/180*pi;          %bow to starboard is positive psi
ratex=+z(3,:)/0.025/180*pi;          %port up is positive phi
ratey=+z(2,:)/0.025/180*pi;          %bow down is positive theta
%ratez=+z(5,:)/0.024985/180*pi;          %bow to starboard is positive psi (left-handed)
ratez=-z(4,:)/0.025023/180*pi;          %bow to port is positive psi (right-handed)

accx=accx.*1.017;
accy=accy.*1.017;
accz=accz.*1.017;

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;