function [nearestNeighborString, markerDefString] = GenerateNearestNeighborHeaderFiles(varargin)

writeFiles = false;
projectRoot = fullfile(fileparts(mfilename('fullpath')), '..', '..', '..', '..');
markerDefFile = 'coretech/vision/shared/MarkerCodeDefinitions.h';
nearestNeighborDataFile = 'engine/vision/nearestNeighborLibraryData.h'; % embed in code
%nearestNeighborDataFile = 'resources/config/basestation/vision/nnLibrary/nnLibrary.bin'; % binary data file
nnLibrary = [];
useHoG = false;

numFractionalBits = 15;

parseVarargin(varargin{:});

labels = nnLibrary.labelNames;

[numDimensions, numDataPoints] = size(nnLibrary.probeValues);

[~,~,ext] = fileparts(nearestNeighborDataFile);
writeBinaryDataFile = strcmp(ext, '.bin');

if useHoG
  if ~isfield(nnLibrary, 'probeHoG')
    error('useHoG==true but no probeHoG field found in nnLibrary.');
  end
  assert(size(nnLibrary.probeHoG,2) == numDataPoints);
  numHoGdims = size(nnLibrary.probeHoG, 1);
else
  numHoGdims = 0;
end



paramStr = '';
if isfield(nnLibrary, 'parameters')
  assert(iscell(nnLibrary.parameters), 'Parameters should be a cell array.');
  
  for i = 1:2:length(nnLibrary.parameters)
    paramStr = [paramStr sprintf('''%s'', %s,\n', nnLibrary.parameters{i}, num2str(nnLibrary.parameters{i+1}))];
  end  
end

dateString = datestr(now);
nearestNeighborString = sprintf( [...
  '// Autogenerated by VisionMarkerTrained.%s() on %s with Matlab %s\n\n' ...
  '// NOTE: (X,Y) probe locations are stored as SQ%d.%d\n\n' ...
  '#ifndef ANKI_COZMO_ROBOT_NEAREST_NEIGHBOR_LIBRARY_DATA_H\n' ...
  '#define ANKI_COZMO_ROBOT_NEAREST_NEIGHBOR_LIBRARY_DATA_H\n\n' ...
  '#include "coretech/vision/shared/MarkerCodeDefinitions.h"\n\n' ...
  'namespace Anki {\n' ...
  'namespace Embedded {\n\n' ...
  'const s32 NUM_MARKERS_IN_LIBRARY = %d;\n' ...
  'const s32 DATA_DIMENSION = %d;\n' ...
  'const s32 NUM_HOG_DIMENSIONS = %d;\n'], ...
  mfilename, dateString, version, 15-numFractionalBits, numFractionalBits, ...
  numDataPoints, numDimensions, numHoGdims);

%% Probe point pattern

VisionMarkerTrained.GenerateProbeDefinitionFiles('writeFiles', writeFiles);

%% Enums and LUTs
numLabels = length(labels);

enumString          = cell(1, numLabels);
enumString_oriented = cell(1, numLabels);
labelNames          = cell(1, numLabels); % same as enumString_oriented without extra () or ,
enumMappingString   = cell(1, numLabels);
reorderingString    = cell(1, numLabels);
orientationString   = cell(1, numLabels);

for i_label = 1:numLabels
    
    % The oriented enums are the raw decision tree labels (if they don't
    % have an underscore, add _000 to the end)
    enumName_oriented = labels{i_label};
    if any(strcmp(enumName_oriented, {'ALL_WHITE', 'ALL_BLACK'}))
        % special case
        % TODO: adjust training to change these special labels to ALLWHITE and ALLBLACK
        underscoreIndex = [];
    else
        underscoreIndex = find(enumName_oriented == '_');
        if strncmpi(enumName_oriented, 'inverted_', 9)
            % Ignore the first underscore found if this is
            % an inverted code name
            underscoreIndex(1) = [];
        end
        
        if length(underscoreIndex) > 1
          % If more than one underscore, use last
          underscoreIndex = underscoreIndex(end);
        end
    end
    if isempty(underscoreIndex) 
       enumName_oriented = [enumName_oriented '_000'];  %#ok<AGROW>
       underscoreIndex = length(enumName_oriented)-3;
    end
    labelNames{i_label} = sprintf('MARKER_%s', upper(enumName_oriented));
    enumString_oriented{i_label} = sprintf('  %s', labelNames{i_label});
    %enumString_oriented_quoted{i_label} = sprintf('  "%s",\n', labelNames{i_label});
    
    % The unoriented enums strip off the _QQQ angle off the end
    enumName = sprintf('MARKER_%s', upper(enumName_oriented(1:(underscoreIndex-1))));
    enumString{i_label} = sprintf('%s', enumName);
    %enumString_quoted{i_label} = sprintf('  "%s",\n', enumName);
    
    % Map oriented to unoriented
    enumMappingString{i_label} = ['Vision::' enumString{i_label}];
    
    % Reorient the corners accroding to the orientation.  Using this will
    % result in the first and third corners being the top side.
    reorder = [1 3; 2 4]; % canonical corner ordering
    orientationAngleStr = enumName_oriented((underscoreIndex+1):end);
    switch(orientationAngleStr)
        case '000'
            % nothing to do
        case '090'
            reorder = rot90(rot90(rot90(reorder)));
            %reorder = rot90(reorder);
        case '180'
            reorder = rot90(rot90(reorder));
        case '270'
            %reorder = rot90(rot90(rot90(reorder)));
            reorder = rot90(reorder);
        otherwise
            error('Unrecognized angle string "%s"', orientationAngleStr);
    end
    reorderingString{i_label} = sprintf('  {%d,%d,%d,%d},\n', reorder(:)-1);
    
    orientationString{i_label} = sprintf('    %f,\n', str2double(orientationAngleStr));
    
end % FOR each label
        
% Create enumerate marker IDs, oriented and unoriented, and a mapping
% between them
nearestNeighborString = [nearestNeighborString sprintf([...
    'enum OrientedMarkerLabel {\n' ....
    '%s' ...
    '  NUM_MARKER_LABELS_ORIENTED,\n' ...
    '  MARKER_UNKNOWN = NUM_MARKER_LABELS_ORIENTED\n' ...
    '};\n\n'], sprintf('%s,\n', enumString_oriented{:}))];

nearestNeighborString = [nearestNeighborString sprintf([ ...
    'const char * const OrientedMarkerLabelStrings[NUM_MARKER_LABELS_ORIENTED] = {\n' ...
    '%s' ...
    '};\n\n'], sprintf('"%s",\n', enumString_oriented{:}))];
      
nearestNeighborString = [nearestNeighborString sprintf([ ...
    'const u32 CornerReorderLUT[NUM_MARKER_LABELS_ORIENTED][4] = {\n' ...
    '%s' ...
    '};\n\n'], [reorderingString{:}])];


%% NearestNeighborData/Weights/Labels

if useHoG 
  assert(isfield(nnLibrary, 'probeHoG'), 'useHoG==true, nnLibrary should have probeHoG field.'); %#ok<UNRCH>
  data = nnLibrary.probeHoG;
  dataDimStr = 'NUM_HOG_DIMENSIONS';
else
  data = nnLibrary.probeValues;
  dataDimStr = 'DATA_DIMENSION';
end

if ~writeBinaryDataFile
  nearestNeighborString = [nearestNeighborString sprintf([ ...
    'const u8 NearestNeighborData[NUM_MARKERS_IN_LIBRARY*%s] = {\n' ...
    '%s' ...
    '};\n\n'], dataDimStr, sprintf('%d, ', data))];
  
  if ~useHoG
    nearestNeighborString = [nearestNeighborString sprintf([ ...
      'const u8 NearestNeighborWeights[NUM_MARKERS_IN_LIBRARY*DATA_DIMENSION] = {\n' ...
      '%s' ...
      '};\n\n'], sprintf('%d, ', im2uint8(nnLibrary.gradMagWeights)))];
  end
  
  nearestNeighborString = [nearestNeighborString sprintf([ ...
    'const u16 NearestNeighborLabels[NUM_MARKERS_IN_LIBRARY] = {\n' ...
    '%s' ...
    '};\n\n'], sprintf('%d, ', nnLibrary.labels-1))];
end

%% Marker Type Definitions
% Different file now!
markerDefString = sprintf([ ...
    '// Autogenerated by VisionMarkerTrained.%s() on %s with Matlab %s\n\n' ...
    '#ifndef ANKI_COZMO_VISIONMARKERTYPES_H\n' ...
    '#define ANKI_COZMO_VISIONMARKERTYPES_H\n\n' ...
    '/* ExtractProbeValues Parameters:\n' ...
    '%s\n' ...
    '*/\n\n' ...
    '#include <map>\n\n' ...
    'namespace Anki {\n' ...
    'namespace Vision {\n\n' ...
    '  static const char* MarkerDefinitionVersionString = "%s";\n\n'], ...
    mfilename, dateString, version, paramStr, dateString);
    
enumString = unique(enumString);
enumStringsWithValues = [enumString(:) num2cell((0:length(enumString)-1)')]';

markerDefString = [markerDefString sprintf([ ...
    '  enum MarkerType {\n' ...
    '%s' ...
    '    NUM_MARKER_TYPES,\n' ...
    '    MARKER_UNKNOWN = NUM_MARKER_TYPES\n' ...
    '  };\n\n'], sprintf('    %s = %d,\n', enumStringsWithValues{:}))];

markerDefString = [markerDefString sprintf([ ...
    '  const char * const MarkerTypeStrings[NUM_MARKER_TYPES+1] = {\n' ...
    '%s' ...
    '    "MARKER_UNKNOWN"\n' ...
    '  };\n\n'], sprintf('    "%s",\n', enumString{:}))];

% We can put this back if we need a lookup from String to MarkerType.
% repeatedEnumString = [enumString(:) enumString(:)]';
% markerDefString = [markerDefString sprintf([ ...
%     '#ifdef COZMO_BASESTATION\n' ...
%     '  const std::map<std::string, Vision::MarkerType> StringToMarkerType = {\n' ...
%     '%s' ...
%     '    {"MARKER_UNKNOWN", MARKER_UNKNOWN}\n' ...
%     '  };\n' ...
%     '#endif\n\n'], sprintf('    {"%s", %s},\n', repeatedEnumString{:}))];

markerDefString = [markerDefString sprintf([ ...
  '} // namespace Vision\n' ...
  '} // namespace Anki\n\n' ...
  '#endif // ANKI_COZMO_VISIONMARKERTYPES_H\n\n'])];
 
%% Move back to LUT defs
nearestNeighborString = [nearestNeighborString sprintf([ ...
    'const Vision::MarkerType RemoveOrientationLUT[NUM_MARKER_LABELS_ORIENTED] = {\n' ...
    '%s' ...
    '};\n\n'], sprintf('  %s,\n', enumMappingString{:}))];

nearestNeighborString = [nearestNeighborString sprintf([ ...
    'const f32 ObservedOrientationLUT[NUM_MARKER_LABELS_ORIENTED] = {\n' ...
    '%s' ...
    '};\n\n'], [orientationString{:}])];

nearestNeighborString = [nearestNeighborString sprintf([ ...
    '} // namespace Embedded\n' ...
    '} // namespace Anki\n\n' ...
    '#endif // ANKI_COZMO_ROBOT_NEAREST_NEIGHBOR_LIBRARY_DATA_H\n'])];



if writeFiles 
  
  nearestNeighborDataFile = fullfile(projectRoot, nearestNeighborDataFile);
  
  if writeBinaryDataFile
    fid = fopen(nearestNeighborDataFile, 'wb');
    if fid == -1
      error('Could not open "%s" for writing binary nearest neighbor data file.', nearestNeighborDataFile);
    else
      fwrite(fid, data, 'uint8');
      fclose(fid);
    end
    
    [p,f,e] = fileparts(nearestNeighborDataFile);
    weightsFile = fullfile(p, [f '_weights' e]);
    fid = fopen(weightsFile, 'wb');
    if fid == -1
      error('Could not open "%s" for writing binary nearest neighbor weights file.', weightsFile);
    else
      fwrite(fid, im2uint8(nnLibrary.gradMagWeights), 'uint8');
      fclose(fid);
    end
    
    labelsFile = fullfile(p, [f '_labels' e]);
    fid = fopen(labelsFile, 'wb');
    if fid == -1
      error('Could not open "%s" for writing binary nearest neighbor labels file.', labelsFile);
    else
      fwrite(fid, uint16(nnLibrary.labels)-1, 'uint16');
      fclose(fid);
    end
    
    fprintf('Wrote binary nearest neighbor data to "%s".\n', nearestNeighborDataFile);
  else 
    
    fid = fopen(nearestNeighborDataFile, 'wt');
    if fid == -1
      error('Could not open "%s" for writing nearest neighbor data file.', nearestNeighborDataFile);
    else
      fprintf(fid, '%s', nearestNeighborString);
      fclose(fid);
      fprintf('Wrote nearest neighbor data to "%s".\n', nearestNeighborDataFile);
    end
  end
  
  markerDefFile = fullfile(projectRoot, markerDefFile);
  fid = fopen(markerDefFile, 'wt');
  if fid == -1
    error('Could not open "%s" for writing marker definition file.', markerDefFile);
  else
    fprintf(fid, '%s', markerDefString);
    fclose(fid);
    fprintf('Wrote marker definitions to "%s".\n', markerDefFile);
  end
    
end % IF writeFiles

% if nargout==0
%     clipboard('copy', decisionTreeString);
%     fprintf('\nCopied header file to clipboard.\n\n');
%     clear outputString;
% end

%sprintf('Maximum differences with %d fractional bits:\n\nTheoretical: %f\n\nMeasured:\nXProbes: %f (%f percent)\nYProbes: %f (%f percent)\nProbeWeights: %f (%f percent)\n',...
%    numFractionalBits, 1/(2^(numFractionalBits+1)), maxX, maxXPercent, maxY, maxYPercent, maxP, maxPPercent)
    
end % FUNCTION GenerateHeaderFile()

function fixedPt = FixedPoint(value, numFractionalBits)

if isempty(value)
    fixedPt = 0;
else
    fixedPt = int32(round(2^numFractionalBits)*value);
end

end % FUNCTION FixedPointHelper()


