#pragma once


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

/**
 * LIS3MDL 3-axis magnetometer module
 * https://www.st.com/resource/en/datasheet/lis3mdl.pdf
 */
template <typename I2C> class LIS3MDL {

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

	static constexpr uint8_t ADDR7			= 0b0011100;
	
	static constexpr uint8_t CTRL_REG1	= 0x20;
	static constexpr uint8_t CTRL_REG2	= 0x21;
	static constexpr uint8_t CTRL_REG3	= 0x22;
	static constexpr uint8_t CTRL_REG4	= 0x23;
	static constexpr uint8_t CTRL_REG5	= 0x24;

	static constexpr uint8_t REG_X		= 0x28;	//0x28(L= + 0x29(H)
	static constexpr uint8_t REG_Y		= 0x2A;	//0x2A(L) + 0x2B(H)
	static constexpr uint8_t REG_Z		= 0x2C;	//0x2C(L) + 0x2D(H)
	
	

public:

	struct Magnetometer {
		int16_t x;
		int16_t y;
		int16_t z;
	};
	
	struct Acceleromter {
		int16_t x;
		int16_t y;
		int16_t z;
	};
	
	enum class Resolution : uint8_t {
		GAUSS_4 = 0b00,
		GAUSS_8 = 0b01,
		GAUSS_12 = 0b10,
		GAUSS_16 = 0b11,
	};
	
	enum class AxisMode : uint8_t {
		LOW_POWER = 0b00,
		MEDIUM_PERFORMANCE = 0b01,
		HIGH_PERFORMANCE = 0b10,
		ULTRA_HIGH_PERFORMANCE = 0b11,
	};
	
	enum class SamplingRate : uint8_t {
		HZ_0_625,
		HZ_1_25,
		HZ_2_5,
		HZ_5,
		HZ_10,
		HZ_20,
		HZ_40,
		HZ_80,
	};
	
	enum class OperationMode : uint8_t {
		CONTINUOUS = 0b00,
		SINGLE = 0b01,
		OFF = 0b11,
	};
	
public:

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

	}


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

	

	void setResolution(Resolution res) {
		getAndSet(CTRL_REG2, 0b01100000, (uint8_t)res << 5);
	}
	
	void setAxisMode(AxisMode mode) {
		getAndSet(CTRL_REG1, 0b01100000, (uint8_t)mode << 5);	// x and y
		getAndSet(CTRL_REG4, 0b00001100, (uint8_t)mode << 2);	// z
	}

	void setSamplingRate(SamplingRate rate) {
		getAndSet(CTRL_REG1, 0b00011100, (uint8_t)rate << 2);
	}

	void setOperationMode(OperationMode mode) {
		getAndSet(CTRL_REG3, 0b00000011, (uint8_t)mode << 0);
	}

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

	void getAndSet(uint8_t reg, uint8_t setMask, uint8_t setVal) {
		uint8_t tmp = i2c.readReg8(ADDR7, reg);
		tmp = (tmp & ~setMask) | setVal;
		i2c.writeReg8(ADDR7, reg, tmp);
	}
	
};