#include <Adafruit_ILI9341.h>
#include "cos_table.h"
#include "motif.h"

Adafruit_ILI9341 tft = Adafruit_ILI9341(9, 10);

void setup(void) {
  tft.begin();
  Serial.begin(115200);
  plot_rle(&smmotif, -32768, 20);
  plot_rle(&bigmotif, -32768, 110);
  delay(2000);
}

void loop() {
  constexpr auto buffer_lines = 3;
  uint16_t buffer[ILI9341_TFTWIDTH * buffer_lines];
  uint16_t *pixel = buffer, accum[9];
  uint8_t dither = 0x2d, delta[9];
  for (auto i = 0; i < 9; i++)
    delta[i] = rand(),  accum[i] = rand();

  for (auto y = 0; y < ILI9341_TFTHEIGHT;) {
    for (auto i = 0; i < 3; i++) {
      accum[i] += delta[i];
      accum[i + 3] = accum[i];
      accum[i + 6] += delta[i + 6];
    }

    dither ^= 0xff;
    for (auto x = 0; x < ILI9341_TFTWIDTH; x++) {
      for (auto i = 3; i < 6; i++) accum[i] += delta[i];
      dither = (dither >> 4) | (dither << 4);
      uint8_t dstep = dither & 2;
      // dstep = 0; // disable dithering
      uint8_t r = qadd8(dstep << 1, cos8u(accum[3] + cos16s(accum[6])));
      uint8_t g = qadd8(dstep << 0, cos8u(accum[4] + cos16s(accum[7])));
      uint8_t b = qadd8(dstep << 1, cos8u(accum[5] + cos16s(accum[8])));
      *pixel++ = ((r & 0xf8) << 8) | ((g & 0xfc) << 3) | (b >> 3);
    }

    if ((++y % buffer_lines) == 0) {
      pixel = buffer;
      tft.drawRGBBitmap(0, y - buffer_lines, buffer, ILI9341_TFTWIDTH, buffer_lines);
    }
  }
  uint8_t rem = ILI9341_TFTHEIGHT % buffer_lines;
  if (rem) tft.drawRGBBitmap(0, ILI9341_TFTHEIGHT - rem, buffer, ILI9341_TFTWIDTH, rem);
  delay(1000);
}



///////////////////////////////////////////////
// consumes GIMP's RLE-compressed RGB565 output
void plot_rle(const RLE565 *motif, int16_t x, int16_t y) {
  uint16_t height = pgm_read_word_near(&motif->height);
  uint16_t width = pgm_read_word_near(&motif->width);
  if (x == -32768) x = ILI9341_TFTWIDTH / 2 - width / 2;
  if (y == -32768) y = ILI9341_TFTHEIGHT / 2 - height / 2;

  const uint8_t *rle = motif->rle_pixel_data;
  uint16_t buffer[width], *dest = buffer;
  uint16_t xpos = 0, ypos = 0, val = 0;
  uint8_t length = 0;

  while (ypos < height) {
    if ((length & 0x7f) == 0) {
      length = pgm_read_byte_near(rle++);
      if (length & 128) {
        val = pgm_read_word_near(rle);
        rle += 2;
      }
    }

    if (length & 128) {
      uint8_t l = length & 0x7f;
      uint8_t plotwidth = l;
      if (xpos + plotwidth > width) {
        plotwidth -= xpos + plotwidth - width;
      }
      length -= plotwidth;
      xpos += plotwidth;
      do { *dest++ = val; } while (--plotwidth);

    } else {
      uint8_t plotwidth = length;
      if (xpos + plotwidth > width) {
        plotwidth -= xpos + plotwidth - width;
      }
      memcpy_P(dest, rle, plotwidth * 2);
      xpos += plotwidth;
      dest += plotwidth;
      rle += plotwidth * 2;
      length -= plotwidth;
    }
    if (xpos >= width) {
      tft.drawRGBBitmap(x, y + ypos, buffer, width, 1);
      dest = buffer;
      ypos += 1;
      xpos = 0;
    }
  }
}




// 8-bit saturating add borrowed from FastLED
__attribute__ ((always_inline)) static inline uint8_t qadd8(uint8_t i, uint8_t j) {
#if defined(__AVR__)
  asm volatile(
      "add %0, %1    \n\t"
      "brcc L_%=     \n\t"
      "ldi %0, 0xFF  \n\t"
      "L_%=: "
      : "+a" (i)
      : "a"  (j)
  );
  return i;
#elif defined(__arm__)
  asm volatile( "uqadd8 %0, %0, %1" : "+r" (i) : "r" (j));
  return i;
#else
  unsigned int t = i + j;
  if (t > 255) t = 255;
  return t;
#endif
}