#define jType_S_MPU 1
#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_set_offsets_acc 0x15
#define jCommand_MPU_set_offsets_gyro 0x16

#define jCommand_MPU_get_offsets_acc 0x17
#define jCommand_MPU_get_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 jMPU

#include <MPU6050_jusax23.h>

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

void initMPU(){
  mpu6050.begin();

  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.calcGyroOffsets(false,50,50);
}

void MPULoop(){
  mpu6050.update();
}

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.getGyroXoffsetGlide();
      data.Y = mpu6050.getGyroYoffsetGlide();
      data.Z = mpu6050.getGyroZoffsetGlide();
      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.calcAccXoffset();
      data.Y = mpu6050.calcAccYoffset();
      data.Z = mpu6050.calcAccZoffset();
      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