#include <AccelStepper.h>
#include <Servo.h> // Include the Servo library

// Define motor connections and setup AccelStepper
#define dirPin1 10
#define stepPin1 11
#define enablePin1 6

#define dirPin2 7  // Direction pin for second stepper motor
#define stepPin2 8  // Step pin for second stepper motor
#define enablePin2 5 // Enable pin for second stepper motor

#define dirPin3 3  // Direction pin for third stepper motor
#define stepPin3 2  // Step pin for third stepper motor
#define enablePin3 4 // Enable pin for third stepper motor

// LED pins
const int ledPin1 = 4;  // Pin connected to the first LED
const int ledPin2 = 25; // Pin connected to the second LED

// Create instances of the stepper motors
AccelStepper stepper1(AccelStepper::DRIVER, stepPin1, dirPin1);
AccelStepper stepper2(AccelStepper::DRIVER, stepPin2, dirPin2); // Second stepper motor
AccelStepper stepper3(AccelStepper::DRIVER, stepPin3, dirPin3); // Third stepper motor

// Create instances of the Servos
Servo myServo1; // First servo object
Servo myServo2; // Second servo object on pin 45
Servo myServo3; // Third servo object for new servo

// Ultrasonic sensor pins
const int trigPin = A4; // Trigger pin
const int echoPin = A3; // Echo pin

// Button pins
const int startButtonPin = 13; // The pin connected to the START button
const int stopButtonPin = 12;   // The pin connected to the STOP button

// Temperature sensor pin
const int tempPin = A5; // Pin for temperature sensor
const float BETA = 3950; // Should match the Beta Coefficient of the thermistor

bool isRunning = false; // Flag to indicate if motor 1 is running
bool servo1Moving = false; // Flag to indicate if the first servo is moving
bool servo1Moved = false;  // Flag to indicate if the first servo has already moved
bool motor2Running = false; // Flag to indicate if motor 2 is running
bool motor3Running = false; // Flag to indicate if motor 3 is running

unsigned long motor2StartTime = 0; 
unsigned long motor3StartTime = 0; 
unsigned long motor3RunDuration = 10000; // 10 seconds for motor 3

void setup() {
  // Set up the motors
  pinMode(enablePin1, OUTPUT);
  digitalWrite(enablePin1, LOW); // Enable the driver for motor 1
  pinMode(enablePin2, OUTPUT);
  digitalWrite(enablePin2, LOW); // Enable the driver for motor 2
  pinMode(enablePin3, OUTPUT);
  digitalWrite(enablePin3, LOW); // Enable the driver for motor 3

  // Set up the LED pins
  pinMode(ledPin1, OUTPUT); // Set LED pin 1 as output
  pinMode(ledPin2, OUTPUT); // Set LED pin 2 as output
  digitalWrite(ledPin2, LOW); // Start with LED 2 off

  // Set up the button
  pinMode(startButtonPin, INPUT_PULLUP); // Use internal pull-up resistor
  pinMode(stopButtonPin, INPUT_PULLUP);   // Use internal pull-up resistor

  // Set up the ultrasonic sensor pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  stepper1.setMaxSpeed(1000); // Set max speed for motor 1 (steps per second)
  stepper2.setMaxSpeed(1000); // Set max speed for motor 2 (steps per second)
  stepper3.setMaxSpeed(1000); // Set max speed for motor 3 (steps per second)

  // Attach the servos to their respective pins
  myServo1.attach(9); // Attach first servo to pin 9
  myServo1.write(0); // Start first servo at 0 degrees

  myServo2.attach(45); // Attach second servo to pin 45
  myServo2.write(0); // Start second servo at 0 degrees

  myServo3.attach(44); // Attach third servo to pin 44
  myServo3.write(0); // Start third servo at 0 degrees
}

