// Path: app/src/main/java/sample/ble/sensortag/config/AppConfig.java
// public class AppConfig {
//     /** Debug flag to enable local sensors fustion. */
//     public static final boolean LOCAL_SENSOR_FUSION = false;
//     /** Indicates whether {@link sample.ble.sensortag.BleSensorsRecordService} would be enabled. */
//     public static final boolean ENABLE_RECORD_SERVICE = false;
//     /** Enables magnet sensor to be used while sensor fusion calculation. */
//     public static final boolean SENSOR_FUSION_USE_MAGNET_SENSOR = false;
// 
//     /** Sensor Tag Device name. */
//     public static final String SENSOR_TAG_DEVICE_NAME = "SensorTag";
//     /** Sensor Tag Device name. */
//     public static final String SENSOR_TAG_V2_DEVICE_NAME = "CC2650 SensorTag";
//     /** Metawear Device name. */
//     public static final String METAWEAR_DEVICE_NAME = "MetaWear";
// 
//     /** Supported devices. */
//     public static final Set<String> SUPPORTED_DEVICES = new HashSet<>();
//     public static final Set<String> SENSOR_FUSION_DEVICES = new HashSet<>();
//     {
//         // supported devices
//         SUPPORTED_DEVICES.add(SENSOR_TAG_DEVICE_NAME);
//         SUPPORTED_DEVICES.add(SENSOR_TAG_V2_DEVICE_NAME);
//         SUPPORTED_DEVICES.add(METAWEAR_DEVICE_NAME);
//         // sensor fusion devices
//         SENSOR_FUSION_DEVICES.add(SENSOR_TAG_DEVICE_NAME);
//     }
// }


import android.hardware.SensorManager;
import java.util.TimerTask;
import sample.ble.sensortag.config.AppConfig;
    private final float[] accMagOrientation = new float[3];

    // final orientation angles from sensor fusion
    private final float[] fusedOrientation = new float[3];

    // accelerometer and magnetometer based rotation matrix
    private final float[] rotationMatrix = new float[9];
    private float timestamp;

    public SensorFusionEngine() {
        gyroOrientation[0] = 0.0f;
        gyroOrientation[1] = 0.0f;
        gyroOrientation[2] = 0.0f;

        // initialise gyroMatrix with identity matrix
        gyroMatrix[0] = 1.0f; gyroMatrix[1] = 0.0f; gyroMatrix[2] = 0.0f;
        gyroMatrix[3] = 0.0f; gyroMatrix[4] = 1.0f; gyroMatrix[5] = 0.0f;
        gyroMatrix[6] = 0.0f; gyroMatrix[7] = 0.0f; gyroMatrix[8] = 1.0f;
    }

    public float[] getFusedOrientation() {
        return fusedOrientation;
    }

    public void onAccDataUpdate(float[] accel) {
        System.arraycopy(accel, 0, this.accel, 0, 3);
        calculateAccMagOrientation();
    }

    public void onMagDataUpdate(float[] magnet) {
// Path: app/src/main/java/sample/ble/sensortag/config/AppConfig.java
// public class AppConfig {
//     /** Debug flag to enable local sensors fustion. */
//     public static final boolean LOCAL_SENSOR_FUSION = false;
//     /** Indicates whether {@link sample.ble.sensortag.BleSensorsRecordService} would be enabled. */
//     public static final boolean ENABLE_RECORD_SERVICE = false;
//     /** Enables magnet sensor to be used while sensor fusion calculation. */
//     public static final boolean SENSOR_FUSION_USE_MAGNET_SENSOR = false;
// 
//     /** Sensor Tag Device name. */
//     public static final String SENSOR_TAG_DEVICE_NAME = "SensorTag";
//     /** Sensor Tag Device name. */
//     public static final String SENSOR_TAG_V2_DEVICE_NAME = "CC2650 SensorTag";
//     /** Metawear Device name. */
//     public static final String METAWEAR_DEVICE_NAME = "MetaWear";
// 
//     /** Supported devices. */
//     public static final Set<String> SUPPORTED_DEVICES = new HashSet<>();
//     public static final Set<String> SENSOR_FUSION_DEVICES = new HashSet<>();
//     {
//         // supported devices
//         SUPPORTED_DEVICES.add(SENSOR_TAG_DEVICE_NAME);
//         SUPPORTED_DEVICES.add(SENSOR_TAG_V2_DEVICE_NAME);
//         SUPPORTED_DEVICES.add(METAWEAR_DEVICE_NAME);
//         // sensor fusion devices
//         SENSOR_FUSION_DEVICES.add(SENSOR_TAG_DEVICE_NAME);
//     }
// }


// Path: app/src/main/java/sample/ble/sensortag/fusion/engine/SensorFusionEngine.java
import android.hardware.SensorManager;
import java.util.TimerTask;
import sample.ble.sensortag.config.AppConfig;

    private final float[] accMagOrientation = new float[3];

    // final orientation angles from sensor fusion
    private final float[] fusedOrientation = new float[3];

    // accelerometer and magnetometer based rotation matrix
    private final float[] rotationMatrix = new float[9];
    private float timestamp;

    public SensorFusionEngine() {
        gyroOrientation[0] = 0.0f;
        gyroOrientation[1] = 0.0f;
        gyroOrientation[2] = 0.0f;

        // initialise gyroMatrix with identity matrix
        gyroMatrix[0] = 1.0f; gyroMatrix[1] = 0.0f; gyroMatrix[2] = 0.0f;
        gyroMatrix[3] = 0.0f; gyroMatrix[4] = 1.0f; gyroMatrix[5] = 0.0f;
        gyroMatrix[6] = 0.0f; gyroMatrix[7] = 0.0f; gyroMatrix[8] = 1.0f;
    }

    public float[] getFusedOrientation() {
        return fusedOrientation;
    }

    public void onAccDataUpdate(float[] accel) {
        System.arraycopy(accel, 0, this.accel, 0, 3);
        calculateAccMagOrientation();
    }

    public void onMagDataUpdate(float[] magnet) {
        if (AppConfig.SENSOR_FUSION_USE_MAGNET_SENSOR)