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

// ---------- Pico 2 WH Pinbelegung ----------
#define TFT_CS   17    // GP17
#define TFT_DC   20    // GP20
#define TFT_RST  21    // GP21
// SPI0: SCK = GP18, MOSI = GP19, MISO = GP16 (MISO optional)

// Poti & Buttons
#define POT_RPM_PIN 26 // GP26 (ADC0)

#define BTN_RUN     2  // GP2
#define BTN_FLOAT   3  // GP3
#define BTN_TS1     4  // GP4
#define BTN_TS2     5  // GP5
#define BTN_TS3     6  // GP6

Adafruit_ILI9341 display(TFT_CS, TFT_DC, TFT_RST);

// -------- Physik / Poti --------
const int  RPM_MIN = 100;
const int  RPM_MAX = 4000;
const float WHEEL_DIAMETER_M = 0.15f;
const float EFFICIENCY       = 0.90f;

// -------- Zustände --------
bool isRunning = false;
unsigned long lastRunToggleMs = 0;
const unsigned long debounceMs = 150;

unsigned long lastCalcMs = 0;
const unsigned long calcIntervalMs = 120; // ~8–10 Hz
float rpmUpper = 0, rpmLower = 0;
float speed_kmh = 0;
float baseRpmSet = 2000, baseRpmFiltered = 2000;
const float POT_ALPHA = 0.25f;

// -------- Mode & Trim --------
enum TrimMode { MODE_FLOAT, MODE_TS1, MODE_TS2, MODE_TS3 };
TrimMode trimMode = MODE_FLOAT;
float trimDelta = 0.0f;
const float PRESET_FLOAT = 0.00f;
const float PRESET_TS1   = 0.02f;
const float PRESET_TS2   = 0.04f;
const float PRESET_TS3   = 0.06f;

// -------- Anzeige-Tracking --------
int   lastUpper = -1, lastLower = -1, lastSpeed = -1;
int   lastTrimPct = -1;
TrimMode lastMode = MODE_FLOAT;
bool  lastRunBlinkOn = false;

// -------- Positionen (Portrait-Layout 240 × 320) --------
const int HDR_OFF_X = 80, HDR_OFF_Y = 15;
const int HDR_RUNNING_X = 20, HDR_RUNNING_Y = 15;
const int UPPER_VAL_X = 90,  UPPER_VAL_Y = 100;
const int LOWER_VAL_X = 90,  LOWER_VAL_Y = 125;
const int MODE_VAL_X  = 10,  MODE_VAL_Y  = 200;

// Speed linksbündig + „km/h“ separat
const int SPEED_X     = 10;
const int SPEED_Y     = 265;

// --------------------------------------------------------
//   Hilfsfunktionen für flackerfreie Anzeige
// --------------------------------------------------------
void drawTextWithBG(int x, int y, const char* txt, uint16_t fg, uint16_t bg, uint8_t size) {
  display.setTextSize(size);
  display.setTextColor(fg, bg);
  display.setCursor(x, y);
  display.print(txt);
}

void eraseText(int x, int y, const char* txt, uint8_t size) {
  drawTextWithBG(x, y, txt, ILI9341_BLACK, ILI9341_BLACK, size);
}

// Int flackerfrei, mit 4 Stellen (führende Nullen)
void drawIntNoFlicker(int x, int y, uint8_t size, uint16_t color, int lastVal, int newVal) {
  char buf[8];
  if (lastVal >= 0) {
    snprintf(buf, sizeof(buf), "%04d", lastVal);
    eraseText(x, y, buf, size);
  }
  snprintf(buf, sizeof(buf), "%04d", newVal);
  drawTextWithBG(x, y, buf, color, ILI9341_BLACK, size);
}

