/*
  Support function for Floyd-Steinberg dithering of an 8bit grey-scale BMP image
  on a Monochrome display:
  https://en.wikipedia.org/wiki/Floyd%E2%80%93Steinberg_dithering

  Bitmap format:
  https://en.wikipedia.org/wiki/BMP_file_format
  
  Example for https://github.com/Bodmer/TFT_eSPI
  
  The MIT License (MIT)
  Copyright (c) 2015 by Bodmer
  Permission is hereby granted, free of charge, to any person obtaining a copy
  of this software and associated documentation files (the "Software"), to deal
  in the Software without restriction, including without limitation the rights
  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  copies of the Software, and to permit persons to whom the Software is
  furnished to do so, subject to the following conditions:
  The above copyright notice and this permission notice shall be included in all
  copies or substantial portions of the Software.
  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYBR_DATUM HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  SOFTWARE.

  Note: drawFSBmp() is a simplified function and does not handle all possible
  BMP file header variants. It works OK with 8 bit per pixel grey-scale images
  generated by MS Paint and IrfanView.
*/

// https://github.com/Bodmer/TFT_eSPI

//====================================================================================
// Draw an 8 bit grey-scale bitmap (*.BMP) on a Monochrome display using dithering
//====================================================================================
// Uses RAM for buffers (3 * width + 4) ( 532 bytes for 176 pixels)

//   Image must be stored in ESP8266 or ESP32 SPIFFS

//    Quantisation error distribution for pixel X
//     (This is for bottum up drawing of the BMP)
//          |-------|-------|-------|
//          | +3/16 | +5/16 | +1/16 |
//          |-------|-------|-------|
//          |       |   X   | +7/16 |
//          |-------|-------|-------|
//

void drawFSBmp(const char *filename, int16_t x, int16_t y) {

  if ((x >= frame.width()) || (y >= frame.height())) return;

  fs::File   bmpFS;

  // Open requested file
  bmpFS = SPIFFS.open( filename, "r");

  if (!bmpFS)
  {
    Serial.print("File not found");
    return;
  }

  uint32_t seekOffset, dib_size;
  uint16_t w, h, row, col, num_colors;
  uint8_t  r, g, b;

  if (read16(bmpFS) == 0x4D42)         // Check it is a valid bitmap header
  {
    read32(bmpFS);
    read32(bmpFS);
    seekOffset = read32(bmpFS);        // Pointer to image start
    dib_size   = read32(bmpFS);        // DIB header size, typically 40 bytes

    w = read32(bmpFS);                 // Get width and height of image
    h = read32(bmpFS);

    //  Check it is 1 plane  and   8 bits per pixel  and       no compression
    if ((read16(bmpFS) == 1) && (read16(bmpFS) == 8) && (read32(bmpFS) == 0))
    {
      read32(bmpFS); // Throw away image size
      read32(bmpFS); // Throw away x pixels per meter
      read32(bmpFS); // Throw away y pixels per meter

      num_colors = read32(bmpFS);           // Number of colours in colour table (usually 256)

      uint8_t pixel_color[num_colors];      // Lookup table for grey-scale

      bmpFS.seek(14 + dib_size);            // Seek to start of colour table

      // Capture the colour lookup table
      for (uint16_t i = 0; i < num_colors; i++)
      {
        uint32_t abgr = read32(bmpFS);      // Assume 4 byte, RGB colours in LS 3 bytes
        pixel_color[i] = (uint8_t) abgr;    // For grey-scale R, G, B are same value
      }

      bmpFS.seek(seekOffset);               // Seek to start of image

      uint16_t padding = (4 - (w & 3)) & 3; // Calculate the BMP line padding

      // Create an zero an 8 bit pixel line buffer
      uint8_t* lineBuffer = ( uint8_t*) calloc(w    , sizeof(uint8_t));

      // Create a 16 bit signed line buffer for the quantisation error
      // Diffusion spreads to x-1 and x+1 so w + 2 avoids a bounds check
      int16_t* qerrBuffer = ( int16_t*) calloc((w + 2)<<1, sizeof(uint8_t));

      y += h - 1; // Start from bottom (assumes bottum up!)

      // Draw row by row from bottom up
      for (row = 0; row < h; row++) {

        // Read a row of pixels
        bmpFS.read(lineBuffer, w);

        // Prep variables
        uint16_t dx = 0;
        uint8_t* bptr = lineBuffer;
        int16_t* qptr = qerrBuffer + 1; // + 1 because diffusion spreads to x-1

        // Lookup color, add quantisation error, clip and clear error buffer
        while(dx < w)
        {
          int16_t depixel =  pixel_color[(uint8_t)*bptr] + *qptr;
          if (depixel >255) depixel = 255;   // Clip pixel to 0-255
          else if (depixel < 0) depixel = 0;
          *bptr++ = (uint8_t) depixel;       // Save new value, inc pointer
          *qptr++ = 0;                       // Zero error, inc pointer
          dx++;                              // Next pixel
        }

        dx = 0;                // Reset varaibles to start of line
        bptr = lineBuffer;
        qptr = qerrBuffer + 1;
        int32_t qerr = 0;
        int32_t qerr16 = 0;

        // Push the pixel row to screen
        while(dx < w)
        {
           // Add 7/16 of error (error = 0 on first entry)
          int16_t pixel = *bptr + (qerr>>1) - qerr16;

          // Do not clip here so quantisation error accumulates correctly?
          // Draw pixel (black or white) and determine new error
          if (pixel < 128) { frame.drawPixel(x + dx, y, INK); qerr = pixel; }
          else qerr = pixel - 255;

          // Diffuse into error buffer for next pixel line
          qerr16 = qerr>>4;                  //     1/16 of error
          *(qptr - 1) += (qerr>>2) - qerr16; // Add 3/16 of error
          *(qptr    ) += (qerr>>2) + qerr16; // Add 5/16 of error
          *(qptr + 1) +=  qerr16;            // Add 1/16 of error

          *bptr++; // Move along pixel and error buffers
          *qptr++;
          dx++;    // Move coordinate along
        }
        y--;

        // Read any line padding (saves a slow seek)
        if (padding) bmpFS.read(lineBuffer, padding);
      }
    }
    else Serial.println("BMP format not recognized.");
  }
  bmpFS.close();
}

//====================================================================================
// Read a 16 bit value from the filing system
//====================================================================================
uint16_t read16(fs::File &f) {
  uint16_t result;
  ((uint8_t *)&result)[0] = f.read(); // LSB
  ((uint8_t *)&result)[1] = f.read(); // MSB
  return result;
}

//====================================================================================
// Read a 32 bit value from the filing system
//====================================================================================
uint32_t read32(fs::File &f) {
  uint32_t result;
  ((uint8_t *)&result)[0] = f.read(); // LSB
  ((uint8_t *)&result)[1] = f.read();
  ((uint8_t *)&result)[2] = f.read();
  ((uint8_t *)&result)[3] = f.read(); // MSB
  return result;
}

//  TODO: Add support for colour images by converting RGB to grey-scale
//  grey = (R+G+B)/3