/*
 * 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 <nanohubPacket.h>
#include <plat/exti.h>
#include <plat/gpio.h>
#include <platform.h>
#include <plat/syscfg.h>
#include <heap.h>
#include <sensors.h>
#include <seos.h>
#include <slab.h>
#include <i2c.h>
#include <timer.h>
#include <stdlib.h>
#include <string.h>
#include <variant/variant.h>

#define LPS22HB_APP_ID              APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 1)

/* Sensor defs */
#define LPS22HB_INT_CFG_REG_ADDR        0x0B
#define LPS22HB_LIR_BIT                 0x04

#define LPS22HB_WAI_REG_ADDR            0x0F
#define LPS22HB_WAI_REG_VAL             0xB1

#define LPS22HB_SOFT_RESET_REG_ADDR     0x11
#define LPS22HB_SOFT_RESET_BIT          0x04
#define LPS22HB_I2C_DIS                 0x08
#define LPS22HB_IF_ADD_INC              0x10

#define LPS22HB_ODR_REG_ADDR            0x10
#define LPS22HB_ODR_ONE_SHOT            0x00
#define LPS22HB_ODR_1_HZ                0x10
#define LPS22HB_ODR_10_HZ               0x20
#define LPS22HB_ODR_25_HZ               0x30
#define LPS22HB_ODR_50_HZ               0x40
#define LPS22HB_ODR_75_HZ               0x50

#define LPS22HB_RPDS_L                  0x18
#define LPS22HB_RPDS_H                  0x19

#define LPS22HB_PRESS_OUTXL_REG_ADDR    0x28
#define LPS22HB_TEMP_OUTL_REG_ADDR      0x2B

#define LPS22HB_HECTO_PASCAL(baro_val)  (baro_val/4096)
#define LPS22HB_CENTIGRADES(temp_val)   (temp_val/100)

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

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

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

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

enum lps22hbSensorEvents
{
    EVT_COMM_DONE = EVT_APP_START + 1,
    EVT_SENSOR_BARO_TIMER,
    EVT_SENSOR_TEMP_TIMER,
    EVT_TEST,
};

enum lps22hbSensorState {
    SENSOR_BOOT,
    SENSOR_VERIFY_ID,
    SENSOR_BARO_POWER_UP,
    SENSOR_BARO_POWER_DOWN,
    SENSOR_BARO_START_CAL,
    SENSOR_BARO_READ_CAL_MEAS,
    SENSOR_BARO_CAL_DONE,
    SENSOR_BARO_SET_OFFSET,
    SENSOR_BARO_CFG_DONE,
    SENSOR_TEMP_POWER_UP,
    SENSOR_TEMP_POWER_DOWN,
    SENSOR_READ_SAMPLES,
};

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

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

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

enum lps22hbSensorIndex {
    BARO = 0,
    TEMP,
    NUM_OF_SENSOR,
};

//#define NUM_OF_SENSOR 1

struct lps22hbSensor {
    uint32_t handle;
};

struct CalibrationData {
    struct HostHubRawPacket header;
    struct SensorAppEventHeader data_header;
    float value;
} __attribute__((packed));

#define LPS22HB_MAX_PENDING_I2C_REQUESTS   4
#define LPS22HB_MAX_I2C_TRANSFER_SIZE      6
#define LPS22HB_MAX_BARO_EVENTS            4

struct I2cTransfer
{
    size_t tx;
    size_t rx;
    int err;
    uint8_t txrxBuf[LPS22HB_MAX_I2C_TRANSFER_SIZE];
    uint8_t state;
    bool inUse;
};

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

    struct SlabAllocator *baroSlab;

    /* timer */
    uint32_t baroTimerHandle;
    uint32_t tempTimerHandle;

    /* sensor flags */
    bool baroOn;
    bool baroReading;
    bool baroWantRead;
    bool tempOn;
    bool tempReading;
    bool tempWantRead;

    uint8_t offset_L;
    uint8_t offset_H;

    //int sensLastRead;

    struct I2cTransfer transfers[LPS22HB_MAX_PENDING_I2C_REQUESTS];

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

    /* sensors */
    struct lps22hbSensor sensors[NUM_OF_SENSOR];
};

static struct lps22hbTask mTask;

