/******************************************************************************
 * 2016 ideasX (Tyler Berezowsky and Marc Jurchak)
 *
 * FileName: LSM6DS3.c
 *
 * Description: SPIdriver for LSM6DS3
 * Modification history:
 *     2015/10/12, v1.0 created this file (Marc)
 *     2016/3/15,  v1.1 added comments for Tyler.
*******************************************************************************/
#include "hal/lsm6ds3.h"
#include "log/esp_log.h"

static const char* TAG = "lsm6ds3.c";

static int32_t ICACHE_FLASH_ATTR Sensor_IO_Write(uint16_t WriteAddr, uint8_t nBytesToWrite, uint32_t *pBuffer)
{
    // Initalize HSPI Interface GPIO
    WRITE_PERI_REG(PERIPHS_IO_MUX, 0x105);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDI_U, 2);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTCK_U, 2);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTMS_U, 2);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDO_U, 2);

    // Setup SPI Attributes Struct
    SpiAttr spiConfig;
    spiConfig.mode =  SpiMode_Master;
    spiConfig.subMode = SpiSubMode_0;
    spiConfig.speed = SpiSpeed_8MHz;
    spiConfig.bitOrder = SpiBitOrder_MSBFirst;

    // Initalize SPI Interface
    SPIInit(SpiNum_HSPI, &spiConfig);

    // Setup SPI Data Struct
    SpiData spiData;
    uint32_t addr = (WriteAddr);

    spiData.cmd = addr;
    spiData.cmdLen = 1;
    spiData.addrLen = 0;
    spiData.addr = 0;
    spiData.data  = pBuffer;
    spiData.dataLen = nBytesToWrite;

    // Write Data to SPI port
    return SPIMasterSendData(SpiNum_HSPI, &spiData);
}

static int32_t ICACHE_FLASH_ATTR Sensor_IO_Read(uint16_t ReadAddr, uint8_t nBytesToRead, uint32_t *pBuffer)
{
    // Initalize HSPI Interface GPIO
    WRITE_PERI_REG(PERIPHS_IO_MUX, 0x105);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDI_U, 2);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTCK_U, 2);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTMS_U, 2);
    PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDO_U, 2);

    // Setup SPI Attributes Struct
    SpiAttr spiConfig;
    spiConfig.mode =  SpiMode_Master;
    spiConfig.subMode = SpiSubMode_0;
    spiConfig.speed = SpiSpeed_8MHz;
    spiConfig.bitOrder = SpiBitOrder_MSBFirst;

    // Initalize SPI Interface
    SPIInit(SpiNum_HSPI, &spiConfig);

    // Setup SPI Data Struct
    SpiData spiData;
    uint32_t addr = (ReadAddr | 0x80);

    spiData.cmd = addr;
    spiData.cmdLen = 1;
    spiData.addrLen = 0;
    spiData.addr = 0;
    spiData.data  = pBuffer;
    spiData.dataLen = nBytesToRead;

    return SPIMasterRecvData(SpiNum_HSPI, &spiData);
}

static int32_t ICACHE_FLASH_ATTR Sensor_IO_Set_Register(uint16_t WriteAddr, uint32_t v0)
{
    uint32_t v1;
    Sensor_IO_Write(WriteAddr, 1, &v0);
    Sensor_IO_Read(WriteAddr, 1, &v1);
    if (v0 == v1)
        return 0;
    else
        return -1;
}
/******************************************************************************
 * FunctionName : lsm6ds3_i2c_enable
 * Description  : Setup LSM6DS3 registers for I2C sensor hub.
 * Parameters   : uint8_t start
 				  1: enable
 				  0: disable
 * Returns      : uint8_t 1: success 0: failed
*******************************************************************************/
int32_t ICACHE_FLASH_ATTR
LSM6DS3_Enable_I2C_Pullups(uint8_t enable)
{
    uint32_t ctrl10_c, master_config;

    Sensor_IO_Read(LSM6DS3_CTRL10_C, 1,  &ctrl10_c);
    Sensor_IO_Read(LSM6DS3_MASTER_CONFIG, 1, &master_config);
    if (enable)
    {
        ctrl10_c |= LSM6DS3_FUNC_EN;
        //master_config |= (PULL_UP_EN + MASTER_ON);
        master_config |= LSM6DS3_PULL_UP_EN;	// pull-ups on
        master_config &= ~LSM6DS3_MASTER_ON;	// disable i2c master function?
    }
    else
    {
        ctrl10_c &= ~LSM6DS3_FUNC_EN;
        master_config &= ~(LSM6DS3_PULL_UP_EN + LSM6DS3_MASTER_ON);
    }
    return (Sensor_IO_Set_Register(LSM6DS3_CTRL10_C, ctrl10_c) & Sensor_IO_Set_Register(LSM6DS3_MASTER_CONFIG, master_config));
}


