// https://forum.arduino.cc/t/how-do-i-get-line-following-robot-to-run-for-10-seconds-when-button-is-pushed/1146946/
// https://wokwi.com/projects/370699668648464385

// 1. Motors stop at distance < 20 if motor is on or within 10 seconds of button press
// 2. If button is pressed with distancd < 20 motors will not run,  timer will count to 10
// 3. If distance > 20 and button is pressed, line follower motors run
// 4. After button is pressed, motors stop after 10 seconds
// 5. After button is pressed and Left/Center/Right IR is ON, corresponding motor stops

#define DEBUG 1 // 0 = packed debug string, 1 = long debug string

// pins
const int rightmotorPin = 10;
const int leftmotorPin = 11;
const int centerirPin = 12;
const int rightirPin = 5;
const int leftirPin = 6;
const int trigPin = 2; //digital pin here
const int echoPin = A5; //analog pin here
const int buttonPin = 3;

const byte PWMspeed = 50;

// ultrasonic sensor (sonar)
int distance;
int time;

// IR line follower
int centerVal;
int rightVal;
int leftVal;

// button detection
int buttonState = 0; // set after button is pressed to make motors run. reset to 0 after trackTime
unsigned long buttonPressed = 0; // holds time button was pressed
int startMotorButton;

// timer
unsigned long trackTime = 10 * 1000; //10 seconds << this line needed a semicolon ;
unsigned long previousMillis = 0; // campare with millis() for event interval

void setup() {
  // put your setup code here, to run once:
  pinMode(rightmotorPin, OUTPUT);
  pinMode(leftmotorPin, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(centerirPin, INPUT);
  pinMode(leftirPin, INPUT);
  pinMode(rightirPin, INPUT);
  pinMode(buttonPin, INPUT_PULLUP); //ALWAYS HIGH
  Serial.begin(115200); // 115200 is much faster than 9600
}

void loop() {

  pingSONAR(); // HCSR04 distance finder
  time = pulseIn(echoPin, HIGH); //measure how much time echoPin was HIGH
  distance = time * 0.0343 / 2; //343 speed of sound in air time in microseconds hence 0.0343

  lineSense(); // IR line sensors
  checkButton(); // for motor start
  if (buttonState == 1) { // only run the motors after a button press
    lineFollow();
  }

  if (DEBUG)
    debugLong  (buttonState, distance, leftVal, centerVal, rightVal, buttonPressed);
  else
    debugShort (buttonState, distance, leftVal, centerVal, rightVal, buttonPressed);
}

//******************************************************************************//
//                          User Defined Functions                              //
//******************************************************************************//

void stopMotors() {
  digitalWrite(rightmotorPin, 0);
  digitalWrite(leftmotorPin, 0);
}

void checkButton() {
  startMotorButton = digitalRead(buttonPin); // need to read the button pin...
  if (startMotorButton == 0) { // 0 (button pressed), 1 (not pressed)
    buttonPressed = millis(); //NOTES DOWN THE TIME WHEN THE BUTTON WAS PRESSED AND STORES IN THE buttonPressed VARIABLE
    buttonState = 1;  // button was pressed... need to clear buttonState after 10 seconds
  }
}

void lineSense() {
  centerVal = digitalRead(centerirPin); // 1 white 0 black
  rightVal = digitalRead(rightirPin);   // 0 IS WHITE AND 1 IS BLACK
  leftVal = digitalRead(leftirPin);     // 0 IS WHITE AND 1 IS BLACK
}

void pingSONAR() {
  digitalWrite(trigPin, LOW); // force trigger low for clean high
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW); // force trigger low for clean echo
}

