#ifndef SENS_MPU6050
#define SENS_MPU6050

#include "../../io/SoftI2C.h"
#include "../../Debug.h"

/**
 * accelereomter/gyroscope
 * https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf
 * https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
 * https://github.com/kriswiner/MPU6050/blob/master/MPU6050BasicExample.ino
 */
class MPU6050 {

private:

	static constexpr const char* NAME		= "MPU6050";

	static constexpr uint8_t ADDR			= 0b1101000 ;

	static constexpr uint8_t REG_CFG_GYRO		= 0x1B;
	static constexpr uint8_t REG_CFG_ACC		= 0x1C;
	static constexpr uint8_t REG_ACCEL_XOUT_H	= 0x3B;

	static constexpr uint8_t REG_SMPLRT_DIV		= 0x19;
	static constexpr uint8_t REG_CONFIG			= 0x1A;

	static constexpr uint8_t REG_PWR_MGMT_1		= 0x6B;

	bool inited = false;


private:

	void init() {

		debugMod(NAME, "init()");

		// set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
		writeRegister(REG_PWR_MGMT_1, 0b001);		// table on page 40

		// Configure Gyro and Accelerometer
		// Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
		// DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
		//writeRegister(REG_CONFIG, 0x03);			// table on page 13
		writeRegister(REG_CONFIG, 0x00);			// table on page 13

		// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
		writeRegister(REG_SMPLRT_DIV, 0x04); // Use a 200 Hz sample rate

		// configure gyro as +/- 4G -> +32767 = +4G, -32768 = -4G
		writeRegister(REG_CFG_ACC, 0b01 << 3);		// table on page 15
		#define oneGto1000(x)	((int)x)*1000/(32768/4)
	}

	bool writeRegister(const uint8_t addr, const uint8_t val) {

		bool ok;

		// address the slave in write mode and select the first register to read
		ok = i2c::startWrite(ADDR);
		if (!ok) {os_printf("failed start write\n"); return false;}
		ok = i2c::writeByteAndCheck(addr);
		if (!ok) {os_printf("failed to select register %d\n", addr); return false;}
		ok = i2c::writeBytesAndCheck(&val, 1);
		if (!ok) {os_printf("failed to write register contents \n"); return false;}

		// done
		i2c::stop();
		return true;

	}




public:

	struct Acc {
		int16_t x;
		int16_t y;
		int16_t z;
	};

	struct Gyro {
		int16_t x;
		int16_t y;
		int16_t z;
	};

	struct Res {
		Acc acc;
		int16_t temp;
		Gyro gyro;
	};

	void initOnce() {
		if (inited) {return;}
		init();
		inited = true;
	}


	bool isPresent() {
		return i2c::query(ADDR);
	}

	bool query(Res& res) {

		bool ok;

		// select register(s) to read
		//i2c::start();
		ok = i2c::startWrite(ADDR);
		if (!ok) {os_printf("failed start write1\n"); return false;}
		ok = i2c::writeByteAndCheck(REG_ACCEL_XOUT_H);
		if (!ok) {os_printf("failed select register\n"); return false;}
		i2c::stop();

		// read registers
		uint8_t buf[14];
		//i2c::start();
		ok = i2c::startRead(ADDR);
		if (!ok) {os_printf("failed start write2\n"); return false;}
		i2c::readBytes(buf, 14);
		i2c::stop();

		res.acc.x = (buf[0] << 8) | buf[1];
		res.acc.y = (buf[2] << 8) | buf[3];
		res.acc.z = (buf[4] << 8) | buf[5];

		res.temp = (buf[6] << 8) | buf[7];

		res.gyro.x = (buf[8] << 8) | buf[9];
		res.gyro.y = (buf[10] << 8) | buf[11];
		res.gyro.z = (buf[12] << 8) | buf[13];

		res.acc.x = oneGto1000(res.acc.x);
		res.acc.y = oneGto1000(res.acc.y);
		res.acc.z = oneGto1000(res.acc.z);


		//os_printf("Acc  (%d, %d, %d)\n", res.acc.x, res.acc.y, res.acc.z);
		//os_printf("Gyro (%d, %d, %d)\n", res.gyro.x, res.gyro.y, res.gyro.z);
		//os_printf("Temp (%d)\n", res.temp);

		return true;

	}


};

#endif