% Clear the workspace
clear all;
close all;
clc;

% Reload breakpoints
ActivateBreakpoints;

% Define program variables
Update_size     = 30;
GCode_size      = 40;
flags.Randomize = false; 

% Update command line
UpdateStr   = sprintf(' -------- PROGRAM START! --------\n');
UpdateStr   = [UpdateStr,sprintf('Started %s\n',datestr(now,'HH:MM:SS dd-mmm-yyyy'))];
fprintf(UpdateStr);
RunTimer    = tic;

% Define bed parameters
Bed_size    = [400,400];
Soft_border = [10,10];

Nozzle_offset = [40,18];

Mesh_edges  = [Soft_border(1),Soft_border(2);
               Bed_size(1)-Soft_border(1),Bed_size(2)-Soft_border(2)];
Mesh_pts    = [3,3];

Z_travel    = 10;
Z_probe     = 5;
Z_liftspeed = 20;
Z_divespeed = 10;
XY_speed    = 50; 

nSamples    = 3;
nReps       = 1;

%% Create coordinates list

% Update command line
UpdateStr   = sprintf('Generating coordinate list...');
Padding     = repmat(',',1,Update_size-length(UpdateStr));
fprintf(UpdateStr);

Coord_timer = tic;

try
    
    X_list      = linspace(Mesh_edges(1,1),Mesh_edges(2,1),Mesh_pts(1));
    Y_list      = linspace(Mesh_edges(1,2),Mesh_edges(2,2),Mesh_pts(2));
    
    Lines_total = Mesh_pts(1)*Mesh_pts(2)*nReps;
    Coord_list  = [];
    
    Line_ind    = 1;
    for Rep_count = 1:nReps
        for X_ind = 1:Mesh_pts(1)
            
            if ~flags.Randomize && mod(X_ind,2) == 0
                Y_start     = Mesh_pts(2);
                Y_step      = -1;
                Y_finish    = 1;
            elseif flags.Randomize || mod(X_ind,2) ~= 0
                Y_start     = 1;
                Y_step      = 1;
                Y_finish    = Mesh_pts(2);
            end
            
            for Y_ind = Y_start:Y_step:Y_finish
            
                Temp_coords = [X_list(X_ind),Y_list(Y_ind)] + Nozzle_offset;
                
                if isempty(Coord_list)
                    Coord_list = Temp_coords;
                else
                    Coord_list = [Coord_list; Temp_coords];
                end
                
            end
        end
    end

    % Randomize coordinate list
    if flags.Randomize 
        randInds    = randperm(length(Coord_list));
        Coord_list  = Coord_list(randInds,:);
    end
        
