
% function convertBlockMarker2dOffsets()

function convertBlockMarker2dOffsets()

numFractionalBits = 15;

image = zeros(480,640);
corners = [0,0; 
           0,200;
           200,0;
           200,200];
tform = maketform('projective', eye(3));
       
marker = BlockMarker2D(image, corners, tform);
 
maxX = max(max(abs(marker.Xprobes - double(int16(round(2^numFractionalBits)*marker.Xprobes))/(2^numFractionalBits))));
maxY = max(max(abs(marker.Yprobes - double(int16(round(2^numFractionalBits)*marker.Yprobes))/(2^numFractionalBits))));
maxP = max(max(abs(marker.ProbeWeights - double(int16(round(2^numFractionalBits)*marker.ProbeWeights))/(2^numFractionalBits))));

maxXPercent = 100 * max(max(abs(marker.Xprobes - double(int16(round(2^numFractionalBits)*marker.Xprobes))/(2^numFractionalBits)) ./ abs(marker.Xprobes)));
maxYPercent = 100 * max(max(abs(marker.Yprobes - double(int16(round(2^numFractionalBits)*marker.Yprobes))/(2^numFractionalBits)) ./ abs(marker.Yprobes)));
maxPPercent = 100 * max(max(abs(marker.ProbeWeights - double(int16(round(2^numFractionalBits)*marker.ProbeWeights))/(2^numFractionalBits)) ./ abs(marker.ProbeWeights)));

fiducialMarkerType = 0;
fiducialMarkerTypeName = sprintf('_FIDICIAL_MARKER_DEFINITION_TYPE_%d_', fiducialMarkerType);

curDateAndTime = round(clock());

outputString = ['// Autogenerated by convertBlockMarker2dOffsets.m on ', sprintf('%04d-%02d-%02d at %02d:%02d:%02d', curDateAndTime(1), curDateAndTime(2), curDateAndTime(3), curDateAndTime(4), curDateAndTime(5), curDateAndTime(6)), '\n\n'];

outputString = [outputString, sprintf('#include "anki/embeddedVision/fiducialMarkers.h"\n\n#ifndef %s\n#define %s\n\n#define NUM_BITS_TYPE_%d %d\n#define NUM_PROBES_PER_BIT_TYPE_%d %d\n#define NUM_FRACTIONAL_BITS_TYPE_%d %d\n\n', fiducialMarkerTypeName, fiducialMarkerTypeName, fiducialMarkerType, size(marker.Xprobes, 1), fiducialMarkerType, size(marker.Xprobes, 2), fiducialMarkerType, numFractionalBits)];

bitTypes = ones(1, size(marker.Xprobes,1));
bitTypes(marker.IdBits('B')) = 2;
bitTypes(marker.IdBits('F')) = 3;
bitTypes(marker.LeftBit) = 4;
bitTypes(marker.RightBit) = 5;
bitTypes(marker.UpBit) = 6;
bitTypes(marker.DownBit) = 7;
bitTypes(marker.CheckBits) = 8;

outputString = [outputString, sprintf('const Anki::Embedded::FiducialMarkerParserBit::Type bitTypes_type%d[NUM_BITS_TYPE_%d] = {\n', fiducialMarkerType, fiducialMarkerType)];
for i = 1:length(bitTypes)
    if bitTypes(i) == 1
        outputString = [outputString, '    Anki::Embedded::FiducialMarkerParserBit::FIDUCIAL_BIT_NONE,\n'];
    elseif bitTypes(i) == 2
        outputString = [outputString, '    Anki::Embedded::FiducialMarkerParserBit::FIDUCIAL_BIT_BLOCK,\n'];
    elseif bitTypes(i) == 3
        outputString = [outputString, '    Anki::Embedded::FiducialMarkerParserBit::FIDUCIAL_BIT_FACE,\n'];
    elseif bitTypes(i) == 4
        outputString = [outputString, '    Anki::Embedded::FiducialMarkerParserBit::FIDUCIAL_BIT_ORIENTATION_LEFT,\n'];
    elseif bitTypes(i) == 5
        outputString = [outputString, '    Anki::Embedded::FiducialMarkerParserBit::FIDUCIAL_BIT_ORIENTATION_RIGHT,\n'];
    elseif bitTypes(i) == 6
        outputString = [outputString, '    Anki::Embedded::FiducialMarkerParserBit::FIDUCIAL_BIT_ORIENTATION_UP,\n'];
    elseif bitTypes(i) == 7
        outputString = [outputString, '    Anki::Embedded::FiducialMarkerParserBit::FIDUCIAL_BIT_ORIENTATION_DOWN,\n'];
    elseif bitTypes(i) == 8
        outputString = [outputString, '    Anki::Embedded::FiducialMarkerParserBit::FIDUCIAL_BIT_CHECKSUM,\n'];
    end
end
outputString = [outputString, '};\n\n'];

outputString = [outputString, sprintf('// SQ%d.%d\nconst short probesX_type%d[NUM_BITS_TYPE_%d][NUM_PROBES_PER_BIT_TYPE_%d] = {\n', 15-numFractionalBits, numFractionalBits, fiducialMarkerType, fiducialMarkerType, fiducialMarkerType)];
for i = 1:size(marker.Xprobes, 1)
    outputString = [outputString, '    {', sprintf('%d, ', int32(round(2^numFractionalBits)*marker.Xprobes(i,:))), '},\n'];
end
outputString = [outputString, '};\n\n'];
    
outputString = [outputString, sprintf('// SQ%d.%d\nconst short probesY_type%d[NUM_BITS_TYPE_%d][NUM_PROBES_PER_BIT_TYPE_%d] = {\n', 15-numFractionalBits, numFractionalBits, fiducialMarkerType, fiducialMarkerType, fiducialMarkerType)];
for i = 1:size(marker.Xprobes, 1)
    outputString = [outputString, '    {', sprintf('%d, ', int32(round(2^numFractionalBits)*marker.Yprobes(i,:))), '},\n'];
end
outputString = [outputString, '};\n\n'];

outputString = [outputString, sprintf('// SQ%d.%d\nconst short probeWeights_type%d[NUM_BITS_TYPE_%d][NUM_PROBES_PER_BIT_TYPE_%d] = {\n', 15-numFractionalBits, numFractionalBits, fiducialMarkerType, fiducialMarkerType, fiducialMarkerType)];
for i = 1:size(marker.Xprobes, 1)
    outputString = [outputString, '    {', sprintf('%d, ', int32(round(2^numFractionalBits)*marker.ProbeWeights(i,:))), '},\n'];
end
outputString = [outputString, sprintf('};\n\n#endif //%s\n\n',fiducialMarkerTypeName)];

disp(sprintf(outputString));

disp(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));

% keyboard
