czy ten program jest prawidłowy i optymalny? #include "FastAccelStepper.h"  // Biblioteka do sterowania silnikami krokowymi
#include <Preferences.h>       // Biblioteka do przechowywania ustawień
#include <Arduino.h>
#include <SPI.h>

#define NUM_STEPPERS 3  // Liczba silników krokowych

#define PIN_SPI_MISO 13
#define PIN_SPI_MOSI 14
#define PIN_SPI_SCK 12
#define PIN_SPI_CS 4

const int limitSwitchPin1 = 7;  // krańcówki
const int limitSwitchPin2 = 6;
const int limitSwitchPin3 = 5;

const int extraLimitSwitchPin1 = 20;  // Dodatkowa krańcówka
const int extraLimitSwitchPin2 = 21;  // Dodatkowa krańcówka
const int extraSafetyButton = 11;     // Drugi grzyb bezpieczeństwa

const int L_LIMIT_ADC_PIN = 5;
const int L_ADC_NUM_SAMPLES = 10;
const int L_ADC_SAMPLE_DELAY_MS = 1;
const int L_ADC_THRESHOLD = 500;

volatile bool softwareStop = false;

// Definicja struktury silnika krokowego
struct StepperMotor {
  FastAccelStepper* stepper;
  int stepPin;
  int dirPin;
  int calibrationPin;
  int alarmPin;
  int stepperResolution;  // Kroki na jednostkę (stopnie lub milimetry)
  int maxSpeed;
  int stepsMin;
  int stepsMax;
  long zeroOffset;
  long calibrationPosition;
  String name;
  bool dirHigh;
  bool calibrationDirectionForward;  // Kierunek bazowania
  String speedPrefKey;
  String accelerationPrefKey;
  String posPrefKey;
  String zeroOffsetPrefKey;
  String emergencyPosPrefKey;
  int speed;
  int acceleration;
  bool speedModifiedForEncoder;
  int prevSpeed;
  int prevAcceleration;
  int calibrationSpeedFactor;
  int calibrationMoveAwaySteps;
  bool calibrationRunInitialDirectionForward;
  bool calibrationSecondMoveDirectionForward;
  int calibrationLimitSwitchActiveState;
  int calibrationDelayBetweenMoves;
  int calibrationLoopDelay;
  int encoderSpeed;
  int encoderAcceleration;
  long targetPosition;
  unsigned long previousMillis;
  unsigned long previousMillisNewZero;

  // Wędka - prędkości
  enum SpeedMode {
    VERY_FAST,
    FAST,
    MEDIUM_FAST,
    MEDIUM,
    SLOW,
    VERY_SLOW
  };

  SpeedMode currentSpeedMode;  // Aktualny tryb prędkości
};

SPIClass spi = SPIClass(FSPI);  // instancja SPI tylko dla enkodera

// even-parity dla 15 bitów danych
bool evenParity(uint16_t v) {
  v &= 0x7FFF;
  bool p = 0;
  while (v) {
    p ^= (v & 1);
    v >>= 1;
  }
  return p == 0;
}

// 16-bitowa transakcja SPI z obsługą CS
uint16_t spiTransfer16(uint16_t dataOut) {
  spi.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE1));
  digitalWrite(PIN_SPI_CS, LOW);
  delayMicroseconds(1);
  uint8_t hi = spi.transfer((dataOut >> 8) & 0xFF);
  uint8_t lo = spi.transfer(dataOut & 0xFF);
  delayMicroseconds(1);
  digitalWrite(PIN_SPI_CS, HIGH);
  spi.endTransaction();
  return (hi << 8) | lo;
}

// surowy odczyt kąta (14 bitów) z czujnika
uint16_t readAngleRaw() {
  uint16_t cmd = 0x3FFF | 0x4000;       // READ z rejestru kąta
  if (!evenParity(cmd)) cmd |= 0x8000;  // bit parzystości
  spiTransfer16(cmd);                   // pusta odpowiedź
  delayMicroseconds(1);
  return spiTransfer16(0x0000);  // dane z czujnika
}

int median(int* arr, int n) {
  for (int i = 1; i < n; i++) {
    int key = arr[i];
    int j = i - 1;
    while (j >= 0 && arr[j] > key) {
      arr[j + 1] = arr[j];
      j = j - 1;
    }
    arr[j + 1] = key;
  }
  return arr[n / 2];
}

bool readLimitSwitchL() {
  int samples[L_ADC_NUM_SAMPLES];
  for (int i = 0; i < L_ADC_NUM_SAMPLES; i++) {
    samples[i] = analogRead(L_LIMIT_ADC_PIN);
    delay(L_ADC_SAMPLE_DELAY_MS);
  }
  int med = median(samples, L_ADC_NUM_SAMPLES);
  return (med < L_ADC_THRESHOLD);  // Zwraca true jeśli krańcówka aktywna
}

Preferences preferences;
float encoderOffset = 0.0f;
/* --- korekcja PE -------------------------------------------------- */
const float ENC_SCALE  = 1.00f;   // ≈ +0,20 %
const float ENC_OFFSET = -0.00f;


StepperMotor steppers[NUM_STEPPERS];
FastAccelStepperEngine engine = FastAccelStepperEngine();

// Definicje pinów
const int safetyButton = 19;  //Wylaczone chwilowo
const int rManipulatorVCC = 38;
const int encoderPinA = 42;
const int encoderPinB = 41;
const int XaxisPin = 38;
const int YaxisPin = 37;
const int ZaxisPin = 36;
const int multiplier1Pin = 35;
const int multiplier10Pin = 45;

// Zmienne globalne
int pinAstateCurrent = LOW;
int pinAStateLast = LOW;

const unsigned long interval = 100;
const unsigned long intervalNewZero = 100;

bool processEncoderActive = false;
bool safetyLocked = false;  // Flaga blokady bezpieczeństwa //wylaczone chwilowo

// Flaga trybu LIVE
volatile bool liveModeActive = false;

int liveDelayOffset = 5;    // delay po wyświetleniu P10
int liveDelayAbsolute = 5;  // delay po wyświetleniu P1

void requestSoftwareStop() {
  softwareStop = true;
  stopAllSteppers();
  Serial.println("[SOFTWARE STOP]");
}

void startLive(float degrees) {
  if (liveModeActive) return;

  long steps = long(degrees * steppers[0].stepperResolution);
  long currentPos = steppers[0].stepper->getCurrentPosition();
  long targetPos = currentPos + steps;

  if (targetPos < steppers[0].stepsMin || targetPos > steppers[0].stepsMax) {
    Serial.println("LIVE: ruch poza zakresem—nie wykonano.");
    return;
  }

  liveModeActive = true;
  steppers[0].stepper->move(steps);

  while (steppers[0].stepper->isRunning()) {
    enforceLimits();
    safetyCheck();
    if (softwareStop) return;
    {
      long stepsOff = steppers[0].stepper->getCurrentPosition() - steppers[0].zeroOffset;
      float degOff = float(stepsOff) / steppers[0].stepperResolution;
      Serial.print("P10: ");
      Serial.println(degOff, 3);
      delay(liveDelayOffset);
    }
    {
      long stepsAbs = steppers[0].stepper->getCurrentPosition();
      float degAbs = float(stepsAbs) / steppers[0].stepperResolution;
      Serial.print("P1:  ");
      Serial.println(degAbs, 3);
      delay(liveDelayAbsolute);
    }
  }
  liveModeActive = false;
}

