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

// Define step motor pins
#define motorPin3 3  // Motor 1 Step pin SCHNECKE
#define motorPin4 4  // Motor 1 Direction pin SCHNECKE
#define motorPin5 5  // Motor 2 Step pin WASSER
#define motorPin6 6  // Motor 2 Direction pin WASSER
#define motorPin7 7  // Motor 3 Step pin PULVER
#define motorPin8 8  // Motor 3 Direction pin PULVER

// Define Buttons as Inputs for the Arduino
#define buttonPin9 9  // Button pin for toggling Extrusion
#define buttonPin10 10  // Button pin for toggling Clean
#define buttonPin11 11  // Button pin for toggling fitting volumeflow

//Encoder pins
#define encPinA 18
#define encPinB 19
#define encButtonPin 2

// LCD setup
LiquidCrystal_I2C lcd(0x27, 16, 2); // Set the LCD address to 0x27 for a 16 chars and 2 line display

// Encoder setup
Encoder myEnc(encPinA, encPinB); // Rotary encoder pins connected to pin 2 and 3
long oldPosition  = 0;
int activeMotor = 1;

// Initialize AccelStepper objects
AccelStepper myStepper(AccelStepper::DRIVER, motorPin3, motorPin4);
AccelStepper myStepper2(AccelStepper::DRIVER, motorPin5, motorPin6);
AccelStepper myStepper3(AccelStepper::DRIVER, motorPin7, motorPin8);

bool cleaningMode = false;
bool running = false; // Motor running state
int lastButtonState9 = HIGH; // Last button state
int lastButtonState10 = HIGH;
int lastButtonState11 = HIGH;

int lastButtonState = HIGH; //Ecnoder

unsigned long extrusionStartTime = 0; // Track start time of extrusion
const unsigned long extrusionDelay = 3000; // 5 seconds delay in milliseconds
bool secondThirdMotorsStarted = false; // Track if the second and third motors have started


  // Speed memory for each motor
int lastSpeedMotor1 = 100;
int lastSpeedMotor2 = 200;
int lastSpeedMotor3 = 200;

void setup() {
  pinMode(encButtonPin, INPUT_PULLUP);

  pinMode(buttonPin9, INPUT_PULLUP);
  pinMode(buttonPin10, INPUT_PULLUP);
  pinMode(buttonPin11, INPUT_PULLUP);

  myStepper.setMaxSpeed(1000);
  myStepper.setAcceleration(500);
  myStepper2.setMaxSpeed(1000);
  myStepper2.setAcceleration(500);
  myStepper3.setMaxSpeed(1000);
  myStepper3.setAcceleration(500);
  myStepper3.setSpeed(0);

  lcd.init();
  //lcd.begin(16, 2); // Initialize the LCD
  lcd.backlight();
  updateLCD();
  
  Serial.begin(9600);
  Serial.println("Setup complete");

}

void loop() {
  handleEncoder();
  handleMotorSelectionButton();
  handleExtrusionButton();
  handleCleaningButton();
  runMotors();
}

void handleEncoder() {
    long newPosition = myEnc.read();
    if (newPosition != oldPosition) {
        long positionDifference = newPosition - oldPosition;
        oldPosition = newPosition;
        int speedChange = positionDifference * 5; // Adjust scale factor as needed
        
        // Update motor speed based on the active motor
        updateMotorSpeed(activeMotor, speedChange);
        updateLCD();
    }
}

void updateMotorSpeed(int motor, int speedChange) {
    switch (motor) {
        case 1:
            lastSpeedMotor1 += speedChange;
            lastSpeedMotor1 = constrain(lastSpeedMotor1, -1000, 1000); // Assuming motor speed limits are -1000 to 1000
            myStepper.setSpeed(lastSpeedMotor1);
            break;
        case 2:
            lastSpeedMotor2 += speedChange;
            lastSpeedMotor2 = constrain(lastSpeedMotor2, -1000, 1000);
            myStepper2.setSpeed(lastSpeedMotor2);
            break;
        case 3:
            lastSpeedMotor3 += speedChange;
            lastSpeedMotor3 = constrain(lastSpeedMotor3, -1000, 1000);
            myStepper3.setSpeed(lastSpeedMotor3);
            break;
    }
}

