/******************************************************************************
 * 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);
}

// /******************************************************************************
//  * FunctionName : lsm6ds3_xl_config
//  * Description  : Setup LSM6DS3 registers for accel. sample and other cool nonsense.
//  				  Note: 20ms is required from boot for the sensor to load some magic before you
//  				  can play.
//  * Parameters   : bool en_hp_mode: Enable or Disable high performance mode. NOTE: HP mode is
//  				  is required for LP filter data and certain sampling rates.
//  				  odr_xl: Output data rate for XL. See .h file for available rates.
//  				  uint8_t scale_xl: Range of senstiviity selection. See .h file for available rates.
//  				  uint8_t bw_xl: Anti-aliasing LP filter selection. See .h file and datasheet.
//  * Returns      : bool 1: success 0: failed
// *******************************************************************************/
// bool ICACHE_FLASH_ATTR
// lsm6ds3_xl_config(bool en_hp_mode, uint8_t odr_xl, uint8_t scale_xl, uint8_t bw_xl)
// {
//     uint8_t ctrl6_c = spi_rx8_address(HSPI, LSM6DS3_CTRL6_C);
//     uint8_t ctrl1_xl = odr_xl | scale_xl | bw_xl;
//
//     if(en_hp_mode)
//     ctrl6_c &= ~LSM6DS3_XL_HM_MODE;
//     else
//     ctrl6_c |= LSM6DS3_XL_HM_MODE;
//     spi_tx8_address(HSPI, LSM6DS3_CTRL6_C, ctrl6_c);
//     spi_tx8_address(HSPI, LSM6DS3_CTRL1_XL, ctrl1_xl);
//     if ( (spi_rx8_address(HSPI, LSM6DS3_CTRL1_XL) == ctrl1_xl) & (spi_rx8_address(HSPI, LSM6DS3_CTRL6_C) == ctrl6_c) )
//     return 1;
//     else
//     return 0;
// }
// /******************************************************************************
//  * FunctionName : lsm6ds3_g_config
//  * Description  : Setup LSM6DS3 registers for accel. sample and other cool nonsense.
//  				  Note: 20ms is required from boot for the sensor to load some magic before you
//  				  can play.
//  * Parameters   : bool en_hp_mode: Enable or Disable high performance mode. NOTE: HP mode is
//  				  is required for LP filter data and certain sampling rates.
//  				  odr_xl: Output data rate for XL. See .h file for available rates.
//  				  uint8_t scale_xl: Range of senstiviity selection. See .h file for available rates.
//  				  uint8_t bw_xl: Anti-aliasing LP filter selection. See .h file and datasheet.
//  * Returns      : bool 1: success 0: failed
// *******************************************************************************/
// bool ICACHE_FLASH_ATTR
// lsm6ds3_g_config(bool en_hp_mode, bool hp_g_en, bool hp_g_rst,
//     uint8_t odr_g, uint8_t scale_g, uint8_t hpcf_g)
// {
//     uint8_t ctrl7_g;
//     uint8_t ctrl2_g = odr_g | scale_g;
//
//     if (en_hp_mode)
//     ctrl7_g = LSM6DS3_G_HM_MODE;
//     else
//     ctrl7_g &= ~LSM6DS3_G_HM_MODE;
//     if (hp_g_en)
//     ctrl7_g |= LSM6DS3_HP_G_EN | hpcf_g;
//     else
//     ctrl7_g &= ~LSM6DS3_HP_G_EN;
//     if (hp_g_rst)
//     ctrl7_g |= LSM6DS3_HP_G_RST;
//     else
//     ctrl7_g &= ~LSM6DS3_HP_G_RST;
//
//     spi_tx8_address(HSPI, LSM6DS3_CTRL2_G, ctrl2_g);
//     spi_tx8_address(HSPI, LSM6DS3_CTRL7_G, ctrl7_g);
//     if ( (spi_rx8_address(HSPI, LSM6DS3_CTRL2_G) == ctrl2_g) & (spi_rx8_address(HSPI, LSM6DS3_CTRL7_G) == ctrl7_g))
//     return 1;
//     else
//     return 0;
// }
// /******************************************************************************
//  * FunctionName : lsm6ds3_fifo_config
//  * Description  : Setup LSM6DS3 registers for FIFO. FIFO is automatically placed into continous mode.
//  				  Decimation is set to zero, and the ODR of the FIFO is set to match that max(XL_ODR, G_ODR)
//  				  INT1 is automatically enabled / disabled for FIFO overflow and threshold.
//  				  If FIFO is enable it will automatically disable the significant motion ISR.
//  * Parameters   : bool enable: Enable or Disable FIFO
//  				  threshold: Set the threshold level of the FIFO
//  * Returns      : bool 1: success 0: failed
// *******************************************************************************/
// bool ICACHE_FLASH_ATTR
// lsm6ds3_fifo_config(uint16_t threshold)
// {
//     uint8_t ctrl1_xl, ctrl2_g, max_odr, fifo_ctrl3;
//
//     // read ODR rates
//     ctrl1_xl = spi_rx8_address(HSPI, LSM6DS3_CTRL1_XL) & 0xF0;			// get ODR bits and clear the rest
//     ctrl2_g = spi_rx8_address(HSPI, LSM6DS3_CTRL2_G) & 0xF0;			// get ODR bits and clear the rest
//     os_printf("CTRL1_XL, CTRL2_G: %x, %x\r\n", ctrl1_xl, ctrl2_g);
//     if (!(ctrl1_xl || ctrl2_g))
//     {
//     // disable FIFO
//     os_printf("LSM6DS3: XL and G disabled. FIFO disabled.\r\n");
//     lsm6ds3_set_register(LSM6DS3_FIFO_CTRL5, 0x00);
//     lsm6ds3_set_register(LSM6DS3_INT1_CTRL, 0x00); 				// disable INT1 for overflow and threshold
//
//     }
//     else
//     {
//
//     // set Decimation Factor to 1 if sensor is active
//     //lsm6ds3_set_register(FIFO_CTRL3, 0b00001001);
//
//     // set threshold
//     lsm6ds3_set_register(LSM6DS3_FIFO_CTRL1, (uint8_t)(0xFF & threshold));
//     lsm6ds3_set_register(LSM6DS3_FIFO_CTRL2, (uint8_t)(0xFF & (threshold >> 8)));
//
//     // set decimation factor to one if sensor ODR is not zero
//     fifo_ctrl3 = 0;
//     if (ctrl1_xl & 0xF0)
//     fifo_ctrl3 = BIT0;
//     if(ctrl2_g & 0xF0)
//     fifo_ctrl3 |= BIT3;
//     lsm6ds3_set_register(LSM6DS3_FIFO_CTRL3, fifo_ctrl3);
//     os_printf("FIFO_CTRL3: %x\r\n", fifo_ctrl3);
//
//     // determine max ODR
//     if ( (ctrl1_xl) >= (ctrl2_g) )
//     max_odr = ctrl1_xl;
//     else
//     max_odr = ctrl2_g;
//     max_odr = max_odr >> 1; 									// format for fifo register
//     lsm6ds3_set_register(LSM6DS3_FIFO_CTRL5, max_odr | 0x06);	// set FIFO ODR and mode to continously
//     lsm6ds3_set_register(LSM6DS3_INT1_CTRL, BIT3); 				// set INT1 for overflow and threshold
//     }
//     return 1;
// }
//
// bool ICACHE_FLASH_ATTR lsm6ds3_reset(void)
// {
//     lsm6ds3_set_register(LSM6DS3_CTRL3_C, BIT0);
//     os_delay_us(30000);
//     return !(bool)(spi_rx8_address(HSPI, LSM6DS3_CTRL3_C) & BIT7);
// }
//
// uint16_t ICACHE_FLASH_ATTR lsm6ds3_fifo_dump(uint16_t *buf, uint16_t buf_len, uint16_t bufPos)
// {
//     uint16_t samples = (uint16_t)spi_rx8_address(HSPI, LSM6DS3_FIFO_STATUS1);
//     samples |= (((uint16_t)spi_rx8_address(HSPI, LSM6DS3_FIFO_STATUS2) & 0x0F) << 8);
//
//     // read and store into buff in multiples of 3
//     if (samples <=  (buf_len - bufPos) && samples != 0)
//     {
//     uint16_t i;
//     for(i=bufPos; i<samples+bufPos; i++)
//     {
//     buf[i] = spi_rx16_address(HSPI, LSM6DS3_FIFO_DATA_OUT_L);
//     }
//     return samples;
//     }
//     else
//     return 0;
//
// }
//
//
// uint8_t ICACHE_FLASH_ATTR lsm6ds3_fifo_getFlags(void)
// {
//     /*	There is (2) possible sources of interrupts caused by the IMU
//     that the ESP8266 actually cares about.
//     1) The significant motion interrupt
//     2) The FIFO interrupts
//
//     LSM6DS3_FIFO_EMPTY
//     LSM6DS3_FIFO_FULL
//     LSM6DS3_FIFO_OVER_RUN
//     LSM6DS3_FIFO_THRESHOLD
//     */
//     return (uint8_t)spi_rx8_address(HSPI, LSM6DS3_FIFO_STATUS2);
// }
//
// bool ICACHE_FLASH_ATTR lsm6ds3_read_motion(void)
// {
//     uint8_t wake_up_src;
//     wake_up_src = SPIread(LSM6DS3_WAKE_UP_SRC);
//     //if (wake_up_src)
//     {
//     os_printf("WAKE_UP_SRC: %x\r\n", wake_up_src);
//     os_printf("Direction ");
//     if (wake_up_src & BIT2)
//     os_printf("x");
//     if (wake_up_src & BIT1)
//     os_printf("y");
//     if (wake_up_src & BIT0)
//     os_printf("z");
//
//     os_printf("\r\n");
//     }
//     return wake_up_src & BIT3;
// }
//
// bool ICACHE_FLASH_ATTR lsm6ds3_motion_enable(void)
// {
//     SPIwrite(LSM6DS3_CTRL2_G, LSM6DS3_ODR_G_POWERDOWN);								// disable gyro
//     SPIwrite(LSM6DS3_CTRL1_XL, (LSM6DS3_ODR_XL_13Hz + LSM6DS3_FS_XL_2G));					// 208Hz 2g scale
//     SPIwrite(LSM6DS3_TAP_CFG, LSM6DS3_LIR); 										// enable interuppt latch and slope filter
//     SPIwrite(LSM6DS3_WAKE_UP_THS, 0x01); 									// set thres 32/64*2G = 1G
//     SPIwrite(LSM6DS3_WAKE_UP_DUR, 0x02); 									// zero duration
//     SPIwrite(LSM6DS3_MD1_CFG, LSM6DS3_INT1_WU);										// set interrupt for INTX (1 is connected to PBA)
//     SPIwrite(LSM6DS3_CTRL3_C, LSM6DS3_H_LACTIVE);                                   // set int lines active low
//     os_printf("MOTION PROCESS: Setup IMU for motion detection\r\n");
//
//     return 1;
// }
//
// bool ICACHE_FLASH_ATTR lsm6ds3_motion_disable(void)
// {
//     lsm6ds3_reset();
//     SPIwrite(LSM6DS3_CTRL2_G, LSM6DS3_ODR_G_POWERDOWN); 							// disable gyro
//     SPIwrite(LSM6DS3_CTRL1_XL, LSM6DS3_ODR_XL_POWERDOWN); 							// disable XL
//     SPIwrite(LSM6DS3_CTRL3_C, LSM6DS3_H_LACTIVE);
//     return 1;
// }
//
// #if LSM6DS3_DEBUG
// void ICACHE_FLASH_ATTR
// lsm6ds3_xl_print(void)
// {
//     os_printf("XL_X: %d\r\nXL_Y: %d\r\n XL_Z: %d\r\n",
//     (SPIread(LSM6DS3_OUTX_H_XL)<<8)+SPIread(LSM6DS3_OUTX_L_XL), (SPIread(LSM6DS3_OUTY_H_XL)<<8)+SPIread(LSM6DS3_OUTY_L_XL),
//     (SPIread(LSM6DS3_OUTZ_H_XL)<<8)+SPIread(LSM6DS3_OUTX_L_XL));
//     return;
// }
// #endif