#include "MPU.hpp"
#include "hwlib.hpp"

xyz xyz::operator-(xyz &rhs)
{
    return xyz(x - rhs.x, y - rhs.y, z - rhs.z);
}

xyz xyz::operator-=(xyz &rhs)
{
    x -= rhs.x;
    y -= rhs.y;
    z -= rhs.z;
    return *this;
}
xyz xyz::operator+(xyz &rhs)
{
    return xyz(x + rhs.x, y + rhs.y, z + rhs.z);
}
xyz xyz::operator+=(xyz &rhs)
{
    x += rhs.x;
    y += rhs.y;
    z += rhs.z;
    return *this;
}
xyz xyz::operator*(xyz &rhs)
{
    return xyz(x * rhs.x, y * rhs.y, z * rhs.z);
}
xyz xyz::operator*=(xyz &rhs)
{
    x *= rhs.x;
    y *= rhs.y;
    z *= rhs.z;
    return *this;
}
xyz xyz::operator*(int16_t &rhs)
{
    return xyz(x * rhs, y * rhs, z * rhs);
}
xyz xyz::operator*=(int16_t &rhs)
{
    x *= rhs;
    y *= rhs;
    z *= rhs;
    return *this;
}

void MPU6050::setup(int8_t range_setting)
{
    switch (range_setting)
    {
    case 0:
        fs_range = 250;
        gyrosensitivity = 131;    
        accelsensitivity = 16384; 
        break;
    case 1:
        fs_range = 500;
        gyrosensitivity = 65.5;  
        accelsensitivity = 8192; 
        break;
    case 2:
        fs_range = 1000;
        gyrosensitivity = 32.8;  
        accelsensitivity = 4096; 
        break;
    case 3:
        fs_range = 2000;
        gyrosensitivity = 16.4;  
        accelsensitivity = 2048; 
        break;
    default:
        if (range_setting > 3) 
        {
            fs_range = 2000;
            gyrosensitivity = 16.4;
            accelsensitivity = 2048;
        }
        else 
        {
            fs_range = 250;
            gyrosensitivity = 131;
            accelsensitivity = 16384;
        }
    }
    auto to_write = (range_setting << 3); 
    writeRegister(PWR_MGMT_1, 0x80);
    hwlib::wait_ms(100);
    writeRegister(PWR_MGMT_1, 0b0001);
    writeRegister(PWR_MGMT_2, 0x00);
    hwlib::wait_ms(200);
    writeRegister(INT_ENABLE, 0x00);
    writeRegister(FIFO_EN, 0x00);
    writeRegister(PWR_MGMT_1, 0x00);
    writeRegister(I2C_MST_CTRL, 0x00);
    writeRegister(USER_CTRL, 0x00);
    writeRegister(USER_CTRL, 0x0c);
    hwlib::wait_ms(15);
    writeRegister(CONFIG, 0b00000001);
    writeRegister(SMPLRT_DIV, 0);
    writeRegister(GYRO_CONFIG, to_write);
    writeRegister(ACCEL_CONFIG, to_write);
    writeRegister(USER_CTRL, 0x40);
    writeRegister(SIGNAL_PATH_RESET, 0b00000111);
    writeRegister(SIGNAL_PATH_RESET, 0b00000000);
    writeRegister(GYRO_CONFIG, 0b00011000);
    writeRegister(ACCEL_CONFIG, 0b00011000);
    writeRegister(INT_PIN_CFG, 0b00100010);
}

void MPU6050::writeRegister(uint8_t sub_adrr, uint8_t data)
{
    auto wbus = hwlib::i2c_write_transaction(I2C_bus, address);
    wbus.write(sub_adrr);
    wbus.write(data);
    wbus.~i2c_write_transaction();
}

uint8_t *MPU6050::readRegister(uint8_t sub_addr, uint8_t *data, uint8_t size)
{
    auto wbus = hwlib::i2c_write_transaction(I2C_bus, address);
    wbus.write(sub_addr); 
    wbus.~i2c_write_transaction();
    auto rbus = hwlib::i2c_read_transaction(I2C_bus, address);
    rbus.read(data, size); 
    rbus.~i2c_read_transaction();
    return data;
}

