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


MPU6050 mpu6050(Wire,IMU_AC,IMU_GC);

float maxAccX;
float maxAccY;
float maxAccZ;


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());*/

    
    maxAccX-=UPDATE_IMU_INTERVAL/1000.0*1.0; //slowly reduce values
    maxAccY-=UPDATE_IMU_INTERVAL/1000.0*1.0;
    maxAccZ-=UPDATE_IMU_INTERVAL/1000.0*1.0;
    static float last_accX;
    static float last_accY;
    static float last_accZ;
    float _accX=getAccX();
    float _accY=getAccY();
    float _accZ=getAccZ();
    maxAccX=max(abs(last_accX-_accX), maxAccX);
    maxAccY=max(abs(last_accY-_accY), maxAccY);
    maxAccZ=max(abs(last_accZ-_accZ), maxAccZ);
    last_accX=_accX;
    last_accY=_accY;
    last_accZ=_accZ;

    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();
}


float getMaxAccX() {
  return maxAccX;
}
float getMaxAccY() {
  return maxAccY;
}
float getMaxAccZ() {
  return maxAccZ;  
}
void resetMaxAcc(){
  maxAccX=0;
  maxAccY=0;
  maxAccZ=0;
}

float getGyroAngleX() {
  return mpu6050.getGyroAngleX();
}
float getGyroAngleY() {
  return mpu6050.getGyroAngleY();
}
float getGyroAngleZ() {
  return mpu6050.getGyroAngleZ();
}