void loop() {  
  // Check if the START button is pressed
  if (digitalRead(startButtonPin) == LOW) {
    isRunning = true; // Start the first motor
    servo1Moved = false; // Reset first servo moved flag to allow movement again
    digitalWrite(ledPin1, LOW); // Ensure the LED is off when starting
  }

  // Read the distance from the ultrasonic sensor
  long distance = getDistance();

  // Stop the first motor if the distance is 200 cm or greater
  if (isRunning && distance >= 200) {
    isRunning = false; 
    stepper1.stop(); 
    servo1Moving = true;  
  }

  // Run the first motor if the flag is set
  if (isRunning) {
    stepper1.setSpeed(500); 
    stepper1.runSpeed(); // Move the first motor
  }

  // Move the first servo from 0 to 180 degrees only once
  if (servo1Moving && !servo1Moved) {
    moveServoTo90(myServo1);
  }

  // Turn on the LED after the first servo has moved
  if (servo1Moved) {
    digitalWrite(ledPin1, HIGH); 
  }

  // Start motor 2 after the servo has moved
  if (!isRunning && !motor2Running && servo1Moved) {
    motor2StartTime = millis(); // Record the start time for motor 2
    motor2Running = true; // Set the flag to indicate motor 2 is about to run
  }

  // Run the second stepper motor for 5 seconds
  if (motor2Running) {
    if (millis() - motor2StartTime < 5000) { 
      stepper2.setSpeed(500); 
      stepper2.runSpeed(); 
    } else {
      motor2Running = false; // Stop motor 2 after 5 seconds
      stepper2.stop(); 
      digitalWrite(enablePin2, HIGH); 
      digitalWrite(ledPin1, LOW);
      digitalWrite(ledPin2, HIGH);
    }
  }

  // Check temperature and run motor 3 if temperature is 60°C or higher
  float temperature = readTemperature();
  if (temperature >= 60.0) {
    if (!motor3Running) {
      motor3Running = true; 
      motor3StartTime = millis(); 
      motor2Running = false; 
    }

    // Run motor 3 for 10 seconds
    if (motor3Running) {
      stepper3.setSpeed(500); 
      stepper3.runSpeed(); 
      
      // Stop motor 3 after 10 seconds
      if (millis() - motor3StartTime >= motor3RunDuration) {
        motor3Running = false; 
        stepper3.stop(); 
        digitalWrite(enablePin3, HIGH); 
      }
    }
  } else {
    motor3Running = false; // Stop motor 3 if temperature is below threshold
    stepper3.disableOutputs(); 
  }

  // Move the second servo from 0 to 180 degrees after motor 2 completes
  if (!motor2Running && servo1Moved) {
    moveServoTo90(myServo2); 
  }

  // Move the third servo from 0 to 180 degrees after servo 2 completes
  if (!motor2Running && servo1Moved && myServo2.read() == 180) {
    moveServoTo90(myServo3);
  }
}

// Function to get distance from the ultrasonic sensor
long getDistance() {
  // Clear the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  
  // Set the trigPin HIGH for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Read the echoPin, pulseIn returns the duration in microseconds
  long duration = pulseIn(echoPin, HIGH);
  
  // Calculate the distance in cm (speed of sound is 34300 cm/s)
  long distance = duration * 0.034 / 2; 
  return distance;
}

// Function to read temperature from the sensor
float readTemperature() {
  int analogValue = analogRead(tempPin); 
  float celsius = 1 / (log(1 / (1023. / analogValue - 1)) / BETA + 1.0 / 298.15) - 273.15; 
  return celsius; // Return the temperature
}

// Function to move a servo from 0 degrees to 180 degrees slowly
void moveServoTo90(Servo &servo) {
  for (int pos = 90; pos <= 180; pos += 1) {
    servo.write(pos); // Move the servo to the desired position
    delay(15); // Wait for the servo to reach the position
  }
  servo1Moved = true; // Set the flag to indicate that the first servo has moved
}

// Function to move a servo from 180 degrees to 0 degrees slowly
void moveServoTo0(Servo &servo) {
  for (int pos = 180; pos >= 90; pos -= 1) {
    servo.write(pos); // Move the servo to the desired position
    delay(15); // Wait for the servo to reach the position
  }
  servo1Moved = false; // Update the flag if needed


  
}