xyz MPU6050::getAccdata_scale(int desired_range)
{
    uint8_t data[6];
    readRegister(ACCEL_XOUT_H, data, 6);
    int16_t x = (data[0] << 8) | data[1]; 
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz((x / (fs_range / desired_range)) % (desired_range + 1), (y / (fs_range / desired_range)) % (desired_range + 1), (z / (fs_range / desired_range)) % (desired_range + 1));
}
xyz MPU6050::getGyrodata_scale(int desired_range)
{
    uint8_t data[6];
    readRegister(GYRO_XOUT_H, data, 6);
    int16_t x = (data[0] << 8) | data[1];
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz((x / (fs_range / desired_range)) % (desired_range + 1), (y / (fs_range / desired_range)) % (desired_range + 1), (z / (fs_range / desired_range)) % (desired_range + 1));
}
int16_t MPU6050::getTempdata()
{
    uint8_t data[2];
    readRegister(TEMP_OUT_H, data, 2);
    int16_t temp = (data[0] << 8) | data[1];

    return temp / 340 + 36.53;
}

all_values MPU6050::getAlldata_scale(int desired_range)
{
    return all_values(getAccdata_scale(desired_range), getGyrodata_scale(desired_range), getTempdata());
}

xyz MPU6050::getAccdata()
{
    uint8_t data[6];
    readRegister(ACCEL_XOUT_H, data, 6);
    int16_t x = (data[0] << 8) | data[1];
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz(x / accelsensitivity, y / accelsensitivity, z / accelsensitivity);
}
xyz MPU6050::getGyrodata()
{
    uint8_t data[6];
    readRegister(GYRO_XOUT_H, data, 6);
    int16_t x = (data[0] << 8) | data[1];
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz(x / gyrosensitivity, y / gyrosensitivity, z / gyrosensitivity);
}

all_values MPU6050::getAlldata()
{
    return all_values(getAccdata(), getGyrodata(), getTempdata()); 
}

xyz MPU6050::getAccdata_raw()
{
    uint8_t data[6];
    readRegister(ACCEL_XOUT_H, data, 6);
    int16_t x = (data[0] << 8) | data[1];
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz(x, y, z);
}
xyz MPU6050::getGyrodata_raw()
{
    uint8_t data[6];
    readRegister(GYRO_XOUT_H, data, 6);
    int16_t x = (data[0] << 8) | data[1];
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz(x, y, z);
}
int16_t MPU6050::getTempdata_raw()
{
    uint8_t data[2];
    readRegister(TEMP_OUT_H, data, 2);
    int16_t temp = (data[0] << 8) | data[1];

    return temp;
}

all_values MPU6050::getAlldata_raw()
{
    return all_values(getAccdata_raw(), getGyrodata_raw(), getTempdata_raw());
}

