/*
 * Copyright (C) 2016 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <atomic.h>
#include <gpio.h>
#include <isr.h>
#include <nanohubPacket.h>
#include <plat/exti.h>
#include <plat/gpio.h>
#include <platform.h>
#include <plat/syscfg.h>
#include <plat/rtc.h>
#include <sensors.h>
#include <seos.h>
#include <halIntf.h>
#include <slab.h>
#include <heap.h>
#include <i2c.h>
#include <timer.h>
#include <variant/sensType.h>
#include <cpu/cpuMath.h>
#include <calibration/magnetometer/mag_cal.h>
#include <floatRt.h>

#include <stdlib.h>
#include <string.h>
#include <variant/variant.h>

#define ST_MAG40_APP_ID            APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 3)

/* Sensor registers */
#define ST_MAG40_WAI_REG_ADDR      0x4F
#define ST_MAG40_WAI_REG_VAL       0x40

#define ST_MAG40_CFG_A_REG_ADDR    0x60
#define ST_MAG40_TEMP_COMP_EN      0x80
#define ST_MAG40_SOFT_RESET_BIT    0x20
#define ST_MAG40_ODR_10_HZ         0x00
#define ST_MAG40_ODR_20_HZ         0x04
#define ST_MAG40_ODR_50_HZ         0x08
#define ST_MAG40_ODR_100_HZ        0x0C
#define ST_MAG40_POWER_ON          0x00
#define ST_MAG40_POWER_IDLE        0x03

#define ST_MAG40_CFG_B_REG_ADDR    0x61
#define ST_MAG40_OFF_CANC          0x02

#define ST_MAG40_CFG_C_REG_ADDR    0x62
#define ST_MAG40_I2C_DIS           0x20
#define ST_MAG40_BDU_ON            0x10
#define ST_MAG40_SELFTEST_EN       0x02
#define ST_MAG40_INT_MAG           0x01

#define ST_MAG40_OUTXL_REG_ADDR    0x68

/* Enable auto-increment of the I2C subaddress (to allow I2C multiple ops) */
#define ST_MAG40_I2C_AUTO_INCR     0x80

enum st_mag40_SensorEvents
{
    EVT_COMM_DONE = EVT_APP_START + 1,
    EVT_SENSOR_INTERRUPT,
};

enum st_mag40_TestState {
    MAG_SELFTEST_INIT,
    MAG_SELFTEST_RUN_ST_OFF,
    MAG_SELFTEST_INIT_ST_EN,
    MAG_SELFTEST_RUN_ST_ON,
    MAG_SELFTEST_VERIFY,
    MAG_SELFTEST_DONE,
};

enum st_mag40_SensorState {
    SENSOR_BOOT,
    SENSOR_VERIFY_ID,
    SENSOR_INITIALIZATION,
    SENSOR_IDLE,
    SENSOR_MAG_CONFIGURATION,
    SENSOR_READ_SAMPLES,
    SENSOR_SELF_TEST,
};

enum st_mag40_subState {
    NO_SUBSTATE = 0,

    INIT_START,
    INIT_ENABLE_DRDY,
    INIT_I2C_DISABLE_ACCEL,
    INIT_DONE,

    CONFIG_POWER_UP,
    CONFIG_POWER_UP_2,

    CONFIG_POWER_DOWN,
    CONFIG_POWER_DOWN_2,

    CONFIG_SET_RATE,
    CONFIG_SET_RATE_2,

    CONFIG_DONE,
};

struct TestResultData {
    struct HostHubRawPacket header;
    struct SensorAppEventHeader data_header;
} __attribute__((packed));

#ifndef ST_MAG40_I2C_BUS_ID
#error "ST_MAG40_I2C_BUS_ID is not defined; please define in variant.h"
#endif

#ifndef ST_MAG40_I2C_SPEED
#error "ST_MAG40_I2C_SPEED is not defined; please define in variant.h"
#endif

#ifndef ST_MAG40_I2C_ADDR
#error "ST_MAG40_I2C_ADDR is not defined; please define in variant.h"
#endif

#ifndef ST_MAG40_INT_PIN
#error "ST_MAG40_INT_PIN is not defined; please define in variant.h"
#endif

#ifndef ST_MAG40_INT_IRQ
#error "ST_MAG40_INT_IRQ is not defined; please define in variant.h"
#endif

#ifndef ST_MAG40_ROT_MATRIX
#error "ST_MAG40_ROT_MATRIX is not defined; please define in variant.h"
#endif

#define ST_MAG40_X_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
                                                      ((r11 == 1 ? x : (r11 == -1 ? -x : 0)) + \
                                                       (r12 == 1 ? y : (r12 == -1 ? -y : 0)) + \
                                                       (r13 == 1 ? z : (r13 == -1 ? -z : 0)))

