/*
 ******************************************************************************
 * @file    self_test.c
 * @author  Sensors Software Solution Team
 * @brief   This file implement the self test procedure.
 *
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
 * All rights reserved.</center></h2>
 *
 * This software component is licensed by ST under BSD 3-Clause license,
 * the "License"; You may not use this file except in compliance with the
 * License. You may obtain a copy of the License at:
 *                        opensource.org/licenses/BSD-3-Clause
 *
 ******************************************************************************
 */

/*
 * This example was developed using the following STMicroelectronics
 * evaluation boards:
 *
 * - STEVAL_MKI109V3 + STEVAL-MKI206V1
 * - NUCLEO_F411RE + STEVAL-MKI206V1
 * - DISCOVERY_SPC584B + STEVAL-MKI206V1
 *
 * Used interfaces:
 *
 * STEVAL_MKI109V3    - Host side:   USB (Virtual COM)
 *                    - Sensor side: SPI(Default) / I2C(supported)
 *
 * NUCLEO_STM32F411RE - Host side: UART(COM) to USB bridge
 *                    - Sensor side: I2C(Default) / SPI(supported)
 *
 * DISCOVERY_SPC584B  - Host side: UART(COM) to USB bridge
 *                    - Sensor side: I2C(Default) / SPI(supported)
 *
 * If you need to run this example on a different hardware platform a
 * modification of the functions: `platform_write`, `platform_read`,
 * `tx_com` and 'platform_init' is required.
 *
 */


/* STMicroelectronics evaluation boards definition
 *
 * Please uncomment ONLY the evaluation boards in use.
 * If a different hardware is used please comment all
 * following target board and redefine yours.
 */

//#define STEVAL_MKI109V3  /* little endian */
#define NUCLEO_F411RE    /* little endian */
//#define SPC584B_DIS      /* big endian */

/* ATTENTION: By default the driver is little endian. If you need switch
 *            to big endian please see "Endianness definitions" in the
 *            header file of the driver (_reg.h).
 */


#if defined(STEVAL_MKI109V3)
/* MKI109V3: Define communication interface */
#define SENSOR_BUS hspi2
/* MKI109V3: Vdd and Vddio power supply values */
#define PWM_3V3 915

#elif defined(NUCLEO_F411RE)
/* NUCLEO_F411RE: Define communication interface */
#define SENSOR_BUS hi2c1

#elif defined(SPC584B_DIS)
/* DISCOVERY_SPC584B: Define communication interface */
#define SENSOR_BUS I2CD1

#endif

/* Includes ------------------------------------------------------------------*/
#include <string.h>
#include <stdio.h>
#include "lis331dlh_reg.h"

#if defined(NUCLEO_F411RE)
#include "stm32f4xx_hal.h"
#include "usart.h"
#include "gpio.h"
#include "i2c.h"

#elif defined(STEVAL_MKI109V3)
#include "stm32f4xx_hal.h"
#include "usbd_cdc_if.h"
#include "gpio.h"
#include "spi.h"

#elif defined(SPC584B_DIS)
#include "components.h"
#endif

/* Private macro -------------------------------------------------------------*/

#define    BOOT_TIME          5 //ms
#define    WAIT_TIME        200 //ms

#define    SAMPLES            5 //number of samples

/* Self test results. */
#define    ST_PASS     1U
#define    ST_FAIL     0U

/* Self test limits changes with the power supply. these are @ 2.5V*/
static const float min_st_limit[] = {120.0f, 120.0f, 140.0f};
static const float max_st_limit[] = {550.0f, 550.0f, 750.0f};

/* Private variables ---------------------------------------------------------*/

/* Extern variables ----------------------------------------------------------*/

/* Private functions ---------------------------------------------------------*/
/*
 *   WARNING:
 *   Functions declare in this section are defined at the end of this file
 *   and are strictly related to the hardware platform used.
 *
 */
static int32_t platform_write(void *handle, uint8_t reg, uint8_t *bufp,
                              uint16_t len);
static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp,
                             uint16_t len);
static void tx_com(uint8_t *tx_buffer, uint16_t len);
static void platform_delay(uint32_t ms);
static void platform_init(void);

