#include "twi.h"  //Two Wire Interface for ISR
#include "mpu6050.h"  //for reading data from the gyroscope + accelerometer

float accelX; //calulated horizontal data
float accelY; //dalculated vertical data
int x = 0;  //variable for horizontal directional translation
int y = 0;  //variable for vertical directional translation

volatile int dataFound = 0; //volatile check for ISR receiving data from IMU

void setup(void) {
  Serial.begin(115200); //open Serial monitor connection
  while (!initMPU()) {  //ensure IMU is connected
    Serial.println("MPU6050 not connected!");
    delay(2000);
  }
  Serial.println("MPU6050 ready!");
 noInterrupts();    // disable all interrupts
 TCNT1 = 3036;    // preload timer 65536-16MHz/256/1Hz
 TCCR1B |= (1 << CS12);    // 256 prescaler 
 interrupts();    // enable all interrupts
}


ISR(TWI_vect){
  dataFound = 1;  //data received from IMU
  UpdateStateMachine();  // Update the twi.h library based on lecture notes
}

AccelTempGyroData data; //setup event for collecting IMU data

void imuRead() { //read from IMU
  accelX = data.AccelX() / 16384. * 9.8;  //convert data to m/s^2
  accelY = data.AccelY() / 16384. * 9.8;  
}

void calcMove() { //calculate movement based on read data
  if (accelX > 5) { //determine horizontal movement translation based on directional readings while accounting for micromovements                                   
    x = 1;
  } else if (accelX < -5) { 
    x = 2;
  } else {
    x = 0;
  } 

  if (accelY > 5) { //determine vertical movement translation based on directional readings while accounting for micromovements                                
    y = 1;
  } else if (accelY < -5) {
    y = 2;
  } else {
    y = 0;
  } 

  Serial.print("[");   //print data and time in milliseconds
  Serial.print(millis());
  Serial.print("] X: ");
  Serial.print(accelX);
  Serial.print(", Y: ");
  Serial.print(accelY);

  if (x == 0) {   //head not turning
    if (y == 0) { //head remains stable 
      Serial.println(", Steady ");
    } else if (y == 1) {  //head tilts upwards
      Serial.println(", Moving Forwards ");
    } else {  //head tilts downwards
      Serial.println(", Moving Backwards ");
    }
  } else if (x == 1) {  //head turns right
    if (y == 0) { 
      Serial.println(", Turning Right ");
    } else if (y == 1) {  //head tilts upwards
      Serial.println(", Moving Forwards and Right ");
    } else {  //head tilts downwards
      Serial.println(", Moving Bacwards and Right ");
    }
  } else {  //head turns left
    if (y == 0) { 
      Serial.println(", Turning Left ");
    } else if (y == 1) {  //head tilts upwards
      Serial.println(", Moving Forwards and Left ");
    } else {  //head tilts downwards
      Serial.println(", Moving Backwards and Left ");
    }
  }
}

void loop() {
  bool dataRead = Read(MPU_ADDRESS, data.reg, data.buffer, sizeof(data.buffer));  //confirms collection of data from IMU
  while (GetState() != States::Ready);  //ensure program is ready to continue
  if (dataFound && dataRead) {  //ensure data IMU is found and data is collected
    imuRead();
    calcMove();
  }
  delay(100); //small delay
}