//Arduino Car project 
//Team -1 
// Brayden Hull, Farit Zinatulin, Grayson Fitzgerald, Santiago Trejo Lopez, Ethan Hacker

#define IR_SMALLD_NEC
#include <IRsmallDecoder.h> // include library for IR remote
#include <RedBot.h> //include library for IR line follow
IRsmallDecoder irsmallDecoder(2);
irSmallD_t irData;

unsigned long startMillis;
unsigned long currentMillis;
const unsigned long eventTime = 5000;

// Motor driver pins
int motor1Pin1 = 9;  // Input1 Pin for motor 1 (Right Motor)
int motor1Pin2 = 8;  // Input2 Pin for motor 1 (Right Motor)
int PWMA = 10;
int motor2Pin1 = 13; // Input1 Pin for motor 2 (Left Motor)
int motor2Pin2 = 12; // Input2 Pin for motor 2 (Left Motor)
int PWMB = 3;

// Ultrasonic sensor pins
int trigPin = 6; // Trig pin for ultrasonic sensor
int echoPin = 7; // Echo pin for ultrasonic sensor
float distance; 

int keyPress; // variable for input from  IR remote

//Pins for IR Linefollowing sensors
RedBotSensor Left = RedBotSensor(A2); // initialize a sensor object on A2
RedBotSensor Center = RedBotSensor(A3); // initialize a sensor object on A3
RedBotSensor Right = RedBotSensor(A4); // initialize a sensor object on A4

//Assigning a Threshold for the IR sensors to hold
#define LINETHRESHOLD 950 //can vary to fine tune following

//Assigned speed to car
int speed = 255; //can vary speed from 0-255
//Line following speeds
int linespeed = 85;  // speed going forward in Line-Following mode
int turnspeed = 125; //variable used to adjust relative speeds for turning
int leftSpeed;   // variable used to store the leftMotor speed
int rightSpeed;  // variable used to store the rightMotor speed


//Roomba and Line-following state
bool running;

