#include "HMC5883L.h"
#ifndef F_CPU
# warning "F_CPU not defined for compass.h"
# define F_CPU 16000000UL
#endif

#include <util/delay.h>
#include <stdio.h>
#include "HMC5883L.h"
#include "uart.h"

char Buffer[50];
int16_t X_axis,Y_axis,Z_axis;
float m_scale = 1.0;

unsigned char HMC5883L_init(void)
{
  i2c_start(HMC5883L_WRITE_ADDR );
  i2c_write(0x02);
  i2c_write(0x00);
  i2c_stop();

  i2c_start(HMC5883L_WRITE_ADDR );
  i2c_write(0x01);
  i2c_write(0x20);
  i2c_stop();

  i2c_start(HMC5883L_WRITE_ADDR );
  i2c_write(0x00);
  i2c_write(0x10);
  i2c_stop();
  
  HMC5883L_set_scale(1.3); 

  _delay_ms(6);
  
	return 1; // command successfully
}

unsigned char HMC5883L_get_axis(void)
{
	i2c_start(HMC5883L_WRITE_ADDR );
	i2c_write(0x03);
	i2c_stop();
	
	i2c_start(HMC5883L_READ_ADDR );
	
	X_axis  = (int16_t)i2c_readAck()<<8;
	X_axis |= (int16_t)i2c_readAck();
	
	Z_axis  = (int16_t)i2c_readAck()<<8;
	Z_axis |= (int16_t)i2c_readAck();
	
	Y_axis  = (int16_t)i2c_readAck()<<8;
	Y_axis |= (int16_t)i2c_readNak();
	
	i2c_stop();
	
	_delay_ms(100);
	
	return 1; // command successfully
}

unsigned char HMC5883L_get_raw(void)
{
	HMC5883L_get_axis();
			
	sprintf(Buffer,"RAW_Data ->     x : %d     ||      y : %d     ||     z : %d     \n",x,y,z);
	uart_puts(Buffer);
	
	return 1; // command successfully
}

unsigned char HMC5883L_set_scale(float gauss)
{
	unsigned char value = 0;
	
	if(gauss == 0.88)
	{
		value = 0x00;
		m_scale = 0.73;
	}
	else if(gauss == 1.3)
	{
		value = 0x01;
		m_scale = 0.92;
	}
	else if(gauss == 1.9)
	{
		value = 0x02;
		m_scale = 1.22;
	}
	else if(gauss == 2.5)
	{
		value = 0x03;
		m_scale = 1.52;
	}
	else if(gauss == 4.0)
	{
		value = 0x04;
		m_scale = 2.27;
	}
	else if(gauss == 4.7)
	{
		value = 0x05;
		m_scale = 2.56;
	}
	else if(gauss == 5.6)
	{
		value = 0x06;
		m_scale = 3.03;
	}
	else if(gauss == 8.1)
	{
		value = 0x07;
		m_scale = 4.35;
	}
		
	value <<= 5;
	i2c_start(HMC5883L_WRITE_ADDR );
	i2c_write(Config_Reg_B);
	i2c_write(value);
	i2c_stop();
	
	return 1; // command successfully
}

unsigned char HMC5883L_scale_axes(void)
{
	X_axis *= m_scale;
	Z_axis *= m_scale;
	Y_axis *= m_scale;
	
	return 1; // command successfully
}

float HMC5883L_heading(void)
{
	register float heading = 0.0;
	
	HMC5883L_get_axis();
	HMC5883L_scale_axes();
	heading = atan2((double)Y_axis,(double)X_axis) * 180 / 3.14159265 + 180;
 	
	if(heading < 0)
 	{
	 	heading += 360; // angle from 0 to 359 instead of plus/minus 180
 	}
	 
	return heading;
}