#define ST_MAG40_Y_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
                                                      ((r21 == 1 ? x : (r21 == -1 ? -x : 0)) + \
                                                       (r22 == 1 ? y : (r22 == -1 ? -y : 0)) + \
                                                       (r23 == 1 ? z : (r23 == -1 ? -z : 0)))

#define ST_MAG40_Z_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
                                                      ((r31 == 1 ? x : (r31 == -1 ? -x : 0)) + \
                                                       (r32 == 1 ? y : (r32 == -1 ? -y : 0)) + \
                                                       (r33 == 1 ? z : (r33 == -1 ? -z : 0)))

#define ST_MAG40_REMAP_X_DATA(...)                     ST_MAG40_X_MAP(__VA_ARGS__)
#define ST_MAG40_REMAP_Y_DATA(...)                     ST_MAG40_Y_MAP(__VA_ARGS__)
#define ST_MAG40_REMAP_Z_DATA(...)                     ST_MAG40_Z_MAP(__VA_ARGS__)

/* Self Test macros */
#define ST_MAG40_ST_NUM_OF_SAMPLES        50
#define ST_MAG40_ST_MIN_THRESHOLD         10 /* 15 mGa */
#define ST_MAG40_ST_MAX_THRESHOLD        333 /* 500 mGa */

#define INFO_PRINT(fmt, ...) \
    do { \
        osLog(LOG_INFO, "%s " fmt, "[ST_MAG40]", ##__VA_ARGS__); \
    } while (0);

#define DEBUG_PRINT(fmt, ...) \
    do { \
        if (ST_MAG40_DBG_ENABLED) { \
            osLog(LOG_DEBUG, "%s " fmt, "[ST_MAG40]", ##__VA_ARGS__); \
        } \
    } while (0);

#define ERROR_PRINT(fmt, ...) \
    do { \
        osLog(LOG_ERROR, "%s " fmt, "[ST_MAG40]", ##__VA_ARGS__); \
    } while (0);

/* DO NOT MODIFY, just to avoid compiler error if not defined using FLAGS */
#ifndef ST_MAG40_DBG_ENABLED
#define ST_MAG40_DBG_ENABLED                           0
#endif /* ST_MAG40_DBG_ENABLED */

#define ST_MAG40_MAX_PENDING_I2C_REQUESTS   4
#define ST_MAG40_MAX_I2C_TRANSFER_SIZE      6
#define ST_MAG40_MAX_MAG_EVENTS             20

struct I2cTransfer
{
    size_t tx;
    size_t rx;
    int err;
    uint8_t txrxBuf[ST_MAG40_MAX_I2C_TRANSFER_SIZE];
    bool last;
    bool inUse;
    uint32_t delay;
};

/* Task structure */
struct st_mag40_Task {
    uint32_t tid;

    struct SlabAllocator *magDataSlab;

    uint64_t timestampInt;

    volatile uint8_t state; //task state, type enum st_mag40_SensorState, do NOT change this directly
    uint8_t subState;

    /* sensor flags */
    uint8_t samplesToDiscard;
    uint32_t rate;
    uint64_t latency;
    bool magOn;
    bool pendingInt;
    uint8_t pendingSubState;

    uint8_t currentODR;

#if defined(ST_MAG40_CAL_ENABLED)
    struct MagCal moc;
#endif

    unsigned char       sens_buf[7];

    struct I2cTransfer transfers[ST_MAG40_MAX_PENDING_I2C_REQUESTS];

    /* Communication functions */
    void (*comm_tx)(uint8_t addr, uint8_t data, uint32_t delay, bool last);
    void (*comm_rx)(uint8_t addr, uint16_t len, uint32_t delay, bool last);

    /* irq */
    struct Gpio *Int1;
    struct ChainedIsr Isr1;

    /* Self Test */
    enum st_mag40_TestState mag_test_state;
    uint32_t mag_selftest_num;
    int32_t dataST[3];
    int32_t dataNOST[3];

    /* sensors */
    uint32_t magHandle;
};

static struct st_mag40_Task mTask;

static void sensorMagConfig(void);

#define PRI_STATE PRIi32
static int32_t getStateName(int32_t s) {
    return s;
}

// Atomic get state
#define GET_STATE() (atomicReadByte(&mTask.state))

// Atomic set state, this set the state to arbitrary value, use with caution
#define SET_STATE(s) do{\
        DEBUG_PRINT("set state %" PRI_STATE "\n", getStateName(s));\
        atomicWriteByte(&mTask.state, (s));\
    }while(0)

// Atomic switch state from IDLE to desired state.
static bool trySwitchState(enum st_mag40_SensorState newState) {
#if DBG_STATE
    bool ret = atomicCmpXchgByte(&mTask.state, SENSOR_IDLE, newState);
    uint8_t prevState = ret ? SENSOR_IDLE : GET_STATE();
    DEBUG_PRINT("switch state %" PRI_STATE "->%" PRI_STATE ", %s\n",
            getStateName(prevState), getStateName(newState), ret ? "ok" : "failed");
    return ret;
#else
    return atomicCmpXchgByte(&mTask.state, SENSOR_IDLE, newState);
#endif
}

static bool magAllocateEvt(struct TripleAxisDataEvent **evPtr)
{
    struct TripleAxisDataEvent *ev;

    ev = *evPtr = slabAllocatorAlloc(mTask.magDataSlab);
    if (!ev) {
        ERROR_PRINT("Failed to allocate mag event memory");
        return false;
    }

    memset(&ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
    return true;
}

static void magFreeEvt(void *ptr)
{
    slabAllocatorFree(mTask.magDataSlab, ptr);
}

// Allocate a buffer and mark it as in use with the given state, or return NULL
// if no buffers available. Must *not* be called from interrupt context.
static struct I2cTransfer *allocXfer(void)
{
    size_t i;

    for (i = 0; i < ARRAY_SIZE(mTask.transfers); i++) {
        if (!mTask.transfers[i].inUse) {
            mTask.transfers[i].inUse = true;
            return &mTask.transfers[i];
        }
    }

    ERROR_PRINT("Ran out of i2c buffers!");
    return NULL;
}

static inline void releaseXfer(struct I2cTransfer *xfer)
{
    xfer->inUse = false;
}

static void i2cCallback(void *cookie, size_t tx, size_t rx, int err);

/* delayed callback */
static void i2cDelayCallback(uint32_t timerId, void *data)
{
    struct I2cTransfer *xfer = data;

    i2cCallback((void *)xfer, xfer->tx, xfer->rx, xfer->err);
}

static void i2cCallback(void *cookie, size_t tx, size_t rx, int err)
{
    struct I2cTransfer *xfer = cookie;

    /* Do not run callback if not the last one in a set of i2c transfers */
    if (xfer && !xfer->last) {
        releaseXfer(xfer);
        return;
    }

    /* delay callback if it is the case */
    if (xfer->delay > 0) {
        xfer->tx = tx;
        xfer->rx = rx;
        xfer->err = err;

        if (!timTimerSet(xfer->delay * 1000, 0, 50, i2cDelayCallback, xfer, true)) {
            ERROR_PRINT("Cannot do delayed i2cCallback\n");
            goto handle_now;
        }

        xfer->delay = 0;
        return;
    }

handle_now:
    xfer->tx = tx;
    xfer->rx = rx;
    xfer->err = err;

    osEnqueuePrivateEvt(EVT_COMM_DONE, cookie, NULL, mTask.tid);
    if (err != 0)
        ERROR_PRINT("i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err);
}

static void i2c_read(uint8_t addr, uint16_t len, uint32_t delay, bool last)
{
    struct I2cTransfer *xfer = allocXfer();

    if (xfer != NULL) {
        xfer->delay = delay;
        xfer->last = last;
        xfer->txrxBuf[0] = ST_MAG40_I2C_AUTO_INCR | addr;
        i2cMasterTxRx(ST_MAG40_I2C_BUS_ID, ST_MAG40_I2C_ADDR, xfer->txrxBuf, 1, xfer->txrxBuf, len, i2cCallback, xfer);
    }
}

static void i2c_write(uint8_t addr, uint8_t data, uint32_t delay, bool last)
{
    struct I2cTransfer *xfer = allocXfer();

    if (xfer != NULL) {
        xfer->delay = delay;
        xfer->last = last;
        xfer->txrxBuf[0] = addr;
        xfer->txrxBuf[1] = data;
        i2cMasterTx(ST_MAG40_I2C_BUS_ID, ST_MAG40_I2C_ADDR, xfer->txrxBuf, 2, i2cCallback, xfer);
    }
}

#define DEC_INFO_BIAS(name, type, axis, inter, samples, rates, raw, scale, bias) \
    .sensorName = name, \
    .sensorType = type, \
    .numAxis = axis, \
    .interrupt = inter, \
    .minSamples = samples, \
    .supportedRates = rates, \
    .rawType = raw, \
    .rawScale = scale, \
    .biasType = bias

#define DEC_INFO(name, type, axis, inter, samples, rates, raw, scale) \
    .sensorName = name, \
    .sensorType = type, \
    .numAxis = axis, \
    .interrupt = inter, \
    .minSamples = samples, \
    .supportedRates = rates, \
    .rawType = raw, \
    .rawScale = scale,

static uint32_t st_mag40_Rates[] = {
    SENSOR_HZ(10.0f),
    SENSOR_HZ(20.0f),
    SENSOR_HZ(50.0f),
    SENSOR_HZ(100.0f),
    0
};

static uint32_t st_mag40_regVal[] = {
    ST_MAG40_ODR_10_HZ,
    ST_MAG40_ODR_20_HZ,
    ST_MAG40_ODR_50_HZ,
    ST_MAG40_ODR_100_HZ,
};

static uint8_t st_mag40_computeOdr(uint32_t rate)
{
    int i;

    for (i = 0; i < (ARRAY_SIZE(st_mag40_Rates) - 1); i++) {
        if (st_mag40_Rates[i] == rate)
            break;
    }
    if (i == (ARRAY_SIZE(st_mag40_Rates) -1 )) {
        ERROR_PRINT("ODR not valid! Choosed smallest ODR available\n");
        i = 0;
    }

    return i;
}


static const struct SensorInfo st_mag40_SensorInfo =
{
#if defined(ST_MAG40_CAL_ENABLED)
    DEC_INFO_BIAS("Magnetometer", SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP,
        600, st_mag40_Rates, 0, 0, SENS_TYPE_MAG_BIAS)
#else
    DEC_INFO("Magnetometer", SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP,
        600, st_mag40_Rates, 0, 0)
#endif
};

/* Sensor Operations */
static bool magPower(bool on, void *cookie)
{
    INFO_PRINT("magPower %s\n", on ? "on" : "off");
    if (trySwitchState(SENSOR_MAG_CONFIGURATION)) {
        mTask.subState = on ? CONFIG_POWER_UP : CONFIG_POWER_DOWN;
        sensorMagConfig();
    } else {
        mTask.pendingSubState = on ? CONFIG_POWER_UP : CONFIG_POWER_DOWN;
    }

    return true;
}

static bool magFwUpload(void *cookie)
{
    return sensorSignalInternalEvt(mTask.magHandle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
}

static bool magSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
    uint8_t num = 0;

    INFO_PRINT("magSetRate %lu Hz - %llu ns\n", rate, latency);

    num = st_mag40_computeOdr(rate);
    mTask.currentODR = st_mag40_regVal[num];
    mTask.rate = rate;
    mTask.latency = latency;
    mTask.samplesToDiscard = 2;

    if (trySwitchState(SENSOR_MAG_CONFIGURATION)) {
        mTask.subState = CONFIG_SET_RATE;
        sensorMagConfig();
    } else {
        mTask.pendingSubState = CONFIG_SET_RATE;
    }

    return true;
}

static bool magFlush(void *cookie)
{
    INFO_PRINT("magFlush\n");
    return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG), SENSOR_DATA_EVENT_FLUSH, NULL);
}

static bool magCfgData(void *data, void *cookie)
{
#if defined(ST_MAG40_CAL_ENABLED)
    const struct AppToSensorHalDataPayload *p = data;

    if (p->type == HALINTF_TYPE_MAG_CAL_BIAS && p->size == sizeof(struct MagCalBias)) {
        const struct MagCalBias *d = p->magCalBias;
        INFO_PRINT("magCfgData: calibration %ldnT, %ldnT, %ldnT\n",
                (int32_t)(d->bias[0] * 1000),
                (int32_t)(d->bias[1] * 1000),
                (int32_t)(d->bias[2] * 1000));

        mTask.moc.x_bias = d->bias[0];
        mTask.moc.y_bias = d->bias[1];
        mTask.moc.z_bias = d->bias[2];
    } else if (p->type == HALINTF_TYPE_MAG_LOCAL_FIELD && p->size == sizeof(struct MagLocalField)) {
        const struct MagLocalField *d = p->magLocalField;
        INFO_PRINT("magCfgData: local field strength %dnT, dec %ddeg, inc %ddeg\n",
                (int)(d->strength * 1000),
                (int)(d->declination * 180 / M_PI + 0.5f),
                (int)(d->inclination * 180 / M_PI + 0.5f));

        // Passing local field information to mag calibration routine
#ifdef DIVERSITY_CHECK_ENABLED
        diversityCheckerLocalFieldUpdate(&mTask.moc.diversity_checker, d->strength);
#endif
        // TODO: pass local field information to rotation vector sensor.
    } else {
        ERROR_PRINT("magCfgData: unknown type 0x%04x, size %d", p->type, p->size);
    }
#endif /* ST_MAG40_CAL_ENABLED */

    return true;
}

static void sendTestResult(uint8_t status, uint8_t sensorType)
{
    struct TestResultData *data = heapAlloc(sizeof(struct TestResultData));
    if (!data) {
        ERROR_PRINT("Couldn't alloc test result packet");
        return;
    }

    data->header.appId = ST_MAG40_APP_ID;
    data->header.dataLen = (sizeof(struct TestResultData) - sizeof(struct HostHubRawPacket));
    data->data_header.msgId = SENSOR_APP_MSG_ID_TEST_RESULT;
    data->data_header.sensorType = sensorType;
    data->data_header.status = status;

    if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
        ERROR_PRINT("Couldn't send test result packet");
}

static void magTestHandling(struct I2cTransfer *xfer)
{
    int32_t dataGap[3];

    switch(mTask.mag_test_state) {
    case MAG_SELFTEST_INIT:
        mTask.mag_selftest_num = 0;
        memset(mTask.dataNOST, 0, 3 * sizeof(int32_t));

        mTask.mag_test_state = MAG_SELFTEST_RUN_ST_OFF;
        mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, ST_MAG40_ODR_100_HZ, 0, false);
        mTask.comm_tx(ST_MAG40_CFG_B_REG_ADDR, ST_MAG40_OFF_CANC, 0, false);
        mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR, ST_MAG40_BDU_ON, 0, true);
        break;

    case MAG_SELFTEST_RUN_ST_OFF:
        if (mTask.mag_selftest_num++ > 0) {
            uint8_t *raw = &xfer->txrxBuf[0];

            mTask.dataNOST[0] += (*(int16_t *)&raw[0]);
            mTask.dataNOST[1] += (*(int16_t *)&raw[2]);
            mTask.dataNOST[2] += (*(int16_t *)&raw[4]);
        }

        if (mTask.mag_selftest_num <= ST_MAG40_ST_NUM_OF_SAMPLES) {
            mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 10000, true);

            break;
        }

        mTask.dataNOST[0] /= ST_MAG40_ST_NUM_OF_SAMPLES;
        mTask.dataNOST[1] /= ST_MAG40_ST_NUM_OF_SAMPLES;
        mTask.dataNOST[2] /= ST_MAG40_ST_NUM_OF_SAMPLES;
        mTask.mag_test_state = MAG_SELFTEST_INIT_ST_EN;
        /* fall through */

    case MAG_SELFTEST_INIT_ST_EN:
        mTask.mag_selftest_num = 0;
        memset(mTask.dataST, 0, 3 * sizeof(int32_t));

        mTask.mag_test_state = MAG_SELFTEST_RUN_ST_ON;
        mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR, ST_MAG40_BDU_ON | ST_MAG40_SELFTEST_EN, 0, true);
        break;

    case MAG_SELFTEST_RUN_ST_ON:
        if (mTask.mag_selftest_num++ > 0) {
            uint8_t *raw = &xfer->txrxBuf[0];

            mTask.dataST[0] += (*(int16_t *)&raw[0]);
            mTask.dataST[1] += (*(int16_t *)&raw[2]);
            mTask.dataST[2] += (*(int16_t *)&raw[4]);
        }

        if (mTask.mag_selftest_num <= ST_MAG40_ST_NUM_OF_SAMPLES) {
            mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 10000, true);

            break;
        }

        mTask.dataST[0] /= ST_MAG40_ST_NUM_OF_SAMPLES;
        mTask.dataST[1] /= ST_MAG40_ST_NUM_OF_SAMPLES;
        mTask.dataST[2] /= ST_MAG40_ST_NUM_OF_SAMPLES;
        mTask.mag_test_state = MAG_SELFTEST_VERIFY;

        /* fall through */

    case MAG_SELFTEST_VERIFY:
        dataGap[0] = abs(mTask.dataST[0] - mTask.dataNOST[0]);
        dataGap[1] = abs(mTask.dataST[1] - mTask.dataNOST[1]);
        dataGap[2] = abs(mTask.dataST[2] - mTask.dataNOST[2]);

        if (dataGap[0] >= ST_MAG40_ST_MIN_THRESHOLD &&
            dataGap[0] <= ST_MAG40_ST_MAX_THRESHOLD &&
            dataGap[1] >= ST_MAG40_ST_MIN_THRESHOLD &&
            dataGap[1] <= ST_MAG40_ST_MAX_THRESHOLD &&
            dataGap[2] >= ST_MAG40_ST_MIN_THRESHOLD &&
            dataGap[2] <= ST_MAG40_ST_MAX_THRESHOLD)
                sendTestResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_MAG);
        else
                sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, SENS_TYPE_MAG);

        mTask.mag_test_state = MAG_SELFTEST_DONE;
        mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, ST_MAG40_TEMP_COMP_EN | ST_MAG40_POWER_IDLE, 0, false);
        mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR, ST_MAG40_BDU_ON | ST_MAG40_INT_MAG, 0, true);
        break;

    case MAG_SELFTEST_DONE:
        break;
    }
}

