#include <AccelStepper.h>
#include <math.h>

// ====== MIDDLE MOTOR PINS ======
#define STEP_MITTE 4
#define DIR_MITTE 5

AccelStepper stepperMitte(AccelStepper::DRIVER, STEP_MITTE, DIR_MITTE);

// ====== RIGHT MOTOR PINS ======
#define STEP_RECHTS 2
#define DIR_RECHTS 3
#define ENABLE_RECHTS 8

AccelStepper stepperRechts(AccelStepper::DRIVER, STEP_RECHTS, DIR_RECHTS);

// ====== LEFT MOTOR PINS ======
#define STEP_LINKS 6
#define DIR_LINKS 7
#define ENABLE_LINKS 9

AccelStepper stepperLinks(AccelStepper::DRIVER, STEP_LINKS, DIR_LINKS);

// ====== CONTROL VARIABLES ======
bool paused = true;               // start paused until 'r' is pressed
unsigned long lastMoveTime = 0;   // timing for 5-second middle motor
const long moveAmount = 500;      // middle motor steps per move
bool forward = true;              // middle motor direction flag
long segmentStart = 0;
long segmentTarget = 0;
bool midPauseDone = false;
bool midPausing = false;
unsigned long midPauseStart = 0;
long segmentCenter = 0;
long basePosition = 0;            // lowest allowed position
long baseOffset = 0;              // shifts base upward in steps

// ====== MIDDLE MOTOR GAUSSIAN PROFILE ======
const float MITTE_MIN_SPEED = 20.0f;
const float MITTE_MAX_SPEED = 200.0f;
const float MITTE_MIN_ACCEL = 10.0f;
const float MITTE_MAX_ACCEL = 80.0f;
const float MITTE_SIGMA = 120.0f;

float gaussianFactor(float distance, float sigma) {
  float x = distance / sigma;
  float dip = expf(-0.5f * x * x);
  return 1.0f - dip;
}

// ====== SIDE MOTOR CONSTANT SPEED ======
const int SPOOL_SPEED = 220; // SAME speed for left & right

int leftDir = -1;
int rightDir = 1;

void applySideMotorSpeeds() {
  stepperLinks.setSpeed(leftDir * SPOOL_SPEED);
  stepperRechts.setSpeed(rightDir * SPOOL_SPEED);
}

void setup() {
  Serial.begin(115200);
  while (!Serial) {}

  // ---- ENABLE MOTORS ----
  pinMode(ENABLE_LINKS, OUTPUT);
  digitalWrite(ENABLE_LINKS, LOW);

  pinMode(ENABLE_RECHTS, OUTPUT);
  digitalWrite(ENABLE_RECHTS, LOW);

  // ---- SET STEP/DIR LOW ----
  pinMode(2, OUTPUT); digitalWrite(2, LOW);
  pinMode(3, OUTPUT); digitalWrite(3, LOW);
  pinMode(6, OUTPUT); digitalWrite(6, LOW);
  pinMode(7, OUTPUT); digitalWrite(7, LOW);

  // ---- MIDDLE MOTOR ----
  stepperMitte.setMaxSpeed(MITTE_MAX_SPEED);
  stepperMitte.setAcceleration(MITTE_MAX_ACCEL);

  // ---- SIDE MOTORS ----
  stepperLinks.setMaxSpeed(SPOOL_SPEED);
  stepperLinks.setAcceleration(0);

  stepperRechts.setMaxSpeed(SPOOL_SPEED);
  stepperRechts.setAcceleration(0);

  applySideMotorSpeeds();

  basePosition = stepperMitte.currentPosition();

  Serial.println("Send 'r' to START, 's' to STOP, 'x' to SWAP, 'u'/'d' to shift base");
}

void loop() {

  // ---- SERIAL CONTROL ----
  if (Serial.available()) {
    char c = Serial.read();

    if (c == 's') {
      paused = true;
      stepperMitte.stop();
      stepperLinks.setSpeed(0);
      stepperRechts.setSpeed(0);
      Serial.println("Motors STOPPED");
    }

    if (c == 'r') {
      paused = false;
      applySideMotorSpeeds();
      Serial.println("Motors RESUMED");
    }

    if (c == 'x') {
      leftDir = -leftDir;
      rightDir = -rightDir;
      applySideMotorSpeeds();
      Serial.println("Left/Right directions swapped");
    }

    if (c == 'u' || c == 'd') {
      long delta = (c == 'u') ? 2000 : -2000;
      baseOffset += delta;
      if (baseOffset < 0) baseOffset = 0;
      long newBase = basePosition + baseOffset;
      stepperMitte.moveTo(newBase);
      segmentStart = stepperMitte.currentPosition();
      segmentTarget = newBase;
      segmentCenter = newBase;
      midPauseDone = true;
      midPausing = false;
      lastMoveTime = millis();
      Serial.println("Base position shifted");
    }
  }

  if (paused) return;

  // ---- MIDDLE MOTOR EVERY 5 SECONDS ----
  if (stepperMitte.distanceToGo() == 0 && millis() - lastMoveTime >= 5000) {
    long target = forward
      ? basePosition + baseOffset + moveAmount
      : basePosition + baseOffset;

    stepperMitte.moveTo(target);
    segmentStart = stepperMitte.currentPosition();
    segmentTarget = target;
    segmentCenter = basePosition + baseOffset + (moveAmount / 2);
    midPauseDone = false;
    midPausing = false;
    forward = !forward;
    lastMoveTime = millis();
  }

  // ---- MID-SEGMENT PAUSE ----
  if (stepperMitte.distanceToGo() != 0 && !midPauseDone && !midPausing) {
    long traveled = labs(stepperMitte.currentPosition() - segmentStart);
    long total = labs(segmentTarget - segmentStart);
    if (total > 0 && traveled >= total / 2) {
      midPausing = true;
      midPauseStart = millis();
    }
  }

  if (midPausing) {
    if (millis() - midPauseStart >= 3000) {
      midPausing = false;
      midPauseDone = true;
    }
  } else {
    float distance = fabsf(stepperMitte.currentPosition() - segmentCenter);
    float factor = gaussianFactor(distance, MITTE_SIGMA);
    float targetSpeed = MITTE_MIN_SPEED + (MITTE_MAX_SPEED - MITTE_MIN_SPEED) * factor;
    float targetAccel = MITTE_MIN_ACCEL + (MITTE_MAX_ACCEL - MITTE_MIN_ACCEL) * factor;
    stepperMitte.setMaxSpeed(targetSpeed);
    stepperMitte.setAcceleration(targetAccel);
    stepperMitte.run();
  }

  // ---- RUN SIDE MOTORS ----
  stepperLinks.runSpeed();
  stepperRechts.runSpeed();
}