// 1. move dedicated sections into their own functions DONE
// 2. allow motors to get affected by the IR inputs DONE
// 3. allow motors to get turned of when distance sensor detects obstacle DONE
// 4. make motors not turn on before button press DONE
// 5. make sure button input will not be read for 10 seconds after button pressed once (no spam pressing) DONE
// 6. make motors run for 10 seconds, STOP for 5 seconds, run for 10 seconds again DONE
// 7. MAKE TIMER STOP WHEN DISTANCE SENSOR DETECTS OBSTACLE

//******************************************************************************//
//                                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

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

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

//flags
bool sonarpauseState;
bool travel1Flag;
bool travel2Flag;
bool stopFlag;

//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() {

  readSonar();
  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();
  
  obstacledetect();

  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
  }

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

  botmovement();

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

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

  void botmovement() {
    if (buttonState == 1) {
      startmotorButton = 7;

      while (travelTime < 750 && sonarpauseState == 0) { //if sonar detected dont run deduct value
        if (travelTime >= 750) {
          break;
        }
        obstacledetect();
        runlinefollower();
        debug (buttonState, distance, leftVal, centerVal, rightVal, buttonpressedTime);
        travelTime++; //THE VALUE DECREASES BY 75 EACH SECOND
      }
    
      while (stopTime < 375 && sonarpauseState == 0 && travelTime > 750) {
        if (stopTime >= 375) {
          break;
        }
        stopMotors();
        debug (buttonState, distance, leftVal, centerVal, rightVal, buttonpressedTime);
        stopTime++;
      }
      
      while (returnTime < 750 && sonarpauseState == 0 && stopTime > 375) {
        if (returnTime >= 750) {
          break;
        }
        obstacledetect();
        runlinefollower();
        debug (buttonState, distance, leftVal, centerVal, rightVal, buttonpressedTime);
        returnTime++;
      }

      if (returnTime > 750) {
        travelTime = 750;
        stopTime = 375;
        returnTime = 750;
        buttonState = 0;
      }
    }
  }
  

  void obstacledetect() {
    if (distance < 50) { //if obstacle detected
      stopMotors();
      sonarpauseState = 1;
    } else {
      x = 0;   
      sonarpauseState = 0;
    }
  }


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

        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();

        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 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
}

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(") (");
    Serial.print("Time1:");
    Serial.print(travelTime);
    Serial.print(") (");
    Serial.print("Time2:");
    Serial.print(stopTime);
    Serial.println(")");
  }
}