// -----------------------------------------------------------------------------
//  FMS 50 – „Fenstermesslanze“
//  Firmware 0.3  – getrennte SPI‑Busse & Menüsystem
//
//  Komponenten
//  • 4 × VL53L1X‑ToF‑Sensoren (I²C, getrennte XSHUT‑Pins)
//  • Analoger Joystick (X, Y, Taster)
//  • Schrittmotor + Treiber (DIR/STEP)
//  • TFT‑Display (VSPI)
//  • SD‑Karte  (HSPI)
// -----------------------------------------------------------------------------
//  Erstellt / geändert: 17 Mai 2025
// -----------------------------------------------------------------------------

#include <Wire.h>
#include <Adafruit_VL53L1X.h>     // https://github.com/adafruit/Adafruit-VL53L1X
#include <AccelStepper.h>         // https://www.airspayce.com/mikem/arduino/AccelStepper/
#include <TFT_eSPI.h>             // https://github.com/Bodmer/TFT_eSPI
#include <SPI.h>
#include <SD.h>

// -----------------------------------------------------------------------------
//  Allgemeine Konfiguration & Pin‑Mapping (ESP32)
// -----------------------------------------------------------------------------
#define NUM_SENSORS 4                // Anzahl VL53L1X‑Module

// I²C (beliebige GPIOs mit Pull‑Ups)
#define I2C_SDA 21
#define I2C_SCL 22

// XSHUT‑Pins der Sensoren
const uint8_t XSHUT_PINS[NUM_SENSORS] = {32, 33, 25, 26};

// Joystick (analog) + Taster
#define JOY_X_PIN   34               // ADC1_CH6
#define JOY_Y_PIN   35               // ADC1_CH7
#define JOY_BTN_PIN 2                // Digital‑Input, interner Pull‑Up

// Schrittmotor (Treiber‑Modus DIR/STEP)
#define STEPPER_STEP_PIN 15          // frei (belegte VSPI/HSPI‑Pins vermieden)
#define STEPPER_DIR_PIN  12          // bleibt auf 12 (teilt HSPI‑MISO, OK für OUT‑Signal)

// ---------------- VSPI  (TFT) ----------------
#define TFT_MOSI 23   // VSPI_MOSI
#define TFT_MISO 19   // VSPI_MISO
#define TFT_SCLK 18   // VSPI_CLK
#define TFT_CS   5
#define TFT_DC   16
#define TFT_RST  17

// ---------------- HSPI  (SD) -----------------
#define SD_MOSI 13    // HSPI_MOSI
#define SD_MISO 12    // HSPI_MISO  (teilt PIN mit DIR, unkritisch)
#define SD_SCLK 14    // HSPI_CLK   (Step‑Pin verlegt nach 15)
#define SD_CS    4

// SPI‑Instanzen
SPIClass SPI_SD(HSPI);   // eigener Bus für SD
// TFT_eSPI verwendet standardmäßig VSPI, Pins bitte in User_Setup angepasst lassen!

// -----------------------------------------------------------------------------
//  Menüdefinition
// -----------------------------------------------------------------------------
enum MenuState { MAIN_MENU, CAL_MENU, MANUAL_MENU, HOME_MENU, MEASURE_MENU };
const char* MENU_ITEMS[] = { "Kalibrierung", "Manuelle Bewegung", "Homing", "Messung" };
const uint8_t MENU_COUNT = sizeof(MENU_ITEMS) / sizeof(MENU_ITEMS[0]);

// -----------------------------------------------------------------------------
//  Hilfsklassen
// -----------------------------------------------------------------------------
class DistanceSensor {
public:
  explicit DistanceSensor(uint8_t xshutPin) : _xshutPin(xshutPin) {}

  bool begin(uint8_t newAddress) {
    pinMode(_xshutPin, OUTPUT);
    digitalWrite(_xshutPin, LOW);
    delay(10);
    digitalWrite(_xshutPin, HIGH);
    delay(10);

    if (!_sensor.begin(0x29, &Wire)) {
      Serial.printf("VL53L1X @XSHUT %d nicht erkannt\n", _xshutPin);
      return false;
    }
        // ab v3.x: Adresse per ST‑API setzen (8‑Bit‑Format!)
    if (_sensor.VL53L1X_SetI2CAddress(newAddress << 1) != 0) { // 7‑Bit → 8‑Bit
      Serial.printf("Adresswechsel zu 0x%02X fehlgeschlagen", newAddress);
      return false;
    }
    // Wrapper erneut auf neue 7‑Bit‑Adresse initialisieren, damit künftige Aufrufe passen
    _sensor.begin(newAddress, &Wire);
    _sensor.startRanging();
    return true;
  }

