#include <SoftwareSerial.h>
#include <DFRobotDFPlayerMini.h>
#include <Stepper.h>
#include <math.h>



unsigned long noObjectStartTime = 0;  // When object disappeared
bool waitingForNoObject = false;       // Is timer running?
const unsigned long noObjectDelay = 1000;  // Delay time in ms (1 second)

////Ultrasonic Sensor
const int trigPin = 7;
const int echoPin = 8;
const int ledPin = 6;

//Distances
const int threshold = 200;

const int level_3 = threshold;
const int level_2 = 150;
const int level_1 = 50;


////Stepper Motor
const int stepsPerRevolution = 2048;
Stepper myStepper(stepsPerRevolution, 2, 3, 4, 5);

//MP3_Player
SoftwareSerial mySerial(10, 9);
DFRobotDFPlayerMini myDFPlayer;






enum SystemState {
  Idle,
  GoingForward,
  Playing,
  GoingBack
};

SystemState currentState = Idle;

bool objectDetected = false;
int currentTrack = 0;

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(ledPin, OUTPUT);

  Serial.begin(9600);
  mySerial.begin(9600);

  if (!myDFPlayer.begin(mySerial)) {
    Serial.println("DFPlayer is playing!");
    while (true);
  }

  Serial.println("DFPlayer not working.");
  myDFPlayer.volume(25);
  myStepper.setSpeed(5);
}

///Function - Measure Distance

int measureDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH, 30000);
  int distance = duration * 0.034 / 2;
  return distance;
}


///Function - change track based on distance

int getTrackFromDistance(int distance) {
  if (distance > level_2 && distance <= level_3) return 1;
  else if (distance > level_1 && distance <= level_2) return 2;
  else if (distance > 0 && distance <= level_1) return 3;
  return 0;
}

///Angle Conversion


int stepperRotation(int angle) {
  return (int)round(((float)angle * stepsPerRevolution) / 360.0);
}


void loop() {
  int distance = measureDistance();
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");


///TRUE IF-
  objectDetected = (distance > 0 && distance <= threshold);

///PICK Condition
  switch (currentState) {
    case Idle:
      if (objectDetected) {
        digitalWrite(ledPin, HIGH);
        currentState = GoingForward;
      }
      break;

    case GoingForward:
      Serial.println("Motor turns front");
      myStepper.step(stepperRotation(180));
      currentTrack = getTrackFromDistance(distance);
      if (currentTrack > 0) {
        myDFPlayer.loop(currentTrack);  // Put the track into loop
        Serial.print("Track start (loop): ");
        Serial.println(currentTrack);
      }
      currentState = Playing;
      break;

    case Playing:
   if (!objectDetected) {
    if (!waitingForNoObject) {
      // Object just disappeared, start timer
      noObjectStartTime = millis();
      waitingForNoObject = true;
    } else {
      // Timer running, check if enough time passed
      if (millis() - noObjectStartTime >= noObjectDelay) {
        digitalWrite(ledPin, LOW);
        currentState = GoingBack;
        waitingForNoObject = false;  // Reset flag
      }
    }
  } else {
    // Object detected again, cancel timer
    waitingForNoObject = false;
    int newTrack = getTrackFromDistance(distance);
    if (newTrack != currentTrack && newTrack > 0) {
      myDFPlayer.stop();
      myDFPlayer.loop(newTrack);
      currentTrack = newTrack;
      Serial.print("Track updated (loop): ");
      Serial.println(currentTrack);
    }
  }
  break;

    case GoingBack:
      Serial.println("Motor turns back");
      myDFPlayer.stop(); // Stop Track 
      myStepper.step(stepperRotation(-180));
      currentTrack = 0;
      currentState = Idle;
      break;
  }

  delay(100);
}