void sweepForEncoderError() {

  StepperMotor& motor = steppers[0];

  /* --- spowalniamy, żeby pomiary były powtarzalne ------------------------- */
  int origSpeed        = motor.speed;
  int origAcceleration = motor.acceleration;

  motor.stepper->setSpeedInHz(origSpeed / 4);          // ~25 % normalnej
  motor.stepper->setAcceleration(origAcceleration / 4);
  motor.stepper->applySpeedAcceleration();

  /* --- start –95° względem zeroOffset ------------------------------------ */
  long startPos = motor.zeroOffset + (-95L) * motor.stepperResolution;
  motor.stepper->moveTo(startPos);
  while (motor.stepper->isRunning()) delay(1);

  Serial.println(F("=== START SWEEP –95 … +95 ° (co 1 °) ==="));

  for (int deg = -95; deg <= 95; ++deg) {

    long target = motor.zeroOffset + long(deg) * motor.stepperResolution;
    motor.stepper->moveTo(target);
    while (motor.stepper->isRunning()) delay(1);

    /* ---------- P1 -------------------------------------------------------- */
    float p1 = float(motor.stepper->getCurrentPosition()) / motor.stepperResolution;
    Serial.print(F("P1: "));
    Serial.println(p1, 3);

    /* ---------- PE -------------------------------------------------------- */
    Serial.print(F("PE: "));
    printEncoderWithOffset();                     // ta funkcja sama kończy linię
  }

  Serial.println(F("=== KONIEC SWEEP ==="));

  /* --- przywracamy stare parametry ruchu ---------------------------------- */
  motor.stepper->setSpeedInHz(origSpeed);
  motor.stepper->setAcceleration(origAcceleration);
  motor.stepper->applySpeedAcceleration();
}

void setup() {
  Serial.begin(460800);
  // ---------- SPI ENCODER ----------
  spi.begin(PIN_SPI_SCK, PIN_SPI_MISO, PIN_SPI_MOSI, PIN_SPI_CS);
  pinMode(PIN_SPI_CS, OUTPUT);
  digitalWrite(PIN_SPI_CS, HIGH);

  uint16_t raw0 = readAngleRaw() & 0x3FFF;
  float angle0 = raw0 * 360.0f / 16384.0f;
  Serial.printf("[log01]", raw0, angle0);
  setupPins();
  engine.init();
  preferences.begin("stepper", false);
  encoderOffset = preferences.getFloat("encOffset", 99.5f);
  setupSteppers();
  readPreferences();
  Serial.println("[log02]");
}

void loop() {
  checkMvsEncoder();
  if (liveModeActive) {
    return;
  }
  if (!safetyLocked) {
    enforceLimits();
    safetyCheck();
    if (processEncoderActive) {
      processEncoderAndMoveSteppers();
    }
    if (Serial.available() > 0) {
      processSerialCommand();
    }
  } else {
    if (Serial.available() > 0) {
      String command = Serial.readStringUntil('\n');
      command.trim();
      if (command == "Odblokuj") {
        unlockSafety();
      } else if (command == "reset") {
        resetESP32();
      } else {
        Serial.println("System zablokowany. Użyj komendy 'Odblokuj' lub 'reset'.");
      }
    }
  }

  /* ======= ENCODER READ co 50 ms ======= */
  static unsigned long lastRead = 0;
  unsigned long now = millis();
  if (now - lastRead >= 50) {
    lastRead += 50;  // minimalny dryft
    uint16_t raw = readAngleRaw() & 0x3FFF;
    float angleDeg = raw * 360.0f / 16384.0f;
    // zastosowanie offsetu
    float angleAdj = encoderOffset - angleDeg;

    // owiń do zakresu –180…+180°
    if (angleAdj > 180.0f) angleAdj -= 360.0f;
    else if (angleAdj < -180.0f) angleAdj += 360.0f;

    // Serial.printf("ENC: RAW=0x%04X | surowy=%.1f° | z offsetem=%.1f°\n", raw, angleDeg, angleAdj);
    // Serial.printf("ENC: RAW=0x%04X | %.1f°\n", raw, angleDeg);
  }

  //Obsługa enkodera i komend niewyłączona
  if (processEncoderActive) {
    processEncoderAndMoveSteppers();
  }
  if (Serial.available() > 0) {
    processSerialCommand();
  }
}

// Funkcje konfigurujące
void setupPins() {
  pinMode(safetyButton, INPUT_PULLUP);  //czasowo wylaczone
  pinMode(rManipulatorVCC, INPUT);
  pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);
  pinMode(XaxisPin, INPUT_PULLDOWN);
  pinMode(YaxisPin, INPUT_PULLDOWN);
  pinMode(ZaxisPin, INPUT_PULLDOWN);
  pinMode(multiplier1Pin, INPUT_PULLDOWN);
  pinMode(multiplier10Pin, INPUT_PULLDOWN);
  pinMode(limitSwitchPin1, INPUT_PULLUP);
  pinMode(limitSwitchPin2, INPUT_PULLUP);
  pinMode(limitSwitchPin3, INPUT_PULLUP);
  pinMode(extraLimitSwitchPin1, INPUT);
  pinMode(extraLimitSwitchPin2, INPUT);
  pinMode(extraSafetyButton, INPUT);

  for (int i = 0; i < NUM_STEPPERS; ++i) {
    pinMode(steppers[i].alarmPin, INPUT_PULLDOWN);
  }
  //zakomentowane alarmy
}

void setupSteppers() {
  steppers[0] = {
    .stepper = nullptr,
    .stepPin = 3,
    .dirPin = 8,
    .calibrationPin = 7,
    .alarmPin = 46,
    .stepperResolution = 320,
    .maxSpeed = 64000,
    .stepsMin = -32000,
    .stepsMax = 32000,
    .zeroOffset = 0,
    .calibrationPosition = 0,
    .name = "M",
    .dirHigh = false,
    .calibrationDirectionForward = false,
    .speedPrefKey = "speed1",
    .accelerationPrefKey = "acceleration1",
    .posPrefKey = "pos1",
    .zeroOffsetPrefKey = "zeroOffset1",
    .emergencyPosPrefKey = "emergencyPos1",
    .speed = 0,
    .acceleration = 0,
    .calibrationSpeedFactor = 5,
    .calibrationMoveAwaySteps = 5000,
    .calibrationRunInitialDirectionForward = false,
    .calibrationSecondMoveDirectionForward = false,
    .calibrationLimitSwitchActiveState = LOW,
    .calibrationDelayBetweenMoves = 500,
    .calibrationLoopDelay = 0,
    .encoderSpeed = 32000,
    .encoderAcceleration = 16000,
    .targetPosition = 0,
    .previousMillis = 0,
    .previousMillisNewZero = 0
  };

  steppers[1] = {
    .stepper = nullptr,
    .stepPin = 16,
    .dirPin = 15,
    .calibrationPin = 6,
    .alarmPin = 9,
    .stepperResolution = 320,
    .maxSpeed = 32000,
    .stepsMin = -32000,
    .stepsMax = 0,
    .zeroOffset = 0,
    .calibrationPosition = 0,
    .name = "N",
    .dirHigh = false,
    .calibrationDirectionForward = true,
    .speedPrefKey = "speed2",
    .accelerationPrefKey = "acceleration2",
    .posPrefKey = "pos2",
    .zeroOffsetPrefKey = "zeroOffset2",
    .emergencyPosPrefKey = "emergencyPos2",
    .speed = 0,
    .acceleration = 0,
    .calibrationSpeedFactor = 10,
    .calibrationMoveAwaySteps = -1600,
    .calibrationRunInitialDirectionForward = true,
    .calibrationSecondMoveDirectionForward = true,
    .calibrationLimitSwitchActiveState = HIGH,
    .calibrationDelayBetweenMoves = 500,
    .calibrationLoopDelay = 0,
    .encoderSpeed = 32000,
    .encoderAcceleration = 32000,
    .targetPosition = 0,
    .previousMillis = 0,
    .previousMillisNewZero = 0
  };

  steppers[2] = {
    .stepper = nullptr,
    .stepPin = 18,
    .dirPin = 17,
    .calibrationPin = 5,
    .alarmPin = 10,
    .stepperResolution = 100,
    .maxSpeed = 100000,
    .stepsMin = -12000,
    .stepsMax = 12000,
    .zeroOffset = 0,
    .calibrationPosition = -12000,
    .name = "L",
    .dirHigh = true,
    .calibrationDirectionForward = false,
    .speedPrefKey = "speed3",
    .accelerationPrefKey = "acceleration3",
    .posPrefKey = "pos3",
    .zeroOffsetPrefKey = "zeroOffset3",
    .emergencyPosPrefKey = "emergencyPos3",
    .speed = 0,
    .acceleration = 0,
    .calibrationSpeedFactor = 20,
    .calibrationMoveAwaySteps = 1000,
    .calibrationRunInitialDirectionForward = false,
    .calibrationSecondMoveDirectionForward = false,
    .calibrationLimitSwitchActiveState = LOW,
    .calibrationDelayBetweenMoves = 500,
    .calibrationLoopDelay = 500,
    .encoderSpeed = 32000,
    .encoderAcceleration = 32000,
    .targetPosition = 0,
    .previousMillis = 0,
    .previousMillisNewZero = 0
  };

  for (int i = 0; i < NUM_STEPPERS; ++i) {
    steppers[i].stepper = engine.stepperConnectToPin(steppers[i].stepPin);
    if (steppers[i].stepper) {
      steppers[i].stepper->setDirectionPin(steppers[i].dirPin, steppers[i].dirHigh);
      steppers[i].stepper->setAcceleration(steppers[i].acceleration);
      steppers[i].stepper->setSpeedInHz(steppers[i].speed);
      steppers[i].stepper->setCurrentPosition(steppers[i].targetPosition);
      steppers[i].targetPosition = steppers[i].stepper->getCurrentPosition();
      steppers[i].currentSpeedMode = StepperMotor::MEDIUM;
      if (steppers[i].name == "M") Serial.println("[log03]");
      else if (steppers[i].name == "N") Serial.println("[log04]");
      else if (steppers[i].name == "L") Serial.println("[log05]");
    } else {
      if (steppers[i].name == "M") Serial.println("[log06]");
      else if (steppers[i].name == "N") Serial.println("[log07]");
      else if (steppers[i].name == "L") Serial.println("[log08]");
    }
  }
}