int32_t ICACHE_FLASH_ATTR
LSM6DS3_Enable_I2C_Bridge(uint8_t enable)
{
    LSM6DS3_Enable_I2C_Pullups(1);

    uint32_t master_config;
    Sensor_IO_Read(LSM6DS3_MASTER_CONFIG, 1, &master_config);
    if (enable)
    {
        master_config |= LSM6DS3_PASS_THROUGH_MODE;
    }
    else
    {
        master_config &= ~LSM6DS3_PASS_THROUGH_MODE;
    }
    ESP_LOGI(TAG, "Writing %x to LSM6DS3_MASTER_CONFIG", master_config);
    return Sensor_IO_Set_Register(LSM6DS3_MASTER_CONFIG, master_config);
}

void ICACHE_FLASH_ATTR LSM6DS3_Reset(void)
{
    Sensor_IO_Set_Register(LSM6DS3_CTRL3_C, BIT0);
    os_delay_us(30000);
}

void ICACHE_FLASH_ATTR LSM6DS3_EnableMotion(void)
{
    LSM6DS3_Reset();
	Sensor_IO_Set_Register(LSM6DS3_CTRL2_G, LSM6DS3_ODR_G_POWERDOWN);						     // disable gyro
	Sensor_IO_Set_Register(LSM6DS3_CTRL1_XL, (LSM6DS3_ODR_XL_13Hz + LSM6DS3_FS_XL_2G));		     // 208Hz 2g scale
    Sensor_IO_Set_Register(LSM6DS3_TAP_CFG, LSM6DS3_LIR); 									     // enable interuppt latch and slope filter
	Sensor_IO_Set_Register(LSM6DS3_WAKE_UP_THS, 0x01); 									         // set thres 32/64*2G = 1G
	Sensor_IO_Set_Register(LSM6DS3_WAKE_UP_DUR, 0x02); 									         // zero duration
	Sensor_IO_Set_Register(LSM6DS3_MD1_CFG, LSM6DS3_INT1_WU);								     // set interrupt for INTX (1 is connected to PBA)
	Sensor_IO_Set_Register(LSM6DS3_CTRL3_C, LSM6DS3_H_LACTIVE);                                  // set int lines active low

}

void ICACHE_FLASH_ATTR LSM6DS3_DisableMotion(void)
{
	LSM6DS3_Reset();
	Sensor_IO_Set_Register(LSM6DS3_CTRL2_G, LSM6DS3_ODR_G_POWERDOWN);      // disable gyro
	Sensor_IO_Set_Register(LSM6DS3_CTRL1_XL, LSM6DS3_ODR_XL_POWERDOWN);    // disable XL
	Sensor_IO_Set_Register(LSM6DS3_CTRL3_C, LSM6DS3_H_LACTIVE);
}

// determine if there has been a FIFO or significant motion interrupt
LSM6DS3_Interrupt_et ICACHE_FLASH_ATTR LSM6DS3_Get_Interrupts(void)
{
    uint32_t fifo_status2, wake_up_src;
    Sensor_IO_Read(LSM6DS3_FIFO_STATUS2, 1, &fifo_status2);
    Sensor_IO_Read(LSM6DS3_WAKE_UP_SRC, 1, &wake_up_src);


    if (wake_up_src)
    {
        ESP_LOGV(TAG, "significant motion interrupt");
        return LSM6DS3_SIGN_MOTION_INT;
    }
    else if (fifo_status2 & LSM6DS3_FIFO_OVER_RUN)
    {
        ESP_LOGV(TAG, "FIFO overflow interrupt");
        return LSM6DS3_FIFO_OVERFLOW_INT;
    }
    else if (fifo_status2 & LSM6DS3_FIFO_FULL)
    {
        ESP_LOGV(TAG, "FIFO full interrupt");
        return LSM6DS3_FIFO_FULL_INT;
    }
    else
    {
        return LSM6DS3_NO_INT;
    }
}