%% get_headcorr.m
%%
%% Example script for making heading correction from VmDAS data - designed
%% to be cut-and-paste to run. WILL need changes for individual
%% data sets, depending on the quality of the ancillary heading,
%% the linearity of the time stamps, availability of quality flags etc.
%% Easy to run it all in matlab (under bash) even though there are some
%% python steps for initial data parsing - run in directory cal/rotate
%%
%% Jules Hummon 1Mar2006; LB annotations

%% ------------ section for user to edit --------------
%% locations of N2R and ENS files
!n2rdir=/Mahe/sadcp_processing/RonBrown/data/rb0505
ensdir='/Mahe/sadcp_processing/RonBrown/data/rb0505';

headcorr_device = 'mahrs';
cruiseid = 'rb0505';

%% need to use *.tem file for timestamps that match CODAS database 
ddayfile = '../../edit/a0505.tem';

%% pick an output anglefile name
angfile = 'head_corr.ang';
%% -------- end of section for user to edit -----------



%% -- Making rbins files with headings for loading into matlab ---

%% first, N2R files for secondary heading, which are parsed OUTSIDE matlab
cd cal/rotate
!mkdir rbin 
cd rbin
!serasc2bin.py -rv -y 2005 -t vmdas -c 'last' -m hdg  $n2rdir/*N2R
%% creates files called *.hdg.rbin which contains secondary heading

%% Second, ENS files for gyro
%% get a matlab structure of directory contents and sort the files 
%% into numerical order 
ensfiles = dirs(fullfile(ensdir,'*.ENS'));    
% convert each file into rbins
for filei = 1:length(ensfiles)
  filename = ensfiles(filei).name
  ensgyro2rbin(os,fullfile(ensdir,filename));
end
% creates files called *.ehd.rbins which contain gyro data
%% -----------------------------------------------------


%% ----- Loading up heading rbins files into matlab -------

%% get gyro - first list *.ehd.rbin files
gyrolist = dirs('rbin/*.ehd.rbin', 'fullfile',1)
% extract specific information (rowname, u_dday etc) from each of
% the gyro files
%% (we will use u_dday, which is COMPUTER time, since it is the only 
%% common timestamp between gyro and secondary heading)
gyro_trinfo = tr2ilist(gyrolist, 'rowname', 'u_dday','sort','time');
% loop through the cell array and write to a structured array
[gdata, gstruct] = tr2ilist(gyro_trinfo);
%% get secondary heading (e.g. MAHRS)
otherlist = dirs(sprintf('rbin/*.hdg.rbin'), 'fullfile',1)
other_trinfo = tr2ilist(otherlist, 'rowname', 'u_dday','sort','time');
[odata, ostruct] = tr2ilist(other_trinfo);
%% -----------------------------------------------------


%% ------------- Fixing time stamps --------------------

%% Check that COMPUTER times are monotonic. Can have problems
%% with non-monotonic time stamps if clock corrections were used
%% to make PC time shift periodically to UTC.
dt = diff(gdata(1,:));
gbadi = find(dt <= 0);
fprintf('there are %d nonmonotonic gyro time stamps\n', length(gbadi))
dt = diff(odata(1,:));
obadi = find(dt <= 0);
fprintf('there are %d nonmonotonic other time stamps\n', length(obadi))

%% Plot gyro and secondary heading time series
clf; subplot(211)
plot(gdata(1,:),gdata(r_row(gstruct, 'heading'),:),'g.-')
hold on
plot(odata(1,:),odata(r_row(ostruct, 'heading'),:),'r-')
yl(0,360);
legend('gyro','secondary')
xlabel('PC dday')
ylabel('heading')


%% Plot difference between PC time and UTC time (UTC has come from
%% ENS file thru VmDAS via NMEA GGA messages) and correct time stamp
subplot(212)
plot(gdata(1,:), 86400*(gdata(1,:) - gdata(2,:)),'.')
xlabel('PC dday')
ylabel('Seconds: PC-UTC')
%% Need to correct u_dday (PC clock) to dday (UTC) - typically
%% there will be an offset and a drift - sometimes, as on the Ron
%% Brown, there will be spikes where the VmDAS system was overwhelmed,
%% due to all the serial input from the MAHRS, and held onto the same 
%% NMEA time stamp for a few seconds
goodi = find(~isnan(gdata(1,:) + gdata(2,:)));
coef = polyfit(gdata(1,goodi), 86400*(gdata(1,goodi) - gdata(2,goodi)),1);
tt = [min(gdata(1,goodi)): .1 : (max(gdata(1,goodi)) +.1)];
%% Plot and have a look
hold on
plot(tt, polyval(coef, tt),'k-')
legend('PC-UTC','linear fit')
%% if fit looks okay, replace "u_dday" with a corrected version for UTC
gdata(1,:) = gdata(1,:) -  polyval(coef,gdata(1,:))/86400;
odata(1,:) = odata(1,:) -  polyval(coef,odata(1,:))/86400;
%% -----------------------------------------------------


%% ---------- Actual Heading correction ---------------

%% mk_avgdh reads in gdata and odata which were created
%% from tr2ilist above, and calculates the difference
%% between gyro and 'hdg' on one-minute averages
[o_tdh,outdata] = mk_avgdh(gdata, gstruct, odata, ostruct, 'hdg', ...
       'logfile', 'rbin.log');

%%% o_tdh is a structured array containing the heading 
%% correction 'ang', (and median correction 'meddh'). 'ang' is
%% clean IF quality flags were available from secondary
%% heading instrument, e.g indicators ('numgood' and 'badmask')
%% Heading devices with QC are: ASHTECH, POSMV, SEAPATH

%% Let's check that the means of the differences and the
%% differences of the means are the same - spikey hdg data
%% could cause discrepancies in the heading correction
%% (shift everything to +/-180)
olddh = rem(o_tdh.meangyro - o_tdh.mean_other+720+20,360)-20;

%% compare heading correction with the differences of the 
%% one-minute means
figure; subplot(211)
plot(o_tdh.dday, o_tdh.ang,'g')
hold on
plot(o_tdh.dday, olddh,'k')
xlabel('decimal day')
ylabel('gyro - other (degs)')
grid
title('Is heading correction good?')
legend('Head Corr','diff of means',2);
%% -----------------------------------------------------


%% ------------ Cleaning up heading correction ------------

%% !! If there are no quality indicators for secondary heading !!
%% e.g. MAHRS, PHINS, OCTANS and can see some spikes, then need to 
%% filter the data (boxfilt) and remove outliers (cleaner).

%% THIS PART OF THE CODE IS MEANT AS AN EXAMPLE AND WILL NEED EDITING
%% TO GET THE BEST RESULTS

%cleandh = o_tdh.ang;
cleandh = olddh;
%smooth_stddh =  boxfilt(cleaner(o_tdh.stddh,1,3),15);
smooth_dh =  boxfilt(cleaner(o_tdh.ang,1,3),15);
%badi = find(abs(olddh-mstdgap(olddh)) > 50);
%badi = [badi find(abs(o_tdh.stddh - smooth_stddh) > .1)];
badi = [badi find(abs(o_tdh.ang - smooth_dh) > .1)];
cleandh(badi) = NaN;
cleandh = fillends(cleandh);
cleandh = boxfilt(cleandh,7);
subplot(212)
plot(o_tdh.dday,o_tdh.ang,'g')
hold on
plot(o_tdh.dday,cleandh,'r')
grid
xlabel('decimal day')
ylabel('gyro - other (degs)')
legend('Head Corr','diff of means',2);
legend('Head Corr','clean Head Corr',2);

%hh=mtext(.35,.15,'no QC');click_rgbk(hh,'c','g')
%hh=mtext(.35,.25,'no QC, filtered');click_rgbk(hh,'c','r')

%% when satisfied with heading correction, save it to o_tdh
o_tdh.ang = cleandh;
%% -----------------------------------------------------


%% -- Interpolate heading correction onto CODAS times --
%% ----------- and write out to angfile ----------------

%% use the *.tem file to get time stamps
temp = load(ddayfile);
enddday = temp(:,1);
dh = table1([o_tdh.dday(:) o_tdh.ang(:)], enddday);
ddaydh = fillends([enddday dh], 'dir','col');

%% Open the angle file for writing - this is the file that
%% gets used by rotate
fid = fopen(angfile,'w');
fprintf(fid, '%10.7f   %5.3f\n', ddaydh');
fclose(fid);
%% -----------------------------------------------------


%% ----- Save and view the final heading correction ----
%% ----------------- and statistics --------------------

o_tdh.badmask(badi) = 1;
fid=fopen('hcorr_avgdh.asc','w');
   fprintf(fid, '%% headings listed are original heading device\n');
   fprintf(fid, '%% sign is correct for use with "rotate"\n');
   fprintf(fid, '%% enddday, mean head, last head, dhfix, stddh, ndh badmask(1is bad)\n');
%% use same variable now that we're done with it
   alldh = [o_tdh.dday(:) ...
            o_tdh.meangyro(:) ...
            o_tdh.lastgyro(:) ...
            o_tdh.ang(:) ...
            o_tdh.stddh(:) ...
            o_tdh.numgood(:)...
            o_tdh.badmask(:)];
   fprintf(fid, '%10.7f   %5.3f     %5.3f    %5.3f   %5.3f   %4d %4d\n', alldh');
fclose(fid);

% more book keeping
cfg.okcutoff         = [];
cfg.stdcutoff        = [.01 1];
cfg.abs_dh_cutoff    = 10;
cfg.ascfile = 'hcorr_avgdh.asc';
cfg.time_angle_file = angfile;
cfg.showverbose = 0;
cfg.interval = 120;
cfg.logfile = 'mk_gyrodh.log';
cfg.yearbase = 2005;
cfg.cruiseid = cruiseid;
cfg.stats_ddrange = [-100 500];
statsfile = 'gyrodh_stats.txt';
plot_basename = 'gyrodh_stats';
gps_attname = headcorr_device;
uhdas = 0;

%% make statistics from file
print_hcorstats('yearbase', cfg.yearbase,...
                    'stats_ddrange', cfg.stats_ddrange,...
                    'hcorfile', cfg.ascfile,...
                    'comment', gps_attname,...
                    'outfile', statsfile);

cfg.plot_ddrange = gdata(1,[1 end]);
plot_hcorstats( 'comment', cfg.cruiseid,...
                    'outfilebase', plot_basename ,...
                    'plot_ddrange', cfg.plot_ddrange,...
                    'plottype', 'fancy',...
                    'gps_attname', gps_attname,...
                    'yearbase', cfg.yearbase,...
                    'hcorfile', cfg.ascfile,...
                    'cruiseid', cfg.cruiseid);

%% end of file