/*
 * compass: for the Orangutan LV, SV, SVP, and X2.
 * >>> MODIFIED FOR SINGLE ATMEGA8 CHIP
 * >>> CURRENTLY A STRIPPED DOWN VERSION FOR TESTING PURPOSES
 *
 * This example program demonstrates how to use the LSM303DLH 3D compass and
 * accelerometer carrier with an Orangutan robot controller.
 *
 * The LSM303DLH carrier should be connected to the Orangutan's I2C pins; on the
 * LV and SV, these are PC5 and PC4 for SCL and SDA, respectively, and on the
 * SVP and X2, these are PC0 and PC1, respectively. (PC0 and PC1 are LCD data
 * lines on the X2, but this code will work on it if the LCD is not used, or
 * used in 4-bit mode.) 
 *
 * http://www.pololu.com
 * http://forum.pololu.com
 */

#define F_CPU 4000000 // 4mhz

#include <avr/io.h>  
#include <util/delay.h>
// #include <pololu/orangutan.h>  
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "vector.h"

/* 
 * This program assumes that the LSM303DLH carrier is oriented with X pointing
 * to the right, Y pointing backward, and Z pointing down (toward the ground).
 * The code compensates for tilts of up to 90 degrees away from horizontal.
 * Vector p should be defined as pointing forward, parallel to the ground,
 * with coordinates {X, Y, Z}.
 */
vector p = {0, -1, 0};
/*
 * m_max and m_min are calibration values for the maximum and minimum
 * measurements recorded on each magnetic axis, which can vary for each
 * LSM303DLH. You should replace the values below with max and min readings from
 * your particular device.
 *
 * To obtain the max and min values, you can use this program's
 * calibration mode, which is enabled by pressing one of the pushbuttons. While
 * calibration mode is active, point each of the axes of the LSM303DLH toward
 * and away from the earth's North Magnetic Pole. Due to space constraints on an
 * 8x2 LCD, only one axis is displayed at a time; each button selects an axis to
 * display (top = X, middle = Y, bottom = Z), and pressing any button a second
 * time exits calibration mode and returns to normal operation.
 */
vector m_max = {340, 517, 416};
vector m_min = {-642, -496, -462};

void i2c_start() {  
	TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN); // send start condition  
	while (!(TWCR & (1 << TWINT)));  
}  
  
void i2c_write_byte(char byte) {  
	TWDR = byte;              
	TWCR = (1 << TWINT) | (1 << TWEN); // start address transmission  
	while (!(TWCR & (1 << TWINT)));  
}  
  
char i2c_read_byte() {  
	TWCR = (1 << TWINT) | (1 << TWEA) | (1 << TWEN); // start data reception, transmit ACK  
	while (!(TWCR & (1 << TWINT)));  
	return TWDR;  
}  

char i2c_read_last_byte() {  
	TWCR = (1 << TWINT) | (1 << TWEN); // start data reception
	while (!(TWCR & (1 << TWINT)));  
	return TWDR;  
}  
  
void i2c_stop() {  
	  TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); // send stop condition  
}  

// Returns a set of acceleration and raw magnetic readings from the cmp01a.
void read_data_raw(vector *a, vector *m)
{
	// read accelerometer values
	i2c_start();
	i2c_write_byte(0x30); // write acc
	i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment
	i2c_start();		  // repeated start
	i2c_write_byte(0x31); // read acc
	unsigned char axl = i2c_read_byte();
	unsigned char axh = i2c_read_byte();
	unsigned char ayl = i2c_read_byte();
	unsigned char ayh = i2c_read_byte();
	unsigned char azl = i2c_read_byte();
	unsigned char azh = i2c_read_last_byte();
	i2c_stop();

	// read magnetometer values
	i2c_start(); 
	i2c_write_byte(0x3C); // write mag
	i2c_write_byte(0x03); // OUTXH_M
	i2c_start();		  // repeated start
	i2c_write_byte(0x3D); // read mag
	unsigned char mxh = i2c_read_byte();
	unsigned char mxl = i2c_read_byte();
	unsigned char myh = i2c_read_byte();
	unsigned char myl = i2c_read_byte();
	unsigned char mzh = i2c_read_byte();
	unsigned char mzl = i2c_read_last_byte();
	i2c_stop();

	a->x = axh << 8 | axl;
	a->y = ayh << 8 | ayl;
	a->z = azh << 8 | azl;
	m->x = mxh << 8 | mxl;
	m->y = myh << 8 | myl;
	m->z = mzh << 8 | mzl;
}

