#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

typedef struct {
    float x;
    float y;
    float z;
} mpuPrefData;

#define mpuOffGyr "/mpuOffGyr"
#define mpuOffAcc "/mpuOffAcc"

#include <MPU6050_jusax23.h>

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

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

    mpuPrefData buffer;

    if (!fs_read(mpuOffGyr, &buffer, sizeof(buffer))) {
        buffer.x = -7.8;
        buffer.y = 2.32;
        buffer.z = -0.22;
    }
    mpu6050.setGyroOffsets(buffer.x, buffer.y, buffer.z);

    if (!fs_read(mpuOffAcc, &buffer, sizeof(buffer))) {
        buffer.x = 0.03;
        buffer.y = 0.0;
        buffer.z = 0.3;
    }
    mpu6050.setAccXoffset(buffer.x, buffer.y, buffer.z);

#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();
            mpuPrefData buffer{.x = offX, .y = offY, .z = offZ};
            fs_write(mpuOffAcc, &buffer, sizeof(buffer));
            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();
            mpuPrefData buffer{.x = offX, .y = offY, .z = offZ};
            fs_write(mpuOffGyr, &buffer, sizeof(buffer));
            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));
            mpuPrefData buffer{.x = data.X, .y = data.Y, .z = data.Z};
            fs_write(mpuOffAcc, &buffer, sizeof(buffer));
            mpu6050.setGyroOffsets(data.X, data.Y, data.Z);
        } break;
        case jCommand_MPU_set_offsets_gyro: {
            mpuSendData data;
            memmove(&data, &jRCbuff, sizeof(data));
            mpuPrefData buffer{.x = data.X, .y = data.Y, .z = data.Z};
            fs_write(mpuOffGyr, &buffer, sizeof(buffer));
            mpu6050.setAccXoffset(data.X, data.Y, data.Z);
        } break;

        case jCommand_MPU_get_offsets_acc: {
            mpuSendData data;
            data.command = jCommand_MPU_get_offsets_acc;

            mpuPrefData buffer;
            if (!fs_read(mpuOffAcc, &buffer, sizeof(buffer))) {
                buffer.x = 0.03;
                buffer.y = 0.0;
                buffer.z = 0.3;
            }
            data.X = buffer.x;
            data.Y = buffer.y;
            data.Z = buffer.z;
            jSendNRF(&data, sizeof(data));
        } break;
        case jCommand_MPU_get_offsets_gyro: {
            mpuSendData data;
            data.command = jCommand_MPU_get_offsets_gyro;

            mpuPrefData buffer;
            if (!fs_read(mpuOffGyr, &buffer, sizeof(buffer))) {
                buffer.x = -7.8;
                buffer.y = 2.32;
                buffer.z = -0.22;
            }
            data.X = buffer.x;
            data.Y = buffer.y;
            data.Z = buffer.z;
            jSendNRF(&data, sizeof(data));
        }
    }
}

#else

void initMPU() {}

void MPULoop() {}

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

#endif
#endif