#include <Adafruit_MPU6050.h>
#include <SPI.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;

int trigPin1 = 3;
int echoPin1 = 2;
int trigPin2 = 5;
int echoPin2 = 4;

float v = 331.5 + 0.6 * 20;
int left_intr = 0;
float diameter_of_wheel = 0.066;
int distance;

void setup() {
  Serial.begin(9600);

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }

  Serial.println("MPU6050 ready!");

  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  attachInterrupt(digitalPinToInterrupt(2), Left_ISR, CHANGE);
}

sensors_event_t event;

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  float dist1 = getDistance(trigPin1, echoPin1);
  float dist2 = getDistance(trigPin2, echoPin2);
  distance = (3.141 * diameter_of_wheel) * (left_intr / 20);

  Serial.println(dist1);
  Serial.println(dist2);

  Serial.print("X: ");
  Serial.print(a.acceleration.x);
  Serial.print(",");
  Serial.print(" Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(",");
  Serial.print(" Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(";");
  Serial.print("gX: ");
  Serial.print(g.gyro.x);
  Serial.print(",");
  Serial.print(" gY: ");
  Serial.print(g.gyro.y);
  Serial.print(",");
  Serial.print(" gZ: ");
  Serial.print(g.gyro.z);
  Serial.println("");
  Serial.println(distance);

  delay(100);
}

float getDistance(int tpin, int epin) {
  digitalWrite(tpin, LOW);
  delayMicroseconds(3);
  digitalWrite(tpin, HIGH);
  delayMicroseconds(5);
  digitalWrite(tpin, LOW);

  float tu = pulseIn(epin, HIGH);
  float t = tu / 2000000;
  float dist = t * v * 100;

  return dist;
}

void Left_ISR() {
  left_intr++; delay(10);
}