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

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

class MPU {
  public:
    MPU(MPU6050_6Axis_MotionApps20 *mpu) {
        assert(mpu != nullptr);
        _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;

        _gx = q.x / 32768.f * 250.f;
        _gy = q.y / 32768.f * 250.f;
        _gz = q.z / 32768.f * 250.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]);
    };

    float ax() {
        return _ax;
    }
    float ay() {
        return _ay;
    }
    float az() {
        return _az;
    }

    float gravityZ() {
        return _gravity;
    }


    float gx() {
        return _gx;
    }
    float gy() {
        return _gy;
    }
    float gz() {
        return _gz;
    }

  private:
    MPU6050_6Axis_MotionApps20 *_mpu = nullptr;
    float _ypr[3];
    float _ax, _ay, _az;
    float _gx, _gy, _gz;
    float _gravity;
    float _AccZInertial = 0;
    uint8_t _fifoBuffer[45];

    const char *_TAG = "MPU6050 module";

    void _calculateZInertial(VectorFloat &gravity) {
        const float anglePitch = _ypr[1];
        const float angleRoll = _ypr[2];
        _gravity = gravity.z;

        _AccZInertial = -sin(anglePitch) * _ax + cos(anglePitch) * sin(angleRoll) * _ay + cos(anglePitch) * cos(angleRoll) * _az;
        _AccZInertial = (_AccZInertial - 1) * gravity.z * 100;
    }
};