void readPreferences() {
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    StepperMotor& motor = steppers[i];
    motor.speed = preferences.getInt(motor.speedPrefKey.c_str(), motor.maxSpeed / 10);
    motor.acceleration = preferences.getInt(motor.accelerationPrefKey.c_str(), motor.speed / 2);
    motor.prevSpeed = motor.speed;
    motor.prevAcceleration = motor.acceleration;
    motor.speedModifiedForEncoder = false;
    long pos = preferences.getLong(motor.posPrefKey.c_str(), 0);
    motor.zeroOffset = preferences.getLong(motor.zeroOffsetPrefKey.c_str(), 0);

    if (motor.stepper) {
      motor.stepper->setSpeedInHz(motor.speed);
      motor.stepper->setAcceleration(motor.acceleration);
      motor.stepper->setCurrentPosition(pos);
      motor.targetPosition = pos;
      if (motor.name == "M") Serial.println("[log09]");
      else if (motor.name == "N") Serial.println("[log10]");
      else if (motor.name == "L") Serial.println("[log11]");
    }
  }
}

void enforceLimits() {
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    StepperMotor& motor = steppers[i];
    long pos = motor.stepper->getCurrentPosition();
    if (pos < motor.stepsMin || pos > motor.stepsMax) {
      motor.stepper->forceStop();
      if (pos < motor.stepsMin) {
        motor.stepper->setCurrentPosition(motor.stepsMin);
        motor.targetPosition = motor.stepsMin;
        if (motor.name == "M") Serial.println("[log12]");
        else if (motor.name == "N") Serial.println("[log13]");
        else if (motor.name == "L") Serial.println("[log14]");
      } else {
        motor.stepper->setCurrentPosition(motor.stepsMax);
        motor.targetPosition = motor.stepsMax;
        if (motor.name == "M") Serial.println("[log15]");
        else if (motor.name == "N") Serial.println("[log16]");
        else if (motor.name == "L") Serial.println("[log17]");
      }

      if (motor.name == "M") Serial.println("[log18]");
      else if (motor.name == "N") Serial.println("[log19]");
      else if (motor.name == "L") Serial.println("[log20]");
    }
  }
}

void stopAllSteppers() {
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (steppers[i].stepper) {
      steppers[i].stepper->forceStop();
      long pos = steppers[i].stepper->getCurrentPosition();
      preferences.putLong(steppers[i].posPrefKey.c_str(), pos);
      preferences.putLong(steppers[i].emergencyPosPrefKey.c_str(), pos);
      steppers[i].targetPosition = pos;
    }
  }
  Serial.println("[log21]");
}


void safetyCheck() {
  bool safetyTriggered = false;

  if (softwareStop) {
    safetyTriggered = true;
  }

  // Sprawdź grzyby bezpieczeństwa
  if (digitalRead(safetyButton) == LOW) {
    Serial.println("[log22]");
    safetyTriggered = true;
  }
  if (digitalRead(extraSafetyButton) == HIGH) {
    Serial.println("[log23]");
    safetyTriggered = true;
  }

  // Sprawdź krańcówki dodatkowe
  if (digitalRead(extraLimitSwitchPin1) == LOW) {
    Serial.println("[log24]).");
    safetyTriggered = true;
  }
  if (digitalRead(extraLimitSwitchPin2) == LOW) {
    Serial.println("[log25]");
    safetyTriggered = true;
  }

  // Sprawdź alarmy silników
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (digitalRead(steppers[i].alarmPin) == LOW) {
      if (steppers[i].name == "M") Serial.println("[log26]");
      else if (steppers[i].name == "N") Serial.println("[log27]");
      else if (steppers[i].name == "L") Serial.println("[log28]");
      safetyTriggered = true;
    }
  }


  if (safetyTriggered) {
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      stopAllSteppers();
      steppers[i].stepper->forceStop();
      long pos = steppers[i].stepper->getCurrentPosition();
      preferences.putLong(steppers[i].posPrefKey.c_str(), pos);
      preferences.putLong(steppers[i].emergencyPosPrefKey.c_str(), pos);
      steppers[i].targetPosition = pos;
    }
    Serial.println("[log29]");
    safetyLocked = true;
  }
  checkMvsEncoder();
}

void checkMvsEncoder() {
  static bool discrepancyDetected = false;  // flaga rozjazdu pozycji
  static float previousDiff = 0.0f;         // poprzednia wartość różnicy

  float posP1 = (float)steppers[0].stepper->getCurrentPosition() / steppers[0].stepperResolution;
  float posPE = readEncoderDegCorrected();
  float diff = posP1 - posPE;

  // Jeśli rozjazd przekracza 2 stopnie i nie został wcześniej wykryty, albo zmienił się o więcej niż np. 0.5°
  if ((abs(diff) > 2.0f && !discrepancyDetected) || (abs(diff - previousDiff) > 0.5f)) {
    discrepancyDetected = true;
    previousDiff = diff;
    Serial.print("[log33]");
    Serial.print(posP1, 2);
    Serial.print("°, PE=");
    Serial.print(posPE, 2);
    Serial.println("°. WYMAGANE BAZOWANIE silnika M!");
  }
  // Jeśli rozjazd spadnie poniżej progu, zresetuj flagę
  else if (abs(diff) <= 2.0f) {
    discrepancyDetected = false;
  }
}

void unlockSafety() {
  if (safetyLocked) {
    safetyLocked = false;
    Serial.println("[log30]");
  } else {
    Serial.println("[log31]");
  }
}

void resetESP32() {
  Serial.println("[log32]");
  delay(1000);
  ESP.restart();
}