void MPU6050::test(hwlib::pin_in &button, hwlib::glcd_oled &oled, hwlib::pin_out &data_rdy, hwlib::pin_out &fifo_overflow)
{

    setup(3);
    hwlib::wait_ms(2000); 
    interrupt_enable();
    for (;;)
    {
        auto f1 = hwlib::font_default_8x8();
        auto d1 = hwlib::terminal_from(oled, f1);
        uint8_t data[1];
        hwlib::wait_ms(10); 
        auto all_data = getAlldata();
        if (button.read())
        {
            fifo_disable();       
            fifo_reset();         
            read_interrupt(data); 
            read_interrupt(data); 
            data_rdy.write(0);
            hwlib::wait_ms(10);

            if ((data[0] & 0b00010000) != 0)
            { 
                fifo_overflow.write(1);
                hwlib::wait_ms(10);
            }
            else
            {
                fifo_overflow.write(0);
                hwlib::wait_ms(10);
            }
            interrupt_disable(); 
            return;
        }
        oled.clear();
        read_interrupt(data);
        if ((data[0] & 0b00000001) != 0)
        { 
            data_rdy.write(1);
            hwlib::wait_ms(10);
        }
        else
        {
            data_rdy.write(0);
            hwlib::wait_ms(10);
        }
        if ((data[0] & 0b00010000) != 0)
        { 
            fifo_overflow.write(1);
            hwlib::wait_ms(10);
        }
        else
        {
            fifo_overflow.write(0);
            hwlib::wait_ms(10);
        }
        hwlib::cout << "\n\nacc_x: " << all_data.acc.x << "   std"
                    << "\nacc_y: " << all_data.acc.y
                    << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z;
        d1 << '\f' << "acc_x: " << all_data.acc.x << "   std"
           << "\nacc_y: " << all_data.acc.y
           << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z << hwlib::flush;
        hwlib::wait_ms(500);
        all_data = fifo_read_test();
        hwlib::cout << "\n\nacc_x: " << all_data.acc.x << "   fifo"
                    << "\nacc_y: " << all_data.acc.y
                    << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z;
        d1 << '\f' << "acc_x: " << all_data.acc.x << "  fifo"
           << "\nacc_y: " << all_data.acc.y
           << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z << hwlib::flush;
        hwlib::wait_ms(500);
        all_data = getAlldata_scale(10);
        hwlib::cout << "\n\nacc_x: " << all_data.acc.x << "   scale"
                    << "\nacc_y: " << all_data.acc.y
                    << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z;
        d1 << '\f' << "acc_x: " << all_data.acc.x << "  scale"
           << "\nacc_y: " << all_data.acc.y
           << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z << hwlib::flush;
        hwlib::wait_ms(500);
        all_data = getAlldata_raw();
        hwlib::cout << "\n\nacc_x: " << all_data.acc.x << "   raw"
                    << "\nacc_y: " << all_data.acc.y
                    << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z;
        d1 << '\f' << "acc_x: " << all_data.acc.x << "  raw"
           << "\nacc_y: " << all_data.acc.y
           << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z << hwlib::flush;
        hwlib::wait_ms(500);
    }
}

void MPU6050::interrupt_enable()
{
    writeRegister(INT_PIN_CFG, 0b11110000);
    writeRegister(INT_ENABLE, 0b00010001); 
}
void MPU6050::interrupt_disable()
{
    writeRegister(INT_PIN_CFG, 0b00000000);
    writeRegister(INT_ENABLE, 0b0000000); 
}

void MPU6050::read_interrupt(uint8_t data[1])
{
    readRegister(INT_STATUS, data, 1);
}

void MPU6050::fifo_enable()
{
    writeRegister(FIFO_EN, 0b11111000); 
}

void MPU6050::fifo_disable()
{
    writeRegister(FIFO_EN, 0b00000000); 
}

all_values MPU6050::fifo_read()
{
    fifo_enable();
    hwlib::wait_ms(50);
    uint8_t count[2];
    readRegister(FIFO_COUNTH, count, 2); 
    int16_t packetcount = (count[0] << 8) | count[1];
    int16_t data[7];
    if (packetcount > 0) 
    {
        uint8_t temp[14];
        readRegister(FIFO_R_W, temp, 14);
        data[0] = (temp[0] << 8) | temp[1];
        data[1] = (temp[2] << 8) | temp[3];
        data[2] = (temp[4] << 8) | temp[5];
        data[3] = (temp[6] << 8) | temp[7];
        data[4] = (temp[8] << 8) | temp[9];
        data[5] = (temp[10] << 8) | temp[11];
        data[6] = (temp[12] << 8) | temp[13];
    }
    fifo_disable(); 
    return all_values(xyz(data[0] / accelsensitivity, data[1] / accelsensitivity, data[2] / accelsensitivity), xyz(data[4] / gyrosensitivity, data[5] / gyrosensitivity, data[6] / gyrosensitivity), data[3] / 340 + 36.53);
}
all_values MPU6050::fifo_read_test()
{
    fifo_enable();
    hwlib::wait_ms(20);
    uint8_t count[2];
    readRegister(FIFO_COUNTH, count, 2);
    int16_t packetcount = (count[0] << 8) | count[1];
    int16_t data[7];
    if (packetcount > 0) 
    {
        uint8_t temp[14];
        readRegister(FIFO_R_W, temp, 14);
        data[0] = (temp[0] << 8) | temp[1];
        data[1] = (temp[2] << 8) | temp[3];
        data[2] = (temp[4] << 8) | temp[5];
        data[3] = (temp[6] << 8) | temp[7];
        data[4] = (temp[8] << 8) | temp[9];
        data[5] = (temp[10] << 8) | temp[11];
        data[6] = (temp[12] << 8) | temp[13];
    }

    return all_values(xyz(data[0] / accelsensitivity, data[1] / accelsensitivity, data[2] / accelsensitivity), xyz(data[4] / gyrosensitivity, data[5] / gyrosensitivity, data[6] / gyrosensitivity), data[3] / 340 + 36.53);
}