static bool magSelfTest(void *cookie)
{
    INFO_PRINT("magSelfTest\n");

    if (!mTask.magOn && trySwitchState(SENSOR_SELF_TEST)) {
        mTask.mag_test_state = MAG_SELFTEST_INIT;
        magTestHandling(NULL);
        return true;
    } else {
        ERROR_PRINT("cannot test mag because sensor is busy\n");
        sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_MAG);
        return false;
    }
}

#define DEC_OPS(power, firmware, rate, flush, test, cal, cfg) \
    .sensorPower = power, \
    .sensorFirmwareUpload = firmware, \
    .sensorSetRate = rate, \
    .sensorFlush = flush, \
    .sensorCalibrate = cal, \
    .sensorSelfTest = test, \
    .sensorCfgData = cfg

static const struct SensorOps st_mag40_SensorOps =
{
    DEC_OPS(magPower, magFwUpload, magSetRate, magFlush, magSelfTest, NULL, magCfgData),
};

static void enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
{
    gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
    syscfgSetExtiPort(pin);
    extiEnableIntGpio(pin, EXTI_TRIGGER_RISING);
    extiChainIsr(ST_MAG40_INT_IRQ, isr);
}

static void disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
{
    extiUnchainIsr(ST_MAG40_INT_IRQ, isr);
    extiDisableIntGpio(pin);
}