void processEncoderAndMoveSteppers() {
  pinAstateCurrent = digitalRead(encoderPinA);
  static long stepsForSteppers[NUM_STEPPERS] = { 0 };
  static unsigned long lastEncoderEventTime = 0;
  static unsigned long prevEncoderEventTime = 0;
  static unsigned long deltaT = 1000;

  if (pinAStateLast == LOW && pinAstateCurrent == HIGH) {
    int stepIncrement = (digitalRead(encoderPinB) == HIGH) ? -1 : 1;
    prevEncoderEventTime = lastEncoderEventTime;
    lastEncoderEventTime = millis();
    if (prevEncoderEventTime != 0) {
      deltaT = lastEncoderEventTime - prevEncoderEventTime;
    }
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      stepsForSteppers[i] += stepIncrement * steppers[i].stepperResolution;
    }
  }
  pinAStateLast = pinAstateCurrent;

  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (stepsForSteppers[i] != 0) {
      int axisPin = (i == 0 ? XaxisPin : i == 1 ? YaxisPin
                                                : ZaxisPin);
      if (digitalRead(axisPin) == HIGH) {
        float multiplier = 1.0;
        if (digitalRead(multiplier1Pin) == HIGH) multiplier = 0.1;
        else if (digitalRead(multiplier10Pin) == HIGH) multiplier = 0.01;

        long movement = stepsForSteppers[i] * multiplier;
        long newTargetPosition = steppers[i].targetPosition + movement;

        if (newTargetPosition >= steppers[i].stepsMin && newTargetPosition <= steppers[i].stepsMax) {
          // histereza...
          StepperMotor::SpeedMode newMode = steppers[i].currentSpeedMode;
          const unsigned long veryFastThreshold = 30;
          const unsigned long fastThreshold = 50;
          const unsigned long mediumFastThreshold = 100;
          const unsigned long mediumThreshold = 150;
          const unsigned long slowThreshold = 200;
          switch (steppers[i].currentSpeedMode) {
            case StepperMotor::VERY_FAST:
              if (deltaT > veryFastThreshold) newMode = StepperMotor::FAST;
              break;
            case StepperMotor::FAST:
              if (deltaT > fastThreshold) newMode = StepperMotor::MEDIUM_FAST;
              else if (deltaT <= veryFastThreshold) newMode = StepperMotor::VERY_FAST;
              break;
            case StepperMotor::MEDIUM_FAST:
              if (deltaT > mediumFastThreshold) newMode = StepperMotor::MEDIUM;
              else if (deltaT <= fastThreshold) newMode = StepperMotor::FAST;
              break;
            case StepperMotor::MEDIUM:
              if (deltaT > mediumThreshold) newMode = StepperMotor::SLOW;
              else if (deltaT <= mediumFastThreshold) newMode = StepperMotor::MEDIUM_FAST;
              break;
            case StepperMotor::SLOW:
              if (deltaT > slowThreshold) newMode = StepperMotor::VERY_SLOW;
              else if (deltaT <= mediumThreshold) newMode = StepperMotor::MEDIUM;
              break;
            case StepperMotor::VERY_SLOW:
              if (deltaT <= slowThreshold) newMode = StepperMotor::SLOW;
              break;
            default:
              newMode = StepperMotor::MEDIUM;
          }
          if (newMode != steppers[i].currentSpeedMode) {
            steppers[i].currentSpeedMode = newMode;
            steppers[i].speedModifiedForEncoder = true;
          }

          int newSpeed, newAcceleration;
          switch (steppers[i].currentSpeedMode) {
            case StepperMotor::VERY_FAST:
              newSpeed = steppers[i].encoderSpeed;
              newAcceleration = steppers[i].encoderAcceleration;
              break;
            case StepperMotor::FAST:
              newSpeed = steppers[i].encoderSpeed / 2;
              newAcceleration = steppers[i].encoderAcceleration / 2;
              break;
            case StepperMotor::MEDIUM_FAST:
              newSpeed = steppers[i].encoderSpeed / 6;
              newAcceleration = steppers[i].encoderAcceleration / 4;
              break;
            case StepperMotor::MEDIUM:
              newSpeed = steppers[i].encoderSpeed / 8;
              newAcceleration = steppers[i].encoderAcceleration / 6;
              break;
            case StepperMotor::SLOW:
              newSpeed = steppers[i].encoderSpeed / 10;
              newAcceleration = steppers[i].encoderAcceleration / 8;
              break;
            case StepperMotor::VERY_SLOW:
              newSpeed = steppers[i].encoderSpeed / 12;
              newAcceleration = steppers[i].encoderAcceleration / 10;
              break;
            default:
              newSpeed = steppers[i].encoderSpeed;
              newAcceleration = steppers[i].encoderAcceleration;
          }

          if (steppers[i].speedModifiedForEncoder) {
            steppers[i].prevSpeed = steppers[i].speed;
            steppers[i].prevAcceleration = steppers[i].acceleration;
            steppers[i].speedModifiedForEncoder = false;
          }
          if (steppers[i].speed != newSpeed || steppers[i].acceleration != newAcceleration) {
            steppers[i].speed = newSpeed;
            steppers[i].acceleration = newAcceleration;
            steppers[i].stepper->setSpeedInHz(newSpeed);
            steppers[i].stepper->setAcceleration(newAcceleration);
          }

          steppers[i].targetPosition = newTargetPosition;
          steppers[i].stepper->move(movement);
        }
        stepsForSteppers[i] = 0;
      }
    }
    // przywracanie oryginalnych prędkości po zakończeniu ruchu
    if (steppers[i].speedModifiedForEncoder && !steppers[i].stepper->isRunning()) {
      steppers[i].speed = steppers[i].prevSpeed;
      steppers[i].acceleration = steppers[i].prevAcceleration;
      steppers[i].stepper->setSpeedInHz(steppers[i].speed);
      steppers[i].stepper->setAcceleration(steppers[i].acceleration);
      steppers[i].speedModifiedForEncoder = false;
    }
  }
}

void powP10(int times = 500) {
  if (softwareStop) return;
  for (int i = 0; i < times; ++i) {
    steppers[0].previousMillisNewZero = 0;
    Serial.print("P10: ");
    printPositionZero(steppers[0]);
    delay(intervalNewZero);
  }
}

