% read_motion
% VOCALS 2008 :: 2008-10-16 :: Simon de Szoeke
% Reads the ship motion data files for a day.

% SPdeS sped by preallocating arrays; textscan of 24*36000 rows is still
% slow.
disp(['read_motion: ', cruise, ' ', year]);
clearit=true;

% motion data is 10 Hz
nsampl=86400*10;
ncols=9;
jd=sprintf('%03i',ddd);
jd_mot=NaN+zeros(nsampl*1.01,1); % preallocate with tiny 1% buffer
mot1=NaN+zeros(nsampl*1.01,ncols);
%jhold=0; % never used
jax1=1;

for jam=00:23
    hr=sprintf('%02i',jam);
    dfl=[way_raw_data_flux, 'day',jd,'\mot0' year(3:4) jd hr '_raw.txt'];
    disp(dfl)
    flist=fopen(dfl,'r');         %disp(dfl)
    if flist>0
        clear temp z
        temp =textscan(flist,'%2f%2f%3f %f %f %f %f %f %f %*[^\n]','delimiter', ', ', 'headerlines', 1,  'emptyvalue', NaN,'treatAsEmpty', 'Ship Motion');
        z=cell2mat(temp); % SPdeS transposed
%         z(4:9,any(isnan(z),1))=NaN;      %put NaN into any rows containing NaNs (to remove bad lines, gap...)
       
        [nl1,nr1]=size(z);
        mot1(jax1:jax1+nl1-1,:)=z; % SPdeS transposed
        jd1=ddd + (jam+(mot1(jax1:jax1+nl1-1,1)+(mot1(jax1:jax1+nl1-1,2)+mot1(jax1:jax1+nl1-1,3)/1000)/60)/60)/24;
        jd_mot(jax1:jax1+nl1-1,:) = jd1;
        jax1=jax1+nl1;
        fclose(flist); % close files SPdeS
    else
        disp('file not found')
    end
end

% trim arrays to their filled size
mot1=mot1(1:jax1-1,:);
jd_mot=jd_mot(1:jax1-1,:);

% % Time in flux data files is the number of 100-nano second intervals since
% % Jan 1, 1601 divided by 10000. It was chosen to maintain compatibility
% % with an HP-UNIX version of the code form long ago
% %  0             'dd-mmm-yyyy HH:MM:SS'  
% %01-Mar-2000 15:45:17
% dates = datestr(datenum(1601,1,1) + datenum(mot1(1,:)*10000*100e-9/86400));
% hh = str2num(dates(:,13:14));
% mm = str2num(dates(:,16:17));
% ss = str2num(dates(:,19:20));
% jd_mot= ddd + (hh+((mm+(ss/60))/60))/24;
% 
% % test to see if first point is from the previous day
% if str2num(dates(1,1:2)) ~= str2num(dates(2,1:2))
%     jd_mot(1) = jd_mot(1)-1;
% end    

%  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=+mot1(:,8)./3.75.*9.81;               %forward is positive 
accy=+(mot1(:,7)-.1)./3.75.*9.81;          %to port is positive
accz=-mot1(:,9)./3.756.*9.81;              %up is positive

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

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

% accx=accx'; % SPdeS already transposed above
% accy=accy';
% ratex=ratex';
% ratey=ratey';
% ratez=ratez';

% trim off bad data at either end
for mm=length(accz):-1:1;
   if accz(mm)>50,
      accz(mm)=0;
   else
      break;
   end;
end;
for mm=1:1:length(accz);
   if accz(mm)>50,
      accz(mm)=0;
   else
      break;
   end;
end;

if clearit
    clear headr dfl G Filtn Cax Cax1 Cay Cay1 Caz Caz1 Crx Crx1 Cry Cry1 %hr jd
    clear Crz Crz1 FF ans ii %tmotion
end;

% zillions of points makes the display slow...
if plotit
    figure;
    subplot(3,2,1);plot(jd_mot,accx,'.','markersize',2);title(['accx (m.s^-^2). JD ' jd]);
    subplot(3,2,2);plot(jd_mot,accy,'.','markersize',2);title(['accy (m.s^-^2). JD ' jd]);
    subplot(3,2,3);plot(jd_mot,accz,'.','markersize',2);title(['accz (m.s^-^2). JD ' jd]);
    subplot(3,2,4);plot(jd_mot,ratex,'.','markersize',2);title(['ratex (rad/s). JD ' jd]);
    subplot(3,2,5);plot(jd_mot,ratey,'.','markersize',2);title(['ratey (rad/s). JD ' jd]);
    subplot(3,2,6);plot(jd_mot,ratez,'.','markersize',2);title(['ratez (rad/s). JD ' jd]);
    if prtit; print('-djpeg90 ',[way_images_flux '\motion_' jd '.jpg']); end
end