#include <TFT_eSPI.h>
#include "Image.h"
TFT_eSPI tft = TFT_eSPI();

uint16_t adjustBrightness(uint16_t color, int brightness) {
  uint8_t r = color >> 11;
  uint8_t g = (color >> 5) & 0x3f;
  uint8_t b = color & 0x1f;

  r = (r*brightness)/100;
  g = (g*brightness)/100;
  b = (b*brightness)/100;

  return (r << 11) | (g << 5) | b;
}

uint8_t sqrt_fraction(uint32_t num) {
  if (num > (0x40000000)) return 0;
  uint32_t bsh = 0x00004000;
  uint32_t fpr = 0;
  uint32_t osh = 0;

  // Auto adjust from U8:8 up to U15:16
  while (num>bsh) {bsh <<= 2; osh++;}

  do {
    uint32_t bod = bsh + fpr;
    if(num >= bod)
    {
      num -= bod;
      fpr = bsh + bod;
    }
    num <<= 1;
  } while(bsh >>= 1);

  return fpr>>osh;
}


void coverSmoothRoundRect(int32_t x, int32_t y, int32_t w, int32_t h, int32_t r, int32_t br, const uint16_t *image, uint32_t bg_color)
{
  Serial.println("Function called");
  tft.startWrite();

  int32_t xs = 0;
  int32_t cx = 0;

  // Limit radius to half width or height
  if (r < 0)   r = 0;
  if (r > w/2) r = w/2;
  if (r > h/2) r = h/2;

  y += r;
  h -= 2*r;
  // tft.fillRect(x, y, w, h, color);
  
  uint16_t tempData[w];
  for(int i=0; i<h; i++) {
    for(int j=0; j<w; j++) {
      uint16_t color = adjustBrightness(image[(i+r)*w+j], br);
      tempData[j] = (color << 8) | (color >> 8);
    }
    tft.pushImage(x,y+i, w,1, tempData);
  }
  Serial.println("Rect filled");

  int32_t width = w;

  h--;
  x += r;
  w -= 2*r+1;

  int32_t r1 = r * r;
  r++;
  int32_t r2 = r * r;

  for (int32_t cy = r - 1; cy > 0; cy--)
  {
    int32_t dy2 = (r - cy) * (r - cy);
    for (cx = xs; cx < r; cx++)
    {
      int32_t hyp2 = (r - cx) * (r - cx) + dy2;
      if (hyp2 <= r1) break;
      if (hyp2 >= r2) continue;

      uint8_t alpha = ~sqrt_fraction(hyp2);
      if (alpha > 246) break;
      xs = cx;
      if (alpha < 9) continue;

      int32_t x1 = x + cx - r, y1 = y + cy - r;
      int32_t x2 = x - cx + r + w, y2 = y - cy + r + h;
      tft.drawPixel(x1, y1, adjustBrightness(image[(y1-y+r-1)*width+(x1-x+r-1)], br), alpha, bg_color);
      tft.drawPixel(x2, y1, adjustBrightness(image[(y1-y+r-1)*width+(x2-x+r-1)], br), alpha, bg_color);
      tft.drawPixel(x2, y2, adjustBrightness(image[(y2-y+r-1)*width+(x2-x+r-1)], br), alpha, bg_color);
      tft.drawPixel(x1, y2, adjustBrightness(image[(y2-y+r-1)*width+(x1-x+r-1)], br), alpha, bg_color);
    }
    int32_t xx1 = x + cx - r, yy1 = y + cy - r;
    int32_t xx2 = x - cx + r + w, yy2 = y - cy + r + h;
    // tft.drawFastHLine(xx1, yy1, 2 * (r - cx) + 1 + w, image[(yy1-y)*w+(xx1-x)]);
    // tft.drawFastHLine(xx1, yy2, 2 * (r - cx) + 1 + w, image[(yy2-y)*w+(xx1-x)]);
    int32_t l = 2 * (r - cx) + 1 + w;
    uint16_t tempData[l];
    for(int i=0; i<l; i++) {
      uint16_t color = adjustBrightness(image[(yy1-y+r-1)*width+(xx1-x+i+r-1)], br);
      tempData[i] = (color << 8) | (color >> 8);
    }
    tft.pushImage(xx1,yy1, l,1, tempData);
    for(int i=0; i<l; i++) {
      uint16_t color = adjustBrightness(image[(yy2-y+r-1)*width+(xx1-x+i+r-1)], br);
      tempData[i] = (color << 8) | (color >> 8);
    }
    tft.pushImage(xx1,yy2, l,1, tempData);
  }
  
  tft.endWrite();
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");

  tft.init();

}

void loop() {
  coverSmoothRoundRect(5,20, 232,232, 30, 40, Image1, 0x0);

  while(1);
}