void processSerialCommand() {
  String command = Serial.readStringUntil('\n');
  command.trim();

  if (command.startsWith("speed1")) {
    Serial.print("speed1: ");
    float speed = command.substring(6).toFloat();
    changeSpeed(steppers[0], speed);
  } else if (command.startsWith("speed2")) {
    Serial.print("speed2: ");
    float speed = command.substring(6).toFloat();
    changeSpeed(steppers[1], speed);
  } else if (command.startsWith("speed3")) {
    Serial.print("speed3: ");
    float speed = command.substring(6).toFloat();
    changeSpeed(steppers[2], speed);
  } else if (command.startsWith("acceleration1")) {
    Serial.print("acceleration1: ");
    int acc = command.substring(13).toInt();
    changeAcceleration(steppers[0], acc);
  } else if (command.startsWith("acceleration2")) {
    Serial.print("acceleration2: ");
    int acc = command.substring(13).toInt();
    changeAcceleration(steppers[1], acc);
  } else if (command.startsWith("acceleration3")) {
    Serial.print("acceleration3: ");
    int acc = command.substring(13).toInt();
    changeAcceleration(steppers[2], acc);
  } else if (command == "H") {
    Serial.print("H: ");
    calibrationM_EncoderBased(steppers[0]);
  } else if (command == "J") {
    Serial.print("J: ");
    calibration(steppers[1]);
  } else if (command == "JS") {
    Serial.print("JS: ");
    calibrationSerwis(steppers[1]);
  } else if (command == "G") {
    Serial.print("G: ");
    calibrationL(steppers[2]);
  } else if (command == "H1") {
    Serial.print("H1: ");
    returnToZero(steppers[0]);
  } else if (command == "J1") {
    Serial.print("J1: ");
    returnToZero(steppers[1]);
  } else if (command == "G1") {
    Serial.print("G1: ");
    returnToZero(steppers[2]);
  } else if (command == "H2") {
    Serial.print("H2: ");
    returnToOffsetZero(steppers[0]);
  } else if (command == "J2") {
    Serial.print("J2: ");
    returnToOffsetZero(steppers[1]);
  } else if (command == "G2") {
    Serial.print("G2: ");
    returnToOffsetZero(steppers[2]);
  } else if (command == "H3") {
    Serial.print("H3: ");
    returnToEmergencyPosition(steppers[0]);
  } else if (command == "J3") {
    Serial.print("J3: ");
    returnToEmergencyPosition(steppers[1]);
  } else if (command == "G3") {
    Serial.print("G3: ");
    returnToEmergencyPosition(steppers[2]);
  } else if (command == "C") {
    Serial.print("C: ");
    calibrationAll();
  } else if (command == "R") {
    Serial.print("R: ");
    returnToZeroAll();
  } else if (command == "T") {
    Serial.print("T: ");
    returnToOffsetZeroAll();
  } else if (command == "Y") {
    Serial.print("Y: ");
    returnToEmergencyPositionAll();
  } else if (command == "A") {
    Serial.print("A: ");
    setNewZero(steppers[0]);
  } else if (command == "S") {
    Serial.print("S: ");
    setNewZero(steppers[1]);
  } else if (command == "D") {
    Serial.print("D: ");
    setNewZero(steppers[2]);
  } else if (command == "PE") {
    Serial.print("PE: ");
    printEncoderWithOffset();
  } else if (command == "P") {
    positionAll();
  } else if (command == "P1") {
    Serial.print("P1: ");
    printPosition(steppers[0]);
  } else if (command == "P2") {
    Serial.print("P2: ");
    printPosition(steppers[1]);
  } else if (command == "P3") {
    Serial.print("P3: ");
    printPosition(steppers[2]);
  } else if (command == "P0") {
    positionAllZero();
  } else if (command == "P10") {
    Serial.print("P10: ");
    printPositionZero(steppers[0]);
  } else if (command == "P20") {
    Serial.print("P20: ");
    printPositionZero(steppers[1]);
  } else if (command == "P30") {
    Serial.print("P30: ");
    printPositionZero(steppers[2]);
  } else if (command == "EP") {
    positionAllEmergency();
  } else if (command == "EP1") {
    Serial.print("EP1: ");
    printEmergencyPosition(steppers[0]);
  } else if (command == "EP2") {
    Serial.print("EP2: ");
    printEmergencyPosition(steppers[1]);
  } else if (command == "EP3") {
    Serial.print("EP3: ");
    printEmergencyPosition(steppers[2]);
  } else if (command == "F") {
    Serial.print("F: ");
    processEncoderActive = true;
    Serial.println("Jog activate");
  } else if (command == "F1") {
    Serial.print("F1: ");
    processEncoderActive = false;
    Serial.println("Jog deactivate");
  } else if (command == "X") {
    Serial.print("X: ");
    checkSafety();
  } else if (command.equalsIgnoreCase("Odblokuj")) {
    unlockSafety();
  } else if (command.equalsIgnoreCase("reset")) {
    resetESP32();
  } else if (command == "baza1") {
    setCurrentPositionToCalibration(steppers[0]);
  } else if (command == "baza2") {
    setCurrentPositionToCalibration(steppers[1]);
  } else if (command == "baza3") {
    setCurrentPositionToCalibration(steppers[2]);
  } else if (command.startsWith("getSpeed1")) {
    Serial.print("getSpeed1: ");
    getSpeed(steppers[0]);
  } else if (command.startsWith("getSpeed2")) {
    Serial.print("getSpeed2: ");
    getSpeed(steppers[1]);
  } else if (command.startsWith("getSpeed3")) {
    Serial.print("getSpeed3: ");
    getSpeed(steppers[2]);
  } else if (command.startsWith("getMaxSpeed1")) {
    Serial.print("maxSpeedStepper1: ");
    getMaxSpeed(steppers[0]);
  } else if (command.startsWith("getMaxSpeed2")) {
    Serial.print("maxSpeedStepper2: ");
    getMaxSpeed(steppers[1]);
  } else if (command.startsWith("getMaxSpeed3")) {
    Serial.print("maxSpeedStepper3: ");
    getMaxSpeed(steppers[2]);
  } else if (command.startsWith("move1Steps")) {
    int steps = command.substring(11).toInt();
    moveStepperSteps(steppers[0], steps);
  } else if (command.startsWith("move2Steps")) {
    int steps = command.substring(11).toInt();
    moveStepperSteps(steppers[1], steps);
  } else if (command.startsWith("move3Steps")) {
    int steps = command.substring(11).toInt();
    moveStepperSteps(steppers[2], steps);
  } else if (command.startsWith("LIVE")) {
    float deg = command.substring(4).toFloat();
    Serial.print("Starting LIVE for ");
    Serial.print(deg, 2);
    Serial.println(" degrees");
    startLive(deg);
  } else if (command == "P10R") {
    powP10(500);
  } else if (command.startsWith("liveDelayOffset")) {
    int v = command.substring(16).toInt();
    liveDelayOffset = v;
    Serial.print("Ustawiono liveDelayOffset = ");
    Serial.println(liveDelayOffset);
  } else if (command.startsWith("liveDelayAbsolute")) {
    int v = command.substring(18).toInt();
    liveDelayAbsolute = v;
    Serial.print("Ustawiono liveDelayAbsolute = ");
    Serial.println(liveDelayAbsolute);
  } else if (command.startsWith("offset ")) {
    float newOff = command.substring(7).toFloat();
    encoderOffset = newOff;
    preferences.putFloat("encOffset", encoderOffset);
    Serial.print("[log34]");
    Serial.println(encoderOffset, 1);
  } else if (command == "RANDOM") {
    randomMoves();
  } else if (command == "SWEEP") {
    Serial.println("[log35]");
    sweepForEncoderError();
  } else if (command.equalsIgnoreCase("STOP")) {
    requestSoftwareStop();
    return;
  } else if (command.equalsIgnoreCase("limits1") || command.equalsIgnoreCase("limitsM")) {
    printLimits(steppers[0]);
  } else if (command.equalsIgnoreCase("limits2") || command.equalsIgnoreCase("limitsN")) {
    printLimits(steppers[1]);
  } else if (command.equalsIgnoreCase("limits3") || command.equalsIgnoreCase("limitsL")) {
    printLimits(steppers[2]);
  } else {
    controlStepperMotor(command);
  }
}

void controlStepperMotor(String command) {
  if (command.length() < 3) {
    Serial.println("Niepoprawny format komendy.");
    return;
  }
  char motorId = command.charAt(0);
  float value = command.substring(2).toFloat();
  Serial.println(String(motorId) + ": " + String(value));
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (steppers[i].name.charAt(0) == motorId) {
      long steps = value * steppers[i].stepperResolution;
      long newTargetPosition = steppers[i].targetPosition + steps;
      if (newTargetPosition >= steppers[i].stepsMin && newTargetPosition <= steppers[i].stepsMax) {
        steppers[i].targetPosition = newTargetPosition;
        steppers[i].stepper->move(steps);
      } else {
        Serial.print("Ruch poza zakresem dla silnika ");
        Serial.println(steppers[i].name);
      }
      break;
    }
  }
}

void changeSpeed(StepperMotor& motor, float newSpeed) {
  int speedInHz = motor.stepperResolution * newSpeed;
  if (speedInHz > motor.maxSpeed) {
    speedInHz = motor.maxSpeed;
    Serial.println("maxSpeedStepper: " + String(motor.maxSpeed / motor.stepperResolution));
  } else {
    Serial.println(newSpeed);
  }
  motor.speed = speedInHz;
  motor.acceleration = speedInHz / 2;
  motor.stepper->setSpeedInHz(motor.speed);
  motor.stepper->setAcceleration(motor.acceleration);
  preferences.putInt(motor.speedPrefKey.c_str(), motor.speed);
  preferences.putInt(motor.accelerationPrefKey.c_str(), motor.acceleration);
}

