// 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 motor stop afer 10 seconds DONE
// 7. MAKE TIMER STOP WHEN DISTANCE SENSOR DETECTS OBSTACLE

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

// track time
unsigned long trackTime = 10 * 1000; //10 seconds

//sonar pause detection
byte sonarpauseState = 0;
unsigned long sonarTime;
unsigned long sonarelapsedTime;
unsigned long sonarreturnTime;

//debug timer
unsigned long debugpreviousMillis = 0; // compare 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);
  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();
  if (buttonState == 0) {
    startmotorButton = digitalRead(buttonPin); // need to read the button pin
  }

  checkButton(); //check button for press

  if (buttonState == 1) {
    runlinefollower();
    if (distance < 50) { //OBSTACLE DETECTED AND TIMER PAUSED
      stopMotors();
      sonarTime = millis();

      sonarelapsedTime = buttonPressedTime - sonarTime;
      sonarreturnTime = trackTime - sonarelapsedTime;

      if (millis() < sonarreturnTime) {
        runlinefollower();
      }
    }
    startmotorButton = 7; //some number that is not 0 or 1
  }

  timeup();

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

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

  
  void timeup() {
    if(millis() - buttonPressedTime > trackTime) {
      digitalWrite(rightmotorPin, 0);
      digitalWrite(leftmotorPin, 0);
      buttonState = 0;
    }
  }
  
  void checkButton() {
    if (startmotorButton == 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();
    }
  }

  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() {
      if (millis() - buttonPressedTime < trackTime) {
        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("(buttonState:");
    Serial.print(buttonState);
    Serial.print(") (");
    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);
    if (millis() - buttonPressedTime > 10000 && buttonState)
      Serial.print(" STOP");
    Serial.print(")");
    Serial.print("(startmotorButton:");
    Serial.print(startmotorButton);
    Serial.print(")");
    Serial.print("(returnTime:");
    Serial.print(sonarreturnTime);
    Serial.println(")");
  }
}