%------ read_gps_HiWinGS_2013_bwb.m

%	reads Hemisphere gps data files from PSD flux system
%   this version reads reformatted files containing only $GPRMC lines
%   execution is much faster than previous version - avoids using importdata

%	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

%   Format for $GPRMC lines in the raw file:
% 0000078 $GPRMC,000000.00,A,5822.95924260,N,04515.92776422,W,1.58,223.96,151013,22.0,W,A*04
% 1 2 3                      4             5 6              7 8    9      10     11
% PSD timecodes              lat             lon              spd  hdg    

%   files are reformatted using a python script:
%{
        """HiWinGS PSD gps file parse: parseGpsFiles.py
        Parses data from Hemisphere gps files saved on the PSD data system.
        saves GPRMC lines with the following format to an output file.
        0000078 $GPRMC,000000.00,A,5822.95924260,N,04515.92776422,W,1.58,223.96,151013,22.0,W,A*04

        source file names follow the convention: gps0yydddhh_raw.txt
        output file names follow the convention: gprmyydddhh_raw.txt

        Using Anaconda iPython installation.
        Versions:anaconda 1.6.0, python 2.7.5

        Byron Blomquist, NOAA/ESRL/PSD3, Aug 2014
        """
        import os

        # assume current directory contains raw gps files,
        # dump list of data file names into a list variable
        files = [f for f in os.listdir('.') if f.startswith('gps')]

        # parse data files and save into txt files
        for file in files:
            # open output file, new name begins with gprmc
            outp = 'gprm'+file[4:]
            fgprmc = open(outp, 'w')
            print file
            fin = open(file)
            for line in fin:
                cleanLine = line.strip()
                lineOut = cleanLine + '\n'
                fields = cleanLine.split(',')
                if '$GPRMC' in fields[0]:
                    fgprmc.write(lineOut)

            print outp+' finished'

        fgprmc.close()
        # end
%}
%% initialize
% gps data is 10 Hz but file may not be exactly 36000 samples
jd=sprintf('%03i',ddd);
hr=sprintf('%02i',hhh);

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

disp(['Reading gps file  ',dfl])
flist=fopen(dfl);

temp={};    % empty arrays for textscan
stx=[];

%% load raw text files
if flist>0   % if file exists
   while feof(flist)==0,	% read entire file into cell array
        try
            % temp will be 1x11 cell array, cells will be ~36000x1 arrays
            temp = textscan(flist,'%2f%2f%3f %*6c %*9c %*c %f %c %f %c %f %f %f %f %*c %*4c %*[^\n]','delimiter',', ','headerlines',0,'emptyvalue',NaN);
            temp{5} = double(temp{5}); % will be ascii code for N or S
            temp{7} = double(temp{7}); % will be ascii code for E or W
            stx = [stx cell2mat(temp)'];  % 11x~36000 array
        catch
			for ii=1:11      % length of last cell reflects missing values from any/all fields
				 if length(temp{1,ii})~=length(temp{1,11})
					 temp{1,ii}(length(temp{1,11})+1)=[];  %truncate length of all cells to length(temp{1,11})
				 end;
			end;
			if ~isempty(temp{1,1}); % save good values from last read, if any
                temp{5} = double(temp{5});
                temp{7} = double(temp{7});
				stx=[stx cell2mat(temp)'];
			end;
        end; 
   end;
   fclose(flist);
else  % no file exists
    disp('file not found')
    stx=NaN+zeros(11,36000);
end;

t_gps=stx(1,:)*60+stx(2,:)+stx(3,:)/1000;	% 1x~36000 PSD timestamp in sec
tref=0:1/10:35999/10;                       % 1x36000 tref assumes exact 10 Hz timing

%% format variables
[b,m,n]=unique(t_gps);      % kill lines with duplicate time stamps
t_gps=t_gps(m);             % is this necessary?
stx=stx(:,m);

glt=stx(4,:); 
gpslat_sign=stx(5,:);       % ascii code for "N" or "S"
gln=stx(6,:); 
gpslon_sign=stx(7,:);       % ascii code for "W" or "E"
%gpsspeed1=stx(8,:).*.514;  % in m/s
gpsspeed1=stx(8,:);         % in knots
gpshead1=stx(9,:);          % deg

bb=floor(glt/100);          % convert lat format to decimal degrees
glt=(glt-bb*100)/60;
glt=glt+bb;
ii=find(gpslat_sign==83);glt(ii)=-glt(ii);
% glt(char(gpslat_sign)=='S')=-glt(char(gpslat_sign)=='S');
gpslat1=glt;

bbn=floor(gln/100);         % convert lat format to decimal degrees
gln=(gln-bbn*100)/60;
gln=gln+bbn;
ii=find(gpslon_sign==87);gln(ii)=-gln(ii);
% gln(char(gpslon_sign)=='W')=-gln(char(gpslon_sign)=='W');
gpslon1=gln;

%% despike variables - difference between successive points should be small
ii=find(abs(diff(gpslon1))>1);gpslon1(ii)=gpslon1(ii-1);
ii=find(abs(diff(gpslat1))>1);gpslat1(ii)=gpslat1(ii-1);
ii=find(gpsspeed1>100);gpsspeed1(ii)=NaN;
ii=find(gpshead1>400);gpshead1(ii)=NaN;

rdcon=pi/180;
gpsspeedx=gpsspeed1'.*cos(gpshead1'*rdcon);    % compute N spd component
gpsspeedy=gpsspeed1'.*sin(gpshead1'*rdcon);    % compute E spd component