void changeAcceleration(StepperMotor& motor, int newAcceleration) {
  motor.acceleration = newAcceleration;
  motor.stepper->setAcceleration(motor.acceleration);
  preferences.putInt(motor.accelerationPrefKey.c_str(), motor.acceleration);
  Serial.println(newAcceleration);
}

void calibration(StepperMotor& motor) {
  int calibrationSpeed = motor.stepperResolution * motor.calibrationSpeedFactor;
  int calibrationAcceleration = calibrationSpeed / 2;
  motor.stepper->setSpeedInHz(calibrationSpeed);
  motor.stepper->setAcceleration(calibrationAcceleration);
  motor.stepper->applySpeedAcceleration();

  // --- START --- //
  if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
    safetyCheck();
    return;
  }

  // 1. Jazda do krańcówki (lub końca zakresu!)
  motor.stepper->moveTo(motor.calibrationPosition);
  while (motor.stepper->isRunning()) {
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    // *** OGRANICZENIE PROGRAMOWE: jeśli dojedzie do końca zakresu, zatrzymaj ***
    long pos = motor.stepper->getCurrentPosition();
    if (pos < motor.stepsMin || pos > motor.stepsMax) {
      motor.stepper->forceStopAndNewPosition(
        pos < motor.stepsMin ? motor.stepsMin : motor.stepsMax);
      motor.targetPosition = pos < motor.stepsMin ? motor.stepsMin : motor.stepsMax;
      return;
    }
    // --- Krańcówka
    if (digitalRead(motor.calibrationPin) == motor.calibrationLimitSwitchActiveState) {
      motor.stepper->forceStop();
      break;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.stepper->forceStopAndNewPosition(motor.calibrationPosition);
  motor.targetPosition = motor.calibrationPosition;
  delay(motor.calibrationDelayBetweenMoves);

  // 2. Odjazd od krańcówki (sprawdź ograniczenie!)
  motor.stepper->move(motor.calibrationMoveAwaySteps);
  while (motor.stepper->isRunning()) {
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    long pos = motor.stepper->getCurrentPosition();
    if (pos < motor.stepsMin || pos > motor.stepsMax) {
      motor.stepper->forceStopAndNewPosition(
        pos < motor.stepsMin ? motor.stepsMin : motor.stepsMax);
      motor.targetPosition = pos < motor.stepsMin ? motor.stepsMin : motor.stepsMax;
      return;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.targetPosition += motor.calibrationMoveAwaySteps;

  // 3. Powolny powrót na krańcówkę (sprawdzaj zakres!)
  motor.stepper->setSpeedInHz(calibrationSpeed / 2);
  motor.stepper->setAcceleration(calibrationAcceleration / 4);
  motor.stepper->applySpeedAcceleration();
  if (motor.calibrationSecondMoveDirectionForward) {
    motor.stepper->runForward();
  } else {
    motor.stepper->runBackward();
  }
  while (digitalRead(motor.calibrationPin) != motor.calibrationLimitSwitchActiveState) {
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    long pos = motor.stepper->getCurrentPosition();
    if (pos < motor.stepsMin || pos > motor.stepsMax) {
      motor.stepper->forceStopAndNewPosition(
        pos < motor.stepsMin ? motor.stepsMin : motor.stepsMax);
      motor.targetPosition = pos < motor.stepsMin ? motor.stepsMin : motor.stepsMax;
      return;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.stepper->forceStopAndNewPosition(motor.calibrationPosition);
  motor.targetPosition = motor.calibrationPosition;

  // --- POWRÓT DO PARAMETRÓW PRACY --- //
  motor.stepper->setSpeedInHz(motor.speed);
  motor.stepper->setAcceleration(motor.acceleration);
  motor.stepper->applySpeedAcceleration();
  printPosition(motor);
}

void calibrationSerwis(StepperMotor& motor) {
  int calibrationSpeed = motor.stepperResolution * motor.calibrationSpeedFactor;
  int calibrationAcceleration = calibrationSpeed / 2;
  motor.stepper->setSpeedInHz(calibrationSpeed);
  motor.stepper->setAcceleration(calibrationAcceleration);
  motor.stepper->applySpeedAcceleration();
  if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
    safetyCheck();
    return;
  }
  motor.stepper->moveTo(motor.calibrationPosition);
  while (motor.stepper->isRunning()) {
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    if (digitalRead(motor.calibrationPin) == motor.calibrationLimitSwitchActiveState) {
      motor.stepper->forceStop();
      break;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.stepper->forceStopAndNewPosition(motor.calibrationPosition);
  motor.targetPosition = motor.calibrationPosition;
  delay(motor.calibrationDelayBetweenMoves);
  motor.stepper->move(motor.calibrationMoveAwaySteps);
  while (motor.stepper->isRunning()) {
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.targetPosition += motor.calibrationMoveAwaySteps;
  motor.stepper->setSpeedInHz(calibrationSpeed / 2);
  motor.stepper->setAcceleration(calibrationAcceleration / 4);
  motor.stepper->applySpeedAcceleration();
  if (motor.calibrationSecondMoveDirectionForward) {
    motor.stepper->runForward();
  } else {
    motor.stepper->runBackward();
  }
  while (digitalRead(motor.calibrationPin) != motor.calibrationLimitSwitchActiveState) {
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.stepper->forceStopAndNewPosition(motor.calibrationPosition);
  motor.targetPosition = motor.calibrationPosition;
  motor.stepper->setSpeedInHz(motor.speed);
  motor.stepper->setAcceleration(motor.acceleration);
  motor.stepper->applySpeedAcceleration();
  printPosition(motor);
}

void calibrationM_EncoderBased(StepperMotor& motor) {
  int calibrationSpeed = motor.stepperResolution * motor.calibrationSpeedFactor;
  int calibrationAcceleration = calibrationSpeed / 2;
  motor.stepper->setSpeedInHz(calibrationSpeed);
  motor.stepper->setAcceleration(calibrationAcceleration);
  motor.stepper->applySpeedAcceleration();

  // 1. Odczytaj PE i wybierz kierunek DOJAZDU
  float pe = readEncoderDegCorrected();
  bool moveForwardToLimit = (pe < 0);  // PE<0: dojazd forward, PE>0: backward
  Serial.print("[log38]");
  Serial.println(pe, 2);

  // 2. Jedź szybko do krańcówki (aż zadziała)
  if (moveForwardToLimit) {
    motor.stepper->runForward();
  } else {
    motor.stepper->runBackward();
  }
  while (digitalRead(motor.calibrationPin) != motor.calibrationLimitSwitchActiveState) {
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.stepper->forceStop();
  delay(motor.calibrationDelayBetweenMoves);

  // 3. Odsuń się ZAWSZE w tym samym kierunku (np. forward)
  long awaySteps = abs(motor.calibrationMoveAwaySteps);  // zawsze dodatnie
  motor.stepper->move(awaySteps);                        // forward
  while (motor.stepper->isRunning()) {
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.targetPosition += awaySteps;

  // 4. Powolny powrót na krańcówkę (w stronę krańcówki, czyli ZAWSZE backward)
  motor.stepper->setSpeedInHz(calibrationSpeed / 2);
  motor.stepper->setAcceleration(calibrationAcceleration / 4);
  motor.stepper->applySpeedAcceleration();

  // *** ZAWSZE BACKWARD (do krańcówki) ***
  motor.stepper->runBackward();

  while (digitalRead(motor.calibrationPin) != motor.calibrationLimitSwitchActiveState) {
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.stepper->forceStopAndNewPosition(motor.calibrationPosition);
  motor.targetPosition = motor.calibrationPosition;

  // Przywróć normalne parametry
  motor.stepper->setSpeedInHz(motor.speed);
  motor.stepper->setAcceleration(motor.acceleration);
  motor.stepper->applySpeedAcceleration();

  printPosition(motor);
  Serial.println("[log39]");
}


void calibrationL(StepperMotor& motor) {
  int calibrationSpeed = motor.stepperResolution * motor.calibrationSpeedFactor;
  int calibrationAcceleration = calibrationSpeed / 2;
  motor.stepper->setSpeedInHz(calibrationSpeed);
  motor.stepper->setAcceleration(calibrationAcceleration);
  motor.stepper->applySpeedAcceleration();

  // 1. Jedź do pozycji bazowej aż zadziała krańcówka analogowa
  motor.stepper->moveTo(motor.calibrationPosition);
  while (motor.stepper->isRunning()) {
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    if (readLimitSwitchL()) {
      motor.stepper->forceStop();
      break;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.stepper->forceStopAndNewPosition(motor.calibrationPosition);
  motor.targetPosition = motor.calibrationPosition;
  delay(motor.calibrationDelayBetweenMoves);

  // 2. Odsuń się kawałek od krańcówki
  motor.stepper->move(motor.calibrationMoveAwaySteps);
  while (motor.stepper->isRunning()) {
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.targetPosition += motor.calibrationMoveAwaySteps;

  // 3. Powoli wracaj aż znów zadziała krańcówka (precyzyjne bazowanie)
  motor.stepper->setSpeedInHz(calibrationSpeed / 2);
  motor.stepper->setAcceleration(calibrationAcceleration / 4);
  motor.stepper->applySpeedAcceleration();

  if (motor.calibrationSecondMoveDirectionForward) {
    motor.stepper->runForward();
  } else {
    motor.stepper->runBackward();
  }
  while (!readLimitSwitchL()) {  // tu negacja, bo czekasz na AKTYWACJĘ
    if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || digitalRead(steppers[0].alarmPin) == LOW || digitalRead(steppers[1].alarmPin) == LOW || digitalRead(steppers[2].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
    delay(motor.calibrationLoopDelay);
  }
  motor.stepper->forceStopAndNewPosition(motor.calibrationPosition);
  motor.targetPosition = motor.calibrationPosition;

  // Przywróć parametry
  motor.stepper->setSpeedInHz(motor.speed);
  motor.stepper->setAcceleration(motor.acceleration);
  motor.stepper->applySpeedAcceleration();
  printPosition(motor);

  Serial.println("[log37]");
}

void calibrationAll() {
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    StepperMotor& motor = steppers[i];
    int calibrationSpeed = motor.stepperResolution * motor.calibrationSpeedFactor;
    int calibrationAcceleration = calibrationSpeed / 2;
    motor.stepper->setSpeedInHz(calibrationSpeed);
    motor.stepper->setAcceleration(calibrationAcceleration);
    motor.stepper->applySpeedAcceleration();
  }
  if (digitalRead(safetyButton) == LOW) {
    safetyCheck();
    return;
  }
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (digitalRead(steppers[i].alarmPin) == LOW) {
      safetyCheck();
      return;
    }
  }
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    steppers[i].stepper->moveTo(steppers[i].calibrationPosition);
  }
  bool allRunning;
  do {
    allRunning = false;
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      if (steppers[i].stepper->isRunning()) {
        allRunning = true;
        if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
          safetyCheck();
          return;
        }
        if (digitalRead(steppers[i].calibrationPin) == steppers[i].calibrationLimitSwitchActiveState) {
          steppers[i].stepper->forceStop();
        }
      }
    }
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      if (steppers[i].stepper->isRunning()) {
        delay(steppers[i].calibrationLoopDelay);
      }
    }
  } while (allRunning);
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    steppers[i].stepper->forceStopAndNewPosition(steppers[i].calibrationPosition);
    steppers[i].targetPosition = steppers[i].calibrationPosition;
  }
  delay(steppers[0].calibrationDelayBetweenMoves);
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    steppers[i].stepper->move(steppers[i].calibrationMoveAwaySteps);
    steppers[i].targetPosition += steppers[i].calibrationMoveAwaySteps;
  }
  bool anyRunning;
  do {
    anyRunning = false;
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      if (steppers[i].stepper->isRunning()) {
        anyRunning = true;
        if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
          safetyCheck();
          return;
        }
      }
    }
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      if (steppers[i].stepper->isRunning()) {
        delay(steppers[i].calibrationLoopDelay);
      }
    }
  } while (anyRunning);
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    steppers[i].stepper->setSpeedInHz(steppers[i].calibrationSpeedFactor * steppers[i].stepperResolution / 2);
    steppers[i].stepper->setAcceleration(steppers[i].calibrationSpeedFactor * steppers[i].stepperResolution / 8);
    steppers[i].stepper->applySpeedAcceleration();
  }
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (steppers[i].calibrationSecondMoveDirectionForward) {
      steppers[i].stepper->runForward();
    } else {
      steppers[i].stepper->runBackward();
    }
  }
  bool calibrationComplete[NUM_STEPPERS] = { false };
  while (true) {
    bool allComplete = true;
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      if (!calibrationComplete[i]) {
        if (digitalRead(steppers[i].calibrationPin) == steppers[i].calibrationLimitSwitchActiveState) {
          steppers[i].stepper->forceStopAndNewPosition(steppers[i].calibrationPosition);
          steppers[i].targetPosition = steppers[i].calibrationPosition;
          calibrationComplete[i] = true;
        }
        if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW) {
          safetyCheck();
          return;
        }
        if (!calibrationComplete[i]) {
          allComplete = false;
          delay(steppers[i].calibrationLoopDelay);
        }
      }
    }
    if (allComplete) break;
  }
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    steppers[i].stepper->setSpeedInHz(steppers[i].speed);
    steppers[i].stepper->setAcceleration(steppers[i].acceleration);
    steppers[i].stepper->applySpeedAcceleration();
  }
  positionAll();
}

