#ifndef HMC5983_H
#define HMC5983_H

#include <spi/spi.h>
#include <gpio/gpio.h>
#include <os/time.h>

class HMC5983 {
	private:
		Pin cs;
		SPI_t& spi;
		uint8_t raw_data[6];
		
		void read_register(uint8_t reg, uint8_t len, uint8_t *data) {
			cs.off();
			if (len > 1) {
				spi.transfer_byte(reg | (1 << 7) | (1 << 6));
			} else {
				spi.transfer_byte(reg | (1 << 7));
			}
			while(len--) {
				*data++ = spi.transfer_byte();
			}
			cs.on();
		}
		
		void write_register(uint8_t reg, uint8_t data) {
			cs.off();
			spi.transfer_byte(reg);
			spi.transfer_byte(data);
			cs.on();
		}
	public:
		float x, y, z;
		uint8_t ident;
		HMC5983(Pin cs_pin, SPI_t& spi_bus) : cs(cs_pin), spi(spi_bus) {}
		
		void init() {
			spi.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (2 << 3) | (1 << 2) | (1 << 1) | (1 << 0);
			spi.reg.CR2 = (1 << 12) | ( 7 << 8);
			Time::sleep(100);
			ident = 'O';
			while(ident != 'H'){
				read_register(0x0A, 1, &ident);
			}
			write_register(0x00, (1 << 7) | (1 << 5) | (2 << 2));
			write_register(0x01, 0x00);
			write_register(0x02, 0x00);
			Time::sleep(600);
		}
		
		uint8_t update() {
			int16_t raw_x, raw_y, raw_z;
			uint8_t status;
			spi.reg.CR1 = (1 << 9) | (1 <<8) | (1 << 6) | (2 << 3) | (1 << 2) | (1 << 1) | (1 << 0);
			spi.reg.CR2 = (1 << 12) | ( 7 << 8);
			// Read status;
			read_register(0x09, 1, &status);
			if (status & 1) {
				read_register(0x03, 6, raw_data);
				raw_x = raw_data[0] << 8;
				raw_x |= raw_data[1];
				raw_z = raw_data[2] << 8;
				raw_z |= raw_data[3];
				raw_y = raw_data[4] << 8;
				raw_y |= raw_data[5]; 
				
				x = float(raw_x);
				y = float(raw_y);
				z = float(raw_z);
			}
			return status;
		}
};

#endif