%% interp if necessary
if length(stx)< 36000
        disp('<<<<<<<<<<<<<<<<<<  achtung >>>>>>>>>>>>>>>>>>>>>>>>')
    if ~isempty(gpslon1) && ~isempty(gpsspeedx)         % if short, interp to fill
        jd_ref=ddd+hhh/24+(0:1/10:35999/10)/60/60/24;   % exact 10Hz timestamp
        gpslon1=interp1(t_gps,gpslon1,jd_ref); ii=find(isnan(gpslon1)); 
        try 
            gpslon1(ii)=gpslon1(ii(end)+1); 
        catch
            disp('')
        end
        gpslat1=interp1(t_gps,gpslat1,jd_ref); ii=find(isnan(gpslat1));
        try 
            gpslat1(ii)=gpslat1(ii(end)+1); 
        catch
            disp ('');
        end
        gpsspeedxi=interp1(t_gps,gpsspeedx,jd_ref); ii=find(~isnan(gpsspeedxi));msu=mean(gpsspeedxi(ii)); 
        try 
            ii=find(isnan(gpsspeedxi));gpsspeedxi(ii)=msu; 
        catch
            disp (''); 
        end
        gpsspeedyi=interp1(t_gps,gpsspeedy,jd_ref); ii=find(~isnan(gpsspeedyi));msu=mean(gpsspeedyi(ii)); 
        try 
            ii=find(isnan(gpsspeedyi));gpsspeedyi(ii)=msu; 
        catch
            disp (''); 
        end
        
        gpsspeed1=sqrt(gpsspeedxi.*gpsspeedxi+gpsspeedyi.*gpsspeedyi);   % 10 Hz sog
        gpshead1=mod(atan2(gpsspeedyi,gpsspeedxi)/rdcon+360,360);        % 10 Hz hdg

        gpslat1(1)=gpslat1(2);
        gpslon1(1)=gpslon1(2);
    end;   
elseif length(gpsspeed1)> 36000     % else if long, truncate
    gpsspeed1=gpsspeed1(1:36000);
    gpshead1=gpshead1(1:36000);
    gpslat1=gpslat1(1:36000);
    gpslon1=gpslon1(1:36000);
end;

gpslon=gpslon1; 
gpslat=gpslat1; 
gpsspeed=gpsspeed1;
gpshead=gpshead1;
    
sog=gpsspeed1;		% in knots as used in programs afterward...!!!
cog=gpshead1;

%% compute ship N & E velocity components from sog & cog
Ngps=sog.*cos(cog./180.*pi)./1.946; % 1x36000 arrays
Egps=sog.*sin(cog./180.*pi)./1.946;
% shipn=sog.*cos(cog./180.*pi)./1.946;
% shipe=sog.*sin(cog./180.*pi)./1.946;
% tgyro=(0:2:3599);
% % Ngps=interp1(tgyro,shipn,tref,'spline','extrap');
% % Egps=interp1(tgyro,shipe,tref,'spline','extrap');
% Ngps=interp1(tgyro,shipn,tref);
% Egps=interp1(tgyro,shipe,tref);
%------correct for some bad points due to spline+extrap at the end.
% Ngps(tgyro(end)*10:36000)=Ngps(tgyro(end)*10);
% Egps(tgyro(end)*10:36000)=Egps(tgyro(end)*10);

%------filter ship velocities
fs=10;         %Sonic are output at 10 Hz
fc=1/200;      %Cutoff frequency for angle calculation
wp=1/25/(fs/2);
ws=0.8*wp;
% [n,wn]=buttord(wp,ws,10,25);
[n,wn]=buttord(wp,ws,3,7);
[bhi,ahi]=butter(n,wn,'low');
Ngps = filtfilt(bhi,ahi,Ngps);
Egps = filtfilt(bhi,ahi,Egps);

%% cleanup
if clearit
    clear gpslon1 gpslat1 gpsspeed1 gpshead1 glt gln gpslat_sign gpslon_sign jd_ref
    clear gpsspeedx gpsspeedy bb bbn headdata stx temp b m n j ii
    clear jd hr flist dfl ncols rdcon t_gps fs fc wp ws n wn bhi ahi
end;
