/*
 * Driver for L3GD20H 3-axes digital output gyroscope connected to I2C or SPI.
 * It can also be used with L3GD20 and L3G4200D.
 *
 * This driver is for the usage with the ESP8266 and FreeRTOS (esp-open-rtos)
 * [https://github.com/SuperHouse/esp-open-rtos]. It is also working with ESP32
 * and ESP-IDF [https://github.com/espressif/esp-idf.git] as well as Linux
 * based systems using a wrapper library for ESP8266 functions.
 *
 * ---------------------------------------------------------------------------
 *
 * The BSD License (3-clause license)
 *
 * Copyright (c) 2017 Gunar Schorcht (https://github.com/gschorcht)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived from this
 * software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * The information provided is believed to be accurate and reliable. The
 * copyright holder assumes no responsibility for the consequences of use
 * of such information nor for any infringement of patents or other rights
 * of third parties which may result from its use. No license is granted by
 * implication or otherwise under any patent or patent rights of the copyright
 * holder.
 */

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

#include "l3gd20h.h"

#ifdef debug
#undef debug
#undef debug_dev
#endif

#ifdef error
#undef error
#undef error_dev
#endif

#if defined(L3GD20H_DEBUG_LEVEL_2)
#define debug(s, f, ...) printf("%s %s: " s "\n", "L3GD20H", f, ## __VA_ARGS__)
#define debug_dev(s, f, d, ...) printf("%s %s: bus %d, addr %02x - " s "\n", "L3GD20H", f, d->bus, d->addr, ## __VA_ARGS__)
#else
#define debug(s, f, ...)
#define debug_dev(s, f, d, ...)
#endif

#if defined(L3GD20H_DEBUG_LEVEL_1) || defined(L3GD20H_DEBUG_LEVEL_2)
#define error(s, f, ...) printf("%s %s: " s "\n", "L3GD20H", f, ## __VA_ARGS__)
#define error_dev(s, f, d, ...) printf("%s %s: bus %d, addr %02x - " s "\n", "L3GD20H", f, d->bus, d->addr, ## __VA_ARGS__)
#else
#define error(s, f, ...)
#define error_dev(s, f, d, ...)
#endif

// register addresses
#define L3GD20H_REG_WHO_AM_I     0x0f
#define L3GD20H_REG_CTRL1        0x20
#define L3GD20H_REG_CTRL2        0x21
#define L3GD20H_REG_CTRL3        0x22
#define L3GD20H_REG_CTRL4        0x23
#define L3GD20H_REG_CTRL5        0x24
#define L3GD20H_REG_REFERENCE    0x25
#define L3GD20H_REG_OUT_TEMP     0x26
#define L3GD20H_REG_STATUS       0x27
#define L3GD20H_REG_OUT_X_L      0x28
#define L3GD20H_REG_OUT_X_H      0x29
#define L3GD20H_REG_OUT_Y_L      0x2a
#define L3GD20H_REG_OUT_Y_H      0x2b
#define L3GD20H_REG_OUT_Z_L      0x2c
#define L3GD20H_REG_OUT_Z_H      0x2d
#define L3GD20H_REG_FIFO_CTRL    0x2e
#define L3GD20H_REG_FIFO_SRC     0x2f
#define L3GD20H_REG_IG_CFG       0x30
#define L3GD20H_REG_IG_SRC       0x31
#define L3GD20H_REG_IG_THS_XH    0x32
#define L3GD20H_REG_IG_THS_XL    0x33
#define L3GD20H_REG_IG_THS_YH    0x34
#define L3GD20H_REG_IG_THS_YL    0x35
#define L3GD20H_REG_IG_THS_ZH    0x36
#define L3GD20H_REG_IG_THS_ZL    0x37
#define L3GD20H_REG_IG_DURATION  0x38
#define L3GD20H_REG_LOW_ODR      0x39

// register structure definitions
#define L3GD20H_ZYXOR             0x80    // L3GD20H_REG_STATUS<7>
#define L3GD20H_ZOR               0x40    // L3GD20H_REG_STATUS<6>
#define L3GD20H_YOR               0x20    // L3GD20H_REG_STATUS<5>
#define L3GD20H_XOR               0x10    // L3GD20H_REG_STATUS<4>
#define L3GD20H_ZYXDA             0x08    // L3GD20H_REG_STATUS<3>
#define L3GD20H_ZDA               0x04    // L3GD20H_REG_STATUS<2>
#define L3GD20H_YDA               0x02    // L3GD20H_REG_STATUS<1>
#define L3GD20H_XDA               0x01    // L3GD20H_REG_STATUS<0>

#define L3GD20H_ANY_DATA_READY    0x07    // L3GD20H_REG_STATUS<2:0>

#define L3GD20H_ODR               0xc0    // L3GD20H_REG_CTRL1<7:6>
#define L3GD20H_BW                0x30    // L3GD20H_REG_CTRL1<5:4>
#define L3GD20H_POWER_MODE        0x08    // L3GD20H_REG_CTRL1<3>
#define L3GD20H_Z_ENABLED         0x04    // L3GD20H_REG_CTRL1<2>
#define L3GD20H_Y_ENABLED         0x02    // L3GD20H_REG_CTRL1<1>
#define L3GD20H_X_ENABLED         0x01    // L3GD20H_REG_CTRL1<0>

#define L3GD20H_HPF_MODE          0x30    // L3GD20H_REG_CTRL2<5:4>
#define L3GD20H_HPF_CUTOFF        0x0f    // L3GD20H_REG_CTRL2<3:0>

