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

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

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

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

//one time pass
bool x = 0;

//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();
  
  if (buttonState == 0 && sonarpauseState == 0) {
    startmotorButton = digitalRead(buttonPin); // need to read the button pin
  }

  checkButton(); //check button for press

  if (buttonState == 1 && sonarpauseState == 0) {
    startmotorButton = 7; //some number that is not 0 or 1
    runlinefollower();
  }

  obstacle();

  if (sonarpauseState == 1) {
    sonarelapsedTime = sonarTime - buttonpressedTime;
    sonarreturnTime = trackTime - sonarelapsedTime;
    sonarCalc = 1;
  }

  if (sonarCalc == 1) {
    sonarCalc = 0;
    if (millis() - buttonpressedTime < sonarreturnTime && sonarpauseState == 0) {
        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();
        }
    }
  }

  
  timeup();

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

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

  
  void obstacle() {
    if (distance < 50) {
      stopMotors();
      if (x == 0) {
        x = 1;
        sonarTime = millis();
      }
      sonarpauseState = 1;
    } else {
      x = 0;
      sonarpauseState = 0;
    }
  }
  
  void timeup() {
    if(millis() - buttonpressedTime > trackTime) {
      stopMotors();
      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 && sonarpauseState == 0) {
        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);
    if (millis() - buttonpressedTime > 10000 && buttonState)
      Serial.print(" STOP");
    Serial.print(")");
    Serial.print("(sonartime:");
    Serial.print(sonarTime);
    Serial.print(") (presstime:");
    Serial.print(buttonpressedTime);
    Serial.print(")");
    Serial.print("(returnTime:");
    Serial.print(sonarreturnTime / 1000);
    Serial.println(")");
  }
}