#include <Arduino.h>
#include <FastAccelStepper.h>

FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *stepper = NULL;

// WICHTIG beim XIAO ESP32-C6: 
// Wenn du die Beschriftung auf der Platine nutzt, schreibe D-Pins (z. B. D9), 
// nicht die nackte Zahl, da GPIO-Nummern abweichen!
#define dirPinStepper    D5   // Bei dir zuvor: 5
#define enablePinStepper D6   // Bei dir zuvor: 6
#define stepPinStepper   D9   // Bei dir zuvor: 9

#define speedUp   D8          // Bei dir zuvor: 8
#define speedDown D7          // Bei dir zuvor: 7

const int32_t ACCEL_RATE = 2000; 
const uint32_t MAX_SPEED_HZ = 106000;

int32_t lastControl = 0;
uint32_t lastPlotTime = 0;

void setup() {
  Serial.begin(115200);
  delay(1000); // Zeit für USB-CDC-Verbindung geben

  Serial.println("--- MOTOR DIAGNOSE START ---");

  engine.init();
  stepper = engine.stepperConnectToPin(stepPinStepper);
  
  if (stepper) {
    Serial.println("[OK] FastAccelStepper erfolgreich an Pin initialisiert.");
    stepper->setDirectionPin(dirPinStepper);
    
    // Hinweis: Manche Optokoppler-Treiber benötigen HIGH-Aktivität zum Aktivieren.
    // Falls der Motor blockiert bleibt, versuche: stepper->setEnablePin(enablePinStepper, false);
    stepper->setEnablePin(enablePinStepper, true);
    stepper->setAutoEnable(true);
    stepper->setSpeedInHz(MAX_SPEED_HZ);
  } else {
    Serial.println("[FEHLER] FastAccelStepper konnte NICHT initialisiert werden (stepper == NULL)!");
    Serial.println("Prüfe, ob der gewählte Step-Pin von der Library auf dem ESP32-C6 unterstützt wird.");
  }

  pinMode(speedUp, INPUT_PULLUP);
  pinMode(speedDown, INPUT_PULLUP);
}

void loop() {
  if (!stepper) {
    Serial.println("[HALT] Kein Stepper-Objekt aktiv.");
    delay(1000);
    return;
  }

  // Bei INPUT_PULLUP: gedrückt = LOW (true)
  bool btnUpPressed   = (digitalRead(speedUp) == LOW);
  bool btnDownPressed = (digitalRead(speedDown) == LOW);

  int32_t control = 0;

  if (btnUpPressed && btnDownPressed) {
    stepper->stopMove();
    control = 0;
  } 
  else if (btnUpPressed) {
    control = ACCEL_RATE;   
  } 
  else if (btnDownPressed) {
    control = -ACCEL_RATE; 
  } 
  else {
    control = 0;     
  }

  if (control != lastControl) {
    stepper->moveByAcceleration(control, true);
    lastControl = control;
  }

  // Plotter-Ausgabe alle 50 ms (überflutet den Bus nicht)
  if (millis() - lastPlotTime >= 50) {
    lastPlotTime = millis();
    
    int32_t currentSpeed = stepper->getCurrentSpeedInUs() == 0 ? 0 : (1000000 / stepper->getCurrentSpeedInUs());
    
    // Format kompatibel für den Arduino Serial Plotter
    Serial.print("Current_Speed_Hz:");
    Serial.print((60*currentSpeed)/3200);
    Serial.print(" Accel_Control:");
    Serial.print(control);
    Serial.print(" Btn_Up:");
    Serial.print(btnUpPressed ? 5000 : 0);
    Serial.print(" Btn_Down:");
    Serial.print(btnDownPressed ? -5000 : 0);
    Serial.println();
  }
}