/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * resarding copyright ownership.  The ASF licenses this file
 * to you 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 <string.h>
#include <errno.h>
#include <assert.h>

#include "os/mynewt.h"
#include "hal/hal_i2c.h"
#include "i2cn/i2cn.h"
#include "sensor/sensor.h"
#include "sensor/accel.h"
#include "sensor/mag.h"
#include "lsm303dlhc/lsm303dlhc.h"
#include "lsm303dlhc_priv.h"
#include "modlog/modlog.h"
#include "stats/stats.h"

/* Define the stats section and records */
STATS_SECT_START(lsm303dlhc_stat_section)
    STATS_SECT_ENTRY(samples_acc_2g)
    STATS_SECT_ENTRY(samples_acc_4g)
    STATS_SECT_ENTRY(samples_acc_8g)
    STATS_SECT_ENTRY(samples_acc_16g)
    STATS_SECT_ENTRY(samples_mag_1_3g)
    STATS_SECT_ENTRY(samples_mag_1_9g)
    STATS_SECT_ENTRY(samples_mag_2_5g)
    STATS_SECT_ENTRY(samples_mag_4_0g)
    STATS_SECT_ENTRY(samples_mag_4_7g)
    STATS_SECT_ENTRY(samples_mag_5_6g)
    STATS_SECT_ENTRY(samples_mag_8_1g)
    STATS_SECT_ENTRY(errors)
STATS_SECT_END

/* Define stat names for querying */
STATS_NAME_START(lsm303dlhc_stat_section)
    STATS_NAME(lsm303dlhc_stat_section, samples_acc_2g)
    STATS_NAME(lsm303dlhc_stat_section, samples_acc_4g)
    STATS_NAME(lsm303dlhc_stat_section, samples_acc_8g)
    STATS_NAME(lsm303dlhc_stat_section, samples_acc_16g)
    STATS_NAME(lsm303dlhc_stat_section, samples_mag_1_3g)
    STATS_NAME(lsm303dlhc_stat_section, samples_mag_1_9g)
    STATS_NAME(lsm303dlhc_stat_section, samples_mag_2_5g)
    STATS_NAME(lsm303dlhc_stat_section, samples_mag_4_0g)
    STATS_NAME(lsm303dlhc_stat_section, samples_mag_4_7g)
    STATS_NAME(lsm303dlhc_stat_section, samples_mag_5_6g)
    STATS_NAME(lsm303dlhc_stat_section, samples_mag_8_1g)
    STATS_NAME(lsm303dlhc_stat_section, errors)
STATS_NAME_END(lsm303dlhc_stat_section)

/* Global variable used to hold stats data */
STATS_SECT_DECL(lsm303dlhc_stat_section) g_lsm303dlhcstats;

#define LSM303DLHC_LOG(lvl_, ...) \
    MODLOG_ ## lvl_(MYNEWT_VAL(LSM303DLHC_LOG_MODULE), __VA_ARGS__)

/* Exports for the sensor API */
static int lsm303dlhc_sensor_read(struct sensor *, sensor_type_t,
        sensor_data_func_t, void *, uint32_t);
static int lsm303dlhc_sensor_get_config(struct sensor *, sensor_type_t,
        struct sensor_cfg *);

static const struct sensor_driver g_lsm303dlhc_sensor_driver = {
    lsm303dlhc_sensor_read,
    lsm303dlhc_sensor_get_config
};

/**
 * Writes a single byte to the specified register
 *
 * @param The sensor interface
 * @param The I2C address to use
 * @param The register address to write to
 * @param The value to write
 *
 * @return 0 on success, non-zero error on failure.
 */
