/*
   Forum: https://forum.arduino.cc/t/stop-the-rotation-of-stepper-motor-when-the-key-is-pressed/1164919/16
   Wokwi: https://wokwi.com/projects/375498290494353409
*/

#include <AccelStepper.h>
#include <Servo.h>

//Initialize servo motor
Servo servo1;
Servo servo2;
Servo servo3;
unsigned long lastOutOfPositionMs = 0; // global variable to remember out-of-position time across loop()s.

//Initialize stepper motor
#define STEPS_PER_REVOLUTION 1000  // Change this to match your stepper motor's specification
#define DIR_PIN 8
#define STEP_PIN 9

constexpr int maxSpeed = 2000;    // Set the maximum speed of the stepper motor (adjust as needed)
constexpr int runAtSpeed = 1000;  // Set the speed of the stepper motor (adjust as needed)

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

// Initialize push button
const int pushButtonPin = 12;  // Replace with the actual pin connected to the push button


boolean stepperOn = false;



void setup() {
  // Initialize servo motors
  servo1.attach(2);
  servo2.attach(3);
  servo3.attach(4);

  mystepper.setMaxSpeed(maxSpeed);
  pinMode(pushButtonPin, INPUT_PULLUP);
  Serial.begin(9600);
  Serial.println("Begin");
}

void returnAllServos(){
  servo1.write(90);
  servo2.write(90);
  servo3.write(90);
}

void handleStepper(){
  if (stepperOn){
    mystepper.runSpeed();
  }
}

void reactOnButton(){
  if (buttonReleased()) {
    if (stepperOn) {
      Serial.println("Button stops");
      stopStepper();
    } else {
      Serial.println("Button starts");
      startStepper();
    }
  }
}

void reactOnSerial(){
  if (Serial.available() > 0) {
    String command = Serial.readStringUntil('\n');

    // Parse command and move servo motors or stepper motor
    if (command.startsWith("s")) {
      // Move stepper motor
      startStepper();

    } 
    
    if (command.startsWith("p")) {
      stopStepper();
    } 

    int servo = command.substring(0, 1).toInt();
    int pos = command.substring(2).toInt();

    if (servo == 1) {
      servo1.write(pos);
      lastOutOfPositionMs = millis();
    }

    if (servo == 2) {
      servo2.write(pos);
      lastOutOfPositionMs = millis();
    }

    if (servo == 3) {
      servo3.write(pos);
      lastOutOfPositionMs = millis();
    }


    /*char command = Serial.read();
    switch (command) {
      case 's' : startStepper();
        break;
      case 'p' : stopStepper();
        break;
    }*/
  }

  if(millis() - lastOutOfPositionMs > 750){ // set long enough to allow servos to make it to position.
    returnAllServos();
  }
}

void startStepper() {
  Serial.println("Start received");
  mystepper.setSpeed(runAtSpeed);
  stepperOn = true;
}

void stopStepper() {
  Serial.println("Stop received");
  mystepper.stop();
  stepperOn = false;
}


boolean buttonReleased() {
  static unsigned long lastTime = 0;
  static int lastState = HIGH;
  static int state = HIGH;
  boolean returnState = false;
  int actState = digitalRead(pushButtonPin);
  if (actState != lastState) {
    lastTime = millis();
    lastState = actState;
  }
  if (millis() - lastTime > 30 && state != lastState) {
    state = lastState;
    if (state) {
      returnState = true;
    }
  }
  return returnState;
}

void loop() {
  reactOnButton();
  reactOnSerial();
  handleStepper();
}