#include "stm32l4xx_hal.h"

#include "sys_i2c.h"
#include "sys_uart.h"

#include "sys_imu.h"


char GyroAddr = 0xd0;


int
sys_imu_init(void) { 
#ifdef  __B1__  
  int   retval;
    
  retval = sys_imu_init_sensor(SYS_I2C_BABY_HANDLE);
  
  if( retval < 0) {
    return retval;
  }
  
  retval = sys_imu_init_sensor(SYS_I2C_CONTROL_BOX_HANDLE);
  
  return retval;
#else // __B1__
  return sys_imu_init_sensor(SYS_I2C_BABY_HANDLE);
#endif // __B1__
}


int
sys_imu_go_to_sleep_mode_x(I2C_HandleTypeDef *hi2c) {
  uint8_t d[2];
    
  d[0] = 0x1f;
  d[1] = 0x00;  // go to sleep mode 
    
  if (HAL_I2C_Master_Transmit(hi2c, GyroAddr, (uint8_t *)d, 2, 1000) != HAL_OK) {
    /* Check error */
    if (HAL_I2C_GetError(hi2c) != HAL_I2C_ERROR_AF) {
      // csprintf("Wakeup Error\r\n");
      return -1;
    }
  }

  return 0;  
}


int
sys_imu_go_to_sleep_mode(void) {
  int   retval;
  
  retval = sys_imu_go_to_sleep_mode_x(SYS_I2C_BABY_HANDLE);
  
  if (retval < 0) {
    return retval;
  }
  
  retval = sys_imu_go_to_sleep_mode_x(SYS_I2C_CONTROL_BOX_HANDLE);
  
  return retval;
}


int
sys_imu_init_sensor(I2C_HandleTypeDef *hi2c) {
  uint8_t       d[2];
  
  uint8_t       buf[8];

  buf[0] = 0;
  
  /* Check who am I */
  HAL_I2C_Mem_Read(hi2c, GyroAddr, 0x75, 1, buf, 1, 100);
  
#ifdef  DEBUG_VERBOSE  
  csprintf("Gyro ID=0x%x \r\n",buf[0]);
#endif // DEBUG_VERBOSE
    
  if (buf[0] != 0x67) {
    //csprintf("Invalid Gyro ID : %d\n", HAL_I2C_GetError(hi2c));
    return -1;
  }

  /* Wakeup MPU6050 */
  d[0] = 0x1f;
  d[1] = 0x0f;
  
  if (HAL_I2C_Master_Transmit(hi2c, GyroAddr, (uint8_t *)d, 2, 1000) != HAL_OK) {
    /* Check error */
    if (HAL_I2C_GetError(hi2c) != HAL_I2C_ERROR_AF) {
      //  csprintf("Wakeup Error\r\n");
      return -1;
    }
  } 

  // Set range: +/- 500 degrees/sec, LSB sensitivity: 65.5 LSB/degrees/sec
  d[0] = 0x20;
  d[1] = 0x46;
  
  /* Send address */
  if (HAL_I2C_Master_Transmit(hi2c, GyroAddr, (uint8_t *)d, 2, 1000) != HAL_OK) {
    /* Check error */
    if (HAL_I2C_GetError(hi2c) != HAL_I2C_ERROR_AF) {
      return -1;
    }
  }
    
  // Set range: +/- 4g, LSB sensitivity: 8192 LSB/g
  d[0] = 0x21;
  d[1] = 0x46;	
    
  if (HAL_I2C_Master_Transmit(hi2c, GyroAddr, (uint8_t *)d, 2, 1000) != HAL_OK) {
    /* Check error */
    if (HAL_I2C_GetError(hi2c) != HAL_I2C_ERROR_AF) {
      return -1;
    }
  }
    
  return 0; 
}


uint16_t        __sys_imu_read_data_error = 0;


int
sys_imu_read_data(I2C_HandleTypeDef *hi2c, SENSORData *data) {
  uint8_t buf[12];
    
  uint8_t reg_addr;

  /* Send register address */
  reg_addr = 0x0b;
    
  if (HAL_I2C_Master_Transmit(hi2c, GyroAddr, &reg_addr, 1, 1000) != HAL_OK) {
    /* Check error */
    if (HAL_I2C_GetError(hi2c) != HAL_I2C_ERROR_AF) {
      __sys_imu_read_data_error++;
      return -1;
    }		
  }
	
  if (HAL_I2C_Master_Receive(hi2c, GyroAddr, buf, 12, 1000) != HAL_OK) {
    /* Check error */
    if (HAL_I2C_GetError(hi2c) != HAL_I2C_ERROR_AF) {
      __sys_imu_read_data_error++;
      return -1;
    }
  }

  data->ax = ((buf[0] & 0xFF) << 8) | (buf[1] & 0xFF);
  data->ay = ((buf[2] & 0xFF) << 8) | (buf[3] & 0xFF);
  data->az = ((buf[4] & 0xFF) << 8) | (buf[5] & 0xFF);

  data->gx = ((buf[ 6] & 0xFF) << 8) | (buf[ 7] & 0xFF);
  data->gy = ((buf[ 8] & 0xFF) << 8) | (buf[ 9] & 0xFF);
  data->gz = ((buf[10] & 0xFF) << 8) | (buf[11] & 0xFF);
  
  return 0;
}