%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ConfigWrite.m - Write Configuration file for BOAT 
%
% File extension is *.BOATCfg
% 
% 06/01/2017, Mitchell Overdick
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %% User Configurable Parameters
    % Folder to place output in             (string, in reference to working directory)
dest_dir    = './Lake Padden/';  
dest_name   = 'LPCfg';          % Name of file to output to             (string)
WPRadius    = 30;               % Radius to complete waypoint           (meters, double precision) (15 Recommended Minimum)
StartRad    = 30;               % Radius to first waypoint for takeoff  (meters, double precision) (15 Recommended Minimum)
StopRad     = 15;                % Radius to hold last waypoint          (meters, double precision) 
Shortcut    = 0;                % How many waypoints can be shortcut    (max 255)
LogPer      = 0;                % Period between taking logspace        (seconds, max 2^32)
minDuty     = 0;                % Minimum motor duty cycle              (percentage, 0-100)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Main Code - DO NOT EDIT
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % Create destination file string
dest_ext    = '.BOATCfg';
dest_file   = strcat(dest_dir,dest_name,dest_ext);

    % Create/Open destination file
fid     = fopen(dest_file,'w');

    % Write configuration to file
fwrite(fid,WPRadius,'float64',0,'l');           % Write WPRadius as double
fwrite(fid,StartRad,'float64',0,'l');           % Write StartRad as double
fwrite(fid,StopRad,'float64',0,'l');            % Write StopRad as double
fwrite(fid,Shortcut,'uint8');                   % Write shotcut as unsigned 8-bit
LogPerH = dec2hex(LogPer,8);                    % Convert LogPer to hex
    % Write LogPer as multiple bytes
for i = [length(LogPerH):-2:2]       
  fwrite(fid,hex2dec(LogPerH(i-1:i)),'uint8');
end
fwrite(fid,minDuty,'uint8');                    % Write minDuty as unsigned 8-bit
fclose(fid);                                    % Close file

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

    % Append Checksum to file
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(['checksum: ' num2str(chksum)]);