static bool baroAllocateEvt(struct SingleAxisDataEvent **evPtr, float sample, uint64_t time)
{
    struct SingleAxisDataEvent *ev;

    ev = *evPtr = slabAllocatorAlloc(mTask.baroSlab);
    if (!ev) {
        ERROR_PRINT("Failed to allocate baro evt memory");
        return false;
    }

    memset(&ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
    ev->referenceTime = time;
    ev->samples[0].firstSample.numSamples = 1;
    ev->samples[0].fdata = sample;

    return true;
}

static void baroFreeEvt(void *ptr)
{
    slabAllocatorFree(mTask.baroSlab, 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(uint8_t state)
{
    size_t i;

    for (i = 0; i < ARRAY_SIZE(mTask.transfers); i++) {
        if (!mTask.transfers[i].inUse) {
            mTask.transfers[i].inUse = true;
            mTask.transfers[i].state = state;
            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)
{
    struct I2cTransfer *xfer = cookie;

    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 bool i2c_read(uint8_t addr, uint16_t len, uint32_t delay, uint8_t state)
{
    struct I2cTransfer *xfer = allocXfer(state);
    int ret = -1;

    if (xfer != NULL) {
        xfer->txrxBuf[0] = 0x80 | addr;
        if ((ret = i2cMasterTxRx(LPS22HB_I2C_BUS_ID, LPS22HB_I2C_ADDR, xfer->txrxBuf, 1, xfer->txrxBuf, len, i2cCallback, xfer)) < 0) {
            releaseXfer(xfer);
            DEBUG_PRINT("i2c_read: i2cMasterTxRx operation failed (ret: %d)\n", ret);
            return false;
        }
    }

    return (ret == -1) ? false : true;
}

static bool i2c_write(uint8_t addr, uint8_t data, uint32_t delay, uint8_t state)
{
    struct I2cTransfer *xfer = allocXfer(state);
    int ret = -1;

    if (xfer != NULL) {
        xfer->txrxBuf[0] = addr;
        xfer->txrxBuf[1] = data;
        if ((ret = i2cMasterTx(LPS22HB_I2C_BUS_ID, LPS22HB_I2C_ADDR, xfer->txrxBuf, 2, i2cCallback, xfer)) < 0) {
            releaseXfer(xfer);
            DEBUG_PRINT("i2c_write: i2cMasterTx operation failed (ret: %d)\n", ret);
            return false;
        }
    }

    return (ret == -1) ? false : true;
}

static void sendCalibrationResult(uint8_t status, float value)
{
    struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData));
    if (!data) {
        ERROR_PRINT("Couldn't alloc cal result pkt\n");
        return;
    }

    data->header.appId = LPS22HB_APP_ID;
    data->header.dataLen = (sizeof(struct CalibrationData) - sizeof(struct HostHubRawPacket));
    data->data_header.msgId = SENSOR_APP_MSG_ID_CAL_RESULT;
    data->data_header.sensorType = SENS_TYPE_BARO;
    data->data_header.status = status;

    data->value = value;

    if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
        ERROR_PRINT("Couldn't send cal result evt\n");
}

/* Sensor Info */
static void sensorBaroTimerCallback(uint32_t timerId, void *data)
{
    osEnqueuePrivateEvt(EVT_SENSOR_BARO_TIMER, data, NULL, mTask.tid);
}

static void sensorTempTimerCallback(uint32_t timerId, void *data)
{
    osEnqueuePrivateEvt(EVT_SENSOR_TEMP_TIMER, data, NULL, mTask.tid);
}

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

static uint32_t lps22hbRates[] = {
    SENSOR_HZ(1.0f),
    SENSOR_HZ(10.0f),
    SENSOR_HZ(25.0f),
    SENSOR_HZ(50.0f),
    SENSOR_HZ(75.0f),
    0
};

// should match "supported rates in length" and be the timer length for that rate in nanosecs
static const uint64_t lps22hbRatesRateVals[] =
{
    1 * 1000000000ULL,
    1000000000ULL / 10,
    1000000000ULL / 25,
    1000000000ULL / 50,
    1000000000ULL / 75,
};


static const struct SensorInfo lps22hbSensorInfo[NUM_OF_SENSOR] =
{
    { DEC_INFO("Pressure", SENS_TYPE_BARO, NUM_AXIS_ONE, NANOHUB_INT_NONWAKEUP,
        300, lps22hbRates) },
    { DEC_INFO("Temperature", SENS_TYPE_AMBIENT_TEMP, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP,
        20, lps22hbRates) },
};

/* Sensor Operations */
static bool baroPower(bool on, void *cookie)
{
    bool oldMode = mTask.baroOn || mTask.tempOn;
    bool newMode = on || mTask.tempOn;
    uint32_t state = on ? SENSOR_BARO_POWER_UP : SENSOR_BARO_POWER_DOWN;
    bool ret = true;

    INFO_PRINT("baroPower %s\n", on ? "enable" : "disable");
    if (!on && mTask.baroTimerHandle) {
        timTimerCancel(mTask.baroTimerHandle);
        mTask.baroTimerHandle = 0;
        mTask.baroReading = false;
    }

    if (oldMode != newMode) {
        if (on)
            ret = mTask.comm_tx(LPS22HB_ODR_REG_ADDR, LPS22HB_ODR_10_HZ, 0, state);
        else
            ret = mTask.comm_tx(LPS22HB_ODR_REG_ADDR, LPS22HB_ODR_ONE_SHOT, 0, state);
    } else
        sensorSignalInternalEvt(mTask.sensors[BARO].handle,
                    SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);

    if (!ret) {
        DEBUG_PRINT("baroPower comm_tx failed\n");
        return(false);
    }

    mTask.baroReading = false;
    mTask.baroOn = on;
    return true;
}

static bool baroFwUpload(void *cookie)
{
    return sensorSignalInternalEvt(mTask.sensors[BARO].handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
}

static bool baroSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
    INFO_PRINT("baroSetRate %lu Hz - %llu ns\n", rate, latency);

    if (mTask.baroTimerHandle)
        timTimerCancel(mTask.baroTimerHandle);

    mTask.baroTimerHandle = timTimerSet(sensorTimerLookupCommon(lps22hbRates,
                lps22hbRatesRateVals, rate), 0, 50, sensorBaroTimerCallback, NULL, false);

    return sensorSignalInternalEvt(mTask.sensors[BARO].handle,
                SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
}

static bool baroFlush(void *cookie)
{
    return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), SENSOR_DATA_EVENT_FLUSH, NULL);
}

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

    if (mTask.baroOn) {
        ERROR_PRINT("cannot calibrate while baro is active\n");
        sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, 0.0f);
        return false;
    }

    mTask.comm_tx(LPS22HB_RPDS_L, 0, 0, SENSOR_BARO_START_CAL);
    return true;
}

/*
 * Offset data is sent in hPa, and must be transformed in 16th of hPa.
 * Since offset is expected to be summed to the out regs but the sensor
 * will actually subctract it then we need to invert the sign.
 */
static bool baroCfgData(void *data, void *cookie)
{
    float offset_f = *((float *)data) * 16;
    int32_t offset;
    bool ret;

    offset_f = (offset_f > 0) ? offset_f + 0.5f : offset_f - 0.5f;
    offset = -(int32_t)offset_f;

    INFO_PRINT("baroCfgData %ld\n", offset);

    mTask.offset_H = (offset >> 8) & 0xff;
    mTask.offset_L = (offset & 0xff);

    ret = mTask.comm_tx(LPS22HB_RPDS_L, mTask.offset_L, 0, SENSOR_BARO_SET_OFFSET);
    if (!ret)
        DEBUG_PRINT("baroCfgData: comm_tx failed\n");

    return ret;
}

static bool tempPower(bool on, void *cookie)
{
    bool oldMode = mTask.baroOn || mTask.tempOn;
    bool newMode = on || mTask.baroOn;
    uint32_t state = on ? SENSOR_TEMP_POWER_UP : SENSOR_TEMP_POWER_DOWN;
    bool ret = true;

    INFO_PRINT("tempPower %s\n", on ? "enable" : "disable");
    if (!on && mTask.tempTimerHandle) {
        timTimerCancel(mTask.tempTimerHandle);
        mTask.tempTimerHandle = 0;
        mTask.tempReading = false;
    }

    if (oldMode != newMode) {
        if (on)
            ret = mTask.comm_tx(LPS22HB_ODR_REG_ADDR, LPS22HB_ODR_10_HZ, 0, state);
        else
            ret = mTask.comm_tx(LPS22HB_ODR_REG_ADDR, LPS22HB_ODR_ONE_SHOT, 0, state);
    } else
        sensorSignalInternalEvt(mTask.sensors[TEMP].handle,
                    SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);

    if (!ret) {
        DEBUG_PRINT("tempPower comm_tx failed\n");
        return(false);
    }

    mTask.tempReading = false;
    mTask.tempOn = on;
    return true;
}

static bool tempFwUpload(void *cookie)
{
    return sensorSignalInternalEvt(mTask.sensors[TEMP].handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
}

static bool tempSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
    if (mTask.tempTimerHandle)
        timTimerCancel(mTask.tempTimerHandle);

    INFO_PRINT("tempSetRate %lu Hz - %llu ns\n", rate, latency);
    mTask.tempTimerHandle = timTimerSet(sensorTimerLookupCommon(lps22hbRates,
                lps22hbRatesRateVals, rate), 0, 50, sensorTempTimerCallback, NULL, false);

    return sensorSignalInternalEvt(mTask.sensors[TEMP].handle,
                SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
}

static bool tempFlush(void *cookie)
{
    return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_AMBIENT_TEMP), SENSOR_DATA_EVENT_FLUSH, NULL);
}

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

