/*
 * 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 <stdlib.h>
#include <string.h>
#include <float.h>

#include <eventnums.h>
#include <heap.h>
#include <hostIntf.h>
#include <i2c.h>
#include <nanohubPacket.h>
#include <sensors.h>
#include <seos.h>
#include <slab.h>
#include <timer.h>
#include <util.h>
#include <variant/variant.h>

#define BMP280_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 5)

#define BMP280_APP_VERSION 3

#ifndef BMP280_I2C_BUS_ID
#define BMP280_I2C_BUS_ID  0
#endif

#define I2C_BUS_ID                      BMP280_I2C_BUS_ID
#define I2C_SPEED                       400000
#define I2C_ADDR                        0x76

#define BOSCH_BMP280_ID                 0x58

#define BOSCH_BMP280_REG_RESET          0x60
#define BOSCH_BMP280_REG_DIG_T1         0x88
#define BOSCH_BMP280_REG_ID             0xd0
#define BOSCH_BMP280_REG_CTRL_MEAS      0xf4
#define BOSCH_BMP280_REG_CONFIG         0xf5
#define BOSCH_BMP280_REG_PRES_MSB       0xf7

#define BOSCH_BMP280_MAX_PENDING_I2C_REQUESTS   4
#define BOSCH_BMP280_MAX_I2C_TRANSFER_SIZE      6

// This defines how many baro events we could handle being backed up in the
// queue. Use this to size our slab
#define MAX_BARO_EVENTS  4

// temp: 2x oversampling, baro: 16x oversampling, power: normal
#define CTRL_ON    ((2 << 5) | (5 << 2) | 3)
// temp: 2x oversampling, baro: 16x oversampling, power: sleep
#define CTRL_SLEEP ((2 << 5) | (5 << 2))

enum BMP280SensorEvents
{
    EVT_SENSOR_I2C = EVT_APP_START + 1,
    EVT_SENSOR_BARO_TIMER,
    EVT_SENSOR_TEMP_TIMER,
};

enum BMP280TaskState
{
    STATE_RESET,
    STATE_VERIFY_ID,
    STATE_AWAITING_COMP_PARAMS,
    STATE_CONFIG,
    STATE_FINISH_INIT,
    STATE_IDLE,
    STATE_ENABLING_BARO,
    STATE_ENABLING_TEMP,
    STATE_DISABLING_BARO,
    STATE_DISABLING_TEMP,
    STATE_SAMPLING,
};

struct BMP280CompParams
{
    uint16_t dig_T1;
    int16_t dig_T2, dig_T3;
    uint16_t dig_P1;
    int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9;
} __attribute__((packed));

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

static struct BMP280Task
{
    struct BMP280CompParams comp;

    struct SlabAllocator *evtSlab;

    uint32_t id;
    uint32_t baroHandle;
    uint32_t tempHandle;
    uint32_t baroTimerHandle;
    uint32_t tempTimerHandle;

    float offset;

    struct I2cTransfer transfers[BOSCH_BMP280_MAX_PENDING_I2C_REQUESTS];

    bool baroOn;
    bool tempOn;
    bool baroReading;
    bool baroCalibrating;
    bool tempReading;
} mTask;

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

static const uint32_t tempSupportedRates[] =
{
    SENSOR_HZ(0.1),
    SENSOR_HZ(1),
    SENSOR_HZ(5),
    SENSOR_HZ(10),
    SENSOR_HZ(25),
    0,
};

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

static const uint32_t baroSupportedRates[] =
{
    SENSOR_HZ(0.1),
    SENSOR_HZ(1),
    SENSOR_HZ(5),
    SENSOR_HZ(10),
    0
};

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

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

// 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];
        }
    }

    osLog(LOG_ERROR, "[BMP280]: Ran out of i2c buffers!");
    return NULL;
}

// Helper function to write a one byte register. Returns true if we got a
// successful return value from i2cMasterTx().
static bool writeRegister(uint8_t reg, uint8_t value, uint8_t state)
{
    struct I2cTransfer *xfer = allocXfer(state);
    int ret = -1;

    if (xfer != NULL) {
        xfer->txrxBuf[0] = reg;
        xfer->txrxBuf[1] = value;
        ret = i2cMasterTx(I2C_BUS_ID, I2C_ADDR, xfer->txrxBuf, 2, i2cCallback, xfer);
    }

    return (ret == 0);
}

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

    *evPtr = slabAllocatorAlloc(mTask.evtSlab);

    ev = *evPtr;
    if (!ev) {
        osLog(LOG_ERROR, "[BMP280] slabAllocatorAlloc() failed\n");
        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.evtSlab, ptr);
}

/* sensor callbacks from nanohub */

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_SENSOR_I2C, cookie, NULL, mTask.id);
    if (err != 0)
        osLog(LOG_INFO, "[BMP280] i2c error (tx: %d, rx: %d, err: %d)\n", tx, rx, err);
}

