#include "MPU6050.h"
#include <Wire.h>
#include "../Config.h"
#include "../Static.h"

MPU5060::MPU5060() : InputSource("MPU5060") {}

void
MPU5060::onStart()
{
    Wire.begin();

    // Turn on the sensor
    Wire.beginTransmission(I2C_ADDRESS);
    Wire.write(PWR_MGMT_1);
    Wire.write(0);
    Wire.endTransmission(true);

    // Configure the filter
    Wire.beginTransmission(I2C_ADDRESS);
    Wire.write(CONFIG_REG);
    Wire.write(3);
    Wire.endTransmission(true);

    // Configure the accel range
    Wire.beginTransmission(I2C_ADDRESS);
    Wire.write(ACCEL_CONFIG_REG);
    // 4G
    Wire.write(2 << 3);
    Wire.endTransmission(true);
}

void
MPU5060::onStop()
{
    Wire.beginTransmission(I2C_ADDRESS);
    // Turn off the sensor
    Wire.write(PWR_MGMT_1);
    Wire.write(1);
    Wire.endTransmission(true);
    //Wire.end();
}

InputEvent
MPU5060::read()
{
    EVERY_N_MILLISECONDS(5) {
        Wire.beginTransmission(I2C_ADDRESS);
        Wire.write(ACCEL_XOUT_HIGH);
        Wire.endTransmission(false);
        Wire.requestFrom(I2C_ADDRESS, 6);
        const int16_t accelX = Wire.read() << 8 | Wire.read();
        const int16_t accelY = Wire.read() << 8 | Wire.read();
        const int16_t accelZ = Wire.read() << 8 | Wire.read();
        const uint16_t accelSum = abs(accelX) + abs(accelY) + abs(accelZ);
        const uint16_t delta = abs(m_value.value() - accelSum);
        m_value.add(accelSum);
        if (delta > 32) {
            return InputEvent{InputEvent::Acceleration, delta};
        }
    }
    return InputEvent{};
}

STATIC_ALLOC(MPU5060);
STATIC_TASK(MPU5060);