static const struct SensorOps lps22hbSensorOps[NUM_OF_SENSOR] =
{
    { DEC_OPS(baroPower, baroFwUpload, baroSetRate, baroFlush, baroCalibrate, baroCfgData) },
    { DEC_OPS(tempPower, tempFwUpload, tempSetRate, tempFlush, NULL, NULL) },
};

static int handleCommDoneEvt(const void* evtData)
{
    uint8_t i;
    int baro_val;
    short temp_val;
    //uint32_t state = (uint32_t)evtData;
    struct SingleAxisDataEvent *baroSample;
    union EmbeddedDataPoint sample;
    struct I2cTransfer *xfer = (struct I2cTransfer *)evtData;
    uint8_t *ptr_samples;

    switch (xfer->state) {
    case SENSOR_BOOT:
        if (!mTask.comm_rx(LPS22HB_WAI_REG_ADDR, 1, 1, SENSOR_VERIFY_ID)) {
            DEBUG_PRINT("Not able to read WAI\n");
            return -1;
        }
        break;

    case SENSOR_VERIFY_ID:
        /* Check the sensor ID */
        if (xfer->err != 0 || xfer->txrxBuf[0] != LPS22HB_WAI_REG_VAL) {
            DEBUG_PRINT("WAI returned is: %02x\n", xfer->txrxBuf[0]);
            break;
        }


        INFO_PRINT("Device ID is correct! (%02x)\n", xfer->txrxBuf[0]);
        for (i = 0; i < NUM_OF_SENSOR; i++)
            sensorRegisterInitComplete(mTask.sensors[i].handle);

        /* TEST the environment in standalone mode */
        //osEnqueuePrivateEvt(EVT_TEST, NULL, NULL, mTask.tid);
        break;

    case SENSOR_BARO_POWER_UP:
        sensorSignalInternalEvt(mTask.sensors[BARO].handle,
                    SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
        break;

    case SENSOR_BARO_POWER_DOWN:
        sensorSignalInternalEvt(mTask.sensors[BARO].handle,
                    SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0);
        break;

    case SENSOR_TEMP_POWER_UP:
        sensorSignalInternalEvt(mTask.sensors[TEMP].handle,
                    SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
        break;

    case SENSOR_TEMP_POWER_DOWN:
        sensorSignalInternalEvt(mTask.sensors[TEMP].handle,
                    SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0);
        break;

    case SENSOR_BARO_START_CAL:
        mTask.comm_tx(LPS22HB_RPDS_H, 0, 0, SENSOR_BARO_READ_CAL_MEAS);
        break;

    case SENSOR_BARO_READ_CAL_MEAS:
        mTask.comm_rx(LPS22HB_PRESS_OUTXL_REG_ADDR, 3, 1, SENSOR_BARO_CAL_DONE);
        break;

    case SENSOR_BARO_CAL_DONE:
        ptr_samples = xfer->txrxBuf;

        baro_val = ((ptr_samples[2] << 16) & 0xff0000) |
                   ((ptr_samples[1] << 8) & 0xff00) | (ptr_samples[0]);

        sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, LPS22HB_HECTO_PASCAL((float)baro_val));
        break;

    case SENSOR_BARO_SET_OFFSET:
        mTask.comm_tx(LPS22HB_RPDS_H, mTask.offset_H, 0, SENSOR_BARO_CFG_DONE);
        break;

    case SENSOR_BARO_CFG_DONE:
        break;

    case SENSOR_READ_SAMPLES:
        if (mTask.baroOn && mTask.baroWantRead) {
            float pressure_hPa;

            mTask.baroWantRead = false;
            ptr_samples = xfer->txrxBuf;

            baro_val = ((ptr_samples[2] << 16) & 0xff0000) |
                       ((ptr_samples[1] << 8) & 0xff00) | (ptr_samples[0]);

            mTask.baroReading = false;
            pressure_hPa = LPS22HB_HECTO_PASCAL((float)baro_val);
            //osLog(LOG_INFO, "baro: %p\n", sample.vptr);
            if (baroAllocateEvt(&baroSample, pressure_hPa, sensorGetTime())) {
                osEnqueueEvtOrFree(sensorGetMyEventType(SENS_TYPE_BARO), baroSample, baroFreeEvt);
            }
        }

        if (mTask.tempOn && mTask.tempWantRead) {
            mTask.tempWantRead = false;
            ptr_samples = &xfer->txrxBuf[3];

            temp_val  = ((ptr_samples[1] << 8) & 0xff00) | (ptr_samples[0]);

            mTask.tempReading = false;
            sample.fdata = LPS22HB_CENTIGRADES((float)temp_val);
            //osLog(LOG_INFO, "temp: %p\n", sample.vptr);
            osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_AMBIENT_TEMP), sample.vptr, NULL);
        }

        break;

    default:
        break;
    }

    releaseXfer(xfer);
    return (0);
}

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);

        mTask.comm_tx(LPS22HB_SOFT_RESET_REG_ADDR,
                    LPS22HB_SOFT_RESET_BIT, 0, SENSOR_BOOT);
        break;

    case EVT_COMM_DONE:
        //INFO_PRINT("EVT_COMM_DONE %d\n", (int)evtData);
        handleCommDoneEvt(evtData);
        break;

    case EVT_SENSOR_BARO_TIMER:
        //INFO_PRINT("EVT_SENSOR_BARO_TIMER\n");

        mTask.baroWantRead = true;

        /* Start sampling for a value */
        if (!mTask.baroReading && !mTask.tempReading) {
            mTask.baroReading = true;

            mTask.comm_rx(LPS22HB_PRESS_OUTXL_REG_ADDR, 5, 1, SENSOR_READ_SAMPLES);
        }

        break;

    case EVT_SENSOR_TEMP_TIMER:
        //INFO_PRINT("EVT_SENSOR_TEMP_TIMER\n");

        mTask.tempWantRead = true;

        /* Start sampling for a value */
        if (!mTask.baroReading && !mTask.tempReading) {
            mTask.tempReading = true;

            mTask.comm_rx(LPS22HB_PRESS_OUTXL_REG_ADDR, 5, 1, SENSOR_READ_SAMPLES);
        }

        break;

    case EVT_TEST:
        INFO_PRINT("EVT_TEST\n");

        baroPower(true, NULL);
        tempPower(true, NULL);
        baroSetRate(SENSOR_HZ(1), 0, NULL);
        tempSetRate(SENSOR_HZ(1), 0, NULL);
        break;

    default:
        break;
    }

}

static bool startTask(uint32_t task_id)
{
    uint8_t i;
    size_t slabSize;

    mTask.tid = task_id;

    INFO_PRINT("task started\n");

    mTask.baroOn = mTask.tempOn = false;
    mTask.baroReading = mTask.tempReading = false;

    mTask.offset_H = 0;
    mTask.offset_L = 0;

    slabSize = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint);

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

    /* Init the communication part */
    i2cMasterRequest(LPS22HB_I2C_BUS_ID, LPS22HB_I2C_SPEED);

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

    for (i = 0; i < NUM_OF_SENSOR; i++) {
        mTask.sensors[i].handle =
            sensorRegister(&lps22hbSensorInfo[i], &lps22hbSensorOps[i], NULL, false);
    }

    osEventSubscribe(mTask.tid, EVT_APP_START);

    return true;
}

static void endTask(void)
{
    INFO_PRINT("task ended\n");
    slabAllocatorDestroy(mTask.baroSlab);
}

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