static void baroTimerCallback(uint32_t timerId, void *cookie)
{
    osEnqueuePrivateEvt(EVT_SENSOR_BARO_TIMER, cookie, NULL, mTask.id);
}

static void tempTimerCallback(uint32_t timerId, void *cookie)
{
    osEnqueuePrivateEvt(EVT_SENSOR_TEMP_TIMER, cookie, NULL, mTask.id);
}

static void setMode(bool on, uint8_t state)
{
    writeRegister(BOSCH_BMP280_REG_CTRL_MEAS, (on) ? CTRL_ON : CTRL_SLEEP, state);
}

static void sendCalibrationResult(uint8_t status, float value) {
    struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData));
    if (!data) {
        osLog(LOG_WARN, "[BMP280] Couldn't alloc cal result pkt");
        return;
    }

    data->header.appId = BMP280_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))
        osLog(LOG_WARN, "[BMP280] Couldn't send cal result evt");
}

// TODO: only turn on the timer when enabled
static bool sensorPowerBaro(bool on, void *cookie)
{
    bool oldMode = mTask.baroOn || mTask.tempOn;
    bool newMode = on || mTask.tempOn;

    if (!on && mTask.baroTimerHandle) {
        timTimerCancel(mTask.baroTimerHandle);
        mTask.baroTimerHandle = 0;
        mTask.baroReading = false;
    }

    if (oldMode != newMode)
        setMode(newMode, (on ? STATE_ENABLING_BARO : STATE_DISABLING_BARO));
    else
        sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);

    mTask.baroOn = on;

    return true;
}

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

static bool sensorRateBaro(uint32_t rate, uint64_t latency, void *cookie)
{
    if (mTask.baroTimerHandle)
        timTimerCancel(mTask.baroTimerHandle);
    mTask.baroTimerHandle = timTimerSet(sensorTimerLookupCommon(baroSupportedRates, rateTimerValsBaro, rate), 0, 50, baroTimerCallback, NULL, false);
    return sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
}

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

static bool sensorCalibrateBaro(void *cookie)
{
    if (mTask.baroOn || mTask.tempOn) {
        osLog(LOG_ERROR, "[BMP280] cannot calibrate while baro or temp are active\n");
        sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, 0.0f);
        return false;
    }

    if (mTask.baroTimerHandle)
        timTimerCancel(mTask.baroTimerHandle);
    mTask.baroTimerHandle = timTimerSet(100000000ull, 0, 50, baroTimerCallback, NULL, false);

    mTask.offset = 0.0f;
    mTask.baroOn = true;
    mTask.baroCalibrating = true;

    return writeRegister(BOSCH_BMP280_REG_CTRL_MEAS, CTRL_ON, STATE_IDLE);
}

static bool sensorCfgDataBaro(void *data, void *cookie)
{
    mTask.offset = *((float*)data) * 100.0f; // offset is given in hPa, but used as Pa in compensation
    return true;
}

static bool sensorPowerTemp(bool on, void *cookie)
{
    bool oldMode = mTask.baroOn || mTask.tempOn;
    bool newMode = on || mTask.baroOn;

    if (!on && mTask.tempTimerHandle) {
        timTimerCancel(mTask.tempTimerHandle);
        mTask.tempTimerHandle = 0;
        mTask.tempReading = false;
    }

    if (oldMode != newMode)
        setMode(newMode, (on ? STATE_ENABLING_TEMP : STATE_DISABLING_TEMP));
    else
        sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);

    mTask.tempOn = on;

    return true;
}

static bool sensorFirmwareTemp(void *cookie)
{
    sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
    return true;
}

static bool sensorRateTemp(uint32_t rate, uint64_t latency, void *cookie)
{
    if (mTask.tempTimerHandle)
        timTimerCancel(mTask.tempTimerHandle);
    mTask.tempTimerHandle = timTimerSet(sensorTimerLookupCommon(tempSupportedRates, rateTimerValsTemp, rate), 0, 50, tempTimerCallback, NULL, false);
    sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
    return true;
}

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

static const struct SensorInfo sensorInfoBaro =
{
    .sensorName = "Pressure",
    .supportedRates = baroSupportedRates,
    .sensorType = SENS_TYPE_BARO,
    .numAxis = NUM_AXIS_ONE,
    .interrupt = NANOHUB_INT_NONWAKEUP,
    .minSamples = 300
};