void returnToZero(StepperMotor& motor) {
  if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || softwareStop) {
    safetyCheck();
    return;
  }
  motor.stepper->moveTo(0);
  motor.targetPosition = 0;
}

void returnToZeroAll() {
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
  }
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    steppers[i].stepper->moveTo(0);
    steppers[i].targetPosition = 0;
  }
  while (steppers[0].stepper->isRunning() || steppers[1].stepper->isRunning() || steppers[2].stepper->isRunning()) {
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW || softwareStop) {
        safetyCheck();
        return;
      }
    }
  }
  delay(500);
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    printPosition(steppers[i]);
  }
}

void returnToOffsetZero(StepperMotor& motor) {
  if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || softwareStop) {
    safetyCheck();
    return;
  }
  motor.stepper->moveTo(motor.zeroOffset);
  motor.targetPosition = motor.zeroOffset;
}

void returnToOffsetZeroAll() {
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
  }
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    steppers[i].stepper->moveTo(steppers[i].zeroOffset);
    steppers[i].targetPosition = steppers[i].zeroOffset;
  }
  while (steppers[0].stepper->isRunning() || steppers[1].stepper->isRunning() || steppers[2].stepper->isRunning()) {
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW || softwareStop) {
        safetyCheck();
        return;
      }
    }
  }
  delay(500);
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    printPositionZero(steppers[i]);
  }
}

void returnToEmergencyPosition(StepperMotor& motor) {
  if (digitalRead(safetyButton) == LOW || digitalRead(motor.alarmPin) == LOW || softwareStop) {
    safetyCheck();
    return;
  }
  long emergencyPos = preferences.getLong(motor.emergencyPosPrefKey.c_str(), 0);
  motor.stepper->moveTo(emergencyPos);
  motor.targetPosition = emergencyPos;
  Serial.print("Silnik ");
  Serial.print(motor.name);
  Serial.print(" porusza się do pozycji awaryjnej: ");
  Serial.println(emergencyPos);
}