/* Main Example --------------------------------------------------------------*/
void lis331dlh_self_test(void)
{
  stmdev_ctx_t dev_ctx_xl;
  uint8_t tx_buffer[1000];
  int16_t data_raw[3];
  float maes_st_off[3];
  float maes_st_on[3];
  lis331dlh_reg_t reg;
  float test_val[3];
  uint8_t st_result;
  uint8_t i, j;

  /* Initialize mems driver interface */
  dev_ctx_xl.write_reg = platform_write;
  dev_ctx_xl.read_reg = platform_read;
  dev_ctx_xl.handle = &SENSOR_BUS;

  /* Initialize self test results */
  st_result = ST_PASS;

  /* Initialize platform specific hardware */
  platform_init();

  /* Wait sensor boot time */
  platform_delay(BOOT_TIME);

  /* Check device ID */
  lis331dlh_device_id_get(&dev_ctx_xl, &reg.byte);
  if (reg.byte != LIS331DLH_ID){
    while(1) {
      /* manage here device not found */
    }
  }

  /* Enable Block Data Update. */
  lis331dlh_block_data_update_set(&dev_ctx_xl, PROPERTY_ENABLE);

  /* Set full scale to 2g. */
  lis331dlh_full_scale_set(&dev_ctx_xl, LIS331DLH_2g);

  /* Set Output Data Rate. */
  lis331dlh_data_rate_set(&dev_ctx_xl, LIS331DLH_ODR_50Hz);

  /* Wait stable output */
  platform_delay(WAIT_TIME);

  /* Check if new value available */
  do {
    lis331dlh_status_reg_get(&dev_ctx_xl, &reg.status_reg);
  } while(!reg.status_reg.zyxda);
  /* Read dummy data and discard it */
  lis331dlh_acceleration_raw_get(&dev_ctx_xl, data_raw);

  /* Read samples and get the average vale for each axis */
  for (i = 0; i < SAMPLES; i++){
    /* Check if new value available */
      do {
        lis331dlh_status_reg_get(&dev_ctx_xl, &reg.status_reg);
      } while(!reg.status_reg.zyxda);
    /* Read data and accumulate the mg value */
    lis331dlh_acceleration_raw_get(&dev_ctx_xl, data_raw);
    for (j=0; j<3; j++){
      maes_st_off[j] += lis331dlh_from_fs2_to_mg(data_raw[j]);
    }
  }
  /* Calculate the mg average values */
  for (i=0; i<3; i++){
    maes_st_off[i] /= SAMPLES;
  }

  /* Enable Self Test positive (or negative) */
  lis331dlh_self_test_set(&dev_ctx_xl, LIS331DLH_ST_POSITIVE);
  //lis331dlh_self_test_set(&dev_ctx_xl, LIS331DLH_ST_NEGATIVE);

  /* Wait stable output */
  platform_delay(WAIT_TIME);

  /* Check if new value available */
  do {
    lis331dlh_status_reg_get(&dev_ctx_xl, &reg.status_reg);
  } while(!reg.status_reg.zyxda);
  /* Read dummy data and discard it */
  lis331dlh_acceleration_raw_get(&dev_ctx_xl, data_raw);

  /* Read samples and get the average vale for each axis */
  for (i = 0; i < SAMPLES; i++){
    /* Check if new value available */
      do {
        lis331dlh_status_reg_get(&dev_ctx_xl, &reg.status_reg);
      } while(!reg.status_reg.zyxda);
    /* Read data and accumulate the mg value */
    lis331dlh_acceleration_raw_get(&dev_ctx_xl, data_raw);
    for (j=0; j<3; j++){
      maes_st_on[j] += lis331dlh_from_fs2_to_mg(data_raw[j]);
    }
  }
  /* Calculate the mg average values */
  for (i=0; i<3; i++){
    maes_st_on[i] /= SAMPLES;
  }

  /* Calculate the mg values for self test */
  for (i=0; i<3; i++){
    test_val[i] = fabs((maes_st_on[i] - maes_st_off[i]));
  }


  /* Check self test limit */
  for (i=0; i<3; i++){
    if (( min_st_limit[i] > test_val[i] ) ||
        ( test_val[i] > max_st_limit[i])){
        st_result = ST_FAIL;
    }
    tx_com(tx_buffer, strlen((char const*)tx_buffer));
  }

  /* Disable Self Test */
  lis331dlh_self_test_set(&dev_ctx_xl, LIS331DLH_ST_DISABLE);

  /* Disable sensor. */
  lis331dlh_data_rate_set(&dev_ctx_xl, LIS331DLH_ODR_OFF);

  /* Print self test result */
  if (st_result == ST_PASS) {
    sprintf((char*)tx_buffer, "Self Test - PASS\r\n" );
  }
  else {
    sprintf((char*)tx_buffer, "Self Test - FAIL\r\n" );
  }
  tx_com(tx_buffer, strlen((char const*)tx_buffer));

}