// Returns a set of acceleration and adjusted magnetic readings from the cmp01a.
void read_data(vector *a, vector *m)
{
	read_data_raw(a, m);

	// shift and scale
	m->x = (m->x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
	m->y = (m->y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
	m->z = (m->z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
}

// Returns a heading (in degrees) given an acceleration vector a due to gravity, a magnetic vector m, and a facing vector p.
int get_heading(const vector *a, const vector *m, const vector *p)
{
	vector E;
	vector N;

	// cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "east"
	vector_cross(m, a, &E);
	vector_normalize(&E);

	// cross "down" with "east" to produce "north" (parallel to the ground)
	vector_cross(a, &E, &N);
	vector_normalize(&N);

	// compute heading
	int heading = round(atan2(vector_dot(&E, p), vector_dot(&N, p)) * 180 / M_PI);
	if (heading < 0)
		heading += 360;
	return heading;
}

void delay_ms(unsigned int time_ms)
{
  while (time_ms--)
    _delay_ms(1);
}

int main()
{
	// DATA OUTPUT TO LED BAR
	DDRD = 255;
	PORTD = 0;

	DDRC = 1; // PC0 as blinking LED
	
	// IT IS NOT NECCESARY TO ENABLE THIS, THE CIRCUIT HAS EXTERNAL 4.7K PULLUPS
	// PORTC = (1 << PORTC4) | (1 << PORTC5); // enable pull-ups on SDA and SCL, respectively

	TWSR = 0;  // clear bit-rate prescale bits
	
	// FOR ATMEGA8
	TWBR = 12; // produces an SCL frequency of 100 kHz with a 4 MHz CPU clock speed

	//enable accelerometer
	i2c_start(); 
	i2c_write_byte(0x30); // write acc
	i2c_write_byte(0x20); // CTRL_REG1_A
	i2c_write_byte(0x27); // normal power mode, 50 Hz data rate, all axes enabled
	i2c_stop();

	//enable magnetometer
	i2c_start(); 
	i2c_write_byte(0x3C); // write mag
	i2c_write_byte(0x02); // MR_REG_M
	i2c_write_byte(0x00); // continuous conversion mode
	i2c_stop();

	// Data variables (high and low) for each axis
	unsigned char axl,axh,ayl,ayh,azl,azh,ret = 0;
	unsigned char mxl,mxh,myl,myh,mzl,mzh = 0;

	// DEBUG CODE TEST BEGIN
	while(1){
		
		// read accelerometer values		
		i2c_start();
		i2c_write_byte(0x30); // write acc
		i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment
		i2c_start();		  // repeated start
		i2c_write_byte(0x31); // read acc
		axl = i2c_read_byte();
		axh = i2c_read_byte();
		ayl = i2c_read_byte();
		ayh = i2c_read_byte();
		azl = i2c_read_byte();
		azh = i2c_read_last_byte();
		i2c_stop();


		// read magnetometer values
		i2c_start(); 
		i2c_write_byte(0x3C); // write mag
		i2c_write_byte(0x03); // OUTXH_M
		i2c_start();		  // repeated start
		i2c_write_byte(0x3D); // read mag
		mxh = i2c_read_byte();
		mxl = i2c_read_byte();
		myh = i2c_read_byte();
		myl = i2c_read_byte();
		mzh = i2c_read_byte();
		mzl = i2c_read_last_byte();
		i2c_stop();

		// Output data to port
		PORTD = axl; // Output the read value for the accelerometer X axis low byte

		// Blink led to know that things are running
		for(int r=0;r<5;r++) delay_ms(100); // Esperar 1 seg
		PORTC = 1;
		for(int r=0;r<5;r++) delay_ms(100); // Esperar 1 seg
		PORTC = 0;
		
	}
}