/* hmc5883l.c */

#include "sensors.h"
#include "hmc5883l.h"
#include "i2c.h"
#include "event.h"
#include "log.h"
#include "uart.h"

bool hmc5883l_state;

i2c_result hmc5883l_result;

unsigned char hmc5883l_sample_data[6];

#define LOG_SIGNATURE_MAGNETOMETER 0xDA7AAA75

unsigned char hmc5883l_init_command[] = {0x00, 0x70};
unsigned char hmc5883l_init_command2[] = {0x01, 0xA0};

struct i2c_transaction hmc5883l_init_transaction2;

struct i2c_transaction hmc5883l_init_transaction = {
	(0x1E << 1) + 0, /* write */
	2,
	hmc5883l_init_command,
	&hmc5883l_result,
	EVENT_HMC5883L_I2C_COMPLETE,
	&hmc5883l_init_transaction2
};

struct i2c_transaction hmc5883l_init_transaction2 = {
	(0x1E << 1) + 0, /* write */
	2,
	hmc5883l_init_command2,
	&hmc5883l_result,
	EVENT_HMC5883L_I2C_COMPLETE,
	NULL
};

unsigned char hmc5883l_start_command[] = {0x02, 0x01};

struct i2c_transaction hmc5883l_start_transaction = {
	(0x1E << 1) + 0, /* write */
	2,
	hmc5883l_start_command,
	&hmc5883l_result,
	EVENT_HMC5883L_I2C_COMPLETE,
	NULL
};

struct i2c_transaction hmc5883l_sample_transaction = {
	(0x1E << 1) + 1, /* read */
	6,
	hmc5883l_sample_data,
	&hmc5883l_result,
	EVENT_HMC5883L_I2C_COMPLETE,
	NULL
};

void hmc5883l_event_handler(void);

bool hmc5883l_init(void)
{
	event_register(EVENT_HMC5883L_I2C_COMPLETE, hmc5883l_event_handler);

	if (!i2c_start_transaction(&hmc5883l_init_transaction))
		return FALSE;
	while (i2c_busy()) ;

	hmc5883l_state = 0;

	return (hmc5883l_result == I2C_SUCCESS);
}

bool hmc5883l_start_sample(void)
{
	hmc5883l_state = !hmc5883l_state;

	if (hmc5883l_state)
	    return i2c_start_transaction(&hmc5883l_start_transaction);
	else
	    return i2c_start_transaction(&hmc5883l_sample_transaction);
}

void hmc5883l_event_handler(void)
{
#if 0
	signed short int xi, yi, zi;
	signed short int tempi;
	signed short int rolli, pitchi, yawi;

	float x, y, z;
	float temp;
	float roll, pitch, yaw;
#endif

	if (hmc5883l_result != I2C_SUCCESS)
		return;

	hmc5883l_result = I2C_IN_PROGRESS;

	sensors_sample_done();

	/* If we just started a conversion, our job is done for now */
	if (hmc5883l_state)
	    return;

#if 0
	xi = (hmc5883l_sample_data[0] << 8) + hmc5883l_sample_data[1];
	yi = (hmc5883l_sample_data[2] << 8) + hmc5883l_sample_data[3];
	zi = (hmc5883l_sample_data[4] << 8) + hmc5883l_sample_data[5];

	tempi = (hmc5883l_sample_data[6] << 8) + hmc5883l_sample_data[7];

	rolli  = (hmc5883l_sample_data[ 8] << 8)+hmc5883l_sample_data[ 9];
	pitchi = (hmc5883l_sample_data[10] << 8)+hmc5883l_sample_data[11];
	yawi   = (hmc5883l_sample_data[12] << 8)+hmc5883l_sample_data[13];

	sensors_write_gyro_data(roll, pitch, yaw);
	sensors_write_accel_data(x, y, z);
	sensors_write_temp_data(temp);

#endif

	log_put_uint(LOG_SIGNATURE_MAGNETOMETER);
	log_put_array((char *)hmc5883l_sample_data, 6);
}