#include "Arduino.h"
#include "imu.h"


MPU6050 mpu6050(Wire,IMU_AC,IMU_GC);


bool imu_bellyup=false;

void initIMU() {
  initIMU(0,0,0);
}

void initIMU(float x,float y,float z) {
  Wire.begin(25,24);
  mpu6050.begin();
  
  if (x==0 && y==0 && z==0) {
    mpu6050.calcGyroOffsets(true);
  }else{
    mpu6050.setGyroOffsets(x, y, z);
  }
}

void loopIMU(unsigned long millis) {
  static unsigned long last_update_imu=0;
  if (millis-last_update_imu>UPDATE_IMU_INTERVAL) {
    last_update_imu=millis;
    mpu6050.update();
    /*Serial.print("angleX : ");
    Serial.print(mpu6050.getAngleX());
    Serial.print("\tangleY : ");
    Serial.print(mpu6050.getAngleY());
    Serial.print("\tangleZ : ");
    Serial.println(mpu6050.getAngleZ());*/

    if(mpu6050.getAccZ()<-0.5) { //is upside down
      imu_bellyup=true;
    }else{
      if (mpu6050.getAccZ()>0.7) { //is on flat surface
        imu_bellyup=false;
      }
    }

  }
}

bool isBellyUp() {
  return imu_bellyup;
}

float getAngleX() {
  return mpu6050.getAngleX();
}

float getAngleY() {
  return mpu6050.getAngleY();
}

float getAngleZ() {
  return mpu6050.getAngleZ();
}

float getAccX() {
  return mpu6050.getAccX();
}
float getAccY() {
  return mpu6050.getAccY();
}
float getAccZ() {
  return mpu6050.getAccZ();
}