#include "AccelStepper.h"

// Пины
#define STEP_PIN 9
#define DIR_PIN 8 
#define ENDSTOP_PIN 2  // Концевик (подтянут к GND)

// Настройки двигателя
#define MAX_SPEED 10000    // Макс. скорость (шагов/сек)
#define ACCELERATION 500  // Ускорение (шагов/сек²)
#define CALIBRATION_SPEED 100000000  // Скорость калибровки

AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// Флаг завершения калибровки
bool isCalibrated = false;

// Переменные для отслеживания времени
unsigned long prevTime = 0;
const long reportInterval = 100;  // Интервал отчетов в мс

// ============================================
// Калибровка (едет к концевику и сбрасывает в 0)
// ============================================
void calibrate() {
  Serial.println("Калибровка... Движение к концевику");
  
  stepper.setSpeed(-CALIBRATION_SPEED);  // Движение в сторону концевика
  
  while (digitalRead(ENDSTOP_PIN) != 1) {  // Пока концевик не сработал
    stepper.runSpeed();
    Serial.print(digitalRead(ENDSTOP_PIN));
    printStatus();  // Вывод статуса во время калибровки
  }
  
  stepper.stop();  // Остановка
  stepper.setCurrentPosition(0);  // Сброс позиции в 0
  isCalibrated = true;
  
  Serial.println("Калибровка завершена! Текущая позиция: 0");
}

// ============================================
// Вывод текущего статуса (позиция, скорость, время)
// ============================================
void printStatus() {
  unsigned long currentTime = millis();
  if (currentTime - prevTime >= reportInterval) {
    prevTime = currentTime;
    
    Serial.print("Позиция: ");
    Serial.print(stepper.currentPosition());
    Serial.print(" шагов | Скорость: ");
    Serial.print(stepper.speed());
    Serial.print(" шаг/сек | Время: ");
    Serial.print(currentTime / 1000.0, 2);
    Serial.print(" сек ");
    Serial.println(stepper.distanceToGo());
  }
}

// ============================================
// Движение к указанной позиции
// ============================================
void moveToPosition(long targetPos) {
  if (!isCalibrated) {
    Serial.println("Ошибка: сначала выполните калибровку!");
    return;
  }
  
  stepper.moveTo(targetPos);
  
  while (stepper.distanceToGo() != 0) {  // Пока не доехал
    stepper.run();
    printStatus();
  }
  
  Serial.print("Приехали на позицию: ");
  Serial.println(targetPos);
}

// ============================================
// Настройка
// ============================================
void setup() {
  Serial.begin(9600);
  pinMode(ENDSTOP_PIN, INPUT);  // Подтяжка к +5V
  
  stepper.setMaxSpeed(MAX_SPEED);
  stepper.setAcceleration(ACCELERATION);
  
  // Запуск калибровки при старте
  calibrate();
}

// ============================================
// Основной цикл
// ============================================
void loop() {
  // Здесь можно добавить управление через Serial
  if (Serial.available()) {
    long target = Serial.parseInt();
    if (target != 0) {
      moveToPosition(target);
    }
  }
  
  // Периодический вывод статуса в режиме ожидания
  printStatus();
  delay(100);
}