// Setup libraries
#include <Wire.h>
#include <VL53L1X.h>

VL53L1X sensor;

// Name pins
const int PWMA = 0;
const int PWMB = 1;
const int INA1 = 2;
const int INA2 = 3;
const int INB1 = 4;
const int INB2 = 5;
const int ledPin = 16;
const int speedMtr = 225;

void forwards(){
  Serial.println("Going forwards");
  
  // Set speeds
  analogWrite(PWMA, speedMtr);
  analogWrite(PWMB, speedMtr);

  // Turn both motors forwards
  digitalWrite(INA1, HIGH);
  digitalWrite(INA2, LOW);
  digitalWrite(INB1, HIGH);
  digitalWrite(INB2, LOW);

  // Turn LED on showing all is well
  digitalWrite(ledPin, HIGH);
}

void backwards(){
  Serial.println("Going backwards");
  
  // Set speeds
  analogWrite(PWMA, speedMtr);
  analogWrite(PWMB, speedMtr);

  // Turn both motors backwards
  digitalWrite(INA1, LOW);
  digitalWrite(INA2, HIGH);
  digitalWrite(INB1, LOW);
  digitalWrite(INB2, HIGH);

  // Turn LED on showing all is well
  digitalWrite(ledPin, HIGH);
}

void right(){
  Serial.println("Turning right");
  
  // Set speeds
  analogWrite(PWMA, speedMtr);
  analogWrite(PWMB, speedMtr);

  // Turn motor A forwards
  digitalWrite(INA1, HIGH);
  digitalWrite(INA2, LOW);
  // Turn motor B backwards
  digitalWrite(INB1, LOW);
  digitalWrite(INB2, HIGH);

  // Turn LED on showing all is well
  digitalWrite(ledPin, HIGH);
}

void left(){
  Serial.println("Turning left");
  
  // Set speeds
  analogWrite(PWMA, speedMtr);
  analogWrite(PWMB, speedMtr);

  // Turn motor A backwards
  digitalWrite(INA1, LOW);
  digitalWrite(INA2, HIGH);
  // Turn motor B forwards
  digitalWrite(INB1, HIGH);
  digitalWrite(INB2, LOW);

  // Turn LED on showing all is well
  digitalWrite(ledPin, HIGH);
}

void stopMtrs(){
  Serial.println("Stopping");
  
  // Set speeds
  analogWrite(PWMA, 0);
  analogWrite(PWMB, 0);

  // Turn both motors backwards
  digitalWrite(INA1, LOW);
  digitalWrite(INA2, LOW);
  digitalWrite(INB1, LOW);
  digitalWrite(INB2, HIGH);
}

void setup() {
  // Start serial and wire
  Serial.begin(9600);
  Wire.begin();
  Wire.setClock(400000); // use 400 kHz I2C

  // Set pins to output
  pinMode(PWMA, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(INA1, OUTPUT);
  pinMode(INA2, OUTPUT);
  pinMode(INB1, OUTPUT);
  pinMode(INB2, OUTPUT);
  pinMode(ledPin, OUTPUT);

  // Blink LED to start
  digitalWrite(ledPin, HIGH);
  delay(1000);
  digitalWrite(ledPin, LOW);
  delay(1000);
  digitalWrite(ledPin, HIGH);

  // Print if Timeout
  sensor.setTimeout(500);
  if (!sensor.init())
  {
    Serial.println("Failed to detect and initialize sensor!");
    digitalWrite(ledPin, LOW);
  }

  // Use long distance mode
  sensor.setDistanceMode(VL53L1X::Long);
  sensor.setMeasurementTimingBudget(50000);

  // Start continous readings
  sensor.startContinuous(50);
}

void loop() {
  // Read sensor and print answer
  Serial.println(sensor.read());
  // If timeout, say it happened
  if (sensor.timeoutOccurred()) { 
    Serial.println(" TIMEOUT"); 
    digitalWrite(ledPin, LOW);
    stopMtrs();
  }
  
  // If an object is closer than 400 mm go backwards
  if (sensor.read()<400 && sensor.read()>1){ backwards();}
  // If an object if further than 400 and closer than 1000 stop
  if (400<sensor.read() && sensor.read()<1000){ stopMtrs();}
  // If an object is further than 1 meter go forwards
  if (sensor.read()>1000){ forwards();}
}