function CreateTestImage(varargin)
% Create an tiled image of all the training images, for testing detection.

markerImageDir = VisionMarkerTrained.TrainingImageDir;
imageSize = [320 240];
resolutionScaling = 2;
headerFilename = '~/Code/victor/lib/anki/cozmo-engine/robot/test/data/newFiducials_320x240';
maxImages = 24;
figureColor = 'k';
parseVarargin(varargin{:});

aspect = imageSize(1)/imageSize(2);
origImageSize = imageSize;
imageSize = resolutionScaling * imageSize;

% For now, assume all the training images were in a "rotated" subdir and
% we want to create the test image from the parent of that subdir

numDirs = length(markerImageDir);
fnames = cell(numDirs,1);
for i_dir = 1:numDirs
  % Remove cozmo images from testing:
  if ~isempty(strfind(markerImageDir{i_dir}, 'Victor'))
    fnames{i_dir} = getfnames(fullfile(markerImageDir{i_dir}, '..'), 'images', 'useFullPath', true);
  end
end
fnames = vertcat(fnames{:});

if isempty(fnames)
    error('No image files found.');
end

numImages = length(fnames);
fprintf('Found %d total images.\n', numImages);

if numImages > maxImages
   fprintf('Will sample the maximum %d images from those %d.\n', ...
       maxImages, numImages);
   
   randIndex = randperm(numImages);
   fnames = fnames(randIndex(1:maxImages));
   numImages = maxImages;   
end

numRows = round(sqrt(numImages / aspect));
numCols = ceil(numImages / numRows);

assert(numRows*numCols >= numImages);

namedFigure('VisionMarkerTrained TestImage', 'Color', figureColor);
clf
delete(findobj(gcf, 'Type', 'uicontrol'));

markerPositions = cell(1, numImages);

markerSize = min(imageSize)/(numRows+1); % the +3 introduces padding by making markerSize smaller
padding = 0.02;
xcenters = linspace(padding,1-padding,numCols);
xcenters = (imageSize(1)-markerSize)*xcenters + markerSize/2;
ycenters = linspace(padding,1-padding,numRows);
ycenters = (imageSize(2)-markerSize)*ycenters + markerSize/2;

[xgrid,ygrid] = meshgrid(xcenters,ycenters);

pos = get(gcf, 'Pos');
set(gcf, 'Units', 'pixels', 'Position', [pos(1:2) imageSize]); 
h_axes = axes('Units', 'pixels', 'Position', [1 1 imageSize]);
set(h_axes, 'XLim', [0.5 imageSize(1)+.5], 'YLim', [0.5 imageSize(2)+.5]);
hold(h_axes, 'on')

colormap(h_axes, gray);

iMarker = 1;
for iRow = 1:numRows
    for iCol = 1:numCols
        if iMarker <= numImages
            %h_axes = subplot(numRows,numCols,i);
            markerPositions{iMarker} = [xgrid(iRow,iCol)-markerSize/2 ygrid(iRow,iCol)-markerSize/2 markerSize*[1 1]];
            %h_axes = axes('Parent', gcf, 'Units', 'pixels', ...
            %    'Position', axesPositions{iMarker});
            
            img = imreadAlphaHelper(fnames{iMarker});
            imagesc(markerPositions{iMarker}(1) + [0 markerPositions{iMarker}(3)], ...
                markerPositions{iMarker}(2) + [0 markerPositions{iMarker}(4)], ...
                img, 'Parent', h_axes);
                        
            rectangle('Pos', markerPositions{iMarker}, ...
                'Parent', h_axes, 'EdgeColor', 'r', 'Tag', 'MarkerRect');
            
            iMarker = iMarker + 1;
            
        end
    end
end

axis(h_axes, 'ij', 'off');

%fix_subplots(numRows,numCols, .2);

uicontrol('Style', 'pushbutton', ...
    'String', 'Write Ground Truth File', ...
    'Callback', @writeFileFcn, ...
    'Units', 'norm', ...
    'Pos', [0 0 0.25 0.05]);

    function writeFileFcn(src,~)
        set(src, 'Visible', 'off');
        
        set(findobj(h_axes, 'Tag', 'MarkerRect'), 'Visible', 'off');
        
        % save the test image as a header file
        frame = getframe(h_axes);
        frame = rgb2gray(frame.cdata);
        if isequal(size(frame), [imageSize(2) imageSize(1)+1])
            frame = frame(:,1:end-1); % getframe seems to tack on an extra column?
        elseif ~isequal(size(frame), fliplr(imageSize))
            error('Captured frame not the expected size.');
        end
        frame = imresize(frame, fliplr(origImageSize), 'lanczos3');
        saveImageAsHeader(frame, [headerFilename '.h']);
        
        % create a ground truth header file for markers and corner
        % locations
        fid = fopen([headerFilename '_markers.h'], 'wt');
        fidCleanup = onCleanup(@()fclose(fid));
        
        fprintf(fid, [...
            '// Autogenerated by VisionMarkerTrained.%s on %s\n\n' ...
            'const s32 numMarkers_groundTruth = %d;\n\n' ...
            'const f32 corners_groundTruth[numMarkers_groundTruth][4][2] = {\n'], ...
            mfilename, datestr(now, 31), numImages);

        x = zeros(numImages,4);
        y = zeros(numImages,4);
        names = cell(1, numImages);
        for iMarkerNest = 1:numImages
            pos = markerPositions{iMarkerNest};
            x(iMarkerNest,:) = ([pos(1)*[1 1] (pos(1)+pos(3))*[1 1]] - 1) / resolutionScaling + 1;
            y(iMarkerNest,:) = ([pos(2) (pos(2)+pos(4)) pos(2) (pos(2)+pos(4))] - 1) / resolutionScaling + 1;
            
            [~,names{iMarkerNest}] = fileparts(fnames{iMarkerNest});
            names{iMarkerNest} = sprintf('%s', upper(names{iMarkerNest}));
            
            fprintf(fid, '\t{{%.2ff,%.2ff},{%.2ff,%.2ff},{%.2ff,%.2ff},{%.2ff,%.2ff}}, // %s\n', ...
                x(iMarkerNest,1)-1, y(iMarkerNest,1)-1, ...
                x(iMarkerNest,2)-1, y(iMarkerNest,2)-1, ...
                x(iMarkerNest,3)-1, y(iMarkerNest,3)-1, ...
                x(iMarkerNest,4)-1, y(iMarkerNest,4)-1, ...
                names{iMarkerNest});
        end
        fprintf(fid, '};\n\n');

        
        fprintf(fid, 'const Anki::Vision::MarkerType markerTypes_groundTruth[numMarkers_groundTruth] = {\n');
        for iMarkerNest = 1:numImages
            fprintf(fid, '\tAnki::Vision::MARKER_%s,\n', names{iMarkerNest});
        end
        fprintf(fid, '};\n\n');
        
        namedFigure('CreateTestImage: Captured Image');
        clf, imshow(frame), hold on
        plot(x(:,[1 2 4 3 1])',y(:,[1 2 4 3 1])', 'r', 'LineWidth', 2);
        h_text = text(mean(x,2), mean(y,2), names);
        
        set(h_text, 'FontWeight', 'b', 'FontSize', 18, ...
            'Interp', 'none', 'Color', 'r', 'Hor', 'center');
        
        
        disp('Header files written.');
        set(src, 'Visible', 'on')
        set(findobj(h_axes, 'Tag', 'MarkerRect'), 'Visible', 'on');
    end

end
    