static bool st_mag40_int1_isr(struct ChainedIsr *isr)
{
    if (!extiIsPendingGpio(mTask.Int1))
        return false;

    /* Start sampling for a value */
    if (!osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT, NULL, NULL, mTask.tid))
        ERROR_PRINT("st_mag40_int1_isr: osEnqueuePrivateEvt() failed\n");

    extiClearPendingGpio(mTask.Int1);
    return true;
}

#define TIME_NS_TO_US(ns)    cpuMathU64DivByU16(ns, 1000)
#define kScale_mag      0.15f /* in uT - (1.5f / 10) */

static void parseRawData(uint8_t *raw)
{
    struct TripleAxisDataEvent *magSample;

    int32_t raw_x = (*(int16_t *)&raw[0]);
    int32_t raw_y = (*(int16_t *)&raw[2]);
    int32_t raw_z = (*(int16_t *)&raw[4]);
    float x, y, z;
    float xs, ys, zs;
    bool newMagnCalibData;
#if defined(ST_MAG40_CAL_ENABLED)
    float xi, yi, zi;
#endif

    mTask.timestampInt = sensorGetTime();

    /* Discard samples generated during sensor turn-on time */
    if (mTask.samplesToDiscard > 0) {
        mTask.samplesToDiscard--;
        return;
    }

    /* in uT */
    xs = (float)raw_x * kScale_mag;
    ys = (float)raw_y * kScale_mag;
    zs = (float)raw_z * kScale_mag;

    /* rotate axes */
    x = ST_MAG40_REMAP_X_DATA(xs, ys, zs, ST_MAG40_ROT_MATRIX);
    y = ST_MAG40_REMAP_Y_DATA(xs, ys, zs, ST_MAG40_ROT_MATRIX);
    z = ST_MAG40_REMAP_Z_DATA(xs, ys, zs, ST_MAG40_ROT_MATRIX);

#if defined(ST_MAG40_CAL_ENABLED)
    magCalRemoveSoftiron(&mTask.moc, x, y, z, &xi, &yi, &zi);

    newMagnCalibData = magCalUpdate(&mTask.moc, TIME_NS_TO_US(mTask.timestampInt), xi, yi, zi);

    magCalRemoveBias(&mTask.moc, xi, yi, zi, &x, &y, &z);
#endif

    if (magAllocateEvt(&magSample) == false)
        return;

    magSample->referenceTime = mTask.timestampInt;
    magSample->samples[0].deltaTime = 0;
    magSample->samples[0].firstSample.numSamples = 1;
    magSample->samples[0].x = x;
    magSample->samples[0].y = y;
    magSample->samples[0].z = z;

#if defined(ST_MAG40_CAL_ENABLED)
    if (newMagnCalibData) {
        magSample->samples[1].deltaTime = 0;
        magCalGetBias(&mTask.moc,
                     &magSample->samples[1].x,
                     &magSample->samples[1].y,
                     &magSample->samples[1].z);

        magSample->referenceTime = mTask.timestampInt;
        magSample->samples[0].firstSample.numSamples = 2;
        magSample->samples[0].firstSample.biasCurrent = true;
        magSample->samples[0].firstSample.biasPresent = 1;
        magSample->samples[0].firstSample.biasSample = 1;
    }
#endif

    osEnqueueEvtOrFree(sensorGetMyEventType(SENS_TYPE_MAG), magSample, magFreeEvt);
}