void MPU6050::fifo_reset()
{
    writeRegister(USER_CTRL, 0b00000100);
}
// Boost Software License - Version 1.0 - August 17th, 2003

// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:

// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
//-------------------------------
// author: Justin van der Wiel 2021
//--------------------------------

#include "MPU.hpp"
#include "hwlib.hpp"

xyz xyz::operator-(xyz &rhs)
{
    return xyz(x - rhs.x, y - rhs.y, z - rhs.z);
}

xyz xyz::operator-=(xyz &rhs)
{
    x -= rhs.x;
    y -= rhs.y;
    z -= rhs.z;
    return *this;
}
xyz xyz::operator+(xyz &rhs)
{
    return xyz(x + rhs.x, y + rhs.y, z + rhs.z);
}
xyz xyz::operator+=(xyz &rhs)
{
    x += rhs.x;
    y += rhs.y;
    z += rhs.z;
    return *this;
}
xyz xyz::operator*(xyz &rhs)
{
    return xyz(x * rhs.x, y * rhs.y, z * rhs.z);
}
xyz xyz::operator*=(xyz &rhs)
{
    x *= rhs.x;
    y *= rhs.y;
    z *= rhs.z;
    return *this;
}
xyz xyz::operator*(int16_t &rhs)
{
    return xyz(x * rhs, y * rhs, z * rhs);
}
xyz xyz::operator*=(int16_t &rhs)
{
    x *= rhs;
    y *= rhs;
    z *= rhs;
    return *this;
}

void MPU6050::setup(int8_t range_setting)
{
    switch (range_setting)
    {
    case 0:
        fs_range = 250;
        gyrosensitivity = 131;    //lsb per second  sensitivty values from: https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf part 6.1 and 6.2
        accelsensitivity = 16384; //lsb per second
        break;
    case 1:
        fs_range = 500;
        gyrosensitivity = 65.5;  //lsb per second
        accelsensitivity = 8192; //lsb per second
        break;
    case 2:
        fs_range = 1000;
        gyrosensitivity = 32.8;  //lsb per second
        accelsensitivity = 4096; //lsb per second
        break;
    case 3:
        fs_range = 2000;
        gyrosensitivity = 16.4;  //lsb per second
        accelsensitivity = 2048; //lsb per second
        break;
    default:
        if (range_setting > 3) //if greater than 3 do the same as for 3
        {
            fs_range = 2000;
            gyrosensitivity = 16.4;
            accelsensitivity = 2048;
        }
        else //if smaller than 0 do the same as for 0
        {
            fs_range = 250;
            gyrosensitivity = 131;
            accelsensitivity = 16384;
        }
    }
    auto to_write = (range_setting << 3); // first three bytes are ignored as such the value needs to be shifted 3 before being written
    writeRegister(PWR_MGMT_1, 0x80);
    hwlib::wait_ms(100);
    writeRegister(PWR_MGMT_1, 0b0001);
    writeRegister(PWR_MGMT_2, 0x00);
    hwlib::wait_ms(200);
    writeRegister(INT_ENABLE, 0x00);
    writeRegister(FIFO_EN, 0x00);
    writeRegister(PWR_MGMT_1, 0x00);
    writeRegister(I2C_MST_CTRL, 0x00);
    writeRegister(USER_CTRL, 0x00);
    writeRegister(USER_CTRL, 0x0c);
    hwlib::wait_ms(15);
    writeRegister(CONFIG, 0b00000001);
    writeRegister(SMPLRT_DIV, 0);
    writeRegister(GYRO_CONFIG, to_write);
    writeRegister(ACCEL_CONFIG, to_write);
    writeRegister(USER_CTRL, 0x40);
    writeRegister(SIGNAL_PATH_RESET, 0b00000111);
    writeRegister(SIGNAL_PATH_RESET, 0b00000000);
    writeRegister(GYRO_CONFIG, 0b00011000);
    writeRegister(ACCEL_CONFIG, 0b00011000);
    writeRegister(INT_PIN_CFG, 0b00100010);
}