void setup() {
  Serial.begin(9600);
  // Set motor control pins as outputs
  pinMode(motor1Pin1, OUTPUT);
  pinMode(motor1Pin2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(motor2Pin1, OUTPUT);
  pinMode(motor2Pin2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  // Set up ultrasonic sensor pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop() {
  //waiting for a key press on the remote
  if(irsmallDecoder.dataAvailable(irData)){
    currentMillis = millis();
    int keyPress = irData.cmd;
    Serial.println(keyPress); //debugging test to see if reading IR remote
    print_mode();
    handleKeyPress(keyPress);
    }
}


void handleKeyPress(int keyPress)
{
  switch (keyPress)
  {
    case 9:  
      //car in idle
      Serial.println("idle"); //debugging test to see if enters case
       loop();
      {
      analogWrite(PWMA, 0); //Stoping motor previous movement
      analogWrite(PWMB, 0); //Stoping motor previous movement
      currentMillis = millis();
  //check for a key press on the remote
      if(irsmallDecoder.dataAvailable(irData)){
      keyPress = irData.cmd;
      Serial.println(keyPress); //debugging test to see if reading IR remote
        }
      } 
    break;  //case 9 break
  
    case 5: 
      //car will drive forward 
      Serial.println("FORWARD"); //debugging test to see if enters case
       startMillis = millis(); 
        while (millis() - startMillis <= eventTime) {
          digitalWrite(motor1Pin1, HIGH); //right motor spins forward
          digitalWrite(motor1Pin2, LOW);  
          analogWrite(PWMA, speed);
          digitalWrite(motor2Pin1, HIGH); //left motor spins forward
          digitalWrite(motor2Pin2, LOW);  
          analogWrite(PWMB, speed);
      //check for a key press on the remote
          if(irsmallDecoder.dataAvailable(irData)){
            keyPress = irData.cmd;
            Serial.println(keyPress); //debugging test to see if reading IR remote
            break;
             }
        } 

        //Switching into idle mode after driving forward
          keyPress = 9;
          Serial.println("Switching Back to Idle");  //debugging test to see if time feature works on while loop
          break;
    break;  //case 5 break

    case 13:
     //car drives backward
     Serial.println("Retreat"); //debugging test to see if enters case
       startMillis = millis(); 
        while (millis() - startMillis <= eventTime){
          digitalWrite(motor1Pin1, LOW);
          digitalWrite(motor1Pin2, HIGH); //right motor spins backwards
          analogWrite(PWMA, speed);
          digitalWrite(motor2Pin1, LOW);
          digitalWrite(motor2Pin2, HIGH);  //left motor spins backwards
          analogWrite(PWMB, speed);
      //check for a key press on the remote
          if(irsmallDecoder.dataAvailable(irData)){  
            keyPress = irData.cmd;
            Serial.println(keyPress); //debugging test to see if reading IR remote
            break;
             }
        } 

       //Switching into idle mode after driving forward
          keyPress = 9;
          Serial.println("Switching Back to Idle"); //debugging test to see if time feature works on while loop
          break;
    break;  //case 13 break

    case 10:
     //car turns right
     Serial.println("to the right");  //debugging test to see if enters case
        startMillis = millis(); 
        while (millis() - startMillis <= eventTime){
          digitalWrite(motor1Pin1, HIGH); // right motor spins forward
          digitalWrite(motor1Pin2, LOW);
          analogWrite(PWMA, speed);
          digitalWrite(motor2Pin1, LOW);
          digitalWrite(motor2Pin2, HIGH); //left motor spins backwards
          analogWrite(PWMB, speed);
       //check for a key press on the remote
          if(irsmallDecoder.dataAvailable(irData)){
            keyPress = irData.cmd;
            Serial.println(keyPress); //debugging test to see if reading IR remote
            break;
             }
        } 

        //Switching into idle mode after driving forward
          keyPress = 9;
          Serial.println("Switching Back to Idle");  //debugging test to see if time feature works on while loop
          break;
    break;  //case 10 break

    case 8:
     //car turns left
     Serial.println("to da left"); //debugging test to see if enters case
        startMillis = millis(); 
        while (millis() - startMillis <= eventTime){
          digitalWrite(motor1Pin1, LOW);
          digitalWrite(motor1Pin2, HIGH); //right motor spins backwards 
          analogWrite(PWMA, speed);
          digitalWrite(motor2Pin1, HIGH); // left motor spins forward
          digitalWrite(motor2Pin2, LOW);
          analogWrite(PWMB, speed);
      //check for a key press on the remote
          if(irsmallDecoder.dataAvailable(irData)){
            keyPress = irData.cmd;
            Serial.println(keyPress); // debugging test to see if reading IR remote
            break;
             }
        } 

        //Switching into idle mode after driving forward
          keyPress = 9;
          Serial.println("Switching Back to Idle"); //debugging test to see if time feature works on while loop
          break;
    break;  //case 8 break

    case 21: //DO NOT HOLD DOWN BUTTON 21 ONLY PRESS ONCE
    
      //Linefollow
      Serial.println("Line follow"); // debuggin test to see if enters case

      running = true; //will run until any button is pressed ending line follow mode
      while(running == true) {
        //Exit Condition
        if(irsmallDecoder.dataAvailable(irData)){
           keyPress = irData.cmd; //if button pressed on remote, change to that mode
           Serial.println("Button Pressed:"); // debuggin test to see if exiting while loop and reading IR remote
           Serial.println(keyPress);
           running = !running;
          }
        //printing Data recieved from line following sensors
          Serial.println("IR Sensor Readings: ");
          Serial.print("Left Sensor: ");
          Serial.println(Left.read());
          Serial.print("Center Sensor: ");
          Serial.println(Center.read());
          Serial.print("Right Sensor: ");
          Serial.println(Right.read());
          Serial.println(running);

	  // if on the line drive left and right at the same speed 
	        if(Center.read() > Right.read() && Center.read() > Left.read()){
		        leftSpeed = linespeed; 
		        rightSpeed = linespeed;

            digitalWrite(motor1Pin1, HIGH); //right motor spins forward
            digitalWrite(motor1Pin2, LOW);
            digitalWrite(motor2Pin1, HIGH); // left motor spins forward
            digitalWrite(motor2Pin2, LOW);
	        }
	
	 // if the line is under the right sensor, adjust relative speeds to turn to the right
	        else if(Right.read() > Center.read() && Right.read() > Left.read()){
		        leftSpeed = (turnspeed); //speed of turn is adjusted so car does not overshoot line
            rightSpeed = (turnspeed);
		        digitalWrite(motor1Pin1, LOW);
            digitalWrite(motor1Pin2, HIGH); //right motor spins backward
            digitalWrite(motor2Pin1, HIGH); // left motor spins forward
            digitalWrite(motor2Pin2, LOW);
	        }

	  // if the line is under the left sensor, adjust relative speeds to turn to the left
	        else if(Left.read() > Right.read() && Left.read() > Center.read()){
		        digitalWrite(motor1Pin1, HIGH); //right motor spins forward
            digitalWrite(motor1Pin2, LOW);
            digitalWrite(motor2Pin1, LOW);
            digitalWrite(motor2Pin2, HIGH); // left motor spins backward
            leftSpeed = (turnspeed);  //speed of turn is adjusted so car does not overshoot line
		        rightSpeed = (turnspeed);
	        }

          analogWrite(PWMA, leftSpeed);
          
          analogWrite(PWMB, rightSpeed);

	      delay(0);  // add a delay to decrease sensitivity.
      }
      
    Serial.print("breaking"); //debugging test to see if broke out of while loop 
    break;  //case 21 break

    case 2:
      //RoombaMode
      Serial.println("ROOMBA"); // debugging test to see if enters case
      
      running = true;  //will run until any button is pressed ending line follow mode
      while(running == true) {
        
        distance = getDistance(); //using get distance function at bottom
        Serial.println("Current Distance: ");
        Serial.println(distance); // debuggin test to ensure ultra sonic distance sensor is wired correctly and on appropiate pins

        //Break feature
            if(irsmallDecoder.dataAvailable(irData)){
              keyPress = irData.cmd; //if button pressed on remote, change to that mode
              Serial.println(keyPress); // debugging test to see if reading IR remote
              running = !running;
              break; //breaks out of roomba mode
                }

        // if less read on sensor is less than 15, car turns left
          while (getDistance() <= 15){
            //Message to test if entering while loop 
              Serial.println("too close");
            
            //Distance Check
              distance = getDistance();
              Serial.println("Current Distance: ");
              Serial.println(distance);

            //Motor Writing
              digitalWrite(motor1Pin1, HIGH); // right motor spins forward
              digitalWrite(motor1Pin2, LOW);
              analogWrite(PWMA, speed);
              digitalWrite(motor2Pin1, LOW);
              digitalWrite(motor2Pin2, HIGH); // left motor spins backward
              analogWrite(PWMB, speed);

          //Break feature
            if(irsmallDecoder.dataAvailable(irData)){
              keyPress = irData.cmd; //if button pressed on remote, change to that mode
              Serial.println(keyPress); // test to see if reading remote
              running = !running;
              break;
                }
          }

      while(getDistance() > 15){
        // test message to see if in while loop
          Serial.print("moving...");

        //Distance Check
          distance = getDistance();
          Serial.println("Current Distance: ");
          Serial.println(distance);
        
        //Motor Writing
          digitalWrite(motor1Pin1, HIGH); // right motor spins forward
          digitalWrite(motor1Pin2, LOW);
          analogWrite(PWMA, speed);
          digitalWrite(motor2Pin1, HIGH); // left motor spins forward
          digitalWrite(motor2Pin2, LOW);
          analogWrite(PWMB, speed);

        //Break feature
          if(irsmallDecoder.dataAvailable(irData)){
            keyPress = irData.cmd; //if button pressed on remote, change to that mode
            Serial.println(keyPress); // debugging test to see if reading IR remote
            running = !running;
            break;
            }
        }
       }
    break;  //case 2 break
  }
}

float getDistance(){   // function used to get distance for the Ultra Sonic Distance Sensor
  float echoTime; //variable to store the time it takes for a ping to bounce off an object
  float calculatedDistance; //variable to store the distance calculated from the echo time
 
  //send out an ultrasonic pulse that's 10ms long
 digitalWrite(trigPin, HIGH);
 delayMicroseconds(10);
 digitalWrite(trigPin, LOW);
 
  echoTime = pulseIn(echoPin, HIGH); //use the pulsein command to see how long it takes for the
									 //pulse to bounce back to the sensor
 calculatedDistance = echoTime / 148.0; //calculate the distance of the object that reflected the pulse
 										// (half the bounce time multiplied by the speed of sound)
 return calculatedDistance; //send back the distance that was calculated
}

void print_mode() { // only for debugging, prints after each button press
  Serial.print("Mode switched to: ");
}