#include <HX711.h>
#include <AccelStepper.h>

// Steppers Initialization Start
const int DIR = 2;   // 6
const int STEP = 3;  // step
#define motorInterfaceType 1
float previousSpeed = 0;  // Variable to store the previous speed
AccelStepper stepper = AccelStepper(motorInterfaceType, STEP, DIR);
static String lastMessage2 = "";  // Keep track of the last printed message
// Steppers Initialization End

// Actuators Initialisation Start
const int ACTUATOR_UN_UP = A0; // Pin for Actuator 1 UP direction
const int ACTUATOR_UN_DOWN = A1; // Pin for Actuator 1 DOWN direction
const int ACTUATOR_DEUX_UP = A2; // Pin for Actuator 2 UP direction
const int ACTUATOR_DEUX_DOWN = A3; // Pin for Actuator 2 DOWN direction
// Actuators Initialisation End

bool finCycle = false;
int currentAction = 0;
unsigned long previousMillis = 0;  // Stores the last time the action was executed
const unsigned long timeoutIntervals[] = {10000, 10000, 5000, 20000};  // Different timeout intervals for each action

void setup() {
  Serial.begin(115200);
  while (!Serial) {
    ;  // Wait for the serial port to connect. Needed for native USB port only
  }

  Serial.println("Please enter the desired motor speed:");
  int desiredSpeed = getUserInput();
  Serial.println("Please enter the limit speed:");
  int limitSpeed = getUserInput();
  setupMotor(desiredSpeed, limitSpeed);
  setupActuators();
}

void loop() {
  unsigned long currentMillis = millis();

  if (Serial.available()) {
    char command = Serial.read();
    if (command == 'n') {  // 'n' could be the command to proceed to the next action
      moveToNextAction();
    }
  }

  // Automatically move to the next action after the specific timeout for the current action
  if (currentMillis - previousMillis >= timeoutIntervals[currentAction]) {
    moveToNextAction();
  }

  if (finCycle == false) executeAction(currentAction, finCycle);
}

void moveToNextAction() {
  if (currentAction == 3) { finCycle = true; }

  currentAction = (currentAction + 1) % 4;  // Cycle through actions

  if (finCycle == true) {
    printMessage("Fin du cycle de nettoyage", lastMessage2);
    endAction4();
  }

  previousMillis = millis();  // Reset the timer when moving to the next action
}

void setupMotor(int speedLimit, int speed) {
  stepper.setMaxSpeed(speedLimit);  // limite de vitesse
  stepper.setSpeed(speed);          //  vitesse de rotation
}

void StartSteppers() {
  stepper.runSpeed();
  float currentSpeed = stepper.speed();

  if (currentSpeed != previousSpeed) {
    Serial.print("Current Speed: ");
    Serial.println(currentSpeed);
    previousSpeed = currentSpeed;
  }
}

void setupActuators() {
  // Set pin modes for actuators
  pinMode(ACTUATOR_UN_UP, OUTPUT);  
  pinMode(ACTUATOR_UN_DOWN, OUTPUT);
   
  pinMode(ACTUATOR_DEUX_UP, OUTPUT);
  pinMode(ACTUATOR_DEUX_DOWN, OUTPUT);
}

void Actuators() {
  // Code to control actuators
   // Actuator 1 and 2 moving UP
  Serial.println("Actuators moving UP");
  digitalWrite(ACTUATOR_UN_UP, HIGH); // Motor 1 runs UP
  digitalWrite(ACTUATOR_DEUX_UP, HIGH); // Motor 2 runs UP
  
  digitalWrite(ACTUATOR_UN_DOWN, LOW); // Motor 1 stops DOWN
  digitalWrite(ACTUATOR_DEUX_DOWN, LOW); // Motor 2 stops DOWN
  delay(2000); // Wait for 5 seconds

  // Actuator 1 and 2 moving DOWN
  Serial.println("Actuators moving DOWN");
  digitalWrite(ACTUATOR_UN_DOWN, HIGH); // Motor 1 runs DOWN
  digitalWrite(ACTUATOR_DEUX_DOWN, HIGH); // Motor 2 runs DOWN
  
  digitalWrite(ACTUATOR_UN_UP, LOW); // Motor 1 stops UP
  digitalWrite(ACTUATOR_DEUX_UP, LOW); // Motor 2 stops UP
  delay(2000); // Wait for 5 seconds
}

void ControlPump() {
  // Code to control the pump
}

void ControlBrushes() {
  // Code to control the brushes
}

void endAction1() {
  // Code to execute at the end of action 1
}

void endAction2() {
  // Code to execute at the end of action 2
}

void endAction3() {
  // Code to execute at the end of action 3
}

void endAction4() {
  // Code to execute at the end of action 4
}

int getUserInput() {
  String inputString = "";  // String to hold input
  while (inputString.length() == 0) {
    if (Serial.available() > 0) {
      inputString = Serial.readStringUntil('\n');  // Read until newline
      inputString.trim();                          // Remove any leading or trailing whitespace
    }
  }

  int value = inputString.toInt();  // Convert the input to an integer
  Serial.print("You entered: ");
  Serial.println(value);
  return value;
}

void printMessage(String currentMessage, String &lastMessage) {
  if (currentMessage != lastMessage) {
    Serial.println(currentMessage);
    lastMessage = currentMessage;
  }
}

void executeAction(int actionId, bool endCycle) {
  static String lastMessage = "";  // Keep track of the last printed message

  switch (actionId) {
    case 0:
      printMessage("Lancement des actuateurs", lastMessage2);
      Actuators();  // Execute the specific action function
      break;

    case 1:
      endAction1();
      printMessage("Fin de l'action 1", lastMessage2);
      printMessage("Lancement des pompes", lastMessage);
      ControlPump();  // Execute the specific action function
      break;

    case 2:
      endAction2();
      printMessage("Fin de l'action 2", lastMessage2);
      printMessage("Lancement des brosses", lastMessage);
      ControlBrushes();  // Execute the specific action function
      break;

    case 3:
      endAction3();
      printMessage("Fin de l'action 3", lastMessage2);
      printMessage("Lancement des steppers", lastMessage);
      StartSteppers();  // Execute the specific action function
      break;
  }
}