//Include Libraries
#include <LiquidCrystal_I2C.h>
#include <Wire.h>
#include <math.h>

//Create LCD Object in the program
//lcd(LCD I2C address, no. of LCD char, no. of segment)
LiquidCrystal_I2C lcd(0x27, 16, 2);

const int MPU_addr = 0x68;        //MPU6050 I2C Address
const int minVal = 265;           //Min. MPU Raw Value
const int maxVal = 402;           //Max. MPU Raw Value
int16_t axis_X, axis_Y, axis_Z;   //X,Y,Z axis variables
int x_prev = 0;                   //store previous x value
int y_prev = 0;                   //store previous y value
int z_prev = 0;                   //store previous z value
bool mpuState = LOW;              //Initialize MPU state to low.

//Variables for non blocking delay
unsigned long previousMillis1 = 0;
const long interval1 = 50;

const int irSens1 = 2;// interrupt vector of the pin is 0
const int irSens2 = 3; // interrupt vector of the pin is 1

//Use volatile variable as the compiler might see it as unused variable
volatile int irSensState1 = 0;
volatile int irSensState2 = 0;
unsigned int timer1;  //current system timer due to irSensor 1
unsigned int timer2;  //current system timer due to irSensor 2

//ir sensor flags
bool timerFlag1 = 0;
bool timerFlag2 = 0;

//Variables for speed calculations
float Time = 0;
float speed = 0;
float distance = 0.050; //this is constant and must be the gap distance between two IR sensors

void setup() {
  Serial.begin(9600);     //Begin serial comms. Uncomment if needed
  lcd.init();
  lcd.backlight();
  lcd.clear();
  pinMode(irSens1, INPUT);  //Set ir sensor 1 as input
  pinMode(irSens2, INPUT);  //Set ir sensor 2 as input

  //attact an interrupt to the ISR Vector
  attachInterrupt(0, irPinInterrupt1, LOW);
  attachInterrupt(1, irPinInterrupt2, LOW);

  //init_LCD();            //Start LCD
  init_Angle_Read();     //Read initial angle
  init_Display_MPU();    //Display MPU first start up
  lcd_Display_Speed();   //Display Speed
  lcd_Display_Angle();   //Display angle
}

void loop() {

  if (timerFlag1 == 1 && timerFlag2 == 1) {
    if (timer1 > timer2) {
      Time = timer1 - timer2;
    }
    else if (timer1 < timer2) {
      Time = timer2 - timer1;
    }
    Time = Time / 1000.000;
    speed = distance / Time;
  }


  if (speed != speed) {
    lcd.setCursor(0, 1);
    lcd.print("Speed:");
    lcd.print(speed, 3);
    lcd.print("m/s");
    speed = speed;
  }
  timerFlag1 = 0;
  timerFlag2 = 0;
  lcd_Display_Angle();
}

void init_Display_MPU() {
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Arduino");
  lcd.setCursor(0, 1);
  lcd.print("MPU6050");
  delay(2000);
  lcd.clear();
}

void init_Angle_Read() {
  Wire.beginTransmission(MPU_addr);     //begin I2C comms to MPU6050
  Wire.write(0x3B);                     //Read X,Y,Z raw command
  Wire.endTransmission(false);          //terminate I2C comms
  Wire.requestFrom(MPU_addr, 14, true); //Request 14bit acceleration values

  //Store X Y Z raw values
  axis_X = Wire.read() << 8 | Wire.read();
  axis_Y = Wire.read() << 8 | Wire.read();
  axis_Z = Wire.read() << 8 | Wire.read();

  //Convert raw values to angle in radians
  int xAng = map(axis_X, minVal, maxVal, -90, 90);
  int yAng = map(axis_Y, minVal, maxVal, -90, 90);
  int zAng = map(axis_Z, minVal, maxVal, -90, 90);

  //Convert radians to degrees
  int x_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);
  int y_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);
  int z_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);

  //Store previous X,Y,Z values
  x_prev = x_curr;
  y_prev = y_curr;
  z_prev = z_curr;
}



void lcd_Display_Angle() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true);

  axis_X = Wire.read() << 8 | Wire.read();
  axis_Y = Wire.read() << 8 | Wire.read();
  axis_Z = Wire.read() << 8 | Wire.read();

  int xAng = map(axis_X, minVal, maxVal, -90, 90);
  int yAng = map(axis_Y, minVal, maxVal, -90, 90);
  int zAng = map(axis_Z, minVal, maxVal, -90, 90);

  //int x_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI); //Comment this block of code if not using X axis
  //int y_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI); //Comment this block of code if not using Y axis
  int z_curr = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI); //Comment this block of code if not using Z axis

  /*
    //For X-axis reference. //Comment this block of code if not using X axis
    if(x_curr >=0 && x_curr <=90){
    if(x_curr != x_prev){
      lcd.setCursor(0, 0);
      lcd.print("Angle:");
      lcd.print(x_curr);
      x_prev = x_curr;
      }
    }
  */
  /*
    //For Y-axis reference. //Comment this block of code if not using Y axis
    if(y_curr >=0 && y_curr <=90){
    if(y_curr != y_prev){
      lcd.setCursor(0, 0);
      lcd.print("Angle:");
      lcd.print(y_curr);
      y_prev = y_curr;
      }
    }
  */


  //For Z-axis reference. //Comment this block of code if not using Z axis
  if (z_curr >= 0 && z_curr <= 90) {
    if (z_curr != z_prev) {
      lcd.setCursor(0, 0);
      lcd.print("Angle:");
      lcd.print(z_curr);
      z_prev = z_curr;
    }
  }
}



void lcd_Display_Speed() {
  if (timerFlag1 == 1 && timerFlag2 == 1) {
    if (timer1 > timer2) {
      Time = timer1 - timer2;
    }
    else if (timer1 < timer2) {
      Time = timer2 - timer1;
    }
    //Calculate speed
    Time = Time / 1000.000;
    speed = distance / Time;
  }
  
  if (speed != speed) {
    lcd.setCursor(0, 1);
    lcd.print("Speed:");
    lcd.print(speed, 3);
    lcd.print("m/s");
    speed = speed;
  }
  //Reset timer flag
  timerFlag1 = 0;
  timerFlag2 = 0;
}

void irPinInterrupt1() {
  timer1 = millis();
  timerFlag1 = 1;
}

void irPinInterrupt2() {
  timer2 = millis();
  timerFlag2 = 1;
}