/*
  ESP32 FreeRTOS OLED Animation Demo (SSD1306 via I2C)
  ----------------------------------------------------
  - Framework: Arduino-ESP32 (uses FreeRTOS under the hood)
  - Display: 128x64 SSD1306 (Adafruit_SSD1306 + Adafruit_GFX)
  - I2C pins (default ESP32): SDA=21, SCL=22 (change below if needed)
  - Buttons: 4 GPIOs with internal pullups (see defines)

  Features
  --------
  • Separate FreeRTOS tasks for Render, Input and Stats
  • Queue-based command channel (pause/resume, speed up/down, switch mode)
  • Mutex-protected display access (avoid tearing)
  • Bouncing logo animation + simple sprite mode
  • FPS and heap usage overlay (toggled)

  Wiring (ESP32 DevKit V1 example)
  --------------------------------
  SSD1306 VCC -> 3V3
  SSD1306 GND -> GND
  SSD1306 SCL -> GPIO22
  SSD1306 SDA -> GPIO21

  Buttons to GND (active LOW):
  BTN_PAUSE -> GPIO18
  BTN_MODE  -> GPIO19
  BTN_SPD+  -> GPIO5
  BTN_SPD-  -> GPIO4

  Libraries
  ---------
  • Adafruit GFX Library
  • Adafruit SSD1306

  (Install from Arduino Library Manager)

  Build
  -----
  • Arduino IDE: Board = ESP32 Dev Module
  • PlatformIO: framework=arduino, platform=espressif32, board=esp32dev
*/

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// -------------------- User Config --------------------
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1           // Shared reset
#define OLED_ADDR     0x3C
#define I2C_SDA_PIN   21
#define I2C_SCL_PIN   22

#define BTN_PAUSE 18
#define BTN_MODE  19
#define BTN_SPD_UP 5
#define BTN_SPD_DN 4

// -----------------------------------------------------
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Task handles
TaskHandle_t renderTaskHandle = nullptr;
TaskHandle_t inputTaskHandle  = nullptr;
TaskHandle_t statsTaskHandle  = nullptr;

// Mutex for display operations
SemaphoreHandle_t dispMutex;

// Command queue
enum CmdType : uint8_t { CMD_TOGGLE_PAUSE, CMD_SPEED_UP, CMD_SPEED_DN, CMD_TOGGLE_MODE, CMD_TOGGLE_OVERLAY };
struct Cmd { CmdType type; };
QueueHandle_t cmdQ;

// Animation state
volatile bool paused = false;
volatile bool overlay = true;     // show FPS/heap
volatile uint8_t mode = 0;        // 0=bounce, 1=sprites

// Physics
float vx = 1.7f;
float vy = 1.1f;
float speedScale = 1.0f;          // adjusted via buttons

// Sprite (little 8x8 smiley)
static const uint8_t PROGMEM SPRITE_8[] = {
  0b00111100,
  0b01000010,
  0b10100101,
  0b10000001,
  0b10100101,
  0b10011001,
  0b01000010,
  0b00111100
};

// Adafruit logo (16x16) packed as bitmap for the bounce demo
static const uint8_t PROGMEM LOGO_16[] = {
  0x00,0x00,0x03,0xC0,0x07,0xE0,0x0E,0x70,0x1C,0x38,0x39,0x9C,0x33,0xCC,0x73,0xCE,
  0x73,0xCE,0x33,0xCC,0x39,0x9C,0x1C,0x38,0x0E,0x70,0x07,0xE0,0x03,0xC0,0x00,0x00
};

// Frame timing
static const TickType_t TICK_60HZ = pdMS_TO_TICKS(16); // ~60 FPS

// Shared frame metrics
volatile uint32_t frameCount = 0;
volatile uint32_t lastFpsCount = 0;
volatile uint32_t lastFpsTimeMs = 0;
volatile float fps = 0.0f;

// Utility: thread-safe display draw lambda
#define WITH_DISPLAY(lock, body)                 \
  do {                                           \
    if (xSemaphoreTake((lock), pdMS_TO_TICKS(50))) { \
      body;                                      \
      xSemaphoreGive((lock));                    \
    }                                            \
  } while(0)

// -------------------- Tasks --------------------
void renderTask(void*);
void inputTask(void*);
void statsTask(void*);

void applyCommand(const Cmd &c) {
  switch (c.type) {
    case CMD_TOGGLE_PAUSE: paused = !paused; break;
    case CMD_SPEED_UP: speedScale = min(4.0f, speedScale + 0.1f); break;
    case CMD_SPEED_DN: speedScale = max(0.1f, speedScale - 0.1f); break;
    case CMD_TOGGLE_MODE: mode = (mode + 1) % 2; break;
    case CMD_TOGGLE_OVERLAY: overlay = !overlay; break;
  }
}

void setupButtons() {
  pinMode(BTN_PAUSE, INPUT_PULLUP);
  pinMode(BTN_MODE,  INPUT_PULLUP);
  pinMode(BTN_SPD_UP, INPUT_PULLUP);
  pinMode(BTN_SPD_DN, INPUT_PULLUP);
}

