#ifndef _J_SYS_SENS_MPU_H
#define _J_SYS_SENS_MPU_H

#define jType_S_MPU 0x21
#define jCommand_MPU_Acc 0
#define jCommand_MPU_Gyro 1
#define jCommand_MPU_Angle 2

#define jCommand_MPU_reset_offset_calc 0x10

#define jCommand_MPU_set_calc_offsets_acc 0x11
#define jCommand_MPU_set_calc_offsets_gyro 0x12

#define jCommand_MPU_get_calc_offsets_acc 0x13
#define jCommand_MPU_get_calc_offsets_gyro 0x14

#define jCommand_MPU_get_offsets_acc 0x15
#define jCommand_MPU_get_offsets_gyro 0x16

#define jCommand_MPU_set_offsets_acc 0x17
#define jCommand_MPU_set_offsets_gyro 0x18

#define jCommand_MPU_fail 0xff

typedef struct {
    byte type = jType_S_MPU;
    byte command;
    float X;
    float Y;
    float Z;
} mpuSendData;

#ifdef jMPU6050

#include <MPU6050_jusax23.h>

MPU6050 mpu6050(Wire, 0.001f, 0.999f);

void initMPU() {
    mpu6050.begin();
#if debug
    Serial.println("began mpu6050");
#endif

    /*mpu6050.setGyroOffsets(preferences.getFloat("mpuOffGyrX", -7.8),
                           preferences.getFloat("mpuOffGyrY", 2.32),
                           preferences.getFloat("mpuOffGyrZ", -0.22));
    mpu6050.setAccXoffset(preferences.getFloat("mpuOffAccX", 0.03),
                          preferences.getFloat("mpuOffAccY", 0.0),
                          preferences.getFloat("mpuOffAccZ", 0.3));*/

    mpu6050.setGyroOffsets(-7.8, 2.32, -0.22);
#if debug
    Serial.println("set mpu6050 gyro");
#endif
    mpu6050.setAccXoffset(0.03, 0.0, 0.3);

#if debug
    Serial.println("set mpu6050 offsets");
#endif
    // mpu6050.calcGyroOffsets(false,50,50);
}

#if debug
int dcounterxy = 0;
#endif

void MPULoop() {
    mpu6050.update();
#if debug
    if ((dcounterxy++) % 100 == 0) {
        Serial.print("AccX: ");
        Serial.print(mpu6050.getAccX());
        Serial.print(" AccY: ");
        Serial.print(mpu6050.getAccY());
        Serial.print(" AccZ: ");
        Serial.print(mpu6050.getAccZ());
        Serial.println();
        Serial.print("GyroX: ");
        Serial.print(mpu6050.getGyroX());
        Serial.print(" GyroY: ");
        Serial.print(mpu6050.getGyroY());
        Serial.print(" GyroZ: ");
        Serial.print(mpu6050.getGyroZ());
        Serial.println();
        Serial.print("AngleX: ");
        Serial.print(mpu6050.getAngleX());
        Serial.print(" AngleY: ");
        Serial.print(mpu6050.getAngleY());
        Serial.print(" AngleZ: ");
        Serial.print(mpu6050.getAngleZ());
        Serial.println();
        Serial.println();
    }
#endif
}

