/*
 * Include Files
 *
 */
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#include "rtwtypes.h"
#endif

#include "hal_api.h"


/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#ifdef HAL_IMPL
#include "spi_devs.h"
#endif
#include <stdint.h>
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define y_width 1

/*
 * Create external references here.  
 *
 */
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
int imu_inited[3];

void initialize_6axis_imu(uint8_t id)
{
  switch (id)
  {
  case 0:
    if (!imu_inited[0])
    {
#ifdef HAL_IMPL
      // TODO initialize 6axis-IMU No.0
      imu_inited[0] = 1;
#endif
    }
    break;

  case 1:
    if (!imu_inited[1])
    {
#ifdef HAL_IMPL
      // TODO initialize 6axis-IMU No.1
      imu_inited[1] = 1;
#endif
    }
    break;

  case 2:
    if (!imu_inited[2])
    {
#ifdef HAL_IMPL
      // TODO initialize 6axis-IMU No.2
      imu_inited[2] = 1;
#endif
    }
    break;
  
  default:
    break;
  }
}
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */

/*
 * Start function
 *
 */
void hal_6axis_imu_Start_wrapper(const uint8_T *id, const int_T p_width0)
{
/* %%%-SFUNWIZ_wrapper_Start_Changes_BEGIN --- EDIT HERE TO _END */
initialize_6axis_imu(id[0]);
/* %%%-SFUNWIZ_wrapper_Start_Changes_END --- EDIT HERE TO _BEGIN */
}
/*
 * Output function
 *
 */
void hal_6axis_imu_Outputs_wrapper(HAL_acc_SI_t *acc,
			HAL_gyro_SI_t *gyro,
			int32_T *ErrorCode,
			const uint8_T *id, const int_T p_width0)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
  ErrorCode[0] = -1;
  switch (id[0])
  {
  case 0:
    if (imu_inited[0])
    {
#ifdef HAL_IMPL
      // read 6axis-IMU
      if (imu_adis1647x.success)
      {
        acc->a[0] = imu_adis1647x.accel[0];
        acc->a[1] = imu_adis1647x.accel[1];
        acc->a[2] = imu_adis1647x.accel[2];
        gyro->v[0] = imu_adis1647x.gyro[0];
        gyro->v[1] = imu_adis1647x.gyro[1];
        gyro->v[2] = imu_adis1647x.gyro[2];
        acc->seq = imu_adis1647x.seq;
        gyro->seq = imu_adis1647x.seq;
        acc->temperature = imu_adis1647x.temp;
        gyro->temperature = imu_adis1647x.temp;

        ErrorCode[0] = 0;
      }
      else if (imu_icm20689.success)
      {
        acc->a[0] = imu_icm20689.accel[0];
        acc->a[1] = imu_icm20689.accel[1];
        acc->a[2] = imu_icm20689.accel[2];
        gyro->v[0] = imu_icm20689.gyro[0];
        gyro->v[1] = imu_icm20689.gyro[1];
        gyro->v[2] = imu_icm20689.gyro[2];
        acc->seq = imu_icm20689.seq;
        gyro->seq = imu_icm20689.seq;
        acc->temperature = imu_icm20689.temp_degc;
        gyro->temperature = imu_icm20689.temp_degc;

        ErrorCode[0] = 0;
      }
      else
        ErrorCode[0] = -1;
#endif
    }
    break;

  case 1:
    if (imu_inited[1])
    {
#ifdef HAL_IMPL
      // read 6axis-IMU
      if (imu_bmi088.success)
      {
        acc->a[0] = imu_bmi088.accel[0];
        acc->a[1] = imu_bmi088.accel[1];
        acc->a[2] = imu_bmi088.accel[2];
        gyro->v[0] = imu_bmi088.gyro[0];
        gyro->v[1] = imu_bmi088.gyro[1];
        gyro->v[2] = imu_bmi088.gyro[2];
        acc->seq = imu_bmi088.accel_seq;
        gyro->seq = imu_bmi088.gyro_seq;
        acc->temperature = imu_bmi088.temp_degc;
        gyro->temperature = imu_bmi088.temp_degc;

        ErrorCode[0] = 0;
      }
      else
        ErrorCode[0] = -1;
#endif
    }
    break;

  case 2:
    if (imu_inited[2])
    {
#ifdef HAL_IMPL
      // TODO read 6axis-IMU
      ErrorCode[0] = -1;
#endif
    }
    break;
  default:
    break;
  }
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}