void lineFollow() {
  if (millis() - buttonPressed < trackTime) { //IF THE TIME RIGHT NOW MINUS TIME WHEN BUTTON WAS PRESSED IS LESS THAN 10 SECONDS THEN FOLLOW LINE

    if (distance < 20) { //OBSTACLE AHEAD STOP
      digitalWrite(rightmotorPin, 0);
      digitalWrite(leftmotorPin, 0);
    } else { // DISTANCE > 20, NO OBSTACLE,  CONTINUE LINE FOLLOWING

      if ((leftVal == 0) && (centerVal == 0) && (rightVal == 0)) { //IDEAL CASE
        digitalWrite(rightmotorPin, PWMspeed);
        digitalWrite(leftmotorPin, PWMspeed);
      }
      else if ((leftVal == 0) && (centerVal == 1) && (rightVal == 1)) { //RIGHT SENSOR ON LINE
        digitalWrite(rightmotorPin, 0);
        digitalWrite(leftmotorPin, PWMspeed);
      }
      else if ((leftVal == 1) && (centerVal == 1) && (rightVal == 0)) { //LEFT SENSOR ON LINE
        digitalWrite(leftmotorPin, 0);
        digitalWrite(rightmotorPin, PWMspeed);
      }
      else if ((leftVal == 1) && (centerVal == 0) && (rightVal == 1)) { //STOP ALL BLACK DETECTED
        digitalWrite(leftmotorPin, PWMspeed);
        digitalWrite(rightmotorPin, 0);
      }
      else if ((leftVal == 0) && (centerVal == 1) && (rightVal == 0)) { // ALL WHITE
        digitalWrite(leftmotorPin, PWMspeed);
        digitalWrite(rightmotorPin, PWMspeed);
      }
      else if ((leftVal == 1) && (centerVal == 0) && (rightVal == 0)) { //LEFT AND CENTER SENSOR SENSOR ON LINE
        digitalWrite(rightmotorPin, PWMspeed);
        digitalWrite(leftmotorPin, 0);
      }
      else if ((leftVal == 0) && (centerVal == 0) && (rightVal == 1)) { //RIGHT AND CENTER SENSOR ON LINE
        digitalWrite(rightmotorPin, 0);
        digitalWrite(leftmotorPin, PWMspeed);
      }
      else if ((leftVal == 1) && (centerVal == 1) && (rightVal == 1)) { //RIGHT AND LEFT SENSOR BLACK CENTER SENSOR WHITE
        digitalWrite(rightmotorPin, PWMspeed);
        digitalWrite(leftmotorPin, PWMspeed);
      }
      else { //IF NONE OF OTHER CONDITIONS
        digitalWrite(rightmotorPin, 0);
        digitalWrite(leftmotorPin, 0);
      }
    }
  }
  else { // after 10 seconds
    // clear buttonState to stop motors
    buttonState = 0;
    // stop motors
    stopMotors();
  }
}

void debugShort(int buttonState, int distance, int leftVal, int centerVal, int rightVal, unsigned long buttonPressed) {
  if (millis() - previousMillis > 1000 ) { // one second printout interval
    previousMillis = millis(); // set timer to current millis for next event

    Serial.print("Time:");
    if (((millis() - buttonPressed) / 1000) < 1000) Serial.print("0"); // zero pad for Serial presentation
    if (((millis() - buttonPressed) / 1000) < 100) Serial.print("0"); // zero pad for Serial presentation
    if (((millis() - buttonPressed) / 1000) < 10) Serial.print("0"); // zero pad for Serial presentation
    // if (buttonState) // only advance/print time if button was pressed
    Serial.print((millis() - buttonPressed) / 1000);

    if (distance < 20)  Serial.print(" distance:");
    else                Serial.print(" DISTANCE:");
    if (distance < 100) Serial.print("0"); // zero pad for Serial presentation
    if (distance < 10)  Serial.print("0"); // zero pad for Serial presentation
    Serial.print(distance);

    Serial.print(" BLCRLR:");
    Serial.print(buttonState);
    Serial.print(leftVal);
    Serial.print(centerVal);
    Serial.print(rightVal);
    Serial.print(digitalRead(leftmotorPin));
    Serial.print(digitalRead(rightmotorPin));
    Serial.println();
  }
}

void debugLong(int buttonState, int distance, int leftVal, int centerVal, int rightVal, unsigned long buttonPressed) {
  if (millis() - previousMillis > 1000 ) { // one second printout interval
    previousMillis = millis(); // set timer to current millis for next event

    Serial.print("(Time:");
    if (((millis() - buttonPressed) / 1000) < 1000) Serial.print("0"); // zero pad for Serial presentation
    if (((millis() - buttonPressed) / 1000) < 100) Serial.print("0"); // zero pad for Serial presentation
    if (((millis() - buttonPressed) / 1000) < 10) Serial.print("0"); // zero pad for Serial presentation
    // if (buttonState) // only advance/print time if button was pressed
    Serial.print((millis() - buttonPressed) / 1000);
    Serial.print(") ");

    if (distance < 20)  Serial.print("(distance:");
    else                Serial.print("(DISTANCE:");
    if (distance < 100) Serial.print("0"); // zero pad for Serial presentation
    if (distance < 10)  Serial.print("0"); // zero pad for Serial presentation
    Serial.print(distance);
    Serial.print(")");

    if (!buttonState) Serial.print(" (buttonState:");
    else              Serial.print(" (BUTTONSTATE:");
    Serial.print(buttonState);

    Serial.print(") (IR:");
    if (leftVal) Serial.print("LEFT.");
    else         Serial.print("left.");
    Serial.print(leftVal);

    if (centerVal) Serial.print(" CENTER.");
    else           Serial.print(" center.");
    Serial.print(centerVal);

    if (rightVal) Serial.print(" RIGHT.");
    else          Serial.print(" right.");
    Serial.print(rightVal);

    Serial.print(") (Motor:");
    if (digitalRead(leftmotorPin)) Serial.print("LEFT.");
    else                           Serial.print("left.");
    Serial.print(digitalRead(leftmotorPin));

    if (digitalRead(rightmotorPin)) Serial.print(" RIGHT.");
    else                            Serial.print(" right.");
    Serial.print(digitalRead(rightmotorPin));
    Serial.println(")");
  }
}