static const struct SensorOps sensorOpsBaro =
{
    .sensorPower = sensorPowerBaro,
    .sensorFirmwareUpload = sensorFirmwareBaro,
    .sensorSetRate = sensorRateBaro,
    .sensorFlush = sensorFlushBaro,
    .sensorCalibrate = sensorCalibrateBaro,
    .sensorCfgData = sensorCfgDataBaro,
};

static const struct SensorInfo sensorInfoTemp =
{
    .sensorName = "Temperature",
    .supportedRates = tempSupportedRates,
    .sensorType = SENS_TYPE_TEMP,
    .numAxis = NUM_AXIS_EMBEDDED,
    .interrupt = NANOHUB_INT_NONWAKEUP,
    .minSamples = 20
};

static const struct SensorOps sensorOpsTemp =
{
    .sensorPower = sensorPowerTemp,
    .sensorFirmwareUpload = sensorFirmwareTemp,
    .sensorSetRate = sensorRateTemp,
    .sensorFlush = sensorFlushTemp,
};

// Returns temperature in units of 0.01 degrees celsius.
static int32_t compensateTemp( int32_t adc_T, int32_t *t_fine)
{
    int32_t var1 =
        (((adc_T >> 3) - ((int32_t)mTask.comp.dig_T1 << 1))
            * (int32_t)mTask.comp.dig_T2) >> 11;

    int32_t tmp = (adc_T >> 4) - (int32_t)mTask.comp.dig_T1;

    int32_t var2 = (((tmp * tmp) >> 12) * (int32_t)mTask.comp.dig_T3) >> 14;

    int32_t sum = var1 + var2;

    *t_fine = sum;

    return (sum * 5 + 128) >> 8;
}

static float compensateBaro(int32_t t_fine, int32_t adc_P)
{
    float f = t_fine - 128000, fSqr = f * f;
    float a = 1048576 - adc_P;
    float v1, v2, p, pSqr;

    v2 = fSqr * mTask.comp.dig_P6 + f * mTask.comp.dig_P5 * (float)(1ULL << 17) + mTask.comp.dig_P4 * (float)(1ULL << 35);
    v1 = fSqr * mTask.comp.dig_P1 * mTask.comp.dig_P3 * (1.0f/(1ULL << 41)) + f * mTask.comp.dig_P1 * mTask.comp.dig_P2 * (1.0f/(1ULL << 21)) + mTask.comp.dig_P1 * (float)(1ULL << 14);

    p = (a * (float)(1ULL << 31) - v2) * 3125 / v1;
    pSqr = p * p;

    return pSqr * mTask.comp.dig_P9 * (1.0f/(1ULL << 59)) + p * (mTask.comp.dig_P8 * (1.0f/(1ULL << 19)) + 1) * (1.0f/(1ULL << 8)) + 16.0f * mTask.comp.dig_P7;
}

static void getTempAndBaro(const uint8_t *tmp, float *pressure_Pa, float *temp_centigrade)
{
    int32_t pres_adc = ((int32_t)tmp[0] << 12) | ((int32_t)tmp[1] << 4) | (tmp[2] >> 4);
    int32_t temp_adc = ((int32_t)tmp[3] << 12) | ((int32_t)tmp[4] << 4) | (tmp[5] >> 4);

    int32_t T_fine;
    int32_t temp = compensateTemp(temp_adc, &T_fine);
    float pres = compensateBaro(T_fine, pres_adc);

    *temp_centigrade = (float)temp * 0.01f;
    *pressure_Pa = pres * (1.0f / 256.0f) + mTask.offset;
}

