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

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

#define dirPinStepper    D5   
#define enablePinStepper D6   
#define stepPinStepper   D9   

#define speedUp   D8
#define speedDown D7   

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

  engine.init();
  stepper = engine.stepperConnectToPin(stepPinStepper);
  
  if (stepper) {
    stepper->setDirectionPin(dirPinStepper);
    stepper->setEnablePin(enablePinStepper, true);
    stepper->setAutoEnable(true);
    stepper->setSpeedInHz(MAX_SPEED_HZ);
  }

  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();
  }
}