%------ read_motion_HiWinGS_2013.m

%   reads 10 Hz MotionPak data files from PSD flux system

%   These variables must exist in the current workspace
%       hhh: numerical value of current hour (00-23)
%       ddd: numerical value of current day of year (0-365)
%       yyyy: string variable of current year (e.g.'2014')
%       way_raw_data_flux: string path to data folder
%       clearit: sets clear flag

%   Script creates these arrays & variables:
%       accx, accy, accz, ratex, ratey, ratez: MotionPak data
%       slopeaccx: linear coefficients for trend in accx
%       G: acceleration of gravity from accelerometers

%% initialize
disp('read_motion_HiWinGS_2013.m')
fclose all;
hr = sprintf('%02i',hhh);
jd = sprintf('%03i',ddd);

% Full path & file name string for Windows
% dfl=[way_raw_data_flux, yyyy(3:4),jd,'\mot0' yyyy(3:4) jd hr '_raw.txt'];

% Full path & file name string for Mac/Unix
dfl = [way_raw_data_flux, yyyy(3:4),jd,'/mot0' yyyy(3:4) jd hr '_raw.txt'];
disp(dfl)
flist = fopen(dfl,'r'); % open text file

temp = {};              % cell array will hold one line of data, as read from file
z = [];                 % destination matrix for data read from file
good_flag = 1;

%% read raw text files
if flist>0                  % test if file exists
   while feof(flist)==0,    % read entire file into cell array
        try
            % temp will be 1x9 cell array, cells will be ~36000x1 arrays
            temp = textscan(flist,'%2f%2f%3f %f %f %f %f %f %f %*[^\n]','delimiter',', ','headerlines',1,'emptyvalue',NaN,'treatAsEmpty','Ship Motion');
            z = [z cell2mat(temp)'];  % 9x~36000 array
        catch
            for ii=1:9      % length of last cell reflects missing values from any/all fields
                if length(temp{1,ii})~=length(temp{1,9})
                    temp{1,ii}(length(temp{1,9})+1) = [];  %truncate length of all cells to length(temp{1,9})
                end;
            end;
            if ~isempty(temp{1,1});
                z = [z cell2mat(temp)'];  % dim 2 will be < 36000 here
            end;
        end;
   end;
   fclose(flist);
   tmotion = z(1,:)*60+z(2,:)+z(3,:)/1000; % raw time in s
   tref = 0:1/10:35999/10;                 % default 10 Hz timestamp, 1x36000
else        % no motion data so return NaN result arrays
    disp('file not found')    
    accx = NaN + zeros(1,36000);
    accy = NaN + zeros(1,36000);
    accz = NaN + zeros(1,36000);
    ratex = NaN + zeros(1,36000);
    ratey = NaN + zeros(1,36000);
    ratez = NaN + zeros(1,36000);
    slopeaccx = [NaN,NaN];
    good_flag = 0;
end;

%% process motion data unless no file found
if good_flag
    %% check for duplicate lines of data
    [b, m, n] = unique(tmotion);
    tmotion = tmotion(m);             % m are indices of unique elements in tmotion
    mot1 = z(:,m)';                   % mot1 is ~36000x1 array

    %% apply calibration constants & swap axes/change sign as needed
    %  As mounted, motion-pak is
    %  xacc=mot1(4) is + toward port
    %  yacc=mot1(5) is + toward bow
    %  zacc=mot1(6) 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 (psi)

    % So, we need to swap x and y and change the sign on z
    accx = (+mot1(:,8)+0)./3.7756.*9.80;         % forward is positive
    accy = (+mot1(:,7)+0)./3.8215.*9.80;         % to port is positive
    accz = (-mot1(:,9)+0)./3.7005.*9.80;         % up is positive
    ratex = (+mot1(:,5)+0)/0.025/180*pi;         % port up is positive phi
    ratey = (+mot1(:,4)+0)/0.025/180*pi;         % bow down is positive theta
    ratez = (-mot1(:,6)+0)/0.025/180*pi;         % bow to port is positive psi (right-handed)

    % figure;
    % subplot(3,2,1);plot(accx);title('accx');
    % subplot(3,2,2);plot(accy);title('accy');
    % subplot(3,2,3);plot(accz);title('accz');
    % subplot(3,2,4);plot(ratex);title('ratex');
    % subplot(3,2,5);plot(ratey);title('ratey');
    % subplot(3,2,6);plot(ratez);title('ratez');

    %% compute gravity and format arrays
    G=sqrt(mean(accx)^2+mean(accy)^2+mean(accz)^2);
    note = ['*** Gravity = ', num2str(G,'%6.5f')];
    disp(note);

    accx = accx';   % transpose to 1x~36000
    accy = accy';
    accz = accz';
    ratex = ratex';
    ratey = ratey';
    ratez = ratez';
    tmotion = tmotion';

    %% Interpolate no matter what length of data set, and screen for NaNs
    %  Fluctuating time series seems to create distortion issues and reduce
    %  effectiveness of motion correction.
    %  Result arrays are exactly 1x36000
    saccx = interp1(tmotion,accx,tref);    saccx(isnan(saccx)) = nanmean1(saccx);
    saccy = interp1(tmotion,accy,tref);    saccy(isnan(saccy)) = nanmean1(saccy);
    saccz = interp1(tmotion,accz,tref);    saccz(isnan(saccz)) = nanmean1(saccz);
    sratex = interp1(tmotion,ratex,tref);  sratex(isnan(sratex)) = nanmean1(sratex);
    sratey = interp1(tmotion,ratey,tref);  sratey(isnan(sratey)) = nanmean1(sratey);
    sratez = interp1(tmotion,ratez,tref);  sratez(isnan(sratez)) = nanmean1(sratez);

    accx = saccx;   % use interpolated result for motion corrections
    accy = saccy;
    accz = saccz;
    ratex = sratex;
    ratey = sratey;
    ratez = sratez;

    slopeaccx = polyfit(tref,accx,1);   % check coefs for linear fit to accx
end;

%% cleanup
if clearit
    clear saccx saccy saccz sratex sratey sratez mot1 z temp
    clear tmotion tref b m n dfl jd hr ans ii good_flag
end;