void MPUradioTask() {
    switch (jRCbuff[1]) {
        case jCommand_MPU_Acc: {
            mpuSendData data;
            data.command = jCommand_MPU_Acc;
            data.X = mpu6050.getAccX();
            data.Y = mpu6050.getAccY();
            data.Z = mpu6050.getAccZ();
            jSendNRF(&data, sizeof(data));
        } break;
        case jCommand_MPU_Gyro: {
            mpuSendData data;
            data.command = jCommand_MPU_Gyro;
            data.X = mpu6050.getGyroX();
            data.Y = mpu6050.getGyroY();
            data.Z = mpu6050.getGyroZ();
            jSendNRF(&data, sizeof(data));
        } break;
        case jCommand_MPU_Angle: {
            mpuSendData data;
            data.command = jCommand_MPU_Angle;
            data.X = mpu6050.getAngleX();
            data.Y = mpu6050.getAngleY();
            data.Z = mpu6050.getAngleZ();
            jSendNRF(&data, sizeof(data));
        } break;

        case jCommand_MPU_reset_offset_calc: {
            mpu6050.restGyroOffsetGlide();
            mpu6050.resetAccOffsetWatch();
        } break;
        case jCommand_MPU_set_calc_offsets_acc: {
            float offX = mpu6050.calcAccXoffset();
            float offY = mpu6050.calcAccYoffset();
            float offZ = mpu6050.calcAccZoffset();
            preferences.putFloat("mpuOffAccX", offX);
            preferences.putFloat("mpuOffAccY", offY);
            preferences.putFloat("mpuOffAccZ", offZ);
            mpu6050.setAccXoffset(offX, offY, offZ);
        } break;
        case jCommand_MPU_set_calc_offsets_gyro: {
            float offX = mpu6050.getGyroXoffsetGlide();
            float offY = mpu6050.getGyroYoffsetGlide();
            float offZ = mpu6050.getGyroZoffsetGlide();
            preferences.putFloat("mpuOffGyrX", offX);
            preferences.putFloat("mpuOffGyrY", offY);
            preferences.putFloat("mpuOffGyrZ", offZ);
            mpu6050.setGyroOffsets(offX, offY, offZ);
        } break;
        case jCommand_MPU_get_calc_offsets_acc: {
            mpuSendData data;
            data.command = jCommand_MPU_get_calc_offsets_acc;
            data.X = mpu6050.calcAccXoffset();
            data.Y = mpu6050.calcAccYoffset();
            data.Z = mpu6050.calcAccZoffset();
            jSendNRF(&data, sizeof(data));
        } break;
        case jCommand_MPU_get_calc_offsets_gyro: {
            mpuSendData data;
            data.command = jCommand_MPU_get_calc_offsets_gyro;
            data.X = mpu6050.getGyroXoffsetGlide();
            data.Y = mpu6050.getGyroYoffsetGlide();
            data.Z = mpu6050.getGyroZoffsetGlide();
            jSendNRF(&data, sizeof(data));
        } break;
        case jCommand_MPU_set_offsets_acc: {
            mpuSendData data;
            memmove(&data, &jRCbuff, sizeof(data));
            preferences.putFloat("mpuOffGyrX", data.X);
            preferences.putFloat("mpuOffGyrY", data.Y);
            preferences.putFloat("mpuOffGyrZ", data.Z);
            mpu6050.setGyroOffsets(data.X, data.Y, data.Z);
        } break;
        case jCommand_MPU_set_offsets_gyro: {
            mpuSendData data;
            memmove(&data, &jRCbuff, sizeof(data));
            preferences.putFloat("mpuOffAccX", data.X);
            preferences.putFloat("mpuOffAccY", data.Y);
            preferences.putFloat("mpuOffAccZ", data.Z);
            mpu6050.setAccXoffset(data.X, data.Y, data.Z);
        } break;

        case jCommand_MPU_get_offsets_acc: {
            mpuSendData data;
            data.command = jCommand_MPU_get_offsets_acc;
            data.X = preferences.getFloat("mpuOffGyrX", -7.8);
            data.Y = preferences.getFloat("mpuOffGyrY", 2.32);
            data.Z = preferences.getFloat("mpuOffGyrZ", -0.22);
            jSendNRF(&data, sizeof(data));
        } break;
        case jCommand_MPU_get_offsets_gyro: {
            mpuSendData data;
            data.command = jCommand_MPU_get_offsets_gyro;
            data.X = preferences.getFloat("mpuOffAccX", 0.03);
            data.Y = preferences.getFloat("mpuOffAccY", 0.0);
            data.Z = preferences.getFloat("mpuOffAccZ", 0.3);
            jSendNRF(&data, sizeof(data));
        }
    }
}

#else

void initMPU() {}

void MPULoop() {}

void MPUradioTask() {
    mpuSendData data;
    data.command = jCommand_MPU_fail;
    jSendNRF(&data, sizeof(data));
}

#endif
#endif