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

// pins
const byte rightmotorPin = 10;
const byte leftmotorPin = 11;

const byte leftmotorPin1 = 8;
const byte rightmotorPin1 = 9;

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

//timers
int travelTime = 0;
int stopTime = 0;
int returnTime = 0;

//one time pass
bool y = 0;

//sonar state
bool sonarpauseState = 0;

//define robot state
enum RobotState {
  IDLE,         // Initial state
  TRAVELLING,   // Going to table
  STOPPING,     // Stopping at table
  RETURNING    // Returning back to base
};

RobotState currentState = IDLE; //initialize as idle in beginning

//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(rightmotorPin1, OUTPUT);
  pinMode(leftmotorPin1, 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() {
  //Find Distance of obstacle (if present) Value//
  readSonar();

  //Read IR Values//
  lineSense();

  //Check for presence obstacle//
  obstacledetect();

  //Run bot for prescribed timings//
  botmovement();

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

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

  void botmovement() {
    switch (currentState) {
      case IDLE:
        while (buttonState == 0) {  //Keep in this while loop if button NOT detected
          checkButton();            // Check if the button is pressed to start
        }
        currentState = TRAVELLING;  //If loop breaks (button detected) run follower
        break;

      case TRAVELLING:
        if (buttonState == 1 && sonarpauseState == 0) {
          runlinefollower(); //Run bot
          travelTime++; // Increment travel time
          if (travelTime >= 750) {
            currentState = STOPPING; // Transition to stopping state
            stopTime = 0; // Reset stop time
          }
        }
        break;

      case STOPPING:
        if (buttonState == 1) {
          stopMotors(); //Stop motors for 5 seconds
          stopTime++; // Increment stop time
          if (stopTime >= 375) {
            currentState = RETURNING; // Transition to returning state
            returnTime = 0; // Reset return time
          }
        }
        break;

      case RETURNING:
        if (buttonState == 1 && sonarpauseState == 0) {
          runlinefollower(); //Run bot
          returnTime++; // Increment return time
          if (returnTime >= 750) {
            travelTime = 0;   // Reset travel time
            stopTime = 0;     // Reset stop time
            returnTime = 0;   // Reset return time
            buttonState = 0;  // Deactivate buttonState
            currentState = IDLE;
          }
        }
        break;
    }
  }

  void obstacledetect() {
    if (distance < 35) { //if obstacle detected
      stopMotors();
      sonarpauseState = 1;
    } else {
      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)
      if (y == 0) {
        y = 1;
        buttonpressedTime = millis(); //NOTES DOWN THE TIMEEEEEEEEEEE WHEN THE BUTTON WAS PRESSED
      }
      buttonState = 1;
    } else {
      stopMotors(); //if button not pressed turn motots off
      y = 0;
    }
  }

  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);
          digitalWrite(rightmotorPin1, LOW);
          digitalWrite(leftmotorPin1, LOW);
        }
        else if ((leftVal == 0) && (centerVal == 1) && (rightVal == 1)) { //RIGHT SENSOR ON LINE
          digitalWrite(rightmotorPin, 0);
          digitalWrite(leftmotorPin, PWMspeed);
          digitalWrite(rightmotorPin1, LOW);
          digitalWrite(leftmotorPin1, LOW);
        }
        else if ((leftVal == 1) && (centerVal == 1) && (rightVal == 0)) { //LEFT SENSOR ON LINE
          digitalWrite(leftmotorPin, 0);
          digitalWrite(rightmotorPin, PWMspeed);
          digitalWrite(rightmotorPin1, LOW);
          digitalWrite(leftmotorPin1, LOW);
        }
        else if ((leftVal == 1) && (centerVal == 0) && (rightVal == 0)) { //LEFT AND CENTER SENSOR SENSOR ON LINE
          digitalWrite(rightmotorPin, PWMspeed);
          digitalWrite(leftmotorPin, 0);
          digitalWrite(rightmotorPin1, LOW);
          digitalWrite(leftmotorPin1, 0);
        }
        else if ((leftVal == 0) && (centerVal == 0) && (rightVal == 1)) { //RIGHT AND CENTER SENSOR ON LINE
          digitalWrite(rightmotorPin, 0);
          digitalWrite(leftmotorPin, PWMspeed);
          digitalWrite(rightmotorPin1, 0);
          digitalWrite(leftmotorPin1, LOW);
        } else { //IF NONE OF OTHER CONDITIONS
          stopMotors();
        }
    }

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

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(") (butState:");
    Serial.print(buttonState);
    Serial.print(") (time1:");
    Serial.print(travelTime);
    Serial.print(") (time2:");
    Serial.print(stopTime);
    Serial.print(") (time3:");
    Serial.print(returnTime);
    Serial.println(")");
  }
}