/*
 * @brief  Write generic device register (platform dependent)
 *
 * @param  handle    customizable argument. In this examples is used in
 *                   order to select the correct sensor bus handler.
 * @param  reg       register to write
 * @param  bufp      pointer to data to write in register reg
 * @param  len       number of consecutive register to write
 *
 */
static int32_t platform_write(void *handle, uint8_t reg, uint8_t *bufp,
                              uint16_t len)
{
#if defined(NUCLEO_F411RE)
  /* Write multiple command */
   reg |= 0x80;
  HAL_I2C_Mem_Write(handle, LIS331DLH_I2C_ADD_L, reg,
                    I2C_MEMADD_SIZE_8BIT, bufp, len, 1000);
#elif defined(STEVAL_MKI109V3)
  /* Write multiple command */
  reg |= 0x40;
  HAL_GPIO_WritePin(CS_up_GPIO_Port, CS_up_Pin, GPIO_PIN_RESET);
  HAL_SPI_Transmit(handle, &reg, 1, 1000);
  HAL_SPI_Transmit(handle, bufp, len, 1000);
  HAL_GPIO_WritePin(CS_up_GPIO_Port, CS_up_Pin, GPIO_PIN_SET);
#elif defined(SPC584B_DIS)
  /* Write multiple command */
  reg |= 0x80;
  i2c_lld_write(handle,  LIS331DLH_I2C_ADD_L & 0xFE, reg, bufp, len);
#endif
  return 0;
}

/*
 * @brief  Read generic device register (platform dependent)
 *
 * @param  handle    customizable argument. In this examples is used in
 *                   order to select the correct sensor bus handler.
 * @param  reg       register to read
 * @param  bufp      pointer to buffer that store the data read
 * @param  len       number of consecutive register to read
 *
 */
static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp,
                             uint16_t len)
{
#if defined(NUCLEO_F411RE)
  /* Read multiple command */
  reg |= 0x80;
  HAL_I2C_Mem_Read(handle, LIS331DLH_I2C_ADD_L, reg,
                   I2C_MEMADD_SIZE_8BIT, bufp, len, 1000);
#elif defined(STEVAL_MKI109V3)
  /* Read multiple command */
  reg |= 0xC0;
  HAL_GPIO_WritePin(CS_up_GPIO_Port, CS_up_Pin, GPIO_PIN_RESET);
  HAL_SPI_Transmit(handle, &reg, 1, 1000);
  HAL_SPI_Receive(handle, bufp, len, 1000);
  HAL_GPIO_WritePin(CS_up_GPIO_Port, CS_up_Pin, GPIO_PIN_SET);
#elif defined(SPC584B_DIS)
  /* Read multiple command */
  reg |= 0x80;
  i2c_lld_read(handle, LIS331DLH_I2C_ADD_L & 0xFE, reg, bufp, len);
#endif
  return 0;
}

/*
 * @brief  Send buffer to console (platform dependent)
 *
 * @param  tx_buffer     buffer to transmit
 * @param  len           number of byte to send
 *
 */
static void tx_com(uint8_t *tx_buffer, uint16_t len)
{
#if defined(NUCLEO_F411RE)
  HAL_UART_Transmit(&huart2, tx_buffer, len, 1000);
#elif defined(STEVAL_MKI109V3)
  CDC_Transmit_FS(tx_buffer, len);
#elif defined(SPC584B_DIS)
  sd_lld_write(&SD2, tx_buffer, len);
#endif
}

/*
 * @brief  platform specific delay (platform dependent)
 *
 * @param  ms        delay in ms
 *
 */
static void platform_delay(uint32_t ms)
{
#if defined(NUCLEO_F411RE) | defined(STEVAL_MKI109V3)
  HAL_Delay(ms);
#elif defined(SPC584B_DIS)
  osalThreadDelayMilliseconds(ms);
#endif
}

/*
 * @brief  platform specific initialization (platform dependent)
 */
static void platform_init(void)
{
#if defined(STEVAL_MKI109V3)
  TIM3->CCR1 = PWM_3V3;
  TIM3->CCR2 = PWM_3V3;
  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
  HAL_Delay(1000);
#endif
}
