#include "MPU6050_6Axis_MotionApps20.h"
#include "board_pins.h"
#include "esp_log.h"

volatile bool _isDMPDataReady = false;
void IRAM_ATTR _dmpInterruption() {_isDMPDataReady = true;}

class MPU {
public:
  MPU(MPU6050_6Axis_MotionApps20 *mpu) {
    _mpu = mpu;
  }

  ~MPU() {
    delete _mpu;
  }

  bool initialize() {
    _mpu->initialize();
    delay(250);
    if(!_mpu->testConnection()) {
      ESP_LOGE(_TAG, "MPU6050 test connection failed!");
      return false;
    }
    _mpu->setDLPFMode(MPU6050_DLPF_BW_10); // 10 Hz bandwidth
    //mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500); // set sensivity, not recomended
    //mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);

    if(_mpu->dmpInitialize()) {
      ESP_LOGE(_TAG, "Failed to initialize DMP!");
      return false;
    }
    _mpu->setDMPEnabled(true);
    attachInterrupt(MPU6050_INT_PIN, _dmpInterruption, RISING);
    return true;
  }

  bool tick(bool &err) {
    err = false;
    if(!_isDMPDataReady) {
      return false;
    }
    if(!_mpu->dmpGetCurrentFIFOPacket(_fifoBuffer)) {
      ESP_LOGE(_TAG, "Failed to get DMP data!");
      err = true;
      return false;
    }
    
    Quaternion q;
    VectorFloat gravity;
    VectorInt16 accel;
    VectorInt16 accelReal;

    _mpu->dmpGetQuaternion(&q, _fifoBuffer);
    _mpu->dmpGetGravity(&gravity, &q);
    _mpu->dmpGetYawPitchRoll(_ypr, &q, &gravity);
    _mpu->dmpGetAccel(&accel, _fifoBuffer);
    _mpu->dmpGetLinearAccel(&accelReal, &accel, &gravity);
    _isDMPDataReady = false;
    
    _ax = accel.x/8192.f;
    _ay = accel.y/8192.f;
    _az = accel.z/8192.f;

    _calculateZInertial(gravity);
    return true;
  }

  /* getters */
  float accZInertial() {return _AccZInertial;}
  float yaw() {return degrees(_ypr[0]);};
  float pitch() {return degrees(_ypr[1]);};
  float roll() {return degrees(_ypr[2]);};

private:
  MPU6050_6Axis_MotionApps20 *_mpu = nullptr;
  float _ypr[3];
  float _ax, _ay, _az;
  float _AccZInertial = 0;
  uint8_t _fifoBuffer[45];
  //volatile bool _isDMPDataReady = false;
  const char *_TAG = "MPU6050 module";

  void _calculateZInertial(VectorFloat &gravity) {
    const float anglePitch = _ypr[1];
    const float angleRoll = _ypr[2];
    _AccZInertial = -sin(anglePitch)*_ax +
                    cos(anglePitch)*
                    sin(angleRoll)*_ay +
                    cos(anglePitch)*
                    cos(angleRoll)*_az;
    _AccZInertial = (_AccZInertial-1)*gravity.z*100;
  }
};