void setup() {
  Serial.begin(115200);
  delay(100);
  Serial.println("ESP32 FreeRTOS OLED Animation");

  // I2C
  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);

  // OLED init
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    Serial.println("SSD1306 allocation failed");
    while (true) { delay(1000); }
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("FreeRTOS + OLED");
  display.display();

  setupButtons();

  // RTOS sync primitives
  dispMutex = xSemaphoreCreateMutex();
  cmdQ = xQueueCreate(10, sizeof(Cmd));

  // Create tasks (pin render to core 1 for smoother FPS)
  xTaskCreatePinnedToCore(renderTask, "render", 4096, nullptr, 2, &renderTaskHandle, 1);
  xTaskCreatePinnedToCore(inputTask,  "input",  2048, nullptr, 3, &inputTaskHandle, 0);
  xTaskCreatePinnedToCore(statsTask,  "stats",  2048, nullptr, 1, &statsTaskHandle, 0);
}

void loop() {
  // not used — all logic in tasks
  vTaskDelay(pdMS_TO_TICKS(1000));
}

// -------------------- Render Task --------------------
void drawOverlay() {
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.fillRect(0,0,SCREEN_WIDTH,8,SSD1306_BLACK); // clear text row
  display.printf("FPS:%2.0f  spd:%.1f  heap:%u", fps, speedScale, (unsigned)ESP.getFreeHeap());
}

void drawBounce(float &x, float &y, int &w, int &h) {
  // Draw 16x16 logo
  display.drawBitmap((int)x, (int)y, LOGO_16, 16, 16, SSD1306_WHITE);
  w = 16; h = 16;
}

void drawSprites(uint32_t tms) {
  // scatter a few 8x8 sprites based on time
  for (int i = 0; i < 8; ++i) {
    int x = (int)((tms / (20 + i*7)) % (SCREEN_WIDTH - 8));
    int y = (int)((tms / (13 + i*5)) % (SCREEN_HEIGHT - 8));
    display.drawBitmap(x, y, SPRITE_8, 8, 8, SSD1306_WHITE);
  }
}

void renderTask(void*) {
  float x = 10, y = 10;
  int bw = 16, bh = 16;
  uint32_t lastMs = millis();
  lastFpsTimeMs = lastMs;

  for (;;) {
    // Process any pending commands quickly before rendering
    Cmd c;
    while (xQueueReceive(cmdQ, &c, 0) == pdTRUE) {
      applyCommand(c);
    }

    uint32_t now = millis();
    uint32_t dt = now - lastMs;
    lastMs = now;

    if (!paused) {
      // update physics for bounce mode
      x += vx * speedScale;
      y += vy * speedScale;
      if (x <= 0) { x = 0; vx = fabs(vx); }
      if (y <= 0) { y = 0; vy = fabs(vy); }
      if (x + bw >= SCREEN_WIDTH)  { x = SCREEN_WIDTH - bw; vx = -fabs(vx); }
      if (y + bh >= SCREEN_HEIGHT) { y = SCREEN_HEIGHT - bh; vy = -fabs(vy); }
    }

    WITH_DISPLAY(dispMutex, {
      display.clearDisplay();

      if (mode == 0) {
        drawBounce(x, y, bw, bh);
      } else {
        drawSprites(now);
      }

      if (overlay) drawOverlay();

      display.display();
    });

    // FPS calc
    frameCount++;
    uint32_t elapsed = now - lastFpsTimeMs;
    if (elapsed >= 500) { // update twice per second
      fps = (float)(frameCount - lastFpsCount) * 1000.0f / (float)elapsed;
      lastFpsCount = frameCount;
      lastFpsTimeMs = now;
    }

    vTaskDelay(TICK_60HZ);
  }
}

// -------------------- Input Task --------------------
static bool readButton(uint8_t pin) {
  // Basic debounce
  static uint32_t lastMs[40] = {0};
  uint32_t now = millis();
  if (digitalRead(pin) == LOW) {
    if (now - lastMs[pin] > 180) { lastMs[pin] = now; return true; }
  }
  return false;
}

void inputTask(void*) {
  // long-press overlay toggle on PAUSE button
  uint32_t pressStart = 0;
  bool pressed = false;

  for (;;) {
    if (readButton(BTN_PAUSE)) {
      Cmd c{CMD_TOGGLE_PAUSE};
      xQueueSend(cmdQ, &c, 0);
      pressed = true; pressStart = millis();
    }
    if (readButton(BTN_MODE)) {
      Cmd c{CMD_TOGGLE_MODE};
      xQueueSend(cmdQ, &c, 0);
    }
    if (readButton(BTN_SPD_UP)) {
      Cmd c{CMD_SPEED_UP};
      xQueueSend(cmdQ, &c, 0);
    }
    if (readButton(BTN_SPD_DN)) {
      Cmd c{CMD_SPEED_DN};
      xQueueSend(cmdQ, &c, 0);
    }

    // Long press on PAUSE toggles overlay
    if (pressed && (millis() - pressStart > 700)) {
      pressed = false;
      Cmd c{CMD_TOGGLE_OVERLAY};
      xQueueSend(cmdQ, &c, 0);
    }

    vTaskDelay(pdMS_TO_TICKS(10));
  }
}

// -------------------- Stats Task --------------------
void statsTask(void*) {
  for (;;) {
    // could do periodic logging or power-saving here
    // For demo, just print every 2 seconds
    Serial.printf("FPS=%.1f, speed=%.1f, heap=%u, mode=%u, paused=%d\n",
                  fps, speedScale, (unsigned)ESP.getFreeHeap(), mode, paused);
    vTaskDelay(pdMS_TO_TICKS(2000));
  }
}