/**************************
  @file QMC5883LCompass.cpp
  put this file in the sketch folder
**************************/

/*
*** This library was trimmed to make it compatible with the small RAM available in ATtiny85 ***
*** No smoothing no calibration, no bearing, just the raw Azimuth data ***
===============================================================================================================
Library for using QMC5583L series chip boards as a compass.
Learn more at [https://github.com/mprograms/QMC5883LCompass]
===============================================================================================================
v1.0 - June 13, 2019
Written by MPrograms 
Github: [https://github.com/mprograms/]

Release under the GNU General Public License v3
[https://www.gnu.org/licenses/gpl-3.0.en.html]
===============================================================================================================
*/

#include "Arduino.h" 
#include "QMC5883LCompass.h"
#include <Wire.h>

QMC5883LCompass::QMC5883LCompass() {
}

void QMC5883LCompass::init(){
	Wire.begin();
	_writeReg(0x0B,0x01);
	setMode(0x01,0x0C,0x10,0X00);
}

void QMC5883LCompass::_writeReg(byte r, byte v){
	Wire.beginTransmission(_ADDR);
	Wire.write(r);
	Wire.write(v);
	Wire.endTransmission();
}

void QMC5883LCompass::setMode(byte mode, byte odr, byte rng, byte osr){
	_writeReg(0x09,mode|odr|rng|osr);
}

void QMC5883LCompass::setMagneticDeclination(int degrees, uint8_t minutes) {
	_magneticDeclinationDegrees = degrees + minutes / 60;
}

void QMC5883LCompass::setReset(){
	_writeReg(0x0A,0x80);
}

void QMC5883LCompass::read(){
	Wire.beginTransmission(_ADDR);
	Wire.write(0x00);
	int err = Wire.endTransmission();
	if (!err) {
		Wire.requestFrom(_ADDR, (byte)6);
		_vRaw[0] = (int)(int16_t)(Wire.read() | Wire.read() << 8);
		_vRaw[1] = (int)(int16_t)(Wire.read() | Wire.read() << 8);
		_vRaw[2] = (int)(int16_t)(Wire.read() | Wire.read() << 8);
	}
}

int QMC5883LCompass::getX(){
	return _get(0);
}

int QMC5883LCompass::getY(){
	return _get(1);
}

int QMC5883LCompass::getZ(){
	return _get(2);
}

int QMC5883LCompass::_get(int i){
  return _vRaw[i];
}

int QMC5883LCompass::getAzimuth(){
	float heading = atan2( getY(), getX() ) * 180.0 / PI;
	heading += _magneticDeclinationDegrees;
	return (int)heading % 360;
}