static void handleI2cEvent(struct I2cTransfer *xfer)
{
    union EmbeddedDataPoint embeddedSample;
    struct SingleAxisDataEvent *baroSample;

    struct I2cTransfer *newXfer;

    switch (xfer->state) {
        case STATE_RESET: {
            newXfer = allocXfer(STATE_VERIFY_ID);
            if (newXfer != NULL) {
                newXfer->txrxBuf[0] = BOSCH_BMP280_REG_ID;
                i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 1, newXfer->txrxBuf, 1, i2cCallback, newXfer);
            }
            break;
        }

        case STATE_VERIFY_ID: {
            /* Check the sensor ID */
            if (xfer->err != 0 || xfer->txrxBuf[0] != BOSCH_BMP280_ID) {
                osLog(LOG_INFO, "[BMP280] not detected\n");
                break;
            }

            /* Get compensation parameters */
            newXfer = allocXfer(STATE_AWAITING_COMP_PARAMS);
            if (newXfer != NULL) {
                newXfer->txrxBuf[0] = BOSCH_BMP280_REG_DIG_T1;
                i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 1, (uint8_t*)&mTask.comp, 24, i2cCallback, newXfer);
            }

            break;
        }

        case STATE_AWAITING_COMP_PARAMS: {
            writeRegister(BOSCH_BMP280_REG_CTRL_MEAS, CTRL_SLEEP, STATE_CONFIG);
            break;
        }

        case STATE_CONFIG: {
            // standby time: 62.5ms, IIR filter coefficient: 4
            writeRegister(BOSCH_BMP280_REG_CONFIG, (1 << 5) | (2 << 2), STATE_FINISH_INIT);
        }

        case STATE_ENABLING_BARO: {
            sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
            break;
        }

        case STATE_ENABLING_TEMP: {
            sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
            break;
        }

        case STATE_DISABLING_BARO: {
            sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0);
            break;
        }

        case STATE_DISABLING_TEMP: {
            sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, false, 0);
            break;
        }

        case STATE_FINISH_INIT: {
            sensorRegisterInitComplete(mTask.baroHandle);
            sensorRegisterInitComplete(mTask.tempHandle);
            break;
        }

        case STATE_SAMPLING: {
            float pressure_Pa, temp_centigrade;
            getTempAndBaro(xfer->txrxBuf, &pressure_Pa, &temp_centigrade);

            if (mTask.baroOn && mTask.baroReading) {
                if (mTask.baroCalibrating) {
                    sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, pressure_Pa * 0.01f);

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

                    mTask.baroOn = false;
                    mTask.baroCalibrating = false;

                    writeRegister(BOSCH_BMP280_REG_CTRL_MEAS, CTRL_SLEEP, STATE_IDLE);
                } else {
                    if (baroAllocateEvt(&baroSample, pressure_Pa * 0.01f, sensorGetTime())) {
                        if (!osEnqueueEvtOrFree(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(SENS_TYPE_BARO), baroSample, baroFreeEvt)) {
                            osLog(LOG_ERROR, "[BMP280] failed to enqueue baro sample\n");
                        }
                    }
                }
            }

            if (mTask.tempOn && mTask.tempReading) {
                embeddedSample.fdata = temp_centigrade;
                osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), embeddedSample.vptr, NULL);
            }

            mTask.baroReading = false;
            mTask.tempReading = false;

            break;
        }

        default:
            break;
    }

    xfer->inUse = false;
}

static void handleEvent(uint32_t evtType, const void* evtData)
{
    struct I2cTransfer *newXfer;

    switch (evtType) {
        case EVT_APP_START:
        {
            osEventUnsubscribe(mTask.id, EVT_APP_START);
            i2cMasterRequest(I2C_BUS_ID, I2C_SPEED);

            /* Reset chip */
            writeRegister(BOSCH_BMP280_REG_RESET, 0xB6, STATE_RESET);
            break;
        }

        case EVT_SENSOR_I2C:
        {
            handleI2cEvent((struct I2cTransfer *)evtData);
            break;
        }

        case EVT_SENSOR_BARO_TIMER:
        {
            /* Start sampling for a value */
            if (!mTask.baroReading && !mTask.tempReading) {
                newXfer = allocXfer(STATE_SAMPLING);
                if (newXfer != NULL) {
                    newXfer->txrxBuf[0] = BOSCH_BMP280_REG_PRES_MSB;
                    i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 1, newXfer->txrxBuf, 6, i2cCallback, newXfer);
                }
            }

            mTask.baroReading = true;
            break;
        }

        case EVT_SENSOR_TEMP_TIMER:
        {
            /* Start sampling for a value */
            if (!mTask.baroReading && !mTask.tempReading) {
                newXfer = allocXfer(STATE_SAMPLING);
                if (newXfer != NULL) {
                    newXfer->txrxBuf[0] = BOSCH_BMP280_REG_PRES_MSB;
                    i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 1, newXfer->txrxBuf, 6, i2cCallback, newXfer);
                }
            }

            mTask.tempReading = true;
            break;
        }
    }
}

static bool startTask(uint32_t taskId)
{
    mTask.id = taskId;
    mTask.offset = 0.0f;

    /* Register sensors */
    mTask.baroHandle = sensorRegister(&sensorInfoBaro, &sensorOpsBaro, NULL, false);
    mTask.tempHandle = sensorRegister(&sensorInfoTemp, &sensorOpsTemp, NULL, false);

    mTask.evtSlab = slabAllocatorNew(sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint), 4, MAX_BARO_EVENTS);
    if (!mTask.evtSlab) {
        osLog(LOG_ERROR, "[BMP280] slabAllocatorNew() failed\n");
        return false;
    }

    osEventSubscribe(taskId, EVT_APP_START);

    return true;
}

static void endTask(void)
{
    slabAllocatorDestroy(mTask.evtSlab);
}

INTERNAL_APP_INIT(BMP280_APP_ID, BMP280_APP_VERSION, startTask, endTask, handleEvent);