void updateLCD() {
    lcd.clear(); // Clear the LCD to refresh the content
    lcd.setCursor(0, 0); // Set cursor at the beginning of the first line

    // Check which motor is active and display the corresponding label and speed
    switch (activeMotor) {
        case 1:
            lcd.print("Dosing speed:"); // Display for Motor 1
            lcd.setCursor(0, 1); // Move cursor to the beginning of the second line
            lcd.print(lastSpeedMotor1);
            break;
        case 2:
            lcd.print("Water speed:"); // Display for Motor 2
            lcd.setCursor(0, 1); // Move cursor to the beginning of the second line
            lcd.print(lastSpeedMotor2);
            break;
        case 3:
            lcd.print("Material speed:"); // Display for Motor 3
            lcd.setCursor(0, 1); // Move cursor to the beginning of the second line
            lcd.print(lastSpeedMotor3);
            break;
    }
}

void handleMotorSelectionButton() {
  bool currentButtonState = digitalRead(encButtonPin);
  if (lastButtonState == HIGH && currentButtonState == LOW) { // Button pressed
    activeMotor = (activeMotor % 3) + 1; // Cycle through 1, 2, 3
    updateLCD();
    delay(200);  // Debouncing
  }
  lastButtonState = currentButtonState;
}

void handleExtrusionButton() {
  int buttonState9 = digitalRead(buttonPin9);
  if (buttonState9 != lastButtonState9) {
    lastButtonState9 = buttonState9;
    if (buttonState9 == LOW) {
      running = true;
      extrusionStartTime  = millis();
      secondThirdMotorsStarted = false;
      // Start the motor with the last known speed
      myStepper.setSpeed(lastSpeedMotor1);
      Serial.println("Extruding");
      }
      
     else {
      running = false;
      // Stop all motors when extruding stops
      myStepper.stop();
      myStepper2.stop();
      myStepper3.stop();
      myStepper.setSpeed(lastSpeedMotor1);
      myStepper2.setSpeed(lastSpeedMotor2);
      myStepper3.setSpeed(lastSpeedMotor3);
      Serial.println("Extrusion stopped");
    }
    delay(200); // Debouncing delay
}
}

void handleCleaningButton() {
  int buttonState10 = digitalRead(buttonPin10);
  if (buttonState10 != lastButtonState10) {
    lastButtonState10 = buttonState10;
    if (buttonState10 == LOW) {
      running = true; // Ensure no other process interferes
      cleaningMode = true;
      myStepper.setSpeed(lastSpeedMotor1); // Set specific or last known speeds
      myStepper2.setSpeed(lastSpeedMotor2);
      myStepper3.setSpeed(0);
      // Optional: set a cleaning-specific speed or pattern
      Serial.println("Cleaning");
    } else {
      // Restore speeds when cleaning stops
      cleaningMode = false;
      running = false; // Stop the motors
      myStepper.stop();
      myStepper2.stop();
      myStepper3.stop();
      // Restore previous speed settings
      myStepper.setSpeed(lastSpeedMotor1);
      myStepper2.setSpeed(lastSpeedMotor2);
      myStepper3.setSpeed(lastSpeedMotor3);
      Serial.println("Cleaning stopped");
    }
    delay(200); // Debouncing delay
  }
}

void runMotors() {
    if (running) {
        // Handle motor operation based on the mode
        if (cleaningMode) {
            // Only run the first and second motors in cleaning mode
            myStepper.runSpeed();
            myStepper2.runSpeed();
            // Motor 3 should not run as its speed is set to zero
        } else {
            // Normal operation with delay handling for motors 2 and 3
            unsigned long currentTime = millis();
            myStepper.runSpeed(); // Always run the first motor

            if (!secondThirdMotorsStarted && (currentTime - extrusionStartTime >= extrusionDelay)) {
                myStepper2.setSpeed(lastSpeedMotor2);
                myStepper3.setSpeed(lastSpeedMotor3);
                secondThirdMotorsStarted = true;
            }

            if (secondThirdMotorsStarted) {
                myStepper2.runSpeed();
                if (myStepper3.speed() != 0) {
                    myStepper3.runSpeed();
                }
            }
        }
    }
}