int
lsm303dlhc_write8(struct sensor_itf *itf, uint8_t addr, uint8_t reg,
                  uint32_t value)
{
    int rc;
    uint8_t payload[2] = { reg, value & 0xFF };

    struct hal_i2c_master_data data_struct = {
        .address = addr,
        .len = 2,
        .buffer = payload
    };

    rc = sensor_itf_lock(itf, MYNEWT_VAL(LSM303DLHC_ITF_LOCK_TMO));
    if (rc) {
        return rc;
    }

    rc = i2cn_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1,
                           MYNEWT_VAL(LSM303DLHC_I2C_RETRIES));
    if (rc) {
        LSM303DLHC_LOG(ERROR,
                       "Failed to write to 0x%02X:0x%02X with value 0x%02lX\n",
                       addr, reg, value);
        STATS_INC(g_lsm303dlhcstats, errors);
    }

    sensor_itf_unlock(itf);

    return rc;
}

/**
 * Reads a single byte from the specified register
 *
 * @param The sensor interface
 * @param The I2C address to use
 * @param The register address to read from
 * @param Pointer to where the register value should be written
 *
 * @return 0 on success, non-zero error on failure.
 */
int
lsm303dlhc_read8(struct sensor_itf *itf, uint8_t addr, uint8_t reg,
                 uint8_t *value)
{
    int rc;
    uint8_t payload;

    struct hal_i2c_master_data data_struct = {
        .address = addr,
        .len = 1,
        .buffer = &payload
    };

    rc = sensor_itf_lock(itf, MYNEWT_VAL(LSM303DLHC_ITF_LOCK_TMO));
    if (rc) {
        return rc;
    }

    /* Register write */
    payload = reg;
    rc = i2cn_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1,
                           MYNEWT_VAL(LSM303DLHC_I2C_RETRIES));
    if (rc) {
        LSM303DLHC_LOG(ERROR, "I2C access failed at address 0x%02X\n", addr);
        STATS_INC(g_lsm303dlhcstats, errors);
        goto err;
    }

    /* Read one byte back */
    payload = 0;
    rc = i2cn_master_read(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1,
                          MYNEWT_VAL(LSM303DLHC_I2C_RETRIES));
    *value = payload;
    if (rc) {
        LSM303DLHC_LOG(ERROR, "Failed to read from 0x%02X:0x%02X\n",
                       addr, reg);
        STATS_INC(g_lsm303dlhcstats, errors);
    }

err:
    sensor_itf_unlock(itf);

    return rc;
}

/**
 * Reads a six bytes from the specified register
 *
 * @param The sensor interface
 * @param The I2C address to use
 * @param The register address to read from
 * @param Pointer to where the register value should be written
 *
 * @return 0 on success, non-zero error on failure.
 */
int
lsm303dlhc_read48(struct sensor_itf *itf, uint8_t addr, uint8_t reg,
                  uint8_t *buffer)
{
    int rc;
    uint8_t payload[7] = { reg, 0, 0, 0, 0, 0, 0 };

    struct hal_i2c_master_data data_struct = {
        .address = addr,
        .len = 1,
        .buffer = payload
    };

    /* Clear the supplied buffer */
    memset(buffer, 0, 6);

    rc = sensor_itf_lock(itf, MYNEWT_VAL(LSM303DLHC_ITF_LOCK_TMO));
    if (rc) {
        return rc;
    }

    /* Register write */
    rc = i2cn_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1,
                           MYNEWT_VAL(LSM303DLHC_I2C_RETRIES));
    if (rc) {
        LSM303DLHC_LOG(ERROR, "I2C access failed at address 0x%02X\n", addr);
        STATS_INC(g_lsm303dlhcstats, errors);
        goto err;
    }

    /* Read six bytes back */
    memset(payload, 0, sizeof(payload));
    data_struct.len = 6;
    rc = i2cn_master_read(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1,
                          MYNEWT_VAL(LSM303DLHC_I2C_RETRIES));

    if (rc) {
        LSM303DLHC_LOG(ERROR, "Failed to read from 0x%02X:0x%02X\n",
                       addr, reg);
        STATS_INC(g_lsm303dlhcstats, errors);
    }

    /* Copy the I2C results into the supplied buffer */
    memcpy(buffer, payload, 6);