void MPU6050::writeRegister(uint8_t sub_adrr, uint8_t data)
{
    auto wbus = hwlib::i2c_write_transaction(I2C_bus, address);
    wbus.write(sub_adrr);
    wbus.write(data);
    wbus.~i2c_write_transaction();
}

uint8_t *MPU6050::readRegister(uint8_t sub_addr, uint8_t *data, uint8_t size)
{
    auto wbus = hwlib::i2c_write_transaction(I2C_bus, address);
    wbus.write(sub_addr); //send the adrress we want to read
    wbus.~i2c_write_transaction();
    auto rbus = hwlib::i2c_read_transaction(I2C_bus, address);
    rbus.read(data, size); //read the register, if you read more than once the chip auto increments the register address
    rbus.~i2c_read_transaction();
    return data;
}

xyz MPU6050::getAccdata_scale(int desired_range)
{
    uint8_t data[6];
    readRegister(ACCEL_XOUT_H, data, 6);//read acclerometer data registers
    int16_t x = (data[0] << 8) | data[1]; //create 16 bit signed integers from your read 8 bit unsigned integers
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz((x / (fs_range / desired_range)) % (desired_range + 1), (y / (fs_range / desired_range)) % (desired_range + 1), (z / (fs_range / desired_range)) % (desired_range + 1));
}
xyz MPU6050::getGyrodata_scale(int desired_range)
{
    uint8_t data[6];
    readRegister(GYRO_XOUT_H, data, 6);//read gyroscope data registers
    int16_t x = (data[0] << 8) | data[1];//create 16 bit signed integers from your read 8 bit unsigned integers
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz((x / (fs_range / desired_range)) % (desired_range + 1), (y / (fs_range / desired_range)) % (desired_range + 1), (z / (fs_range / desired_range)) % (desired_range + 1));
}
int16_t MPU6050::getTempdata()
{
    uint8_t data[2];
    readRegister(TEMP_OUT_H, data, 2);
    int16_t temp = (data[0] << 8) | data[1];

    return temp / 340 + 36.53;
}

all_values MPU6050::getAlldata_scale(int desired_range)
{
    return all_values(getAccdata_scale(desired_range), getGyrodata_scale(desired_range), getTempdata());
}

xyz MPU6050::getAccdata()
{
    uint8_t data[6];
    readRegister(ACCEL_XOUT_H, data, 6);
    int16_t x = (data[0] << 8) | data[1];
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz(x / accelsensitivity, y / accelsensitivity, z / accelsensitivity);
}
xyz MPU6050::getGyrodata()
{
    uint8_t data[6];
    readRegister(GYRO_XOUT_H, data, 6);
    int16_t x = (data[0] << 8) | data[1];
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz(x / gyrosensitivity, y / gyrosensitivity, z / gyrosensitivity);
}

all_values MPU6050::getAlldata()
{
    return all_values(getAccdata(), getGyrodata(), getTempdata()); //get all values and put them in an all_values
}