// Links­bündig, 3-stellige Zahl mit führenden Nullen (für km/h)
void drawIntLeftAligned(int x, int y, uint8_t size, uint16_t color, int lastVal, int newVal) {
  char buf[8];
  if (lastVal >= 0) {
    snprintf(buf, sizeof(buf), "%03d", lastVal);
    drawTextWithBG(x, y, buf, ILI9341_BLACK, ILI9341_BLACK, size);
  }
  snprintf(buf, sizeof(buf), "%03d", newVal);
  drawTextWithBG(x, y, buf, color, ILI9341_BLACK, size);
}

// --------------------------------------------------------
void drawStaticUI() {
  display.fillScreen(ILI9341_BLACK);
  display.setTextSize(2);
  display.setTextColor(ILI9341_WHITE);
  display.setCursor(10, 70);    display.print("Roller Speed");
  display.setCursor(10, 100);   display.print("Upper:");
  display.setCursor(10, 125);   display.print("Lower:");
  display.setCursor(150, 100);  display.print("RPM");
  display.setCursor(150, 125);  display.print("RPM");
  display.setCursor(10, 170);   display.print("Selected Mode");
  display.setCursor(10, 240);   display.print("Ball Speed");
  display.setCursor(160, 305);  display.print("km/h");
}

// --------------------------------------------------------
void updateRunHeader(bool force = false) {
  static bool lastIsRunning = false;
  static bool lastBlinkOn   = false;

  bool blinkOn = isRunning && ((millis() / 500) % 2 == 0);
  bool stateChanged = (isRunning != lastIsRunning);
  if (!(force || stateChanged || (isRunning && blinkOn != lastBlinkOn))) return;

  const char* TXT_OFF = "OFF";
  const char* TXT_RUN = "RUNNING";
  const uint8_t SIZE = 5;

  if (isRunning) {
    eraseText(HDR_OFF_X, HDR_OFF_Y, TXT_OFF, SIZE);
    if (blinkOn)
      drawTextWithBG(HDR_RUNNING_X, HDR_RUNNING_Y, TXT_RUN, ILI9341_GREEN, ILI9341_BLACK, SIZE);
    else
      eraseText(HDR_RUNNING_X, HDR_RUNNING_Y, TXT_RUN, SIZE);
  } else {
    eraseText(HDR_RUNNING_X, HDR_RUNNING_Y, TXT_RUN, SIZE);
    drawTextWithBG(HDR_OFF_X, HDR_OFF_Y, TXT_OFF, ILI9341_RED, ILI9341_BLACK, SIZE);
  }

  lastIsRunning = isRunning;
  lastBlinkOn = blinkOn;
}

// --------------------------------------------------------
void updateTrimDisplay(bool force = false) {
  static int lastTrimPct = -1;
  static TrimMode lastMode = MODE_FLOAT;

  int pct = (int)(trimDelta * 100.0f + 0.5f);
  if (!(force || pct != lastTrimPct || trimMode != lastMode)) return;

  char oldBuf[40], newBuf[40];
  const char* oldName =
    (lastMode == MODE_FLOAT) ? "Float   " :
    (lastMode == MODE_TS1  ) ? "TopSpin1" :
    (lastMode == MODE_TS2  ) ? "TopSpin2" : "TopSpin3";
  snprintf(oldBuf, sizeof(oldBuf), "%s Delta %d%%", oldName, lastTrimPct);
  eraseText(MODE_VAL_X, MODE_VAL_Y, oldBuf, 2);

  const char* newName =
    (trimMode == MODE_FLOAT) ? "Float   " :
    (trimMode == MODE_TS1  ) ? "TopSpin1" :
    (trimMode == MODE_TS2  ) ? "TopSpin2" : "TopSpin3";
  snprintf(newBuf, sizeof(newBuf), "%s Delta %d%%", newName, pct);
  drawTextWithBG(MODE_VAL_X, MODE_VAL_Y, newBuf, ILI9341_CYAN, ILI9341_BLACK, 2);

  lastTrimPct = pct;
  lastMode = trimMode;
}

// --------------------------------------------------------
static inline float mapFloat(long x, long in_min, long in_max, float out_min, float out_max) {
  return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}

