// bitmap stuff
#pragma pack(push, 2)
typedef struct tagBITMAPFILEHEADER {
  uint16_t  bfType;
  uint32_t bfSize;
  uint16_t  bfReserved1;
  uint16_t  bfReserved2;
  uint32_t bfOffBits;
} BITMAPFILEHEADER;

typedef struct tagBITMAPINFOHEADER {
  uint32_t biSize;
  int32_t  biWidth;
  int32_t  biHeight;
  uint16_t  biPlanes;
  uint16_t  biBitCount;
  uint32_t biCompression;
  uint32_t biSizeImage;
  int32_t  biXPelsPerMeter;
  int32_t  biYPelsPerMeter;
  uint32_t biClrUsed;
  uint32_t biClrImportant;
} BITMAPINFOHEADER;
#pragma pack(pop)

BITMAPFILEHEADER bmpHeader;
BITMAPINFOHEADER bmpInfo;
uint32_t colorTab[256];
uint8_t imageBuffer[18720];
uint8_t twoLineBuffer[2*192];


void getImage() {
	uint8_t fp_result = finger.getImage();
	printf("genImage result: 0x%0x\n", fp_result);

	finger.LEDcontrol(FINGERPRINT_LED_ON, 0, 0x01);

	fp_result = finger.uploadImage(imageBuffer);
	printf("uploadImage result: 0x%0x\n block count: %d\n", fp_result, finger.packetCount);

	finger.LEDcontrol(FINGERPRINT_LED_OFF, 0, 0x01);

	bmpHeader.bfType = 0x4d42; // magic number "BM"
	bmpHeader.bfOffBits = sizeof(bmpHeader) + sizeof(bmpInfo) + sizeof(colorTab);
	bmpHeader.bfSize = 0; // bmpHeader.bfOffBits + 192*192;

	bmpInfo.biSize = sizeof(bmpInfo);
	bmpInfo.biWidth = 192;
	bmpInfo.biHeight = 192;
	bmpInfo.biPlanes = 1;
	bmpInfo.biBitCount = 8;
	bmpInfo.biCompression = 0;
	bmpInfo.biSizeImage = 0; // 192 * 192 * 1;
	bmpInfo.biXPelsPerMeter = 0; // 20000;		// 508 DPI
	bmpInfo.biYPelsPerMeter = 0; // 20000;		// 508 DPI
	bmpInfo.biClrUsed = 0;					// max. no. of colors
	bmpInfo.biClrImportant = 0;				// all colors used


	File imageFile(&fs, "R503-Image.bmp", O_RDWR | O_CREAT);
	// headers
	imageFile.write(&bmpHeader, sizeof(bmpHeader));
	imageFile.write(&bmpInfo, sizeof(bmpInfo));

	// colortable
	uint32_t colorItem = 0;
	for(int i=0; i<256; i++) {
		colorTab[i] = colorItem;
		colorItem += 0x00010101;
	}
	imageFile.write(colorTab, sizeof(colorTab));

	// image data 4 Bit -> 8 Bit
	for(int y=0; y < 192/2; y++) {
		for(int x=0; x < 192; x++) {
			uint8_t val1 = (imageBuffer[x + y*192] >> 4) * 16;
			uint8_t val2 = (imageBuffer[x + y*192] & 0xf) * 16;
			twoLineBuffer[x] = val1;
			twoLineBuffer[x + 192] = val2;
		}
		imageFile.write(twoLineBuffer, sizeof(twoLineBuffer));
	}

	imageFile.close();
}