catch ErrorData
    
    ErrorMsg    = sprintf('\n----- CRITICAL STOP! -----\n');
    
    ErrorMsg    = ErrorData.message;
    ErrorFiles  = split(ErrorData.stack(end).file,'\');
    ErrorFile   = ErrorFiles(end);
    ErrorLine   = ErrorData.stack(end).line;
    
    ErrorOutput = sprintf('\tCritical error in %s, line %g\n\tError: %s\n\n',ErrorFile{1},ErrorLine,ErrorMsg);
    throw(MException('MeshMaker:FailedCoordList',ErrorOutput));
        
    x = 1;
    
end
    
fprintf([Padding,sprintf('Done!\n')]);
Coord_time  = toc(Coord_timer);

%% Prepare gcode file

UpdateStr   = sprintf('Preparing G-code...');
Padding     = repmat('.',1,Update_size-length(UpdateStr)+1);
fprintf(UpdateStr);

GCode_timer = tic;

try

    % Define log file name
    Logfile_name  = 'eventlog_14-Jan-202.txt';

    % Define startup text
    CodeStr         = {sprintf(' ; ----- High-density Bed Meshing ----- \n')};
    CodeStr{end+1}  = sprintf(' ; Authored by Rob Smith\n');
    CodeStr{end+1}  = sprintf(' ; Prepared on %s\n',datestr(now,'HH:MM:SS dd-mmm-yyyy'));
    CodeStr{end+1}  = sprintf(' ; \n');
    CodeStr{end+1}  = sprintf(' ; Mesh parameters:\n');
    CodeStr{end+1}  = sprintf(' ;   Bed dimensions:\t%gmm x %gmm\n',Bed_size(1),Bed_size(2));

    X_spacing       = Bed_size(1)/Mesh_pts(1);
    Y_spacing       = Bed_size(2)/Mesh_pts(2);

    CodeStr{end+1}  = sprintf(' ;   Site spacing:\t\t%3.2fmm\n',(Bed_size(1)-2*Soft_border(1))/(Mesh_pts(1)+1));
    CodeStr{end+1}  = sprintf(' ;   Probe sites:\t\t%g\n',Mesh_pts(1)*Mesh_pts(2));
    CodeStr{end+1}  = sprintf(' ;   Sample size:\t\t%g\n',nSamples);
    CodeStr{end+1}  = sprintf(' ;   Repetitions:\t\t%g\n',nReps);
    CodeStr{end+1}  = sprintf(' ;   Datapoints:\t\t%g\n',Mesh_pts(1)*Mesh_pts(2)*nSamples*nReps);
    CodeStr{end+1}  = sprintf(' ; \n');    
    
    CodeStr{end+1}  = sprintf(' ; ----- PROGRAM START! -----\n\n');
    CodeStr{end+1}  = PadGCode('G28','Home the printer',GCode_size);
    CodeStr{end+1}  = PadGCode(sprintf('M929 P"%s" S1',Logfile_name),'Enable communication logging\n');
    CodeStr{end+1}  = PadGCode('M503','Copy original EEPROM settings\n');
    CodeStr{end+1}  = PadGCode(sprintf('M558 F%g H3 T%g A%g S0.001',Z_divespeed*60,XY_speed*60,nSamples),'Modify z-probe configuration\n');
    CodeStr{end+1}  = PadGCode(sprintf('G1 Z%g F%g',Z_travel,Z_liftspeed*60),'Lift effector to start position\n\n');

    i = 1;
    Spinner     = ['|';'/';'-';'\'];
    Spinner_ind = 1;
    WipeCoords  = '';
    while i <= length(Coord_list)

        NewCoordStr = sprintf('\n%g - (%3.3f,%3.3f)',i,Coord_list(i,1),Coord_list(i,2));
        fprintf([WipeCoords,NewCoordStr]);
        WipeCoords  = repmat(sprintf('\b'),1,length(NewCoordStr));
        
        % Check for previous repetitions
        CheckStr = sprintf('X%3.3f Y%3.3f',Coord_list(i,1),Coord_list(i,2));
        RepCheck = cellfun(@(x) contains(x,CheckStr),CodeStr,'UniformOutput',false);
        RepCount = numel(find(cell2mat(RepCheck))) + 1;
        
        % Generate gcode set
        CodeStr{end+1} = sprintf('\n ; -- Probe site %s%g, rep %s%g\n',char(35),i,char(35),RepCount);
        CodeStr{end+1} = PadGCode(sprintf('G1 X%3.3f Y%3.3f F%g',Coord_list(i,1),Coord_list(i,2),XY_speed*60),'Move to coordinates\n');
        CodeStr{end+1} = PadGCode(sprintf('G1 Z%3.3f F%g',Z_travel,Z_divespeed*60),'Dive to probe height\n');
        CodeStr{end+1} = PadGCode(sprintf('G30 S-1'),'Invoke probe series\n');
        CodeStr{end+1} = PadGCode(sprintf('G1 Z%3.3f F%g',Z_travel,Z_liftspeed*60),'Lift to travel height\n');

        % Spin the spinny thing
        if mod(i,5) == 0

            Spinner_ind = Spinner_ind + 1;
            if (Spinner_ind + 1) > length(Spinner)
                Spinner_ind = 1;
            end

            fprintf('\b%s',Spinner(Spinner_ind));

        end

        % Increment to next coordinate pair
        i = i + 1;

    end

    fprintf([WipeCoords,sprintf('\b')]);
    
    CodeStr{end+1} = sprintf('\n ; Data collection complete!\n');
    CodeStr{end+1} = PadGCode(sprintf('G1 X%3.3f Y%3.3f F%g',Bed_size(1)/2,Bed_size(2)/2,XY_speed*60),'Move to bed center\n');
    CodeStr{end+1} = PadGCode(sprintf('G1 Z30 F%g',Z_liftspeed*60),'Lift effector to end position\n');
    CodeStr{end+1} = PadGCode('M929 S0','Disable communication logging\n');
    CodeStr{end+1} = PadGCode('M84','Disable stepper motors\n');
    CodeStr{end+1} = sprintf('\n\n ----- PROGRAM COMPLETE! -----\n\n');
    
    
catch ErrorData
    
    fprintf('\n----- CRITICAL STOP! -----\n');
    
    ErrorMsg    = ErrorData.message;
    ErrorFiles  = split(ErrorData.stack(end).file,'\');
    ErrorFile   = ErrorFiles(end);
    ErrorLine   = ErrorData.stack(end).line;
    
    ErrorOutput = sprintf('\tCritical error in %s, line %g\n\tError: %s\n\n',ErrorFile{1},ErrorLine,ErrorMsg);
    throw(MException('MeshMaker:FailedCoordList',ErrorOutput));
    
    x = 1;
    
end
       
fprintf([Padding,sprintf('Done!\n')]);
GCode_time  = toc(GCode_timer);


%% Output code to file
UpdateStr   = sprintf('Writing gcode to file...');
Padding     = repmat('.',1,Update_size-length(UpdateStr));
fprintf(UpdateStr);

File_timer  = tic;

try
    
    File_name   = 'MajorRandomness.gcode';
    File_ID     = fopen(File_name,'w+');
    for i = 1:length(CodeStr)
        newLine = CodeStr{i};
        if iscell(newLine)
            newLine = newLine{1};
        end
        fprintf(File_ID,newLine);
    end
    fclose(File_ID);

catch ErrorData
    
    ErrorMsg    = fprintf('\n----- CRITICAL STOP! -----\n');
    
    OpenFiles   = fopen('all');
    if ~isempty(OpenFiles)
        UpdateStr = sprintf('<Closing file(s)...>');
        fprintf(UpdateStr);
        for i = 1:numel(OpenFiles)
            fclose(OpenFiles(i));
        end
        Padding = repmat(',',1,Update_size-length(UpdateStr));
        fprintf([Padding,sprintf('Done!\n')]);
    end
    
    ErrorMsg    = ErrorData.message;
    ErrorFiles  = split(ErrorData.stack(end).file,'\');
    ErrorFile   = ErrorFiles(end);
    ErrorLine   = ErrorData.stack(end).line;
    
    ErrorOutput = sprintf('\tCritical error in %s, line %g\n\tError: %s\n\n',ErrorFile{1},ErrorLine,ErrorMsg(1:end-1));
    throw(MException('MeshMaker:FailedCoordList',ErrorOutput));
    
    x = 1;
    
end

    
fprintf([Padding,sprintf('Done!\n')]);

File_time =  toc(File_timer);


%% End program

fprintf('----- PROGRAM COMPLETE! -----\n\n');
File_length = 5 + (i*4);
UpdateStr   = sprintf('Gcode size: %g lines\n',File_length);
Runtime     = toc(RunTimer);
Run_mins    = floor(Runtime/60);
Run_secs    = Runtime - Run_mins*60;
UpdateStr   = [UpdateStr,sprintf('Generating coordinate list: %gs\n',Coord_time)];
UpdateStr   = [UpdateStr,sprintf('Preparing G-code: %gs\n',GCode_time)];
UpdateStr   = [UpdateStr,sprintf('Output to file: %gs\n\n',File_time)];
UpdateStr   = [UpdateStr,sprintf('Run time: %gm%gs\n\n',Run_mins,Run_secs)];
fprintf(UpdateStr);