#define L3GD20H_INT1_IG           0x80    // L3GD20H_REG_CTRL3<7>
#define L3GD20H_INT1_BOOT         0x40    // L3GD20H_REG_CTRL3<6>
#define L3GD20H_HL_ACTIVE         0x20    // L3GD20H_REG_CTRL3<5>
#define L3GD20H_PP_OD             0x10    // L3GD20H_REG_CTRL3<4>
#define L3GD20H_INT2_DRDY         0x08    // L3GD20H_REG_CTRL3<3>
#define L3GD20H_INT2_FTH          0x04    // L3GD20H_REG_CTRL3<2>
#define L3GD20H_INT2_ORUN         0x02    // L3GD20H_REG_CTRL3<1>
#define L3GD20H_INT2_EMPTY        0x01    // L3GD20H_REG_CTRL3<0>

#define L3GD20H_BLOCK_DATA_UPDATE 0x80    // L3GD20H_REG_CTRL4<7>
#define L3GD20H_BIG_LITTLE_ENDIAN 0x40    // L3GD20H_REG_CTRL4<6>
#define L3GD20H_FULL_SCALE        0x30    // L3GD20H_REG_CTRL4<5:4>

#define L3GD20H_BOOT              0x80    // L3GD20H_REG_CTRL5<7>
#define L3GD20H_FIFO_EN           0x40    // L3GD20H_REG_CTRL5<6>
#define L3GD20H_STOP_ON_FTH       0x20    // L3GD20H_REG_CTRL5<5>
#define L3GD20H_HP_ENABLED        0x10    // L3GD20H_REG_CTRL5<4>
#define L3GD20H_IG_SEL            0x0c    // L3GD20H_REG_CTRL5<3:2>
#define L3GD20H_OUT_SEL           0x03    // L3GD20H_REG_CTRL5<1:0>

#define L3GD20H_FIFO_MODE         0xe0    // L3GD20H_REG_FIFO_CTRL<7:5>
#define L3GD20H_FIFO_THRESH       0x1f    // L3GD20H_REG_FIFO_CTRL<4:0>

#define L3GD20H_FIFO_THS          0x80    // L3GD20H_REG_FIFO_SRC<7>
#define L3GD20H_FIFO_OVR          0x40    // L3GD20H_REG_FIFO_SRC<6>
#define L3GD20H_FIFO_EMPTY        0x20    // L3GD20H_REG_FIFO_SRC<5>
#define L3GD20H_FIFO_FFS          0x1f    // L3GD20H_REG_FIFO_SRC<4:0>

#define L3GD20H_INT1_AND_OR       0x80    // L3GD20H_REG_IG_CFG<7>
#define L3GD20H_INT1_LATCH        0x40    // L3GD20H_REG_IG_CFG<6>
#define L3GD20H_INT1_Z_HIGH       0x20    // L3GD20H_REG_IG_CFG<5>, L3GD20H_REG_IG_SRC<5>
#define L3GD20H_INT1_Z_LOW        0x10    // L3GD20H_REG_IG_CFG<4>, L3GD20H_REG_IG_SRC<4>
#define L3GD20H_INT1_Y_HIGH       0x08    // L3GD20H_REG_IG_CFG<3>, L3GD20H_REG_IG_SRC<3>
#define L3GD20H_INT1_Y_LOW        0x04    // L3GD20H_REG_IG_CFG<2>, L3GD20H_REG_IG_SRC<2>
#define L3GD20H_INT1_X_HIGH       0x02    // L3GD20H_REG_IG_CFG<1>, L3GD20H_REG_IG_SRC<1>
#define L3GD20H_INT1_X_LOW        0x01    // L3GD20H_REG_IG_CFG<0>, L3GD20H_REG_IG_SRC<0>

#define L3GD20H_INT1_ACTIVE       0x40    // L3GD20H_REG_IG_SRC<7>

#define L3GD20H_INT1_WAIT         0x80    // L3GD20H_REG_IG_DURATION<7>
#define L3GD20H_INT1_DURATION     0x3f    // L3GD20H_REG_IG_DURATION<6:0>

#define L3GD20H_DRDY_HL           0x20    // L3GD20H_REG_LOW_ODR<5>
#define L3GD20H_SW_RESET          0x04    // L3GD20H_REG_LOW_ODR<2>
#define L3GD20H_LOW_ODR           0x01    // L3GD20H_REG_LOW_ODR<0>

/** Forward declaration of functions for internal use */

static bool    l3gd20h_reset       (l3gd20h_sensor_t* dev);
static bool    l3gd20h_is_available(l3gd20h_sensor_t* dev);

static bool    l3gd20h_update_reg  (l3gd20h_sensor_t* dev, uint8_t reg, uint8_t mask, uint8_t  val);

static uint8_t l3gd20h_get_reg_bit (uint8_t  byte, uint8_t mask);
static void    l3gd20h_set_reg_bit (uint8_t* byte, uint8_t mask, uint8_t bit);

static bool    l3gd20h_i2c_read    (l3gd20h_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len);
static bool    l3gd20h_i2c_write   (l3gd20h_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len);
static bool    l3gd20h_spi_read    (l3gd20h_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len);
static bool    l3gd20h_spi_write   (l3gd20h_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len);

#define msb_lsb_to_type(t,b,o) (t)(((t)b[o] << 8) | b[o+1])
#define lsb_msb_to_type(t,b,o) (t)(((t)b[o+1] << 8) | b[o])
#define lsb_to_type(t,b,o)     (t)(b[o])

