/*
// GND ----- GND
// VCC ----- 3V3
// SCL ----- D18
// MOSI ----- D23
// RESET ----- D4
// DC ------ D2
// CS ------ D15
// BLK -----   
// MISO ---- D19

  Based on https://wokwi.com/projects/342032431249883731
*/

#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>

#include "photo.h"

#define BTN_PIN 5
#define TFT_DC 2
#define TFT_CS 15
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

#define RGB565_R(p) ((p) & 0x00F8)
#define RGB565_G(p) (((p) & 0x0007) << 5 | ((p) & 0xE000) >> 11)
#define RGB565_B(p) (((p) & 0x1F00) >> 5)
#define RGB565_GS(p) ((RGB565_R(p) + RGB565_R(p) + RGB565_R(p) + RGB565_B(p) + (RGB565_G(p) << 2)) >> 3)
#define RGB565(r, g, b) (((g) & 0x1C) << 11 | ((b) & 0xF8) << 5 | ((r) & 0xF8) | ((g) & 0xE0) >> 5)

uint16_t swap16(uint16_t value) {
    return (value >> 8) | (value << 8);
}

void setup() {
  Serial.begin(115200);
  Serial.print("Application " __FILE__ " compiled " __DATE__ " at " __TIME__ "\n");

  pinMode(BTN_PIN, INPUT_PULLUP);

  tft.begin();
  tft.setRotation(1);

  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.fillScreen(ILI9341_BLACK);

  tft.drawRGBBitmap(0, 0, photo_rgb565, photo_w, photo_h);

  int term_w = 50;
  int term_h = 30;
  for (int y = 0; y < term_h-2; y += 2) {
    int top_y = map(y, 0, term_h, 0, photo_h);
    int bottom_y = map(y+1, 0, term_h, 0, photo_h);
    for (int x = 0; x < term_w; x++) {
      int px = map(x, 0, term_w, 0, photo_w);
      
      uint16_t top_pixel = swap16(photo_rgb565[top_y * photo_w + px]);
      int top_r = RGB565_R(top_pixel);
      int top_g = RGB565_G(top_pixel);
      int top_b = RGB565_B(top_pixel);

      uint16_t bottom_pixel = swap16(photo_rgb565[bottom_y * photo_w + px]);
      int bottom_r = RGB565_R(bottom_pixel);
      int bottom_g = RGB565_G(bottom_pixel);
      int bottom_b = RGB565_B(bottom_pixel);

      Serial.printf("\033[38;2;%d;%d;%dm\033[48;2;%d;%d;%dm▄",
            top_r, top_g, top_b, bottom_r, bottom_g, bottom_b);
    }
    Serial.printf("\033[0m\n");
  }
}

#define ABS(x) (((x) > 0) ? (x) : (-(x)))
#define SGN(x) (((x) > 0) ? 1 : -1)
#define MIN(a,b) (((a)>(b)) ? (b) : (a))
#define MAX(a,b) (((a)>(b)) ? (a) : (b))

int octant(int dx, int dy) {
  int o = ((int)(4.5 + atan2f(1.0f * dy, -1.0f * dx) / (3.141593f / 4.0f)) % 8);
  return o;
}

int octant2(int dx, int dy) {
  int ax = ABS(dx);
  int ay = ABS(dy);
  #define TAN_PI_8 0.414
  if (ay <= ax * TAN_PI_8) { // mostly horizontal
    return (dx > 0) ? 0 : 4;
  } else if (ax < ay * TAN_PI_8) { // mostly vertical
    return (dy > 0) ? 2 : 6;
  } else {  // diagonal direction
    return (dx > 0) ? ((dy > 0) ? 1 : 7) : ((dy > 0) ? 3 : 5);
  }
}

uint16_t blend565(uint16_t c1, uint16_t c2, uint8_t alpha) {
    // Alpha converted from [0..255] to [0..31]
    uint32_t fg = c1;
    uint32_t bg = c2;
    uint32_t a = alpha >> 3;
    fg = (fg | fg << 16) & 0x07e0f81f;
    bg = (bg | bg << 16) & 0x07e0f81f;
    bg += (fg - bg) * a >> 5;
    bg &= 0x07e0f81f;
    return (uint16_t)(bg | bg >> 16);
}