static uint8_t *wai;

static void int2Evt(void)
{
    if (trySwitchState(SENSOR_READ_SAMPLES)) {
        mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 0, true);
    } else {
        mTask.pendingInt = true;
    }
}

static void processPendingEvt(void)
{
    if (mTask.pendingInt) {
        mTask.pendingInt = false;
        int2Evt();
        return;
    }

    if (mTask.pendingSubState != NO_SUBSTATE) {
        if (trySwitchState(SENSOR_MAG_CONFIGURATION)) {
            mTask.subState = mTask.pendingSubState;
            mTask.pendingSubState = NO_SUBSTATE;
            sensorMagConfig();
        }
    }
}

static void sensorMagConfig(void)
{
    uint8_t tmp;

    switch (mTask.subState) {
    case CONFIG_POWER_UP:
        mTask.subState = CONFIG_POWER_UP_2;
        mTask.comm_tx(ST_MAG40_CFG_B_REG_ADDR, ST_MAG40_OFF_CANC, 0, false);
        mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR,
                      ST_MAG40_TEMP_COMP_EN | ST_MAG40_POWER_ON | mTask.currentODR, 0, true);
        break;

    case CONFIG_POWER_UP_2:
        mTask.subState = CONFIG_DONE;
        mTask.magOn = true;
        sensorSignalInternalEvt(mTask.magHandle,
            SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
        break;

    case CONFIG_POWER_DOWN:
        mTask.subState = CONFIG_POWER_DOWN_2;
        mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR,
                      ST_MAG40_TEMP_COMP_EN | ST_MAG40_POWER_IDLE | mTask.currentODR, 0, true);
        break;

    case CONFIG_POWER_DOWN_2:
        mTask.subState = CONFIG_DONE;
        mTask.magOn = false;
        sensorSignalInternalEvt(mTask.magHandle,
            SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0);
        break;

    case CONFIG_SET_RATE:
        mTask.subState = CONFIG_SET_RATE_2;
        tmp = mTask.magOn ? ST_MAG40_POWER_ON : ST_MAG40_POWER_IDLE;
        tmp |= mTask.currentODR;
        mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR, ST_MAG40_TEMP_COMP_EN | tmp, 0, true);
        break;

    case CONFIG_SET_RATE_2:
        mTask.subState = CONFIG_DONE;
        sensorSignalInternalEvt(mTask.magHandle,
                SENSOR_INTERNAL_EVT_RATE_CHG, mTask.rate, mTask.latency);
        break;

    default:
        /* Something weird happened */
        ERROR_PRINT("sensorMagConfig() subState=%d\n", mTask.subState);
        mTask.subState = CONFIG_DONE;
        break;
    }
}