  uint16_t read() {
    if (_sensor.dataReady()) {
      uint16_t dist = _sensor.distance();
      _sensor.clearInterrupt();
      return dist;
    }
    return 0xFFFF;
  }

private:
  uint8_t _xshutPin;
  Adafruit_VL53L1X _sensor;
};

// ---------------------------------------------------------------------------
class Joystick {
public:
  void begin() { pinMode(JOY_BTN_PIN, INPUT_PULLUP); }
  int16_t x()        { return analogRead(JOY_X_PIN); }
  int16_t y()        { return analogRead(JOY_Y_PIN); }
  bool    pressed()  { return digitalRead(JOY_BTN_PIN) == LOW; }
};

// ---------------------------------------------------------------------------
class FMSDisplay {
public:
  void begin() {
    tft.init();
    tft.setRotation(1);
    tft.fillScreen(TFT_BLACK);

    // SD‑Bus initialisieren (getrennter SPI)
    SPI_SD.begin(SD_SCLK, SD_MISO, SD_MOSI, SD_CS);
    if (!SD.begin(SD_CS, SPI_SD)) {
      tft.setTextColor(TFT_RED);
      tft.println("SD init failed!");
    }
  }

  void showMainMenu(uint8_t highlight) {
    tft.fillScreen(TFT_BLACK);
    for (uint8_t i = 0; i < MENU_COUNT; ++i) {
      tft.setTextColor((i == highlight) ? TFT_YELLOW : TFT_WHITE, TFT_BLACK);
      tft.printf("%c %s\n", (i == highlight) ? '>' : ' ', MENU_ITEMS[i]);
    }
  }

  void showSubTitle(const char* title) {
    tft.fillScreen(TFT_BLACK);
    tft.setTextColor(TFT_CYAN, TFT_BLACK);
    tft.println(title);
  }

  void showDistances(uint16_t* d) {
    tft.setTextColor(TFT_WHITE, TFT_BLACK);
    for (uint8_t i = 0; i < NUM_SENSORS; ++i) {
      tft.setCursor(0, 20 + i * 12);
      tft.printf("S%d: %4d mm   ", i, (d[i] == 0xFFFF) ? 0 : d[i]);
    }
  }

  void showMessage(const char* msg) {
    tft.setTextColor(TFT_GREEN, TFT_BLACK);
    tft.setCursor(0, 100);
    tft.println(msg);
  }

private:
  TFT_eSPI tft = TFT_eSPI(); // nutzt VSPI default pins aus User_Setup
};

// ---------------------------------------------------------------------------
class FMSController {
public:
  bool begin() {
    // --- I²C ----------------------------------------------------------------
    Wire.begin(I2C_SDA, I2C_SCL);

    bool ok = true;
    for (uint8_t i = 0; i < NUM_SENSORS; ++i) ok &= _sensors[i].begin(0x30 + i);

    _joystick.begin();
    _display.begin();
    _display.showMainMenu(_menuIndex);

    stepper.setMaxSpeed(1500);
    stepper.setAcceleration(800);

    return ok;
  }

  void update() {
    readJoystick();
    switch (_state) {
      case MAIN_MENU:    updateMainMenu();    break;
      case CAL_MENU:     updateCalMenu();     break;
      case MANUAL_MENU:  updateManualMenu();  break;
      case HOME_MENU:    updateHomeMenu();    break;
      case MEASURE_MENU: updateMeasureMenu(); break;
    }
    stepper.run();
  }

private:
  // ------------------------------------------------------------
  void readJoystick() {
    _joyX   = _joystick.x();
    _joyY   = _joystick.y();
    _joyBtn = _joystick.pressed();
    _joyDX  = _joyX - _centre;
    _joyDY  = _joyY - _centre;
  }

  bool exitRequested() {
    if (_joyDX < -_moveThreshold) { // kräftig nach links
      _state = MAIN_MENU;
      _display.showMainMenu(_menuIndex);
      stepper.setSpeed(0);
      return true;
    }
    return false;
  }