xyz MPU6050::getAccdata_raw()
{
    uint8_t data[6];
    readRegister(ACCEL_XOUT_H, data, 6);
    int16_t x = (data[0] << 8) | data[1];
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz(x, y, z);
}
xyz MPU6050::getGyrodata_raw()
{
    uint8_t data[6];
    readRegister(GYRO_XOUT_H, data, 6);
    int16_t x = (data[0] << 8) | data[1];
    int16_t y = (data[2] << 8) | data[3];
    int16_t z = (data[4] << 8) | data[5];
    return xyz(x, y, z);
}
int16_t MPU6050::getTempdata_raw()
{
    uint8_t data[2];
    readRegister(TEMP_OUT_H, data, 2);
    int16_t temp = (data[0] << 8) | data[1];

    return temp;
}

all_values MPU6050::getAlldata_raw()
{
    return all_values(getAccdata_raw(), getGyrodata_raw(), getTempdata_raw());
}

void MPU6050::test(hwlib::pin_in &button, hwlib::glcd_oled &oled, hwlib::pin_out &data_rdy, hwlib::pin_out &fifo_overflow)
{

    setup(3);
    hwlib::wait_ms(2000); //wait a while to allow data to get written out to the registers
    interrupt_enable();
    for (;;)
    {
        auto f1 = hwlib::font_default_8x8();
        auto d1 = hwlib::terminal_from(oled, f1);
        uint8_t data[1];
        hwlib::wait_ms(10); //wait to give the sensor some time to start
        auto all_data = getAlldata();
        if (button.read())
        {
            fifo_disable();       //shut off fifo
            fifo_reset();         //clear fifo
            read_interrupt(data); //read once to clear register
            read_interrupt(data); //read again to get data
            data_rdy.write(0);
            hwlib::wait_ms(10);

            if ((data[0] & 0b00010000) != 0)
            { //fifo overflow interrupt
                fifo_overflow.write(1);
                hwlib::wait_ms(10);
            }
            else
            {
                fifo_overflow.write(0);
                hwlib::wait_ms(10);
            }
            interrupt_disable(); //shut off interrupt
            return;
        }
        oled.clear();
        read_interrupt(data);
        if ((data[0] & 0b00000001) != 0)
        { //data ready interrupt
            data_rdy.write(1);
            hwlib::wait_ms(10);
        }
        else
        {
            data_rdy.write(0);
            hwlib::wait_ms(10);
        }
        if ((data[0] & 0b00010000) != 0)
        { //fifo overflow interrupt
            fifo_overflow.write(1);
            hwlib::wait_ms(10);
        }
        else
        {
            fifo_overflow.write(0);
            hwlib::wait_ms(10);
        }//output results to the oled and to the terminal
        hwlib::cout << "\n\nacc_x: " << all_data.acc.x << "   std"
                    << "\nacc_y: " << all_data.acc.y
                    << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z;
        d1 << '\f' << "acc_x: " << all_data.acc.x << "   std"
           << "\nacc_y: " << all_data.acc.y
           << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z << hwlib::flush;
        hwlib::wait_ms(500);
        all_data = fifo_read_test();
        hwlib::cout << "\n\nacc_x: " << all_data.acc.x << "   fifo"
                    << "\nacc_y: " << all_data.acc.y
                    << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z;
        d1 << '\f' << "acc_x: " << all_data.acc.x << "  fifo"
           << "\nacc_y: " << all_data.acc.y
           << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z << hwlib::flush;
        hwlib::wait_ms(500);
        all_data = getAlldata_scale(10);
        hwlib::cout << "\n\nacc_x: " << all_data.acc.x << "   scale"
                    << "\nacc_y: " << all_data.acc.y
                    << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z;
        d1 << '\f' << "acc_x: " << all_data.acc.x << "  scale"
           << "\nacc_y: " << all_data.acc.y
           << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z << hwlib::flush;
        hwlib::wait_ms(500);
        all_data = getAlldata_raw();
        hwlib::cout << "\n\nacc_x: " << all_data.acc.x << "   raw"
                    << "\nacc_y: " << all_data.acc.y
                    << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z;
        d1 << '\f' << "acc_x: " << all_data.acc.x << "  raw"
           << "\nacc_y: " << all_data.acc.y
           << "\nacc_z: " << all_data.acc.z << "\ntemp: " << all_data.temp << "\ngyro_x: " << all_data.gyr.x << "\ngyro_y: " << all_data.gyr.y << "\ngyro_z: " << all_data.gyr.z << hwlib::flush;
        hwlib::wait_ms(500);
    }
}