/* initial sensor configuration */
static void sensorInit(void)
{
    switch (mTask.subState) {
    case INIT_START:
        mTask.subState = INIT_ENABLE_DRDY;
        mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR,
                    ST_MAG40_SOFT_RESET_BIT, 0, true);
        break;

    case INIT_ENABLE_DRDY:
        mTask.subState = INIT_DONE;
        mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 0, false);
        mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR,
                    ST_MAG40_BDU_ON | ST_MAG40_INT_MAG, 0, true);
        break;

    default:
        /* Something weird happened */
        ERROR_PRINT("sensorInit() subState=%d\n", mTask.subState);
        mTask.subState = INIT_DONE;
        break;
    }
}

static void handleCommDoneEvt(const void* evtData)
{
    bool returnIdle = false;
    struct I2cTransfer *xfer = (struct I2cTransfer *)evtData;

    switch (GET_STATE()) {
    case SENSOR_BOOT:
        SET_STATE(SENSOR_VERIFY_ID);

        mTask.comm_rx(ST_MAG40_WAI_REG_ADDR, 1, 0, true);
        break;

    case SENSOR_VERIFY_ID:
        /* Check the sensor ID */
        wai = &xfer->txrxBuf[0];

        if (ST_MAG40_WAI_REG_VAL != wai[0]) {
            DEBUG_PRINT("WAI returned is: %02x\n\n", *wai);
            SET_STATE(SENSOR_BOOT);
            mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR,
                        ST_MAG40_SOFT_RESET_BIT, 0, true);
            break;
        }

        INFO_PRINT( "Device ID is correct! (%02x)\n", *wai);
        SET_STATE(SENSOR_INITIALIZATION);
        mTask.subState = INIT_START;
        sensorInit();

        break;

    case SENSOR_INITIALIZATION:
        if (mTask.subState == INIT_DONE) {
            INFO_PRINT( "Initialization completed\n");
            returnIdle = true;
            sensorRegisterInitComplete(mTask.magHandle);
        } else {
            sensorInit();
        }

        break;

    case SENSOR_MAG_CONFIGURATION:
        if (mTask.subState != CONFIG_DONE)
            sensorMagConfig();
        if (mTask.subState == CONFIG_DONE)
            returnIdle = true;
        break;

    case SENSOR_READ_SAMPLES:
        returnIdle = true;

        if (gpioGet(mTask.Int1)) {
            ERROR_PRINT("error read sensor, retry!\n");
        }
        if (mTask.magOn)
            parseRawData(&xfer->txrxBuf[0]);
        break;

    case SENSOR_SELF_TEST:
        if (mTask.mag_test_state == MAG_SELFTEST_DONE)
            returnIdle = true;
        else
            magTestHandling(xfer);

        break;

    case SENSOR_IDLE:
    default:
        break;
    }

    releaseXfer(xfer);

    if (returnIdle) {
        SET_STATE(SENSOR_IDLE);
        processPendingEvt();
    }
}

