#ifndef _SH200Q_H_
#define _SH200Q_H_

#include <Wire.h>
#include <Arduino.h>

#define SH200I_ADDRESS 0x6C //7bit i2c address 
#define SH200I_WHOAMI 0x30
#define SH200I_ACC_CONFIG 0x0E
#define SH200I_GYRO_CONFIG 0x0F
#define SH200I_GYRO_DLPF 0x11
#define SH200I_FIFO_CONFIG 0x12
#define SH200I_ACC_RANGE 0x16
#define SH200I_GYRO_RANGE 0x2B
#define SH200I_OUTPUT_ACC 0x00
#define SH200I_OUTPUT_GYRO 0x06
#define SH200I_OUTPUT_TEMP 0x0C
#define SH200I_REG_SET1 0xBA
#define SH200I_REG_SET2 0xCA   //ADC reset
#define SH200I_ADC_RESET  0xC2   //drive reset
#define SH200I_SOFT_RESET 0x7F
#define SH200I_RESET 0x75

//#define G (9.8)
#define RtA     57.324841
#define AtR     0.0174533
#define Gyro_Gr 0.0010653

class SH200Q {
public:
  enum Ascale {
    AFS_4G = 0,
    AFS_8G,
    AFS_16G
  };

  enum Gscale {
    GFS_2000DPS = 0,
    GFS_1000DPS,
    GFS_500DPS,
    GFS_250DPS,
    GFS_125DPS,
  };



public:
  SH200Q();

  int Init(void);

  void getAccelAdc(int16_t* ax, int16_t* ay, int16_t* az);
  void getGyroAdc(int16_t* gx, int16_t* gy, int16_t* gz);
  void getTempAdc(int16_t *t);

  void getAccelData(float* ax, float* ay, float* az);
  void getGyroData(float* gx, float* gy, float* gz);
  void getTempData(float *t);
  void getAhrsData(float *pitch,float *roll,float *yaw);

  void setGyroFsr(Gscale scale);
  void setAccelFsr(Ascale scale);

public:
  float aRes, gRes;
  uint8_t imuId;
  Gscale Gyscale;
  Ascale Acscale;

protected:
  void I2C_Read_NBytes(uint8_t driver_Addr, uint8_t start_Addr, uint8_t number_Bytes, uint8_t *read_Buffer);
  void I2C_Write_NBytes(uint8_t driver_Addr, uint8_t start_Addr, uint8_t number_Bytes, uint8_t *write_Buffer);
  void sh200i_ADCReset(void);
  void sh200i_Reset(void);
  void updateGres();
  void updateAres();
};
#endif