void MPU6050::interrupt_enable()
{
    writeRegister(INT_PIN_CFG, 0b11110000);
    writeRegister(INT_ENABLE, 0b00010001); //all interrupts except i2c master enabled
}
void MPU6050::interrupt_disable()
{
    writeRegister(INT_PIN_CFG, 0b00000000);
    writeRegister(INT_ENABLE, 0b0000000); //all interrupts disabled
}

void MPU6050::read_interrupt(uint8_t data[1])
{
    readRegister(INT_STATUS, data, 1);
}

void MPU6050::fifo_enable()
{
    writeRegister(FIFO_EN, 0b11111000); //set accelerometer and gyroscope fifo_en flags to 1
}

void MPU6050::fifo_disable()
{
    writeRegister(FIFO_EN, 0b00000000); //set accelerometer and gyroscope fifo_en flags to 0
}

all_values MPU6050::fifo_read()
{
    fifo_enable();//open fifo 
    hwlib::wait_ms(50);
    uint8_t count[2];
    readRegister(FIFO_COUNTH, count, 2); //read the amount of bytes in fifo buffer
    int16_t packetcount = (count[0] << 8) | count[1];
    int16_t data[7];
    if (packetcount > 0) //don't read if it's empty
    {
        uint8_t temp[14];
        readRegister(FIFO_R_W, temp, 14);
        data[0] = (temp[0] << 8) | temp[1];
        data[1] = (temp[2] << 8) | temp[3];
        data[2] = (temp[4] << 8) | temp[5];
        data[3] = (temp[6] << 8) | temp[7];
        data[4] = (temp[8] << 8) | temp[9];
        data[5] = (temp[10] << 8) | temp[11];
        data[6] = (temp[12] << 8) | temp[13];
    }
    fifo_disable(); //close to prevent overflow
    return all_values(xyz(data[0] / accelsensitivity, data[1] / accelsensitivity, data[2] / accelsensitivity), xyz(data[4] / gyrosensitivity, data[5] / gyrosensitivity, data[6] / gyrosensitivity), data[3] / 340 + 36.53);
}
all_values MPU6050::fifo_read_test()
{
    fifo_enable();//open
    hwlib::wait_ms(20);
    uint8_t count[2];
    readRegister(FIFO_COUNTH, count, 2);//read the amount of bytes in fifo buffer
    int16_t packetcount = (count[0] << 8) | count[1];
    int16_t data[7];
    if (packetcount > 0) //don't read if it's empty
    {
        uint8_t temp[14];
        readRegister(FIFO_R_W, temp, 14);
        data[0] = (temp[0] << 8) | temp[1];
        data[1] = (temp[2] << 8) | temp[3];
        data[2] = (temp[4] << 8) | temp[5];
        data[3] = (temp[6] << 8) | temp[7];
        data[4] = (temp[8] << 8) | temp[9];
        data[5] = (temp[10] << 8) | temp[11];
        data[6] = (temp[12] << 8) | temp[13];
    }
    //don't close to test oveflow interrupt
    return all_values(xyz(data[0] / accelsensitivity, data[1] / accelsensitivity, data[2] / accelsensitivity), xyz(data[4] / gyrosensitivity, data[5] / gyrosensitivity, data[6] / gyrosensitivity), data[3] / 340 + 36.53);
}

void MPU6050::fifo_reset()
{
    writeRegister(USER_CTRL, 0b00000100);
}