#include "math.h"
#include "Mag_HMC5883L.h"
#include <stdio.h>

int Mag_HMC5883_init(Mag_HMC5883_t *mag, const char *name, I2C_DEV_t *dev,
		GPIO_EXIT_t *exti) {
	uint8_t RxdBuf[3];
	int rslt;

	mag->name = name;
	mag->_dev = dev;
	mag->exti = exti;

	mag->cnt = 0;
	mag->last_cnt = 0;
	mag->pps = 0;

	I2C_DEV_begin(mag->_dev, -1);

	rslt = 0;
	if (I2C_DEV_read_registers(mag->_dev, 0x0A, RxdBuf, 3)) {
		if (RxdBuf[0] == 'H' && RxdBuf[1] == '4' && RxdBuf[2] == '3') {
			//TxdBuf[0] = 0x01; //Register B
			//TxdBuf[1] = 0x20; // default 1.3Ga 1090LSB/Gause, 0xF800-0x7FF
			//I2C_DEV_transfer(mag->_dev, TxdBuf, 2, NULL, 0);
			I2C_DEV_write_register(mag->_dev, 0x01, 0x20);

			I2C_DEV_write_register(mag->_dev, 0x00, 0x71);
			I2C_DEV_write_register(mag->_dev, 0x02, 0x01);
			HAL_Delay(10);
			I2C_DEV_read_registers(mag->_dev, 0x03, RxdBuf, 6);
			mag->magnet_xyz[0] =
					(int16_t) ((RxdBuf[0] << 8) | RxdBuf[1]);
			mag->magnet_xyz[2] =
					(int16_t) ((RxdBuf[2] << 8) | RxdBuf[3]);
			mag->magnet_xyz[1] =
					(int16_t) ((RxdBuf[4] << 8) | RxdBuf[5]);
			for (int i=0;i<3;++i)
			{
				if (mag->magnet_xyz[i]<900)
				{
					rslt = i+1;
				}
			}

			I2C_DEV_write_register(mag->_dev, 0x00, 0x72);
			I2C_DEV_write_register(mag->_dev, 0x02, 0x01);
			HAL_Delay(10);
			I2C_DEV_read_registers(mag->_dev, 0x03, RxdBuf, 6);
			mag->magnet_xyz[0] =
					(int16_t) ((RxdBuf[0] << 8) | RxdBuf[1]);
			mag->magnet_xyz[2] =
					(int16_t) ((RxdBuf[2] << 8) | RxdBuf[3]);
			mag->magnet_xyz[1] =
					(int16_t) ((RxdBuf[4] << 8) | RxdBuf[5]);
			for (int i=0;i<3;++i)
			{
				if (mag->magnet_xyz[i]>-900)
				{
					rslt = i+1;
				}
			}

			//TxdBuf[0] = 0x00; //Register A
			//TxdBuf[1] = 0x70; // 8 sample averge, 15Hz output, normal config
			//I2C_DEV_transfer(mag->_dev, TxdBuf, 2, NULL, 0);
			I2C_DEV_write_register(mag->_dev, 0x00, 0x70);

			//TxdBuf[0] = 0x02; // Mode Register
			//TxdBuf[1] = 0x00; // continuse mode
			//I2C_DEV_transfer(mag->_dev, TxdBuf, 2, NULL, 0);
			I2C_DEV_write_register(mag->_dev, 0x02, 0x00);

			HAL_Delay(10);

			if (I2C_DEV_read_registers(mag->_dev, 0x00, RxdBuf, 3)) {
				if (RxdBuf[0] == 0x70 && RxdBuf[1] == 0x20
						&& RxdBuf[2] == 0x00) {
					;//pass
				} else {
					rslt = -4;
				}
			} else {
				rslt = -3;
			}
		} else {
			rslt = -2;
		}
	} else {
		rslt = -1;
	}

	I2C_DEV_end(mag->_dev);
	return rslt;
}

bool Mag_HMC5883_update(Mag_HMC5883_t *mag) {
	bool rslt;
	uint8_t RxdBuf[6];

	rslt = false;

	if (I2C_DEV_begin(mag->_dev, 1)) {
		if (mag->exti)
		{
		   mag->exti->cnt = 0;
		   rslt = true;
		}
		else
		{
			if (I2C_DEV_read_registers(mag->_dev, 0x09, RxdBuf, 1)) {
				if (RxdBuf[0] & 0x01) {
					rslt = true;
				}
			}
		}
		if (rslt && I2C_DEV_read_registers(mag->_dev, 0x03, RxdBuf, 6)) {
			mag->magnet_xyz[0] =
					(int16_t) ((RxdBuf[0] << 8) | RxdBuf[1]);
			mag->magnet_xyz[2] =
					(int16_t) ((RxdBuf[2] << 8) | RxdBuf[3]);
			mag->magnet_xyz[1] =
					(int16_t) ((RxdBuf[4] << 8) | RxdBuf[5]);
			mag->cnt++;
		} else {
			rslt = false;
		}

		I2C_DEV_end(mag->_dev);
	} else {
		rslt = false;
	}

	return rslt;
}

void Mag_HMC5883_stats(Mag_HMC5883_t *mag) {
	mag->pps = mag->cnt - mag->last_cnt;
	mag->last_cnt = mag->cnt;
}