static void handleEvent(uint32_t evtType, const void* evtData)
{
    switch (evtType) {
    case EVT_APP_START:
        INFO_PRINT("EVT_APP_START\n");
        osEventUnsubscribe(mTask.tid, EVT_APP_START);

        SET_STATE(SENSOR_BOOT);
        mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR,
                        ST_MAG40_SOFT_RESET_BIT, 0, true);

        break;

    case EVT_COMM_DONE:
        handleCommDoneEvt(evtData);
        break;

    case EVT_SENSOR_INTERRUPT:
        int2Evt();
        break;

    default:
        break;
    }

}

static bool startTask(uint32_t task_id)
{
    size_t slabSize;

    mTask.tid = task_id;

    INFO_PRINT("I2C DRIVER started\n");

    mTask.magOn = false;
    mTask.pendingInt = false;
    mTask.pendingSubState = NO_SUBSTATE;

    mTask.currentODR = ST_MAG40_ODR_10_HZ;
    mTask.timestampInt = 0;

    slabSize = sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint);
#if defined(ST_MAG40_CAL_ENABLED)
    slabSize += sizeof(struct TripleAxisDataPoint);
#endif

    mTask.magDataSlab = slabAllocatorNew(slabSize, 4, ST_MAG40_MAX_MAG_EVENTS);
    if (!mTask.magDataSlab) {
        ERROR_PRINT("Failed to allocate magDataSlab memory\n");
        return false;
    }

    /* Init the communication part */
    i2cMasterRequest(ST_MAG40_I2C_BUS_ID, ST_MAG40_I2C_SPEED);

    mTask.comm_tx = i2c_write;
    mTask.comm_rx = i2c_read;

    /* irq */
    mTask.Int1 = gpioRequest(ST_MAG40_INT_PIN);
    gpioConfigInput(mTask.Int1, GPIO_SPEED_LOW, GPIO_PULL_NONE);
    mTask.Isr1.func = st_mag40_int1_isr;
    enableInterrupt(mTask.Int1, &mTask.Isr1);

