#include <AccelStepper.h>

// Moottori 1
#define STEP_PIN_1 5
#define DIR_PIN_1 6
AccelStepper stepper1(AccelStepper::DRIVER, STEP_PIN_1, DIR_PIN_1);

// Moottori 2
#define STEP_PIN_2 9
#define DIR_PIN_2 10
AccelStepper stepper2(AccelStepper::DRIVER, STEP_PIN_2, DIR_PIN_2);

// Moottori 3
#define STEP_PIN_3 11
#define DIR_PIN_3 12
AccelStepper stepper3(AccelStepper::DRIVER, STEP_PIN_3, DIR_PIN_3);

// Ultraäänisensori
#define TRIG_PIN 7
#define ECHO_PIN 8

// Painonappi
#define BUTTON_PIN 2


void setup() {
  Serial.begin(9600);
  
  // Moottori 1 - Aseta perusasetukset
  stepper1.setMaxSpeed(1000.0);
  stepper1.setAcceleration(500.0);

  // Moottori 2 - Aseta perusasetukset
  stepper2.setMaxSpeed(1000.0);
  stepper2.setAcceleration(500.0);

  // Moottori 3 - Aseta perusasetukset
  stepper3.setMaxSpeed(1000.0);
  stepper3.setAcceleration(500.0);

  // Ultraäänisensori - Aseta pinnit
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Painonappi - Aseta pinnit
  pinMode(BUTTON_PIN, INPUT_PULLUP);
}

float getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  float duration = pulseIn(ECHO_PIN, HIGH);
  // Etäisyys = (ääninopeus ilmassa) * (aika / 2)
  float distance = (duration * 0.0343) / 2;

  return distance;
}

void loop() {
  // Mittaa etäisyys
  float distance = getDistance();
  Serial.println("Etäisyys: " + String(distance) + " cm");

  // Jos etäisyys on pienempi tai yhtä suuri kuin 30 cm, pysäytä moottorit
  if (distance <= 30.0) {
    stepper1.stop();
    stepper2.stop();
    stepper3.stop();
    delay(1000);
  }
  else {
    // Painonappi - Tarkista onko painonappi painettu
    if (digitalRead(BUTTON_PIN) == LOW) {
      // Liikuta moottoria 1 eteenpäin ja taaksepäin
      stepper1.moveTo(1000);
      stepper1.runToPosition();
      delay(1000);
      stepper1.moveTo(0);
      stepper1.runToPosition();
      delay(1000);

      // Liikuta moottoria 2 eteenpäin ja taaksepäin
      stepper2.moveTo(1000);
      stepper2.runToPosition();
      delay(1000);
      stepper2.moveTo(0);
      stepper2.runToPosition();
      delay(1000);

      // Liikuta moottoria 3 eteenpäin ja taaksepäin
      stepper3.moveTo(1000);
      stepper3.runToPosition();
      delay(1000);
      stepper3.moveTo(0);
      stepper3.runToPosition();
      delay(1000);
    }
  }
}