  // ------------------------------------------------------------
  // Hauptmenü
  void updateMainMenu() {
    static unsigned long lastMove = 0;
    unsigned long now = millis();

    if (abs(_joyDY) > _moveThreshold && now - lastMove > _repeatDelay) {
      _menuIndex = (_joyDY < 0) ? (_menuIndex == 0 ? MENU_COUNT - 1 : _menuIndex - 1)
                                : (_menuIndex + 1) % MENU_COUNT;
      _display.showMainMenu(_menuIndex);
      lastMove = now;
    }

    if (_joyBtn && !_btnLock) {
      _btnLock = true;
      enterSubMenu(static_cast<MenuState>(_menuIndex + 1)); // +1 da MAIN_MENU=0
    }
    if (!_joyBtn) _btnLock = false;
  }

  void enterSubMenu(MenuState target) {
    _state = target;
    switch (_state) {
      case CAL_MENU:     _display.showSubTitle("Kalibrierung");          break;
      case MANUAL_MENU:  _display.showSubTitle("Manuelle Bewegung");     break;
      case HOME_MENU:    _display.showSubTitle("Homing"); startHoming();  break;
      case MEASURE_MENU: _display.showSubTitle("Messung");               break;
      default: break;
    }
  }

  // ------------------------------------------------------------
  // Kalibrierung (Platzhalter)
  void updateCalMenu() {
    if (exitRequested()) return;
    if (_joyBtn && !_btnLock) {
      _btnLock = true;
      _display.showMessage("Kalibrierung OK");
    }
    if (!_joyBtn) _btnLock = false;
  }

  // ------------------------------------------------------------
  // Manuelle Bewegung
  void updateManualMenu() {
    if (exitRequested()) return;
    if (abs(_joyDX) > _deadZone) {
      float speed = map(abs(_joyDX), 0, 2048, 0, 1200);
      stepper.setSpeed((_joyDX > 0) ? speed : -speed);
    } else {
      stepper.setSpeed(0);
    }
  }

  // ------------------------------------------------------------
  // Homing
  void startHoming() {
    stepper.setMaxSpeed(800);
    stepper.moveTo(-10000); // grob Richtung Home fahren
  }

  void updateHomeMenu() {
    if (exitRequested()) {
      stepper.stop();
      return;
    }
    if (!stepper.isRunning()) {
      _display.showMessage("Home erreicht");
    }
  }

  // ------------------------------------------------------------
  // Messung – Live‑Distanzen anzeigen
  void readSensors() {
    for (uint8_t i = 0; i < NUM_SENSORS; ++i) _distances[i] = _sensors[i].read();
  }

  void updateMeasureMenu() {
    if (exitRequested()) return;
    readSensors();
    _display.showDistances(_distances);
  }

  // ------------------------------------------------------------
  // Mitgliedsvariablen
  DistanceSensor _sensors[NUM_SENSORS] = {
      DistanceSensor(XSHUT_PINS[0]),
      DistanceSensor(XSHUT_PINS[1]),
      DistanceSensor(XSHUT_PINS[2]),
      DistanceSensor(XSHUT_PINS[3]) };

  uint16_t        _distances[NUM_SENSORS] = {0};
  Joystick        _joystick;
  FMSDisplay      _display;
  AccelStepper    stepper { AccelStepper::DRIVER, STEPPER_STEP_PIN, STEPPER_DIR_PIN };

  // Menu/Joystick State
  MenuState _state = MAIN_MENU;
  uint8_t   _menuIndex = 0;
  bool      _btnLock   = false;
  int16_t   _joyX = 0, _joyY = 0, _joyDX = 0, _joyDY = 0;
  bool      _joyBtn = false;

  // Konstanten
  static constexpr int16_t _centre        = 2048;
  static constexpr int16_t _deadZone      = 300;
  static constexpr int16_t _moveThreshold = 600;
  static constexpr uint16_t _repeatDelay  = 200; // ms
};

// -----------------------------------------------------------------------------
//  Globale Instanz
// -----------------------------------------------------------------------------
FMSController fms;

void setup() {
  Serial.begin(115200);
  Serial.println("\nFMS 50 startet …");
  if (!fms.begin()) {
    Serial.println("Initialisierung fehlgeschlagen – Verdrahtung prüfen!");
    while (true) delay(1000);
  }
}

void loop() {
  fms.update();
}