#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <AccelStepper.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

// motor 2

#define directionPin2 2
#define stepPin2 3
#define enPin2 4
#define switchPin1 6  //toggle switch motor 2  on   oranje
#define switchPin2 5  //toggle switch motor 2  off  Bruin
#define potentiometerPin2 A1

AccelStepper stepper2(AccelStepper::DRIVER, stepPin2, directionPin2);

// MOTOR 1

#define directionPin1 7
#define stepPin1 9
#define enPin1 8
#define switchPin3 11  // toggle switch motor 1 on  Blauw
#define switchPin4 10  // toggle switch motor 1 off  Groen
#define potentiometerPin1 A0

AccelStepper stepper1(AccelStepper::DRIVER, stepPin1, directionPin1);

int previousPotValue1 = 0;
int previousPotValue2 = 0;

bool motor1Running = false;
bool motor2Running = false;

void setup() {

  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("    Stretcher  ");
  lcd.setCursor(0, 1 );
  lcd.print("Made WE62 Ugent");
  delay(6000);
  lcd.clear();

  lcd.setCursor(0, 0);
  lcd.print("Speed M1: ");
  lcd.setCursor(13, 0);
  lcd.print("  % ");
  lcd.setCursor(0, 1);
  lcd.print("Speed M2: ");
  lcd.setCursor(13, 1);
  lcd.print("  % ");

  pinMode(switchPin1, INPUT);  // Schakelaar 1 heeft een pull-up weerstand
  pinMode(switchPin2, INPUT);  // Schakelaar 2 heeft een pull-up weerstand
  pinMode(switchPin3, INPUT);  // Schakelaar 3 heeft een pull-up weerstand
  pinMode(switchPin4, INPUT);  // Schakelaar 4 heeft een pull-up weerstand

  pinMode(directionPin1, OUTPUT);
  pinMode(stepPin1, OUTPUT);
  pinMode(enPin1, OUTPUT);

  digitalWrite(directionPin1, HIGH);
  digitalWrite(enPin1, HIGH);

  pinMode(directionPin2, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(enPin2, OUTPUT);

  digitalWrite(directionPin2, LOW);
  digitalWrite(enPin2, HIGH);

  digitalWrite(switchPin1, LOW);  // Schakelaar 1 heeft een pull-up weerstand
  digitalWrite(switchPin2, LOW);  // Schakelaar 1 heeft een pull-up weerstand
  digitalWrite(switchPin3, LOW);  // Schakelaar 1 heeft een pull-up weerstand
  digitalWrite(switchPin4, LOW);  // Schakelaar 1 heeft een pull-up weerstand

  stepper1.setMaxSpeed(4000);  // Set max speed in steps per second
  stepper2.setMaxSpeed(4000);

  stepper1.setAcceleration(500);  // Set acceleration in steps per second^2
  stepper2.setAcceleration(500);

  stepper1.setCurrentPosition(0);
  stepper2.setCurrentPosition(0);
}

void updateLCD(float speedPercent, int motor) {
  lcd.setCursor(10, motor);
  lcd.print(speedPercent, 0);
  lcd.print(" ");
}

void loop() {

  // Controleer de status van de toggle switches voor motor 2
  if (digitalRead(switchPin1) == HIGH) {  // start motor 2
    motor2Running = true;
    digitalWrite(enPin2, LOW);

  } else if (digitalRead(switchPin2) == HIGH) {  // start motor 2
    motor2Running = false;
    Serial.println(switchPin2);
    digitalWrite(enPin2, HIGH);
  }

  // Controleer de status van de toggle switches voor motor 1
  if (digitalRead(switchPin3) == HIGH) {  // start motor 1
    motor1Running = true;
    digitalWrite(enPin1, LOW);

  } else if (digitalRead(switchPin4) == HIGH) {  // stop motor 1
    digitalWrite(enPin1, HIGH);
    motor1Running = false;
  }

  // Steppermotor 1
  if (motor1Running) {
    int potValue1 = analogRead(potentiometerPin1);
    float speedPercent1 = map(potValue1, 0, 1023, 100, 1);
    if (abs(potValue1 - previousPotValue1) > 10) {
      updateLCD(speedPercent1, 0);
      previousPotValue1 = potValue1;
    }
    int speed1 = map(potValue1, 1023, 0, 0, 4000);  // Map potentiometer value to speed
    stepper1.setSpeed(speed1);
    stepper1.runSpeed();
  } else {
    stepper1.stop();
  }

  // Stepper motor 2
  if (digitalRead(switchPin2) == LOW) {
    int potValue2 = analogRead(potentiometerPin2);
    float speedPercent2 = map(potValue2, 0, 1023, 100, 1);
    if (abs(potValue2 - previousPotValue2) > 10) {
      updateLCD(speedPercent2, 1);
      previousPotValue2 = potValue2;
    }
    int speed2 = map(potValue2, 1023, 0, 0, 4000);  // Map potentiometer value to speed
    stepper2.setSpeed(speed2);
    stepper2.runSpeed();
  } else {
    stepper2.stop();
  }
}