void loop() {
  Serial.print("Draw screen \n");
  tft.fillScreen(ILI9341_BLACK);
  #define W 160
  #define H 120
  int w = W;
  int h = H;
  
  static uint16_t frame[W*H*sizeof(uint16_t)];

  for (int y = 0; y < h; y+=1) {
    for (int x = 0; x < w; x+=1) {
      int dx = x - (w/2);
      int dy = y - (h/2);

      uint8_t r, g, b;
      HSV2RGB(octant2(dx, dy) * 32, 255, 255, &r, &g, &b);
      //HSV2RGB((uint8_t)(128.0 * atan2f(dy, dx) / 3.141692 + 127), 255, 255, &r, &g, &b);

      uint16_t color = tft.color565(r, g, b);
      color = blend565(color, photo_rgb565[w*y + x], 127);
      //tft.writePixel(x, y, color);
      //tft.fillRect(x, y, 1, 1, color);
      frame[h*y + x] = color;
    }
  }
  tft.drawRGBBitmap(0, 0, frame, w, h);

  tft.startWrite();
  for (int y = 0; y < 10; y++) {
    for (int x = 0; x < w; x++) {
      uint8_t r, g, b;
      uint8_t h = (255 * x) / w;
      HSV2RGB(h, 255, 255, &r, &g, &b);
      uint16_t color = tft.color565(r, g, b);
      tft.writePixel(x, y, color);
    }
    delay(1);
  }
  tft.endWrite();

  for (int y = 0; y < h; y += 20) {
    for (int x = 0; x < w; x += 20) {
      tft.setCursor(x,y);
      tft.print(octant2(x - w/2, y - h/2));
    }
    delay(1);
  }

  Serial.print("Done ! \n");
  delay(30000);
}


/**
 * @brief HSV to RGB conversion, using integer arithmetic only
 * @param h hue (0..255) 
 * @param s saturation (0..255) 
 * @param v value (0..255) 
 * @param *r, *g, *b pointers to red, green, blue
 */
void HSV2RGB(uint8_t h, uint8_t s, uint8_t v, uint8_t *r, uint8_t *g, uint8_t *b) {
  #define HEXANT (255/6)
  int i = MIN(5, h/HEXANT);   // i \in [0, 5]
  uint8_t f = (255 * (h - i*HEXANT)) / HEXANT;  // f \in [0, 255]
  uint8_t p = v * (255 - s) / 255;
  uint8_t q = v * (255 - s * f / 255) / 255;
  uint8_t t = v * (255 - s * (255 - f) / 255) / 255;
  switch(i) {
    case 0: *r = v; *g = t; *b = p; break;
    case 1: *r = q; *g = v; *b = p; break;
    case 2: *r = p; *g = v; *b = t; break;
    case 3: *r = p; *g = q; *b = v; break;
    case 4: *r = t; *g = p; *b = v; break;
    default:*r = v; *g = p; *b = q; break;
  }
}

#if 0
// Function to output the image to an ANSI-capable terminal, with resizing
void print_image_ansi_resized(const img_t *img, int term_w, int term_h) {
    // Calculate the scaling factors to fit the image within the terminal size
    float scale_x = (float)img->w / term_w;
    float scale_y = (float)img->h / (term_h * 2); // 2 pixels per character height

    for (int y = 0; y < term_h; y++) {
        for (int x = 0; x < term_w; x++) {
            // Compute the source pixels for top and bottom parts of each character cell
            int src_x = x * scale_x;
            int src_y_top = (y * 2) * scale_y;
            int src_y_bottom = (y * 2 + 1) * scale_y;

            // Get top pixel color
            int top_r = 0, top_g = 0, top_b = 0;
            if (src_y_top < img->h && src_x < img->w) {
              uuint16_t c = img->data[src_y_top * img->w + src_x];
              top_r = RGB565_R(
                (, &top_r, &top_g, &top_b);
            }

            // Get bottom pixel color
            int bottom_r = 0, bottom_g = 0, bottom_b = 0;
            if (src_y_bottom < img->h && src_x < img->w) {
                (img->data[src_y_bottom * img->w + src_x], &bottom_r, &bottom_g, &bottom_b);
            }

            // Print ANSI escape codes for the colors

        }
        // Reset colors and move to the next line
        printf("\033[0m\n");
    }
}
#endif