#if defined(ST_MAG40_CAL_ENABLED)
#ifdef DIVERSITY_CHECK_ENABLED
    initMagCal(&mTask.moc,
            0.0f, 0.0f, 0.0f,      // bias x, y, z
            1.0f, 0.0f, 0.0f,      // c00, c01, c02
            0.0f, 1.0f, 0.0f,      // c10, c11, c12
            0.0f, 0.0f, 1.0f,      // c20, c21, c22
            3000000,               // min_batch_window_in_micros
            8,                     // min_num_diverse_vectors
            1,                     // max_num_max_distance
            6.0f,                  // var_threshold
            10.0f,                 // max_min_threshold
            48.f,                  // local_field
            0.5f,                  // threshold_tuning_param
            2.552f);               // max_distance_tuning_param
#else
    initMagCal(&mTask.moc,
            0.0f, 0.0f, 0.0f,      // bias x, y, z
            1.0f, 0.0f, 0.0f,      // c00, c01, c02
            0.0f, 1.0f, 0.0f,      // c10, c11, c12
            0.0f, 0.0f, 1.0f,      // c20, c21, c22
            3000000);              // min_batch_window_in_micros
#endif /* DIVERSITY_CHECK_ENABLED */
#endif /* ST_MAG40_CAL_ENABLED */

    mTask.magHandle =
            sensorRegister(&st_mag40_SensorInfo, &st_mag40_SensorOps, NULL, false);

    osEventSubscribe(mTask.tid, EVT_APP_START);

    return true;
}

static void endTask(void)
{
    INFO_PRINT("ended\n");
#if defined(ST_MAG40_CAL_ENABLED)
    magCalDestroy(&mTask.moc);
#endif /* ST_MAG40_CAL_ENABLED */
    slabAllocatorDestroy(mTask.magDataSlab);
    disableInterrupt(mTask.Int1, &mTask.Isr1);
}

INTERNAL_APP_INIT(ST_MAG40_APP_ID, 0, startTask, endTask, handleEvent);