#ifndef STA_DRIVERS_BMI088_HPP
#define STA_DRIVERS_BMI088_HPP

//#include <sta/devices/stm32/bus/spi.hpp>
//#include <sta/devices/stm32/bus/i2c.hpp>
#include <sta/bus/spi/device.hpp>
#include <sta/bus/i2c/device.hpp>

#include <sta/drivers/bmi088_defs.hpp>

namespace sta
{
    class BMI088
    {
    public:
    	enum GyroMode
		{
    		NORMAL_AWAKE	= 0x00,
			SUSPEND			= 0x80,
			DEEP_SUSPEND	= 0x20
    	};
    	enum GyroRange
		{
    		_2000	= 0x00,
			_1000	= 0x01,
			_500 	= 0x02,
			_250	= 0x03,
			_125	= 0x04
		};
    	enum GyroBandwidth
		{
    		_2000_532	= 0x00,
			_2000_230	= 0x01,
			_1000_116	= 0x02,
			_400_47		= 0x03,
			_200_23		= 0x04,
			_100_12		= 0x05,
			_200_64		= 0x06,
			_100_32		= 0x07
		};

    	enum AccelMode
		{
    		OFF	= 0x00,
			ON	= 0x04
		};
    	enum AccelRange
		{
			_3G		= 0x00,
			_6G		= 0x01,
			_12G		= 0x02,
			_24G	= 0x03
		};
    	enum AccelBandwidth
		{
    		OSR4				= 0x08,
			OSR2				= 0x09,
			NORMAL_BANDWIDTH	= 0x0A
		};
    	enum AccelODR
		{
    		_12_5	= 0x05,
			_25		= 0x06,
			_50		= 0x07,
			_100	= 0x08,
			_200	= 0x09,
			_400	= 0x0A,
			_800	= 0x0B,
			_1600	= 0x0C
		};

    private:
		enum Part
		{
			GYROSCOPE,
			ACCELEROMETER
		};

    public:
        BMI088(Device* gyro_device, Device* accel_device);

        bool init();

        void setGyroscopeMode(GyroMode mode);
        void setGyroscopeRange(GyroRange range);
        void setGyroscopeBandwidth(GyroBandwidth bandwidth);
        void getRotation(float* x, float* y, float* z);

        void setAccelerometerMode(AccelMode mode);
        void setAccelerometerRange(AccelRange range);
        void setAccelerometerBandwidth(AccelBandwidth bandwidth, AccelODR odr);
        void getAcceleration(float* x, float* y, float* z);

    private:
        bool busRead(Part part, uint8_t reg, uint8_t * buffer);
        bool busWrite(Part part, uint8_t reg, uint8_t value);

        void getRawRotation(int16_t* x, int16_t* y, int16_t* z);
        void getRawAcceleration(int16_t* x, int16_t* y, int16_t* z);

        float lsbToMps2(int16_t val);
        float lsbToDPS(int16_t val);
        void convertRawToActual(uint16_t* i, float* f, float f_range);

        Device* gyro_device;
		Device* accel_device;

        float f_gyro_range = 0;
        float f_accel_range = 0;
    };
} // namespace sta

#endif // STA_DRIVERS_BMI088_HPP