l3gd20h_sensor_t* l3gd20h_init_sensor (uint8_t bus, uint8_t addr, uint8_t cs)
{
    l3gd20h_sensor_t* dev;

    if ((dev = malloc (sizeof(l3gd20h_sensor_t))) == NULL)
        return NULL;

    // init sensor data structure
    dev->bus    = bus;
    dev->addr   = addr;
    dev->cs     = cs;

    dev->error_code = L3GD20H_OK;
    dev->scale      = l3gd20h_scale_245_dps;
    dev->fifo_mode  = l3gd20h_bypass;
    
    // if addr==0 then SPI is used and has to be initialized
    if (!addr && !spi_device_init (bus, cs))
    {
        error_dev ("Could not initialize SPI interface.", __FUNCTION__, dev);
        free (dev);
        return NULL;
    }

    // check availability of the sensor
    if (!l3gd20h_is_available (dev))
    {
        error_dev ("Sensor is not available.", __FUNCTION__, dev);
        free (dev);
        return NULL;
    }

    // reset the sensor
    if (!l3gd20h_reset(dev))
    {
        error_dev ("Could not reset the sensor device.", __FUNCTION__, dev);
        free (dev);
        return NULL;
    }
    
    l3gd20h_update_reg (dev, L3GD20H_REG_CTRL4, L3GD20H_FULL_SCALE, l3gd20h_scale_245_dps);
    l3gd20h_update_reg (dev, L3GD20H_REG_CTRL4, L3GD20H_BLOCK_DATA_UPDATE, 1);

    return dev;
}


bool l3gd20h_set_mode (l3gd20h_sensor_t* dev, l3gd20h_mode_t mode, uint8_t bw,
                       bool x, bool y, bool z)
{
    if (!dev) return false;

    if (bw > 3)
    {
        error_dev ("Bandwidth value %d not in range 0 ... 3", __FUNCTION__, dev, bw);
        dev->error_code = L3GD20H_WRONG_BANDWIDTH;
        return false;
    }
    
    if (dev->mode != l3gd20h && mode != l3gd20h_power_down && mode < l3gd20h_normal_odr_100)
    {
        error_dev ("Low ODRs are not available for this sensor", __FUNCTION__, dev);
        dev->error_code = L3GD20H_ODR_NOT_AVAILABLE;
        return false;
    }
    
    dev->error_code = L3GD20H_OK;
    
    uint8_t reg1 = 0;
    uint8_t reg2 = 0;

    if (mode != l3gd20h_power_down)
    {
        // read current register values
        if (!l3gd20h_reg_read (dev, L3GD20H_REG_CTRL1, &reg1, 1) ||
            !l3gd20h_reg_read (dev, L3GD20H_REG_LOW_ODR, &reg2, 1))
            return false;
   
        // if sensor is in power mode it takes at least 100 ms to start in another mode
        if (!l3gd20h_get_reg_bit (reg1, L3GD20H_POWER_MODE))
            vTaskDelay (200/portTICK_PERIOD_MS);
            
        if (mode >= l3gd20h_normal_odr_100)
        {
            // high output data rate
            l3gd20h_set_reg_bit (&reg2, L3GD20H_LOW_ODR, 0);
            l3gd20h_set_reg_bit (&reg1, L3GD20H_ODR, mode - l3gd20h_normal_odr_100);
        }
        else
        {
            // low output data rate
            l3gd20h_set_reg_bit (&reg2, L3GD20H_LOW_ODR, 1);
            l3gd20h_set_reg_bit (&reg1, L3GD20H_ODR, mode - l3gd20h_normal_odr_12_5);
        }
    
        l3gd20h_set_reg_bit (&reg1, L3GD20H_POWER_MODE, 1);
        l3gd20h_set_reg_bit (&reg1, L3GD20H_BW, bw);
        l3gd20h_set_reg_bit (&reg1, L3GD20H_X_ENABLED, x);
        l3gd20h_set_reg_bit (&reg1, L3GD20H_Y_ENABLED, y);
        l3gd20h_set_reg_bit (&reg1, L3GD20H_Z_ENABLED, z);
        
        if (dev->mode == l3gd20h && 
            !l3gd20h_reg_write (dev, L3GD20H_REG_LOW_ODR, &reg2, 1))
            return false;
    } 
    else
        l3gd20h_set_reg_bit (&reg1, L3GD20H_POWER_MODE, 0);

    if (!l3gd20h_reg_write (dev, L3GD20H_REG_CTRL1, &reg1, 1))
        return false;
        
    return true;
}


bool l3gd20h_set_scale (l3gd20h_sensor_t* dev, l3gd20h_scale_t scale)
{
    if (!dev) return false;
    
    dev->error_code = L3GD20H_OK;
    dev->scale = scale;
    
    // read CTRL4 register and write scale
    return l3gd20h_update_reg (dev, L3GD20H_REG_CTRL4, L3GD20H_FULL_SCALE, scale);
}


bool l3gd20h_set_fifo_mode (l3gd20h_sensor_t* dev, l3gd20h_fifo_mode_t mode,
                            uint8_t thresh)
{
    if (!dev) return false;
    
    if (dev->mode != l3gd20h && mode > l3gd20h_bypass_to_stream)
    {
        error_dev ("FIFO mode is not available for this sensor", __FUNCTION__, dev);
        dev->error_code = L3GD20H_FIFO_MODE_NOT_AVAILABLE;
        return false;
    }
        
    dev->error_code = L3GD20H_OK;
    dev->fifo_mode = mode;
    
    // read CTRL5 register and write FIFO_EN flag
    if (!l3gd20h_update_reg (dev, L3GD20H_REG_CTRL5, L3GD20H_FIFO_EN, (mode != l3gd20h_bypass)))
        return false;

    // read FIFO_CTRL register and write FIFO mode
    if (!l3gd20h_update_reg (dev, L3GD20H_REG_FIFO_CTRL, L3GD20H_FIFO_THRESH, thresh) ||
        !l3gd20h_update_reg (dev, L3GD20H_REG_FIFO_CTRL, L3GD20H_FIFO_MODE, mode))
        return false;

    return true;
}


