#include <AccelStepper.h>
#include <MultiStepper.h>

// Definicje pinów dla fotorezystorów
#define PIN_FR_1 A0
#define PIN_FR_2 A1
#define PIN_FR_3 A2
#define PIN_FR_4 A3

// Definicje pinów dla silników krokowych
#define PIN_STEP_HORIZONTAL 2
#define PIN_DIR_HORIZONTAL 3
#define PIN_STEP_VERTICAL 4
#define PIN_DIR_VERTICAL 5

// Definicje pinów dla czujników krańcowych
#define PIN_LIMIT_HORIZONTAL 6
#define PIN_LIMIT_VERTICAL 7

// Obiekty dla silników krokowych
AccelStepper stepperHorizontal(AccelStepper::DRIVER, PIN_STEP_HORIZONTAL, PIN_DIR_HORIZONTAL);
AccelStepper stepperVertical(AccelStepper::DRIVER, PIN_STEP_VERTICAL, PIN_DIR_VERTICAL);
MultiStepper multiStepper;

// Stałe do kalibracji
int calibrationDelay = 1000; // ms
int calibrationThreshold = 100; // Wartość graniczna dla kalibracji

void setup() {
  Serial.begin(9600);

  // Ustawienia dla silnika poziomego
  stepperHorizontal.setMaxSpeed(1000);
  stepperHorizontal.setAcceleration(500);

  // Ustawienia dla silnika pionowego
  stepperVertical.setMaxSpeed(1000);
  stepperVertical.setAcceleration(500);

  // Ustawienia dla czujnika krańcowego poziomego
  pinMode(PIN_LIMIT_HORIZONTAL, INPUT_PULLUP);

  // Ustawienia dla czujnika krańcowego pionowego
  pinMode(PIN_LIMIT_VERTICAL, INPUT_PULLUP);

  // Kalibracja
  calibrate();
}

void loop() {
  // Odczyt wartości z fotorezystorów
  int sensorValue1 = analogRead(PIN_FR_1);
  int sensorValue2 = analogRead(PIN_FR_2);
  int sensorValue3 = analogRead(PIN_FR_3);
  int sensorValue4 = analogRead(PIN_FR_4);

  // Przesunięcie wartości o wartość kalibracyjną
  sensorValue1 -= calibrationThreshold;
  sensorValue2 -= calibrationThreshold;
  sensorValue3 -= calibrationThreshold;
  sensorValue4 -= calibrationThreshold;

  // Korekcja wartości ujemnych (jeśli występują)
  sensorValue1 = max(0, sensorValue1);
  sensorValue2 = max(0, sensorValue2);
  sensorValue3 = max(0, sensorValue3);
  sensorValue4 = max(0, sensorValue4);

  // Wyświetlanie odczytów w konsoli
  Serial.print("FR1: ");
  Serial.print(sensorValue1);
  Serial.print("\tFR2: ");
  Serial.print(sensorValue2);
  Serial.print("\tFR3: ");
  Serial.print(sensorValue3);
  Serial.print("\tFR4: ");
  Serial.println(sensorValue4);

  // Sterowanie silnikami na podstawie odczytów z fotorezystorów
  controlMotors(sensorValue1, sensorValue2, sensorValue3, sensorValue4);

  // Sprawdzenie czy silniki osiągnęły krańce
  checkLimits();
}

void calibrate() {
  Serial.println("Rozpocznij kalibrację...");

  delay(calibrationDelay);

  int sensorValue1 = analogRead(PIN_FR_1);
  int sensorValue2 = analogRead(PIN_FR_2);
  int sensorValue3 = analogRead(PIN_FR_3);
  int sensorValue4 = analogRead(PIN_FR_4);

  calibrationThreshold = max(sensorValue1, max(sensorValue2, max(sensorValue3, sensorValue4)));

  Serial.println("Kalibracja zakończona.");
}

void controlMotors(int fr1, int fr2, int fr3, int fr4) {
  // Wylicz różnicę pomiędzy odczytami z przeciwległych fotorezystorów
  int horizontalDiff = fr1 - fr3;
  int verticalDiff = fr2 - fr4;

  // Wylicz korektę dla silnika poziomego
  int horizontalCorrection = map(horizontalDiff, -calibrationThreshold, calibrationThreshold, -100, 100);

  // Wylicz korektę dla silnika pionowego
  int verticalCorrection = map(verticalDiff, -calibrationThreshold, calibrationThreshold, -100, 100);

  // Zastosuj korekty
  multiStepper.moveTo(horizontalCorrection, verticalCorrection);
  multiStepper.runSpeedToPosition();
}

void checkLimits() {
  // Sprawdzenie czy silnik poziomy osiągnął krańce
  if (digitalRead(PIN_LIMIT_HORIZONTAL) == LOW) {
    stepperHorizontal.setCurrentPosition(0);
  }

  // Sprawdzenie czy silnik pionowy osiągnął krańce
  if (digitalRead(PIN_LIMIT_VERTICAL) == LOW) {
    stepperVertical.setCurrentPosition(0);
  }
}