/*
 * Copyright (C) 2014 PHYTEC Messtechnik GmbH
 *               2016 Freie Universität Berlin
 *
 * This file is subject to the terms and conditions of the GNU Lesser
 * General Public License v2.1. See the file LICENSE in the top level
 * directory for more details.
 *
 */

/**
 * @ingroup     drivers_mma8x5x
 * @{
 *
 * @file
 * @brief       Driver for the Freescale MMA8x5x accelerometer family
 *
 * @author      Johann Fischer <j.fischer@phytec.de>
 * @author      Peter Kietzmann <peter.kietzmann@haw-hamburg.de>
 * @author      Hauke Petersen <hauke.petersen@fu-berlin.de>
 *
 * @}
 */

#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#include "assert.h"
#include "periph/i2c.h"
#include "mma8x5x.h"
#include "mma8x5x_regs.h"

#define ENABLE_DEBUG        (0)
#include "debug.h"

#define I2C_SPEED           I2C_SPEED_FAST

#define BUS                 (dev->params.i2c)
#define ADDR                (dev->params.addr)

int mma8x5x_init(mma8x5x_t *dev, const mma8x5x_params_t *params)
{
    uint8_t reg;

    assert(dev && params);

    /* write device descriptor */
    memcpy(dev, params, sizeof(mma8x5x_params_t));

    /* initialize the I2C bus */
    i2c_acquire(BUS);
    if (i2c_init_master(BUS, I2C_SPEED) < 0) {
        i2c_release(BUS);
        DEBUG("[mma8x5x] init - error: unable to initialize I2C bus\n");
        return MMA8X5X_NOI2C;
    }

    /* test if the target device responds */
    i2c_read_reg(BUS, ADDR, MMA8X5X_WHO_AM_I, &reg);
    if (reg != dev->params.type) {
        i2c_release(BUS);
        DEBUG("[mma8x5x] init - error: invalid WHO_AM_I value [0x%02x]\n",
               (int)reg);
        return MMA8X5X_NODEV;
    }

    /* reset the device */
    i2c_write_reg(BUS, ADDR, MMA8X5X_CTRL_REG2, MMA8X5X_CTRL_REG2_RST);
    do {
        i2c_read_reg(BUS, ADDR, MMA8X5X_CTRL_REG2, &reg);
    } while (reg & MMA8X5X_CTRL_REG2_RST);
    /* configure the user offset */
    i2c_write_regs(BUS, ADDR, MMA8X5X_OFF_X, dev->params.offset, 3);
    /* configure range, rate, and activate the device */
    reg = (dev->params.range & MMA8X5X_XYZ_DATA_CFG_FS_MASK);
    i2c_write_reg(BUS, ADDR, MMA8X5X_XYZ_DATA_CFG, reg);
    reg = ((dev->params.rate & MMA8X5X_CTRL_REG1_DR_MASK) |
           MMA8X5X_CTRL_REG1_ACTIVE);
    i2c_write_reg(BUS, ADDR, MMA8X5X_CTRL_REG1, reg);

    /* finally release the bus */
    i2c_release(BUS);

    DEBUG("[mma8x5x] init: successful\n");

    return MMA8X5X_OK;
}

void mma8x5x_set_user_offset(const mma8x5x_t *dev, int8_t x, int8_t y, int8_t z)
{
    uint8_t buf[3];

    assert(dev);

    buf[0] = (uint8_t)x;
    buf[1] = (uint8_t)y;
    buf[2] = (uint8_t)z;

    DEBUG("[mma8x5x] setting user offset to X: %3i, Y: %3i, Z: %3i\n",
          (int)x, (int)y, (int)z);

    i2c_acquire(BUS);
    i2c_write_regs(BUS, ADDR, MMA8X5X_OFF_X, buf, 3);
    i2c_release(BUS);
}

static int _get_reg(const mma8x5x_t *dev, uint8_t addr)
{
    uint8_t reg;

    assert(dev);

    DEBUG("[mma8x5x] getting reg 0x%02x\n", (unsigned)addr);

    i2c_acquire(BUS);
    i2c_read_reg(BUS, ADDR, addr, &reg);
    i2c_release(BUS);

    DEBUG("[mma8x5x] reg 0x%02x=0x%02x\n", (unsigned)addr, (unsigned)reg);
    return reg;
}