void readPotAsBaseRpm() {
  // Pico ADC: 0..4095 (12-bit)
  int raw = analogRead(POT_RPM_PIN);
  baseRpmSet = mapFloat(raw, 0, 4095, RPM_MIN, RPM_MAX);
  baseRpmFiltered = POT_ALPHA * baseRpmSet + (1.0f - POT_ALPHA) * baseRpmFiltered;
}

void calcRPMandSpeed() {
  if (millis() - lastCalcMs < calcIntervalMs) return;

  if (!isRunning) {
    rpmUpper = rpmLower = 0;
    speed_kmh = 0;
    lastCalcMs = millis();
    return;
  }

  readPotAsBaseRpm();
  rpmLower = baseRpmFiltered;
  rpmUpper = baseRpmFiltered * (1.0f + trimDelta);

  const float circumference = 3.14159265f * WHEEL_DIAMETER_M;
  float vTop    = circumference * rpmUpper  / 60.0f;
  float vBottom = circumference * rpmLower  / 60.0f;
  float vTrans  = EFFICIENCY * 0.5f * (vTop + vBottom);
  speed_kmh = vTrans * 3.6f;

  lastCalcMs = millis();
}

// --------------------------------------------------------
void updateDynamicUI(bool force = false) {
  updateRunHeader(force);

  int up = (int)(rpmUpper + 0.5f);
  int lo = (int)(rpmLower + 0.5f);
  if (force || up != lastUpper) {
    drawIntNoFlicker(UPPER_VAL_X, UPPER_VAL_Y, 2, ILI9341_GREEN, lastUpper, up);
    lastUpper = up;
  }
  if (force || lo != lastLower) {
    drawIntNoFlicker(LOWER_VAL_X, LOWER_VAL_Y, 2, ILI9341_GREEN, lastLower, lo);
    lastLower = lo;
  }

  updateTrimDisplay(force);

  int spd = (int)(speed_kmh + 0.5f);
  if (spd > 999) spd = 999; // Sicherheit
  if (force || spd != lastSpeed) {
    drawIntLeftAligned(SPEED_X, SPEED_Y, 8, ILI9341_WHITE, lastSpeed, spd);
    lastSpeed = spd;
  }
}

// --------------------------------------------------------
void updateTrimByButtons() {
  if (!digitalRead(BTN_FLOAT)) trimMode = MODE_FLOAT;
  if (!digitalRead(BTN_TS1))   trimMode = MODE_TS1;
  if (!digitalRead(BTN_TS2))   trimMode = MODE_TS2;
  if (!digitalRead(BTN_TS3))   trimMode = MODE_TS3;

  switch (trimMode) {
    case MODE_FLOAT: trimDelta = PRESET_FLOAT; break;
    case MODE_TS1:   trimDelta = PRESET_TS1;   break;
    case MODE_TS2:   trimDelta = PRESET_TS2;   break;
    case MODE_TS3:   trimDelta = PRESET_TS3;   break;
  }
}

void handleRunButton() {
  if (!digitalRead(BTN_RUN)) {
    unsigned long now = millis();
    if (now - lastRunToggleMs > debounceMs) {
      isRunning = !isRunning;
      lastRunToggleMs = now;
      updateDynamicUI(true);
    }
  }
}

// --------------------------------------------------------
void setup() {
  pinMode(BTN_RUN,   INPUT_PULLUP);
  pinMode(BTN_FLOAT, INPUT_PULLUP);
  pinMode(BTN_TS1,   INPUT_PULLUP);
  pinMode(BTN_TS2,   INPUT_PULLUP);
  pinMode(BTN_TS3,   INPUT_PULLUP);
  pinMode(POT_RPM_PIN, INPUT);

  display.begin();
  display.setSPISpeed(32000000); // Pico + ILI9341: 32 MHz meist stabil
  display.setRotation(0);        // Portrait
  drawStaticUI();
  updateDynamicUI(true);
}

// --------------------------------------------------------
void loop() {
  handleRunButton();
  updateTrimByButtons();
  calcRPMandSpeed();
  updateDynamicUI(false);
  delay(20);
}