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

// Pin definitions (match your wiring)
#define TFT_CS   10
#define TFT_DC    9
#define TFT_RST   8  // you can also use -1 if connected to Arduino reset

// Color definitions
#define BLACK    0x0000
#define BLUE     0x001F
#define RED      0xF800
#define GREEN    0x07E0
#define CYAN     0x07FF
#define MAGENTA  0xF81F
#define YELLOW   0xFFE0 
#define WHITE    0xFFFF

// Create the display object using hardware SPI
Adafruit_ILI9341 tft(TFT_CS, TFT_DC, TFT_RST);
float pixels[64];
uint16_t heatmap[64];

uint8_t mix8(uint8_t a, uint8_t b, float t) {
    return a + (b - a) * t;
}

uint16_t mixColor(uint16_t colorA, uint16_t colorB, float t) {
    // --- Extract RGB565 A
    uint8_t rA = ((colorA >> 11) & 0x1F) * 255 / 31;
    uint8_t gA = ((colorA >> 5)  & 0x3F) * 255 / 63;
    uint8_t bA = ( colorA        & 0x1F) * 255 / 31;

    // --- Extract RGB565 B
    uint8_t rB = ((colorB >> 11) & 0x1F) * 255 / 31;
    uint8_t gB = ((colorB >> 5)  & 0x3F) * 255 / 63;
    uint8_t bB = ( colorB        & 0x1F) * 255 / 31;

    // --- Interpolate channels
    uint8_t r = mix8(rA, rB, t);
    uint8_t g = mix8(gA, gB, t);
    uint8_t b = mix8(bA, bB, t);

    // --- Convert back to RGB565
    uint16_t r5 = (r * 31) / 255;
    uint16_t g6 = (g * 63) / 255;
    uint16_t b5 = (b * 31) / 255;

    return (r5 << 11) | (g6 << 5) | b5;
}

float lerpFloat(float a, float b, float t) {
    return a + (b - a) * t;
}

uint16_t colorFor(float n) {
    if (n < 0.33f) {
        float t = n / 0.33f;
        return mixColor(BLUE, RED, t);
    }
    if (n < 0.66f) {
        float t = (n - 0.33f) / 0.33f;
        return mixColor(RED, YELLOW, t);
    }
    float t = (n - 0.66f) / 0.34f;
    return mixColor(YELLOW, WHITE, t);
}

void generateFrame() {
    for (int row = 0; row < 8; row++) {
        for (int col = 0; col < 8; col++) {
            float t = random(0, 20);
            pixels[row * 8 + col] = t;
        };
    };
};

float normalize(float x, float minVal, float maxVal) {
    if (maxVal == minVal) return 0.0f;         // or 1.0f
    return (x - minVal) / (maxVal - minVal);
}

void generateHeatmap() {

    float minVal = 9999;
    float maxVal = -9999;

    for (int i = 0; i < 64; i++) {
        if (pixels[i] < minVal) minVal = pixels[i];
        if (pixels[i] > maxVal) maxVal = pixels[i];
    };

    for (int i = 0; i < 64; i++) {
        pixels[i] = normalize(pixels[i], minVal, maxVal);
        heatmap[i] = colorFor(pixels[i]);
    };

};

void generateUpscaledHeatmap() {
    int srcSize = 8;
    int upscaleFactor = 4;
    int outSize = srcSize * upscaleFactor;
    float srcScale = float(srcSize) / float(outSize);
    int targetSize = min(tft.width(), tft.height());
    float pxSize = targetSize / outSize;

    float hottestValue = -1.0;
    int hottestGX = 0;
    int hottestGY = 0;

    int x0 = 0;
    int y0 = 40;

    for (int gy = 0; gy < outSize; gy++) {
        float srcY = gy * srcScale;
        int yBase = (int)srcY;
        float fy = srcY - yBase;
        int y1 = yBase + 1;
        if (y1 > srcSize) y1 = srcSize - 1;

        for (int gx = 0; gx < outSize; gx++) {
            float srcX = gx * srcScale;
            int xBase = (int)srcX;
            float fx = srcX - xBase;
            int x1 = xBase + 1;
            if (x1 > srcSize) x1 = srcSize - 1;
            
            float v00 = pixels[yBase * srcSize + xBase];
            float v10 = pixels[yBase * srcSize + x1];
            float v01 = pixels[y1 * srcSize + xBase];
            float v11 = pixels[y1 * srcSize + x1];

            float vx0 = lerpFloat(v00, v10, fx);
            float vx1 = lerpFloat(v01, v11, fx);
            float v = lerpFloat(vx0, vx1, fy);

            if (v > hottestValue) {
                hottestValue = v;
                hottestGX = gx;
                hottestGY = gy;
            }

            uint16_t c = colorFor(v);

            int screenX = x0 + (int)(gx * pxSize);
            int screenY = y0 + (int)(gy * pxSize);
            int w = (int)(pxSize + 1);
            int h = (int)(pxSize + 1);

            tft.fillRect(screenX, screenY, w, h, c);
        }

    }
    
    int hotspotScreenX = x0 + int(hottestGX * pxSize);
    int hotspotScreenY = y0 + int(hottestGY * pxSize);

    tft.drawCircle(hotspotScreenX, hotspotScreenY, 4, GREEN);
    
};


// 240x320 px display

void drawHeatmap() {
    tft.fillScreen(BLACK);
    int cellSize = tft.width() / 8;
    int yMargin = 40;
    for (int row = 0; row < 8; row++) {
        for (int col = 0; col < 8; col++) {
            tft.fillRect(col * cellSize, row * cellSize + yMargin, cellSize, cellSize, heatmap[row * 8 + col]);
        };
    };
};

void setup() {
    Serial.begin(115200);
    tft.begin();
    tft.setRotation(0);
};

void loop() {
    generateFrame();
    generateHeatmap();
    generateUpscaledHeatmap();
    delay(100);
};