void returnToEmergencyPositionAll() {
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW || softwareStop) {
      safetyCheck();
      return;
    }
  }
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    long emergencyPos = preferences.getLong(steppers[i].emergencyPosPrefKey.c_str(), 0);
    steppers[i].stepper->moveTo(emergencyPos);
    steppers[i].targetPosition = emergencyPos;
    Serial.print("Silnik ");
    Serial.print(steppers[i].name);
    Serial.print(" porusza się do pozycji awaryjnej: ");
    Serial.println(emergencyPos);
  }
  while (steppers[0].stepper->isRunning() || steppers[1].stepper->isRunning() || steppers[2].stepper->isRunning()) {
    for (int i = 0; i < NUM_STEPPERS; ++i) {
      if (digitalRead(safetyButton) == LOW || digitalRead(steppers[i].alarmPin) == LOW || softwareStop) {
        safetyCheck();
        return;
      }
    }
  }
  delay(500);
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    printEmergencyPosition(steppers[i]);
  }
}

void setNewZero(StepperMotor& motor) {
  motor.zeroOffset = motor.stepper->getCurrentPosition();
  preferences.putLong(motor.zeroOffsetPrefKey.c_str(), motor.zeroOffset);

  if (motor.name == "M") Serial.println("[log40]");
  else if (motor.name == "N") Serial.println("[log41]");
  else if (motor.name == "L") Serial.println("[log42]");
}

void positionAll() {
  Serial.print("P1: ");
  printPosition(steppers[0]);
  Serial.print("P2: ");
  printPosition(steppers[1]);
  Serial.print("P3: ");
  printPosition(steppers[2]);
}

void printPosition(StepperMotor& motor) {
  unsigned long currentMillis = millis();
  if (currentMillis - motor.previousMillis >= interval) {
    motor.previousMillis = currentMillis;
    float position = (float)motor.stepper->getCurrentPosition() / motor.stepperResolution;
    Serial.println(position, 3);
    preferences.putLong(motor.posPrefKey.c_str(), motor.stepper->getCurrentPosition());
  }
}

void positionAllZero() {
  Serial.print("P10: ");
  printPositionZero(steppers[0]);
  Serial.print("P20: ");
  printPositionZero(steppers[1]);
  Serial.print("P30: ");
  printPositionZero(steppers[2]);
}

void printPositionZero(StepperMotor& motor) {
  unsigned long currentMillis = millis();
  if (currentMillis - motor.previousMillisNewZero >= intervalNewZero) {
    motor.previousMillisNewZero = currentMillis;
    float position = (float)(motor.stepper->getCurrentPosition() - motor.zeroOffset) / motor.stepperResolution;
    Serial.println(position, 3);
  }
}

void positionAllEmergency() {
  Serial.print("EP1: ");
  printEmergencyPosition(steppers[0]);
  Serial.print("EP2: ");
  printEmergencyPosition(steppers[1]);
  Serial.print("EP3: ");
  printEmergencyPosition(steppers[2]);
}

void printEmergencyPosition(StepperMotor& motor) {
  unsigned long currentMillis = millis();
  if (currentMillis - motor.previousMillis >= interval) {
    motor.previousMillis = currentMillis;
    long emergencyPos = preferences.getLong(motor.emergencyPosPrefKey.c_str(), 0);
    float position = (float)emergencyPos / motor.stepperResolution;
    Serial.println(position, 3);
  }
}

void getSpeed(StepperMotor& motor) {
  float speed = (float)motor.speed / motor.stepperResolution;
  Serial.println(speed);
}

void getMaxSpeed(StepperMotor& motor) {
  float maxSpeed = (float)motor.maxSpeed / motor.stepperResolution;
  Serial.println(maxSpeed);
}

void moveStepperSteps(StepperMotor& motor, int steps) {
  motor.stepper->move(steps);
  motor.targetPosition += steps;
  Serial.print("Silnik ");
  Serial.print(motor.name);
  Serial.print(" porusza się o ");
  Serial.print(steps);
  Serial.println(" kroków.");
}

void setCurrentPositionToCalibration(StepperMotor& motor) {
  motor.stepper->forceStopAndNewPosition(motor.calibrationPosition);
  motor.targetPosition = motor.calibrationPosition;
  Serial.print("Pozycja silnika ");
  Serial.print(motor.name);
  Serial.print(" została ustawiona na pozycję kalibracyjną: ");
  Serial.println(motor.calibrationPosition);
  preferences.putLong(motor.posPrefKey.c_str(), motor.calibrationPosition);
}

void checkSafety() {
  bool safetyTriggered = false;

  if (digitalRead(safetyButton) == LOW) {
    Serial.println("[log22]");
    safetyTriggered = true;
  }

  if (digitalRead(extraSafetyButton) == HIGH) {
    Serial.println("[log23]");
    safetyTriggered = true;
  }

  if (digitalRead(extraLimitSwitchPin1) == LOW) {
    Serial.println("[log24]");
    safetyTriggered = true;
  }

  if (digitalRead(extraLimitSwitchPin2) == LOW) {
    Serial.println("[log25]");
    safetyTriggered = true;
  }

  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (digitalRead(steppers[i].alarmPin) == LOW) {
      if (steppers[i].name == "M") Serial.println("[log26]");
      else if (steppers[i].name == "N") Serial.println("[log27]");
      else if (steppers[i].name == "L") Serial.println("[log28]");
      safetyTriggered = true;
    }
  }

  if (safetyTriggered) {
    stopAllSteppers();          // zatrzymanie silników i zapis pozycji
    Serial.println("[log29]");  // Potwierdzenie zatrzymania alarmowego systemu
  }
}


void updateTargetPositions() {
  for (int i = 0; i < NUM_STEPPERS; ++i) {
    if (!steppers[i].stepper->isRunning()) {
      steppers[i].targetPosition = steppers[i].stepper->getCurrentPosition();
      Serial.print("Stepper ");
      Serial.print(steppers[i].name);
      Serial.print(" targetPosition updated to: ");
      Serial.println(steppers[i].targetPosition);
    }
  }
}

float readEncoderDegCorrected() {
  uint16_t raw = readAngleRaw() & 0x3FFF;
  float angleDeg = raw * 360.0f / 16384.0f;  // 0…360°
  float pe = encoderOffset - angleDeg;       // stara korekcja zerem

  if (pe > 180.0f) pe -= 360.0f;  // wrap –180…+180
  else if (pe < -180.0f) pe += 360.0f;

  return ENC_SCALE * pe + ENC_OFFSET;  // <-- NOWA poprawka
}

void printEncoderWithOffset() {
  Serial.println(readEncoderDegCorrected(), 1);
}

void randomMoves() {
  randomSeed(micros());

  for (int i = 0; i < 15; ++i) {
    long steps;
    int deg;
    long currentPos = steppers[0].stepper->getCurrentPosition();
    long newPos;

    // losuj aż trafisz na legalny ruch
    do {
      deg = random(-120, 121);
      steps = (long)deg * steppers[0].stepperResolution;
      newPos = currentPos + steps;
    } while (newPos < steppers[0].stepsMin || newPos > steppers[0].stepsMax);

    Serial.print("Ruch ");
    Serial.print(i + 1);
    Serial.print(": ");
    Serial.print(deg);
    Serial.println("°");

    // wykonaj ruch
    steppers[0].stepper->move(steps);
    while (steppers[0].stepper->isRunning()) {
      delay(10);
    }

    // wypisz stan po ruchu
    Serial.print("P1:  ");
    printPosition(steppers[0]);
    Serial.print("PE: ");
    printEncoderWithOffset();

    delay(200);
  }
}

void printLimits(StepperMotor& motor) {
  float minVal = (float)motor.stepsMin / motor.stepperResolution;
  float maxVal = (float)motor.stepsMax / motor.stepperResolution;
  Serial.print("Ograniczenia silnika ");
  Serial.print(motor.name);
  Serial.print(": min = ");
  Serial.print(minVal, 3);
  Serial.print(", max = ");
  Serial.print(maxVal, 3);

  // Jednostka zależna od silnika:
  // M = stopnie, N = mm, L = stopnie
  if (motor.name == "N") {
    Serial.println(" [mm]");
  } else {
    Serial.println(" [stopnie]");
  }
}