#ifndef _J_SYS_SENS_COMPASS_H
#define _J_SYS_SENS_COMPASS_H

#define jType_S_COMP 0x22
#define jCommand_COMP_fail 0xff
#define jCommand_Compass_Data 0
#define jCommand_Compass_Calibrate 1

typedef struct {
    byte type = jType_S_COMP;
    byte command = 0;
    int X;
    int Y;
    int Z;
    int angle;
} compData;

#ifdef jCOMP

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

#include <QMC5883LCompass.h>
QMC5883LCompass compass;

bool compass_is_calibrating = false;

void initCOMP() {
    compass.init();

    compassPrefData buffer;

    if (!fs_read("compOff", &buffer, sizeof(buffer))) {
        buffer.x = 0;
        buffer.y = 0;
        buffer.z = 0;
    }
    compass.setCalibrationOffsets(buffer.x, buffer.y, buffer.z);
    if (!fs_read("compScale", &buffer, sizeof(buffer))) {
        buffer.x = 1;
        buffer.y = 1;
        buffer.z = 1;
    }
    compass.setCalibrationScales(buffer.x, buffer.y, buffer.z);
}

void COMPLoop() {
    if (!compass_is_calibrating) compass.read();
}

/*float Altitude = mpu6050.getAngleX()/180*PI;
float Roll = mpu6050.getAngleY()/180*PI;
float xh=compass.getX()*cos(Altitude)+compass.getZ()*sin(Altitude);
float
yh=compass.getX()*sin(Roll)*sin(Altitude)+compass.getY()*cos(Roll)-compass.getZ()*sin(Roll)*cos(Altitude);
float heading = atan2(yh, xh);*/

void COMPradioTask() {
    switch (jRCbuff[1]) {
        case jCommand_Compass_Data: {
            compData data;
            data.X = compass.getX();
            data.Y = compass.getY();
            data.Z = compass.getZ();
            data.angle = compass.getAzimuth();
            jSendNRF(&data, sizeof(data));
        } break;
        case jCommand_Compass_Calibrate: {
            compass_is_calibrating = true;
            delay(10);
            compass.calibrate();

            compassPrefData buffer;
            buffer.x = compass.getCalibrationOffset(0);
            buffer.y = compass.getCalibrationOffset(1);
            buffer.z = compass.getCalibrationOffset(2);
            fs_write("compOff", &buffer, sizeof(buffer));
            buffer.x = compass.getCalibrationScale(0);
            buffer.y = compass.getCalibrationScale(1);
            buffer.z = compass.getCalibrationScale(2);
            fs_write("compScale", &buffer, sizeof(buffer));
            compass_is_calibrating = false;
        } break;
    }
}

#else

void initCOMP() {}

void COMPLoop() {}

void MPUradioTask() {
    compData data;
    data.command = jCommand_COMP_fail;
    jSendNRF(&data, sizeof(data));
}

#endif

#endif