err:
    sensor_itf_unlock(itf);

    return rc;
}

/**
 * Expects to be called back through os_dev_create().
 *
 * @param The device object associated with this accelerometer
 * @param Argument passed to OS device init, unused
 *
 * @return 0 on success, non-zero error on failure.
 */
int
lsm303dlhc_init(struct os_dev *dev, void *arg)
{
    struct lsm303dlhc *lsm;
    struct sensor *sensor;
    int rc;

    if (!arg || !dev) {
        rc = SYS_ENODEV;
        goto err;
    }

    lsm = (struct lsm303dlhc *) dev;

    lsm->cfg.mask = SENSOR_TYPE_ALL;

    sensor = &lsm->sensor;

    /* Initialise the stats entry */
    rc = stats_init(
        STATS_HDR(g_lsm303dlhcstats),
        STATS_SIZE_INIT_PARMS(g_lsm303dlhcstats, STATS_SIZE_32),
        STATS_NAME_INIT_PARMS(lsm303dlhc_stat_section));
    SYSINIT_PANIC_ASSERT(rc == 0);
    /* Register the entry with the stats registry */
    rc = stats_register(dev->od_name, STATS_HDR(g_lsm303dlhcstats));
    SYSINIT_PANIC_ASSERT(rc == 0);

    rc = sensor_init(sensor, dev);
    if (rc != 0) {
        goto err;
    }

    /* Add the accelerometer/magnetometer driver */
    rc = sensor_set_driver(sensor, SENSOR_TYPE_ACCELEROMETER |
            SENSOR_TYPE_MAGNETIC_FIELD,
            (struct sensor_driver *) &g_lsm303dlhc_sensor_driver);
    if (rc != 0) {
        goto err;
    }

    /* Set the interface */
    rc = sensor_set_interface(sensor, arg);
    if (rc) {
        goto err;
    }

    rc = sensor_mgr_register(sensor);
    if (rc != 0) {
        goto err;
    }

    return (0);
err:
    return (rc);
}

int
lsm303dlhc_config(struct lsm303dlhc *lsm, struct lsm303dlhc_cfg *cfg)
{
    int rc;
    struct sensor_itf *itf;

    itf = SENSOR_GET_ITF(&(lsm->sensor));

    /* Most sensor chips have a single address and just use different
     * registers to get data for different sensors
     */
    if (!cfg->acc_addr || !cfg->mag_addr) {
        rc = SYS_EINVAL;
        goto err;
    }

    /* Set accel data rate (or power down) and enable XYZ output */
    rc = lsm303dlhc_write8(itf, cfg->acc_addr,
                           LSM303DLHC_REGISTER_ACCEL_CTRL_REG1_A,
                           cfg->accel_rate | 0x07);
    if (rc) {
        goto err;
    }

    lsm->cfg.accel_rate = cfg->accel_rate;

    /* Set accel scale */
    rc = lsm303dlhc_write8(itf, cfg->acc_addr,
                           LSM303DLHC_REGISTER_ACCEL_CTRL_REG4_A,
                           cfg->accel_range);
    if (rc) {
        goto err;
    }

    lsm->cfg.accel_range = cfg->accel_range;

    /* Enable the magnetomer (set to continuous conversion mode) */
    rc = lsm303dlhc_write8(itf, cfg->mag_addr,
                           LSM303DLHC_REGISTER_MAG_MR_REG_M, 0x00);
    if (rc) {
        goto err;
    }

    /* Set mag rate */
    rc = lsm303dlhc_write8(itf, cfg->mag_addr,
                           LSM303DLHC_REGISTER_MAG_CRA_REG_M,
                           cfg->mag_rate);
    if (rc) {
        goto err;
    }

    lsm->cfg.mag_rate = cfg->mag_rate;

    /* Set mag gain */
    rc = lsm303dlhc_write8(itf, cfg->mag_addr,
                           LSM303DLHC_REGISTER_MAG_CRB_REG_M,
                           cfg->mag_gain);
    if (rc) {
        goto err;
    }

    lsm->cfg.mag_gain = cfg->mag_gain;

    rc = sensor_set_type_mask(&(lsm->sensor),  cfg->mask);
    if (rc) {
        goto err;
    }

    lsm->cfg.mask = cfg->mask;
    lsm->cfg.mag_addr = cfg->mag_addr;
    lsm->cfg.acc_addr = cfg->acc_addr;

    return 0;
err:
    return (rc);
}

