#pragma once


#include "../../Platforms.h"
#include "../../Debug.h"

/**
 * CMPS10 3-axis magnetometer/accelerometer module
 * https://www.robot-electronics.co.uk/htm/cmps10i2c.htm
 */
template <typename I2C> class CMPS10 {

	I2C& i2c;
	
	static constexpr const char* NAME		= "CMPS10";

	static constexpr uint8_t ADDR7			= 0xC0>>1;
	
	static constexpr uint8_t REG_MAG_X		= 10;	//10+11;
	static constexpr uint8_t REG_MAG_Y		= 12;	//12+13;
	static constexpr uint8_t REG_MAG_Z		= 14;	//14+15;
	
	static constexpr uint8_t REG_ACC_X		= 16;	//16+17;
	static constexpr uint8_t REG_ACC_Y		= 18;	//18+19;
	static constexpr uint8_t REG_ACC_Z		= 20;	//20+21;
	

public:

	struct Magnetometer {
		int16_t x;
		int16_t y;
		int16_t z;
	};
	
	struct Acceleromter {
		int16_t x;
		int16_t y;
		int16_t z;
	};
	
public:

	CMPS10(I2C& i2c, uint8_t addrOffset = 0) : i2c(i2c){

	}


	bool isPresent() {
		return i2c.query(ADDR7);
	}

	Magnetometer getMagnetometer() {
		uint8_t res[6];
		i2c.readReg(ADDR7, REG_MAG_X, 6, res);
		Magnetometer mag;
		mag.x = ((res[0] << 8) | (res[1] << 0));
		mag.y = ((res[2] << 8) | (res[3] << 0));
		mag.z = ((res[4] << 8) | (res[5] << 0));
		return mag;
	}

	Acceleromter getAcceleromter() {
		uint8_t res[6];
		i2c.readReg(ADDR7, REG_ACC_X, 6, res);
		Acceleromter acc;
		acc.x = ((res[0] << 8) | (res[1] << 0));
		acc.y = ((res[2] << 8) | (res[3] << 0));
		acc.z = ((res[4] << 8) | (res[5] << 0));
		return acc;
	}
	
};