static void _reg_setbits(const mma8x5x_t *dev, uint8_t reg, uint8_t val)
{
    uint8_t tmp;

    assert(dev);

    i2c_acquire(BUS);
    i2c_read_reg(BUS, ADDR, reg, &tmp);
    DEBUG("[mma8x5x] 0x%02x: 0x%02x | 0x%02x = 0x%02x\n",
            (unsigned)reg, (unsigned)tmp, (unsigned)val, (unsigned) tmp | val);
    tmp |= val;
    i2c_write_reg(BUS, ADDR, reg, tmp);
    i2c_release(BUS);
}

static void _reg_clearbits(const mma8x5x_t *dev, uint8_t reg, uint8_t val)
{
    uint8_t tmp;

    assert(dev);

    i2c_acquire(BUS);
    i2c_read_reg(BUS, ADDR, reg, &tmp);
    DEBUG("[mma8x5x] 0x%02x: 0x%02x &= ~0x%02x = 0x%02x\n",
            (unsigned)reg, (unsigned)tmp, (unsigned)val, (unsigned) tmp & ~val);
    tmp &= ~val;
    i2c_write_reg(BUS, ADDR, reg, tmp);
    i2c_release(BUS);
}

void mma8x5x_set_active(const mma8x5x_t *dev)
{
    DEBUG("[mma8x5x] put device to active mode\n");
    _reg_setbits(dev, MMA8X5X_CTRL_REG1, MMA8X5X_CTRL_REG1_ACTIVE);
}

void mma8x5x_set_standby(const mma8x5x_t *dev)
{
    DEBUG("[mma8x5x] put device to standby mode\n");
    _reg_clearbits(dev, MMA8X5X_CTRL_REG1, MMA8X5X_CTRL_REG1_ACTIVE);
}

void mma8x5x_set_motiondetect(const mma8x5x_t *dev, uint8_t int_pin, uint8_t threshold)
{
    DEBUG("[mma8x5x] put device to motion detect mode (ELE=1, OAE=1)\n");
    assert(int_pin < 3);

    mma8x5x_set_standby(dev);

    _reg_setbits(dev, MMA8X5X_FF_MT_CFG, \
            MMA8X5X_FF_MT_CFG_XEFE | \
            MMA8X5X_FF_MT_CFG_YEFE | \
            MMA8X5X_FF_MT_CFG_ZEFE | \
            MMA8X5X_FF_MT_CFG_OAE | MMA8X5X_FF_MT_CFG_ELE);

    switch(int_pin) {
        case 0:
            _reg_clearbits(dev, MMA8X5X_CTRL_REG4, MMA8X5X_CTRL_REG4_INT_EN_FF_MT);
            goto out;
        case 1:
            _reg_setbits(dev, MMA8X5X_CTRL_REG5, MMA8X5X_CTRL_REG5_INT_CFG_FF_MT);
            break;
        case 2:
            _reg_clearbits(dev, MMA8X5X_CTRL_REG5, MMA8X5X_CTRL_REG5_INT_CFG_FF_MT);
            break;
    }

    _reg_setbits(dev, MMA8X5X_FF_MT_THS, threshold & 0x7f);
    _reg_setbits(dev, MMA8X5X_CTRL_REG4, MMA8X5X_CTRL_REG4_INT_EN_FF_MT);

out:
    mma8x5x_set_active(dev);
}

void mma8x5x_ack_int(const mma8x5x_t *dev)
{
    _get_reg(dev, MMA8X5X_FF_MT_SRC);
}

int mma8x5x_is_ready(const mma8x5x_t *dev)
{
    DEBUG("[mma8x5x] checking for new available data\n");

    uint8_t reg = _get_reg(dev, MMA8X5X_STATUS);

    if (reg & MMA8X5X_STATUS_ZYXDR) {
        return MMA8X5X_DATA_READY;
    }
    else {
        return MMA8X5X_NODATA;
    }
}

void mma8x5x_read(const mma8x5x_t *dev, mma8x5x_data_t *data)
{
    uint8_t buf[7];

    assert(dev);

    i2c_acquire(BUS);
    i2c_read_regs(BUS, ADDR, MMA8X5X_STATUS, buf, 7);
    i2c_release(BUS);

    data->x = ((int16_t)(buf[1] << 8 | buf[2])) / (16 >> dev->params.range);
    data->y = ((int16_t)(buf[3] << 8 | buf[4])) / (16 >> dev->params.range);
    data->z = ((int16_t)(buf[5] << 8 | buf[6])) / (16 >> dev->params.range);
}