#ifndef STA_DRIVERS_BMI088_HPP
#define STA_DRIVERS_BMI088_HPP

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

#include <sta/drivers/bmi088_defs.hpp>

namespace sta
{
    class BMI088
    {
    public:
    	enum GyroMode
		{
    		NORMAL_AWAKE	= 0x00,
			SUSPEND			= 0x80,
			DEEP_SUSPEND	= 0x20
    	};
    	enum GyroRange
		{
    		TWOTHOUSAND		= 0x00,
			ONETHOUSAND		= 0x01,
			FIVEHUNDRED 	= 0x02,
			TWOFIFTY		= 0x03,
			ONETWENTYFIVE	= 0x04
		};
    	enum GyroBandwidth
		{
    		TWOTHOUSAND_FIVETHIRTYTWO	= 0x00,
			TWOTHOUSAND_TWOTHIRTY		= 0x01,
			ONETHOUSAND_ONESIXTEEN		= 0x02,
			FOURHUNDRED_FORTYSEVEN		= 0x03,
			TWOHUNDRED_TWENTYTHREE		= 0x04,
			ONEHUNDRED_TWELVE			= 0x05,
			TWOHUNDRED_SIXTYFOUR		= 0x06,
			ONEHUNDRED_THIRTYTWO		= 0x07
		};

    	enum AccelMode
		{
    		OFF	= 0x00,
			ON	= 0x04
		};
    	enum AccelRange
		{
			THREEG		= 0x00,
			SIXG		= 0x01,
			TWELVEG		= 0x02,
			TWENTYFOURG	= 0x03
		};
    	enum AccelBandwidth
		{
    		OSR4				= 0x08,
			OSR2				= 0x09,
			NORMAL_BANDWIDTH	= 0x0A
		};
    	enum AccelODR
		{
    		TWELVE_FIVE		= 0x05,
			TWENTYFIVE		= 0x06,
			FIFTY			= 0x07,
			ONEHUNDRED		= 0x08,
			TWOHUNDRED		= 0x09,
			FOURHUNDRED		= 0x0A,
			EIGHTHUNDRED	= 0x0B,
			ONESIXHUNDRED	= 0x0C
		};

    private:
		enum Part
		{
			GYROSCOPE,
			ACCELEROMETER
		};

    public:
        BMI088(STM32SPIDevice* gyro_device, STM32SPIDevice* 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(uint16_t* x, uint16_t* y, uint16_t* z);
        void getRawAcceleration(uint16_t* x, uint16_t* y, uint16_t* z);

        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