#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);

//Motor pins
int motorLeftBackward = 2  ;
int motorLeftForward = 3;
int motorRightBackward = 4;
int motorRightForward = 9;

//Echo pins
int ultraSoundTrigger = 13; // Ultrasound signal pin
int ultraSoundEcho = 12; // Ultrasound signal pin
long ultraSoundValue, cm;

//Infrared pins
int infraLeft = 10;
int infraRight = 11;
int ledLight = 13;

int i = 0;
char input;
const int MPU_addr = 0x68; // I2C address of the MPU-6050
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
int roll = 2300;
int pitch = 2300;

void forward(int time, int speed)
{
  analogWrite(motorRightForward, speed + 30);
  analogWrite(motorLeftForward, speed - 10 );
  delay(time);
  if (time > 0) {
    stop();
  }
  delay(50);
}

void backward(int time)
{
  digitalWrite(motorRightBackward, HIGH);
  digitalWrite(motorLeftBackward, HIGH);
  delay(time);
  if (time > 90) {
    stop();
  }
  delay(50);
}

void turn(String direction, int amount)
{
  if (direction == "right") {
    digitalWrite(motorRightBackward, HIGH);
    digitalWrite(motorLeftForward, HIGH);
    delay(amount);
    stop();
  }
  else if (direction == "left") {
    digitalWrite(motorRightForward, HIGH);
    digitalWrite(motorLeftBackward, HIGH);
    delay(amount);
    stop();
  } else {
    Serial.println("Direction needs to be 'left' or 'right'");
  }
  delay(50);
}

void stop()
{
  digitalWrite(motorRightForward, LOW);
  digitalWrite(motorLeftForward, LOW);
  digitalWrite(motorRightBackward, LOW);
  digitalWrite(motorLeftBackward, LOW);
}

int speed()
{
  ping();
  int speed = 205 - cm;
  if (speed < 80) {
    speed = 90;
  }

  return speed;
}

unsigned long ping() //read ultrasound
{
  digitalWrite(ultraSoundTrigger, LOW); // Send low pulse
  delayMicroseconds(5);
  digitalWrite(ultraSoundTrigger, HIGH); // Send high pulse
  delayMicroseconds(10);
  digitalWrite(ultraSoundTrigger, LOW); // Holdoff
  pinMode(ultraSoundEcho, INPUT);

  ultraSoundValue = pulseIn(ultraSoundEcho, HIGH);
  cm = (ultraSoundValue / 2) / 29.1;
  if (cm < 2500 && !cm == 0 ) {
    Serial.println (cm);
    delay (100);
    return cm;
  }
}

int irRead(int readPin)
{
  int halfPeriod = 13; //one period at 38.5khZ is aproximately 26 microseconds
  int cycles = 38; //26 microseconds * 38 is more or less 1 millisecond
  int i;
  for (i = 0; i <= cycles; i++)
  {
    pinMode(readPin, OUTPUT);
    digitalWrite(readPin, HIGH);
    delayMicroseconds(halfPeriod);

    pinMode(readPin, INPUT);

    digitalWrite(readPin, LOW);
    delayMicroseconds(halfPeriod - 1);     // - 1 to make up for digitaWrite overhead
  }
  return digitalRead(readPin);
}

void irCheck()
{
  Serial.print(irRead(infraLeft));
  Serial.print("\t");
  Serial.print(irRead(infraRight));
  Serial.print("\n");
  delay(10);
}

void gyrMeter() {
  ping();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  if (cm >= 6 && cm < 200) {
    lcd.clear();
    if (AcX <= -roll) {
      lcd.setCursor(0, 0);
      lcd.print("Roll-r");
    } else if (AcX >= roll) {
      lcd.setCursor(0, 0);
      lcd.print("Roll-l");
    } else {
      lcd.setCursor(0, 0);
      lcd.print("No-roll");
    }
    if (AcY <= -pitch) {
      lcd.setCursor(8, 0);
      lcd.print("Tilt-fw");
    } else if (AcY >= pitch) {
      lcd.setCursor(8, 0);
      lcd.print("Tilt-bw");
    } else {
      lcd.setCursor(8, 0);
      lcd.print("No-tilt");
    }
    lcd.setCursor(0, 1);
    lcd.print(round(cm));
    lcd.print ("cm.");
  }

  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.print(AcZ);
  Serial.print(" | Tmp = "); Serial.print(Tmp / 340.00 + 36.53); //equation for temperature in degrees C from datasheet
  Serial.print(" | GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
}

void linestay()
{

  if (irRead(infraLeft) == 0 && irRead(infraRight) == 0) {
    forward(0, speed());
  }
  else if (irRead(infraLeft) == 1 && irRead (infraRight)  == 1) {
    stop();
    backward(300);
    turn("right", 800);
  }
  else if (irRead(infraLeft) == 1) {
    stop();
    backward(300);
    turn("left", 500);
  }
  else if (irRead(infraRight) == 1) {
    stop();
    backward(300);
    turn("right", 500);
  }
}

void linefollow()
{
  if (irRead(infraLeft) == 0 && irRead(infraRight) == 0) {
    forward(0, 70);
  }
  else if (irRead(infraLeft) == 1 && irRead (infraRight)  == 1) {
    forward(0, 70);
  }
  else if (irRead(infraLeft) == 1) {
    stop();
    turn("left", 20);
  }
  else if (irRead(infraRight) == 1) {
    stop();
    turn("right", 20);
  }
}

void setup() {
  pinMode(motorLeftForward, OUTPUT);
  pinMode(motorLeftBackward, OUTPUT);
  pinMode(motorRightForward, OUTPUT);
  pinMode(motorRightBackward, OUTPUT);
  pinMode(ultraSoundTrigger, OUTPUT);
  pinMode(ultraSoundEcho, INPUT);
  pinMode(ledLight, OUTPUT);

  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  lcd.begin();
}

void loop() {
//gyrMeter();
forward(0, 70);
}