% WPwrite.m
% writes comma separated values(csv) of the format lat,long,alt to a binary
% .boat file for the B.O.A.T.
% Compatible with MATLAB and GNU Octave
clear all;
src_dir     = './Ross/';
src_name    = 'RossQuickTest420';
src_ext     = '.kml';
src_file    = strcat(src_dir,src_name,src_ext);

dest_dir    = src_dir;  
dest_name   = src_name;
dest_ext    = '.BOATVoyage';
dest_file   = strcat(dest_dir,dest_name,dest_ext);

% Read kml file for waypoints
[lon,lat,z] = read_kml(src_file);

% write lat and long data to single vector
vector(1:2:(2*length(lon))) = lat(1:end); % latitude
vector(2:2:(2*length(lon))) = lon(1:end); % longitude

% Calculate number of waypoints in vector
numWP   = length(vector)/2;

% Write number of waypoints to file as 32 bit unsigned integer
fid     = fopen(dest_file,'w');
numWPH = dec2hex(numWP,8);
for i = [length(numWPH):-2:2]
  fwrite(fid,hex2dec(numWPH(i-1:i)),'uint8');
end
fclose(fid);

% Append waypoints to file as 64 bit floating point numbers
fid     = fopen(dest_file,'a');
fwrite(fid,vector,'float64',0,'l');
fclose(fid);

% Calculate checksum of file so far
chksum  = Fletcher16(dest_file);

% Append checksum to end of file as 32 bit unsigned integer
fid     = fopen(dest_file,'a');
chksumH = dec2hex(chksum,4);
for i = [1:2:length(chksumH)]
  fwrite(fid,hex2dec(chksumH(i:i+1)),'uint8');
end
fclose(fid);

% Display a friendly message to brighten the user's day
display(['writing ' num2str(numWP) ' waypoints from ' num2str(src_name) num2str(src_ext) ' to ' num2str(dest_name) num2str(dest_ext)])

display(['checksum: ' num2str(chksum)]);



