//******************************************************************************//
//                                Global Variables                              //
//******************************************************************************//

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

byte buttonPin = 3;

const byte PWMspeed = 80;

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

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

// button detection
byte buttonState = 0; //holds state of button after pressed
unsigned long buttonpressedTime = 0; // holds time when button was pressed
byte startmotorButton; //DETECTS BUTTON PRESS

// stop point detection
unsigned long stoppointTime = 0; //holds time of when car stops

// stop ended detection
unsigned long stopendTime = 0; //holds time of when 5 sec stop time over

//sonar pause detection
byte sonarpauseState = 0;

//one time pass
bool x = 0;
bool y = 0;
bool a = 0;
bool b = 0;

int time1 = 10000;
int time2 = 5000;
int time3 = 10000;


//debug timer
unsigned long debugpreviousMillis = 0; // compare with millis() for event interval

//******************************************************************************//
//                          Setup & Loop Functions                              //
//******************************************************************************//

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);
  Serial.begin(115200); // 115200 is much faster than 9600 ._.

}

void loop() {
  
  obstacledetect();

  lineSense();

  checkButton(); //check button for press set button state 1 if pressed

  botmovement(); // run if button has been pressed

  debug (buttonState, distance, leftVal, centerVal, rightVal, buttonpressedTime);
}

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


  void botmovement() {
    if (buttonState == 1) { // if button has been pressed
      startmotorButton = 7; //some number that is not 0 or 1 to prevent button spamming
      botmovementtimings();
      if (millis() - buttonpressedTime < time1) { //If button pressed run bot for 10 seconds
        runlinefollower();
        obstacledetect();
      } else {
        if (millis() - stoppointTime < time2) { //Stop robot for 5 seconds
          stopMotors();
        } else {
          if (millis() - stopendTime < time3) { //Run robot for 10 seonds
            runlinefollower();
            obstacledetect();
          } else { //Reset all values, stop motor, read for button press
            stopMotors();
            a = 0;
            b = 0;
            buttonState = 0;
            stoppointTime = 0;
            stopendTime = 0;
          }
        }
      }
    }
  }

  void botmovementtimings() {
    if (buttonState == 1) {
      if (millis() - buttonpressedTime > 10000) { //if 10 seconds over read current time ONCE
        if (a == 0) {
          a = 1;
          stoppointTime = millis();
        }
      }
      if (stoppointTime > 0) {
        if (millis() - stoppointTime > 5000) { //If 5 seconds over read current time ONCE
          if (b == 0) {
            b = 1;
            stopendTime = millis();
          }
        }
      }
    }
  }


  void obstacledetect() {
      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

    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

    if (distance < 50) { //If obstacle detected
      stopMotors();
      sonarpauseState = 1;
    } else { //No obstacle
      sonarpauseState = 0;
    }
  }
  
  void checkButton() {
    if (buttonState == 0 && sonarpauseState == 0) { //only read the button if obstacle not present and bot movement over
      startmotorButton = digitalRead(buttonPin); // need to read the button pin
    }

    if (startmotorButton == 0 && sonarpauseState == 0) { //button was pressed if 0 (button pressed), 1 (not pressed)
      buttonpressedTime = millis(); //NOTES DOWN THE TIMEEEEEEEEEEE WHEN THE BUTTON WAS PRESSED
      buttonState = 1;
    } else {
      stopMotors(); //if button not pressed turn motots off
    }
  }

  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 stopMotors() {
    digitalWrite(rightmotorPin, LOW);
    digitalWrite(leftmotorPin, LOW);
  }

    void runlinefollower() {
        lineSense();
        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
          stopMotors();
        }
      }

void debug(int buttonState, int distance, int leftVal, int centerVal, int rightVal, unsigned long buttonpressedTime) {
  if (millis() - debugpreviousMillis > 1000 ) { // one second
    debugpreviousMillis = millis(); // set timer to current millis for next event
    Serial.print("(distance:");
    Serial.print(distance);
    Serial.print(") (IR:L");
    Serial.print(leftVal);
    Serial.print(":C");
    Serial.print(centerVal);
    Serial.print(":R");
    Serial.print(rightVal);
    Serial.print(") (motor:L");
    Serial.print(digitalRead(leftmotorPin));
    Serial.print(":R");
    Serial.print(digitalRead(rightmotorPin));
    Serial.print(") (");
    Serial.print("Time:");
    Serial.print((millis() - buttonpressedTime) / 1000);
    Serial.print(") (PTime:");
    Serial.print(millis() / 1000);
    Serial.print(") (stoppoint:");
    Serial.print(stoppointTime / 1000);
    Serial.print(") (stopend: ");
    Serial.println(stopendTime / 1000);
  }
}