static int
lsm303dlhc_sensor_read(struct sensor *sensor, sensor_type_t type,
        sensor_data_func_t data_func, void *data_arg, uint32_t timeout)
{
    int rc;
    int16_t x, y, z;
    float mg_lsb;
    int16_t gauss_lsb_xy;
    int16_t gauss_lsb_z;
    uint8_t payload[6];
    struct sensor_itf *itf;
    struct lsm303dlhc *lsm;
    union {
        struct sensor_accel_data sad;
        struct sensor_mag_data smd;
    }databuf;

    /* If the read isn't looking for accel or mag data, don't do anything. */
    if (!(type & SENSOR_TYPE_ACCELEROMETER) &&
       (!(type & SENSOR_TYPE_MAGNETIC_FIELD))) {
        rc = SYS_EINVAL;
        goto err;
    }

    itf = SENSOR_GET_ITF(sensor);
    lsm = (struct lsm303dlhc *) SENSOR_GET_DEVICE(sensor);

    /* Get a new accelerometer sample */
    if (type & SENSOR_TYPE_ACCELEROMETER) {
        x = y = z = 0;
        rc = lsm303dlhc_read48(itf, lsm->cfg.acc_addr,
                               LSM303DLHC_REGISTER_ACCEL_OUT_X_L_A | 0x80,
                               payload);
        if (rc != 0) {
            goto err;
        }

        /* Shift 12-bit left-aligned accel values into 16-bit int */
        x = ((int16_t)(payload[0] | (payload[1] << 8))) >> 4;
        y = ((int16_t)(payload[2] | (payload[3] << 8))) >> 4;
        z = ((int16_t)(payload[4] | (payload[5] << 8))) >> 4;

        /* Determine mg per lsb based on range */
        switch(lsm->cfg.accel_range) {
            case LSM303DLHC_ACCEL_RANGE_2:
                STATS_INC(g_lsm303dlhcstats, samples_acc_2g);
                mg_lsb = 0.001F;
                break;
            case LSM303DLHC_ACCEL_RANGE_4:
                STATS_INC(g_lsm303dlhcstats, samples_acc_4g);
                mg_lsb = 0.002F;
                break;
            case LSM303DLHC_ACCEL_RANGE_8:
                STATS_INC(g_lsm303dlhcstats, samples_acc_8g);
                mg_lsb = 0.004F;
                break;
            case LSM303DLHC_ACCEL_RANGE_16:
                STATS_INC(g_lsm303dlhcstats, samples_acc_16g);
                mg_lsb = 0.012F;
                break;
            default:
                LSM303DLHC_LOG(
                    ERROR, "Unknown accel range: 0x%02X. Assuming +/-2G.\n",
                    lsm->cfg.accel_range);
                mg_lsb = 0.001F;
                break;
        }

        /* Convert from mg to Earth gravity in m/s^2 */
        databuf.sad.sad_x = (float)x * mg_lsb * 9.80665F;
        databuf.sad.sad_y = (float)y * mg_lsb * 9.80665F;
        databuf.sad.sad_z = (float)z * mg_lsb * 9.80665F;

        databuf.sad.sad_x_is_valid = 1;
        databuf.sad.sad_y_is_valid = 1;
        databuf.sad.sad_z_is_valid = 1;

        /* Call data function */
        rc = data_func(sensor, data_arg, &databuf.sad, SENSOR_TYPE_ACCELEROMETER);
        if (rc != 0) {
            goto err;
        }
    }

    /* Get a new magnetometer sample */
    if (type & SENSOR_TYPE_MAGNETIC_FIELD) {
        x = y = z = 0;
        rc = lsm303dlhc_read48(itf, lsm->cfg.mag_addr,
                               LSM303DLHC_REGISTER_MAG_OUT_X_H_M,
                               payload);
        if (rc != 0) {
            goto err;
        }

        /* Shift mag values into 16-bit int */
        x = (int16_t)(payload[1] | ((int16_t)payload[0] << 8));
        y = (int16_t)(payload[3] | ((int16_t)payload[2] << 8));
        z = (int16_t)(payload[5] | ((int16_t)payload[4] << 8));

        /* Determine gauss per lsb based on gain */
        switch (lsm->cfg.mag_gain) {
            case LSM303DLHC_MAG_GAIN_1_3:
                STATS_INC(g_lsm303dlhcstats, samples_mag_1_3g);
                gauss_lsb_xy = 1100;
                gauss_lsb_z = 980;
                break;
            case LSM303DLHC_MAG_GAIN_1_9:
                STATS_INC(g_lsm303dlhcstats, samples_mag_1_9g);
                gauss_lsb_xy = 855;
                gauss_lsb_z = 760;
                break;
            case LSM303DLHC_MAG_GAIN_2_5:
                STATS_INC(g_lsm303dlhcstats, samples_mag_2_5g);
                gauss_lsb_xy = 670;
                gauss_lsb_z = 600;
                break;
            case LSM303DLHC_MAG_GAIN_4_0:
                STATS_INC(g_lsm303dlhcstats, samples_mag_4_0g);
                gauss_lsb_xy = 450;
                gauss_lsb_z = 400;
                break;
            case LSM303DLHC_MAG_GAIN_4_7:
                STATS_INC(g_lsm303dlhcstats, samples_mag_4_7g);
                gauss_lsb_xy = 400;
                gauss_lsb_z = 355;
                break;
            case LSM303DLHC_MAG_GAIN_5_6:
                STATS_INC(g_lsm303dlhcstats, samples_mag_5_6g);
                gauss_lsb_xy = 330;
                gauss_lsb_z = 295;
                break;
            case LSM303DLHC_MAG_GAIN_8_1:
                STATS_INC(g_lsm303dlhcstats, samples_mag_8_1g);
                gauss_lsb_xy = 230;
                gauss_lsb_z = 205;
                break;
            default:
                LSM303DLHC_LOG(ERROR,
                               "Unknown mag gain: 0x%02X. Assuming +/-1.3g.\n",
                               lsm->cfg.mag_gain);
                gauss_lsb_xy = 1100;
                gauss_lsb_z = 980;
                break;
        }

        /* Convert from gauss to micro Tesla */
        databuf.smd.smd_x = (float)x / gauss_lsb_xy * 100.0F;
        databuf.smd.smd_y = (float)y / gauss_lsb_xy * 100.0F;
        databuf.smd.smd_z = (float)z / gauss_lsb_z * 100.0F;

        databuf.smd.smd_x_is_valid = 1;
        databuf.smd.smd_y_is_valid = 1;
        databuf.smd.smd_z_is_valid = 1;

        /* Call data function */
        rc = data_func(sensor, data_arg, &databuf.smd, SENSOR_TYPE_MAGNETIC_FIELD);
        if (rc != 0) {
            goto err;
        }
    }

    return (0);
err:
    return (rc);
}

static int
lsm303dlhc_sensor_get_config(struct sensor *sensor, sensor_type_t type,
        struct sensor_cfg *cfg)
{
    int rc;

    if ((type != SENSOR_TYPE_ACCELEROMETER) &&
        (type != SENSOR_TYPE_MAGNETIC_FIELD)) {
        rc = SYS_EINVAL;
        goto err;
    }

    cfg->sc_valtype = SENSOR_VALUE_TYPE_FLOAT_TRIPLET;

    return (0);
err:
    return (rc);
}