bool l3gd20h_select_output_filter (l3gd20h_sensor_t* dev,
                                   l3gd20h_filter_t filter)
{
    if (!dev) return 0;

    dev->error_code = L3GD20H_OK;

    if (// try to set the register OUT_SEL in any case
        !l3gd20h_update_reg (dev, L3GD20H_REG_CTRL5, L3GD20H_OUT_SEL, filter) ||
        // try to set HPen in case LPF2 and HPF is used
        (filter == l3gd20h_hpf_and_lpf2 &&
         !l3gd20h_update_reg (dev, L3GD20H_REG_CTRL5, L3GD20H_HP_ENABLED, 1)))
    {   
        error_dev ("Could not select filters for output data", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_SEL_OUT_FILTER_FAILED;
        return false;
    }

    return true;
}


bool l3gd20h_new_data (l3gd20h_sensor_t* dev)
{
    if (!dev) return false;

    dev->error_code = L3GD20H_OK;

    uint8_t reg;

    if (dev->fifo_mode == l3gd20h_bypass)
    {
        if (!l3gd20h_reg_read (dev, L3GD20H_REG_STATUS, &reg, 1))
        {
            error_dev ("Could not get sensor status", __FUNCTION__, dev);
            return false;
        }
        return l3gd20h_get_reg_bit (reg, L3GD20H_ANY_DATA_READY);
    }
    else
    {
        if (!l3gd20h_reg_read (dev, L3GD20H_REG_FIFO_SRC, &reg, 1))
        {
            error_dev ("Could not get fifo source register data", __FUNCTION__, dev);
            return false;
        }
        return l3gd20h_get_reg_bit (reg, L3GD20H_FIFO_FFS);
    }
}

// scale factors for conversion of raw sensor data to degree for possible
// sensitivities according to mechanical characteristics in datasheet
const static float L3GD20H_SCALES[3] = { (0.00875F), (0.0175F), (0.070F) };


bool l3gd20h_get_float_data (l3gd20h_sensor_t* dev, l3gd20h_float_data_t* data)
{
    if (!dev || !data) return false;

    l3gd20h_raw_data_t raw;
    
    if (!l3gd20h_get_raw_data (dev, &raw))
        return false;
        
    data->x = raw.x * L3GD20H_SCALES[dev->scale];
    data->y = raw.y * L3GD20H_SCALES[dev->scale];
    data->z = raw.z * L3GD20H_SCALES[dev->scale];
    
    return true;
}


uint8_t l3gd20h_get_float_data_fifo (l3gd20h_sensor_t* dev, l3gd20h_float_data_fifo_t data)
{
    if (!dev) return 0;

    l3gd20h_raw_data_fifo_t raw;
    
    uint8_t num = l3gd20h_get_raw_data_fifo (dev, raw);

    for (int i = 0; i < num; i++)
    {
        data[i].x = raw[i].x * L3GD20H_SCALES[dev->scale];
        data[i].y = raw[i].y * L3GD20H_SCALES[dev->scale];
        data[i].z = raw[i].z * L3GD20H_SCALES[dev->scale];
    }

    return num;
}


bool l3gd20h_get_raw_data (l3gd20h_sensor_t* dev, l3gd20h_raw_data_t* raw)
{
    if (!dev || !raw) return false;

    dev->error_code = L3GD20H_OK;

    // abort if not in bypass mode
    if (dev->fifo_mode != l3gd20h_bypass)
    {
        dev->error_code = L3GD20H_SENSOR_IN_BYPASS_MODE;
        error_dev ("Sensor is in FIFO mode, use l3gd20h_get_*_data_fifo to get data",
                   __FUNCTION__, dev);
        return false;
    }

    // read raw data sample
    if (!l3gd20h_reg_read (dev, L3GD20H_REG_OUT_X_L, (uint8_t*)raw, 6))
    {
        error_dev ("Could not get raw data", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_GET_RAW_DATA_FAILED;
        return false;
    }

    return true;
}

uint8_t l3gd20h_get_raw_data_fifo (l3gd20h_sensor_t* dev, l3gd20h_raw_data_fifo_t raw)
{
    if (!dev) return 0;

    dev->error_code = L3GD20H_OK;

    // in bypass mode, use lis3dh_get_raw_data to return one sample
    if (dev->fifo_mode == l3gd20h_bypass)
        return l3gd20h_get_raw_data (dev, raw) ? 1 : 0;

    uint8_t reg;

    // read FIFO state
    if (!l3gd20h_reg_read (dev, L3GD20H_REG_FIFO_SRC, &reg, 1))
    {
        error_dev ("Could not get fifo source register data", __FUNCTION__, dev);
        return 0;
    }

    // if nothing is in the FIFO, just return with 0
    if (reg & L3GD20H_FIFO_EMPTY)
        return 0;

    // read samples from FIFO
    uint8_t samples = (reg & L3GD20H_FIFO_FFS) + (reg & L3GD20H_FIFO_OVR ? 1 : 0);

    // read samples from FIFO
    for (int i = 0; i < samples; i++)
        if (!l3gd20h_reg_read (dev, L3GD20H_REG_OUT_X_L, (uint8_t*)&raw[i], 6))
        {
            error_dev ("Could not get raw data", __FUNCTION__, dev);
            dev->error_code |= L3GD20H_GET_RAW_DATA_FIFO_FAILED;
            return i;
        }

    l3gd20h_reg_read (dev, L3GD20H_REG_FIFO_SRC, &reg, 1);

    if (reg & L3GD20H_FIFO_FFS)
    {
        dev->error_code = L3GD20H_ODR_TOO_HIGH;
        error_dev ("New samples stored in FIFO while reading, "
                   "output data rate (ODR) too high", __FUNCTION__, dev);
    }

    if (dev->fifo_mode == l3gd20h_fifo && samples == 32)
    {
        // clean FIFO (see app note)
        l3gd20h_update_reg (dev, L3GD20H_REG_FIFO_CTRL, L3GD20H_FIFO_MODE, l3gd20h_bypass);
        l3gd20h_update_reg (dev, L3GD20H_REG_FIFO_CTRL, L3GD20H_FIFO_MODE, l3gd20h_fifo);
    }

    return samples;
}


bool l3gd20h_enable_int (l3gd20h_sensor_t* dev,
                         l3gd20h_int_types_t type, bool value)
{
    if (!dev) return false;

    dev->error_code = L3GD20H_OK;

    uint8_t mask;
    
    switch (type)
    {
        case l3gd20h_int_data_ready:     mask  = L3GD20H_INT2_DRDY;  break;
        case l3gd20h_int_fifo_threshold: mask  = L3GD20H_INT2_FTH;   break;
        case l3gd20h_int_fifo_overrun:   mask  = L3GD20H_INT2_ORUN;  break;
        case l3gd20h_int_fifo_empty:     mask  = L3GD20H_INT2_EMPTY; break;
        case l3gd20h_int_event:          mask  = L3GD20H_INT1_IG;    break;
        default: dev->error_code = L3GD20H_WRONG_INT_TYPE; 
                 error_dev ("Wrong interrupt type", __FUNCTION__, dev);
                 return false;
    }        
        
    if (!l3gd20h_update_reg (dev, L3GD20H_REG_CTRL3, mask, value))
    {
        error_dev ("Could not %s interrupt INT2", __FUNCTION__, dev, value ? "enable" : "disable");
        dev->error_code |= L3GD20H_CONFIG_INT2_FAILED;
        return false;
    }
    
    return true;
}


bool l3gd20h_set_int_event_config (l3gd20h_sensor_t* dev, 
                                   l3gd20h_int_event_config_t* config)
{
    if (!dev || !config) return false;

    dev->error_code = L3GD20H_OK;

    uint8_t ig_cfg = 0;
    uint8_t ig_dur = 0;
    uint8_t ig_ths[6] = { 0 };
    
    l3gd20h_set_reg_bit (&ig_cfg, L3GD20H_INT1_X_LOW , config->x_low_enabled);
    l3gd20h_set_reg_bit (&ig_cfg, L3GD20H_INT1_X_HIGH, config->x_high_enabled);
    
    l3gd20h_set_reg_bit (&ig_cfg, L3GD20H_INT1_Y_LOW , config->y_low_enabled);
    l3gd20h_set_reg_bit (&ig_cfg, L3GD20H_INT1_Y_HIGH, config->y_high_enabled);

    l3gd20h_set_reg_bit (&ig_cfg, L3GD20H_INT1_Z_LOW , config->z_low_enabled);
    l3gd20h_set_reg_bit (&ig_cfg, L3GD20H_INT1_Z_HIGH, config->z_high_enabled);
    
    l3gd20h_set_reg_bit (&ig_cfg, L3GD20H_INT1_LATCH , config->latch);
    l3gd20h_set_reg_bit (&ig_cfg, L3GD20H_INT1_AND_OR, config->and_or);

    l3gd20h_set_reg_bit (&ig_dur, L3GD20H_INT1_WAIT    , config->wait);
    l3gd20h_set_reg_bit (&ig_dur, L3GD20H_INT1_DURATION, config->duration);

    ig_ths[0] = (config->x_threshold >> 8) & 0x7f;
    ig_ths[1] = (config->x_threshold & 0xff);
    ig_ths[2] = (config->y_threshold >> 8) & 0x7f;
    ig_ths[3] = (config->y_threshold & 0xff);
    ig_ths[4] = (config->z_threshold >> 8) & 0x7f;
    ig_ths[5] = (config->z_threshold & 0xff);

    if (// write the thresholds to registers IG_THS_*
        !l3gd20h_reg_write (dev, L3GD20H_REG_IG_THS_XH, ig_ths, 6) ||
        
        // write duration configuration to IG_DURATION 
        !l3gd20h_reg_write (dev, L3GD20H_REG_IG_DURATION, &ig_dur, 1) ||
        
        // write INT1 configuration  to IG_CFG
        !l3gd20h_reg_write (dev, L3GD20H_REG_IG_CFG, &ig_cfg, 1))
    {   
        error_dev ("Could not configure interrupt INT1", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_CONFIG_INT1_FAILED;
        return false;
    }


    if (// ouput value selection used for threshold comparison for INT1 generation
        !l3gd20h_update_reg (dev, L3GD20H_REG_CTRL5, L3GD20H_IG_SEL, config->filter) ||

        // try to set HPen in case LPF2 and HPF is used
        (config->filter == l3gd20h_hpf_and_lpf2 &&
         !l3gd20h_update_reg (dev, L3GD20H_REG_CTRL5, L3GD20H_HP_ENABLED, 1)))
    {   
        error_dev ("Could not configure interrupt INT1", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_CONFIG_INT1_FAILED;
        return false;
    }

    return true;
}


bool l3gd20h_get_int_event_config (l3gd20h_sensor_t* dev, 
                                   l3gd20h_int_event_config_t* config)
{
    if (!dev || !config) return false;

    dev->error_code = L3GD20H_OK;

    uint8_t ig_cfg;
    uint8_t ig_dur;
    uint8_t ig_ths[6];
    uint8_t ctrl3;
    uint8_t ctrl5;

    if (!l3gd20h_reg_read (dev, L3GD20H_REG_IG_THS_XH, ig_ths, 6) ||
        !l3gd20h_reg_read (dev, L3GD20H_REG_IG_CFG, &ig_cfg, 1) ||
        !l3gd20h_reg_read (dev, L3GD20H_REG_IG_DURATION, &ig_dur, 1) ||
        !l3gd20h_reg_read (dev, L3GD20H_REG_CTRL3, &ctrl3, 1) ||
        !l3gd20h_reg_read (dev, L3GD20H_REG_CTRL5, &ctrl5, 1))
    {   
        dev->error_code |= L3GD20H_CONFIG_INT1_FAILED;
        error_dev ("Could not read configuration for interrupt INT1 from sensor",
                   __FUNCTION__, dev);
        return false;
    }
    
    config->x_low_enabled   = l3gd20h_get_reg_bit (ig_cfg, L3GD20H_INT1_X_LOW);
    config->x_high_enabled  = l3gd20h_get_reg_bit (ig_cfg, L3GD20H_INT1_X_HIGH);
    
    config->y_low_enabled   = l3gd20h_get_reg_bit (ig_cfg, L3GD20H_INT1_Y_LOW);
    config->y_high_enabled  = l3gd20h_get_reg_bit (ig_cfg, L3GD20H_INT1_Y_HIGH);

    config->z_low_enabled   = l3gd20h_get_reg_bit (ig_cfg, L3GD20H_INT1_Z_LOW);
    config->z_high_enabled  = l3gd20h_get_reg_bit (ig_cfg, L3GD20H_INT1_Z_HIGH);
    
    config->x_threshold     = msb_lsb_to_type(uint16_t, ig_ths, 0);
    config->y_threshold     = msb_lsb_to_type(uint16_t, ig_ths, 2);
    config->z_threshold     = msb_lsb_to_type(uint16_t, ig_ths, 4);
    
    config->filter          = l3gd20h_get_reg_bit (ctrl5, L3GD20H_IG_SEL);
    
    config->and_or          = l3gd20h_get_reg_bit (ig_cfg, L3GD20H_INT1_AND_OR);
    config->latch           = l3gd20h_get_reg_bit (ig_cfg, L3GD20H_INT1_LATCH);
    
    config->wait            = l3gd20h_get_reg_bit (ig_dur, L3GD20H_INT1_WAIT);
    config->duration        = l3gd20h_get_reg_bit (ig_dur, L3GD20H_INT1_DURATION);
    
    config->counter_mode    = 0;

    return true;
}


bool l3gd20h_get_int_event_source (l3gd20h_sensor_t* dev, l3gd20h_int_event_source_t* source)
{
    if (!dev || !source) return false;

    dev->error_code = L3GD20H_OK;

    l3gd20h_int_event_source_t ig_cfg;
    l3gd20h_int_event_source_t ig_src;

    if (!l3gd20h_reg_read (dev, L3GD20H_REG_IG_CFG, (uint8_t*)&ig_cfg, 1) ||
        !l3gd20h_reg_read (dev, L3GD20H_REG_IG_SRC, (uint8_t*)&ig_src, 1))
    {   
        error_dev ("Could not read source of interrupt INT1 from sensor", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_INT1_SOURCE_FAILED;
        return false;
    }

    source->active = ig_src.active;
    source->x_low  = ig_src.x_low  & ig_cfg.x_low;
    source->x_high = ig_src.x_high & ig_cfg.x_high;
    source->y_low  = ig_src.y_low  & ig_cfg.y_low;
    source->y_high = ig_src.y_high & ig_cfg.y_high;
    source->z_low  = ig_src.z_low  & ig_cfg.z_low;
    source->z_high = ig_src.z_high & ig_cfg.z_high;
    
    return true;
}


bool l3gd20h_get_int_data_source (l3gd20h_sensor_t* dev, l3gd20h_int_data_source_t* source)
{
    if (!dev || !source) return false;

    dev->error_code = L3GD20H_OK;

    uint8_t fifo_src;
    uint8_t status;

    if (!l3gd20h_reg_read (dev, L3GD20H_REG_STATUS, &status, 1) ||
        !l3gd20h_reg_read (dev, L3GD20H_REG_FIFO_SRC, &fifo_src, 1))
    {   
        error_dev ("Could not read source of interrupt INT2 from sensor", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_INT2_SOURCE_FAILED;
        return false;
    }

    source->data_ready     = l3gd20h_get_reg_bit (status, L3GD20H_ANY_DATA_READY);
    source->fifo_threshold = l3gd20h_get_reg_bit (fifo_src, L3GD20H_FIFO_THS);
    source->fifo_overrun   = l3gd20h_get_reg_bit (fifo_src, L3GD20H_FIFO_OVR);
    source->fifo_empty     = l3gd20h_get_reg_bit (fifo_src, L3GD20H_FIFO_EMPTY);

    return true;
}


bool l3gd20h_config_int_signals (l3gd20h_sensor_t* dev,
                                 l3gd20h_signal_type_t type,
                                 l3gd20h_signal_level_t level)
{
    if (!dev) return false;

    dev->error_code = L3GD20H_OK;

    if (!l3gd20h_update_reg (dev, L3GD20H_REG_CTRL3, L3GD20H_HL_ACTIVE, level) ||
        !l3gd20h_update_reg (dev, L3GD20H_REG_CTRL3, L3GD20H_PP_OD, type) ||
        !l3gd20h_update_reg (dev, L3GD20H_REG_LOW_ODR, L3GD20H_DRDY_HL, level))
    {   
        error_dev ("Could not configure interrupt signals", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_CONFIG_INT_SIGNALS_FAILED;
        return false;
    }

    return true;
}


bool l3gd20h_config_hpf (l3gd20h_sensor_t* dev, l3gd20h_hpf_mode_t mode, 
                         uint8_t cutoff)
{
    if (!dev) return false;

    dev->error_code = L3GD20H_OK;

    if (!l3gd20h_update_reg (dev, L3GD20H_REG_CTRL2, L3GD20H_HPF_MODE, mode) ||
        !l3gd20h_update_reg (dev, L3GD20H_REG_CTRL2, L3GD20H_HPF_CUTOFF, cutoff))
    {   
        error_dev ("Could not configure high pass filter", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_CONFIG_HPF_FAILED;
        return false;
    }

    return true;
}


bool l3gd20h_set_hpf_ref (l3gd20h_sensor_t* dev, int8_t ref)
{
    if (!dev) return false;

    dev->error_code = L3GD20H_OK;

    if (!l3gd20h_reg_write (dev, L3GD20H_REG_REFERENCE, (uint8_t*)&ref, 1))
    {   
        error_dev ("Could not set high pass filter reference", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_CONFIG_HPF_FAILED;
        return false;
    }

    return true;
}

int8_t l3gd20h_get_hpf_ref (l3gd20h_sensor_t* dev)
{
    if (!dev) return 0;

    dev->error_code = L3GD20H_OK;

    int8_t ref = 0;
    
    if (!l3gd20h_reg_read (dev, L3GD20H_REG_REFERENCE, (uint8_t*)&ref, 1))
    {   
        error_dev ("Could not get high pass filter reference", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_CONFIG_HPF_FAILED;
        return 0;
    }

    return ref;
}


int8_t l3gd20h_get_temperature (l3gd20h_sensor_t* dev)
{
    if (!dev) return 0;

    dev->error_code = L3GD20H_OK;

    int8_t reg;
    
    if (!l3gd20h_reg_read (dev, L3GD20H_REG_OUT_TEMP, (uint8_t*)(&reg), 1))
    {   
        error_dev ("Could not get temperature", __FUNCTION__, dev);
        return false;
    }
    
    return reg;
}


/** Functions for internal use only */

/**
 * @brief   Check the chip ID to test whether sensor is available
 */
static bool l3gd20h_is_available (l3gd20h_sensor_t* dev)
{
    uint8_t chip_id;

    if (!dev) return false;

    dev->error_code = L3GD20H_OK;

    if (!l3gd20h_reg_read (dev, L3GD20H_REG_WHO_AM_I, &chip_id, 1))
        return false;

    switch (chip_id)
    {
        case L3GD20H_CHIP_ID:  dev->mode = l3gd20h ; break;
        case L3GD20_CHIP_ID:   dev->mode = l3gd20  ; break;
        case L3G4200D_CHIP_ID: dev->mode = l3g4200d; break;
        default: error_dev ("Chip id %02x is wrong, should be %02x.",
                            __FUNCTION__, dev, chip_id, L3GD20H_CHIP_ID);
                 dev->error_code = L3GD20H_WRONG_CHIP_ID;
                 return false;
    }

    return true;
}


static bool l3gd20h_reset (l3gd20h_sensor_t* dev)
{
    if (!dev) return false;

    dev->error_code = L3GD20H_OK;

    if (!l3gd20h_update_reg (dev, L3GD20H_REG_LOW_ODR, L3GD20H_SW_RESET, 1))
        return false;
        
    vTaskDelay(100/portTICK_PERIOD_MS);

    uint8_t reg[6] = { 0 };
    
    // initialize sensor completely including setting in power down mode
    l3gd20h_reg_write (dev, L3GD20H_REG_CTRL1    , reg, 6);
    l3gd20h_reg_write (dev, L3GD20H_REG_FIFO_CTRL, reg, 1);
    l3gd20h_reg_write (dev, L3GD20H_REG_IG_CFG   , reg, 1);
    l3gd20h_reg_write (dev, L3GD20H_REG_IG_THS_XH, reg, 6);
    
    return true;
}


static bool l3gd20h_update_reg(l3gd20h_sensor_t* dev, uint8_t reg, uint8_t mask, uint8_t val)
{
    if (!dev) return false;

    uint8_t reg_val;
    uint8_t shift = 0;
    
    while (!((mask >> shift) & 0x01)) shift++;

    // read current register value
    if (!l3gd20h_reg_read (dev, reg, &reg_val, 1))
        return false;

    // set masked bits to the given value 
    reg_val = (reg_val & ~mask) | ((val << shift) & mask);

    // write back new register value
    if (!l3gd20h_reg_write (dev, reg, &reg_val, 1))
        return false;
        
    return true;
}

bool l3gd20h_reg_read(l3gd20h_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len)
{
    if (!dev || !data) return false;

    return (dev->addr) ? l3gd20h_i2c_read (dev, reg, data, len)
                       : l3gd20h_spi_read (dev, reg, data, len);
}


bool l3gd20h_reg_write(l3gd20h_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len)
{
    if (!dev || !data) return false;

    return (dev->addr) ? l3gd20h_i2c_write (dev, reg, data, len)
                       : l3gd20h_spi_write (dev, reg, data, len);
}


static void l3gd20h_set_reg_bit (uint8_t* byte, uint8_t mask, uint8_t bit)
{ 
    if (byte)
    {
        uint8_t shift = 0;
        while (!((mask >> shift) & 0x01)) shift++;
        *byte = ((*byte & ~mask) | ((bit << shift) & mask));
    }
}


static uint8_t l3gd20h_get_reg_bit(uint8_t byte, uint8_t mask)
{ 
    uint8_t shift = 0;
    while (!((mask >> shift) & 0x01)) shift++;
    return (byte & mask) >> shift;
}


#define L3GD20H_SPI_BUF_SIZE 64      // SPI register data buffer size

#define L3GD20H_SPI_READ_FLAG      0x80
#define L3GD20H_SPI_WRITE_FLAG     0x00
#define L3GD20H_SPI_AUTO_INC_FLAG  0x40

static bool l3gd20h_spi_read(l3gd20h_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len)
{
    if (!dev || !data) return false;

    if (len >= L3GD20H_SPI_BUF_SIZE)
    {
        dev->error_code |= L3GD20H_SPI_BUFFER_OVERFLOW;
        error_dev ("Error on read from SPI slave on bus 1. Tried to transfer "
                   "more than %d byte in one read operation.",
                   __FUNCTION__, dev, L3GD20H_SPI_BUF_SIZE);
        return false;
    }

    uint8_t addr = (reg & 0x3f) | L3GD20H_SPI_READ_FLAG | L3GD20H_SPI_AUTO_INC_FLAG;
    
    static uint8_t mosi[L3GD20H_SPI_BUF_SIZE];
    static uint8_t miso[L3GD20H_SPI_BUF_SIZE];

    memset (mosi, 0xff, L3GD20H_SPI_BUF_SIZE);
    memset (miso, 0xff, L3GD20H_SPI_BUF_SIZE);

    mosi[0] = addr;
    
    if (!spi_transfer_pf (dev->bus, dev->cs, mosi, miso, len+1))
    {
        error_dev ("Could not read data from SPI", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_SPI_READ_FAILED;
        return false;
    }
    
    // shift data one by left, first byte received while sending register address is invalid
    for (int i=0; i < len; i++)
      data[i] = miso[i+1];

    #ifdef L3GD20H_DEBUG_LEVEL_2
    printf("L3GD20H %s: read the following bytes from reg %02x: ", __FUNCTION__, reg);
    for (int i=0; i < len; i++)
        printf("%02x ", data[i]);
    printf("\n");
    #endif

    return true;
}


static bool l3gd20h_spi_write(l3gd20h_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len)
{
    if (!dev || !data) return false;

    uint8_t addr = (reg & 0x3f) | L3GD20H_SPI_WRITE_FLAG | L3GD20H_SPI_AUTO_INC_FLAG;

    static uint8_t mosi[L3GD20H_SPI_BUF_SIZE];

    if (len >= L3GD20H_SPI_BUF_SIZE)
    {
        dev->error_code |= L3GD20H_SPI_BUFFER_OVERFLOW;
        error_dev ("Error on write to SPI slave on bus 1. Tried to transfer more"
                   "than %d byte in one write operation.", __FUNCTION__, dev, L3GD20H_SPI_BUF_SIZE);

        return false;
    }

    reg &= 0x7f;

    // first byte in output is the register address
    mosi[0] = addr;

    // shift data one byte right, first byte in output is the register address
    for (int i = 0; i < len; i++)
        mosi[i+1] = data[i];

    #ifdef L3GD20H_DEBUG_LEVEL_2
    printf("L3GD20H %s: Write the following bytes to reg %02x: ", __FUNCTION__, reg);
    for (int i = 1; i < len+1; i++)
        printf("%02x ", mosi[i]);
    printf("\n");
    #endif

    if (!spi_transfer_pf (dev->bus, dev->cs, mosi, NULL, len+1))
    {
        error_dev ("Could not write data to SPI.", __FUNCTION__, dev);
        dev->error_code |= L3GD20H_SPI_WRITE_FAILED;
        return false;
    }

    return true;
}


#define I2C_AUTO_INCREMENT    (0x80)

static bool l3gd20h_i2c_read(l3gd20h_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len)
{
    if (!dev || !data) return false;

    debug_dev ("Read %d byte from i2c slave register %02x.", __FUNCTION__, dev, len, reg);

    if (len > 1)
        reg |= I2C_AUTO_INCREMENT;
    
    int result = i2c_slave_read(dev->bus, dev->addr, &reg, data, len);

    if (result)
    {
        dev->error_code |= (result == -EBUSY) ? L3GD20H_I2C_BUSY : L3GD20H_I2C_READ_FAILED;
        error_dev ("Error %d on read %d byte from I2C slave register %02x.",
                    __FUNCTION__, dev, result, len, reg);
        return false;
    }

#   ifdef L3GD20H_DEBUG_LEVEL_2
    printf("L3GD20H %s: Read following bytes: ", __FUNCTION__);
    printf("%02x: ", reg & 0x7f);
    for (int i=0; i < len; i++)
        printf("%02x ", data[i]);
    printf("\n");
#   endif

    return true;
}


static bool l3gd20h_i2c_write(l3gd20h_sensor_t* dev, uint8_t reg, uint8_t *data, uint16_t len)
{
    if (!dev || !data) return false;

    debug_dev ("Write %d byte to i2c slave register %02x.", __FUNCTION__, dev, len, reg);

    if (len > 1)
        reg |= I2C_AUTO_INCREMENT;
    
    int result = i2c_slave_write(dev->bus, dev->addr, &reg, data, len);

    if (result)
    {
        dev->error_code |= (result == -EBUSY) ? L3GD20H_I2C_BUSY : L3GD20H_I2C_WRITE_FAILED;
        error_dev ("Error %d on write %d byte to i2c slave register %02x.",
                    __FUNCTION__, dev, result, len, reg);
        return false;
    }

#   ifdef L3GD20H_DEBUG_LEVEL_2
    printf("L3GD20H %s: Wrote the following bytes: ", __FUNCTION__);
    printf("%02x: ", reg);
    for (int i=0; i < len; i++)
        printf("%02x ", data[i]);
    printf("\n");
#   endif

    return true;
}