/* mpl3115a2.c */

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

bool mpl3115a2_state;

i2c_result mpl3115a2_result;

unsigned char mpl3115a2_sample_data[5];

#define LOG_SIGNATURE_ALTIMETER 0xDA7AAA70

unsigned char mpl3115a2_start_command[] = {0x26, 0x82};
unsigned char mpl3115a2_read_address[] = {0x01};

struct i2c_transaction mpl3115a2_start_transaction = {
	(0x60 << 1) + 0, /* write */
	sizeof(mpl3115a2_start_command),
	mpl3115a2_start_command,
	&mpl3115a2_result,
	EVENT_MPL3115A2_I2C_COMPLETE,
	NULL
};

struct i2c_transaction mpl3115a2_sample_transaction2;

struct i2c_transaction mpl3115a2_sample_transaction = {
	(0x60 << 1) + 0, /* write */
	sizeof(mpl3115a2_read_address),
	mpl3115a2_read_address,
	&mpl3115a2_result,
	EVENT_MPL3115A2_I2C_COMPLETE,
	&mpl3115a2_sample_transaction2
};
struct i2c_transaction mpl3115a2_sample_transaction2 = {
	(0x60 << 1) + 1, /* read */
	5,
	mpl3115a2_sample_data,
	&mpl3115a2_result,
	EVENT_MPL3115A2_I2C_COMPLETE,
	NULL
};

void mpl3115a2_event_handler(void);

bool mpl3115a2_init(void)
{
	event_register(EVENT_MPL3115A2_I2C_COMPLETE, mpl3115a2_event_handler);

	mpl3115a2_state = 0;

	return TRUE;

#if 0
	if (!i2c_start_transaction(&mpl3115a2_init_transaction))
		return FALSE;
	while (i2c_busy()) ;


	return (mpl3115a2_result == I2C_SUCCESS);
#endif
}

bool mpl3115a2_start_sample(void)
{
	mpl3115a2_state = !mpl3115a2_state;

	putstr("X");

	if (mpl3115a2_state)
	    return i2c_start_transaction(&mpl3115a2_start_transaction);
	else
	    return i2c_start_transaction(&mpl3115a2_sample_transaction);
}

void mpl3115a2_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 (mpl3115a2_result != I2C_SUCCESS)
		return;

	mpl3115a2_result = I2C_IN_PROGRESS;

	sensors_sample_done();

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

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

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

	rolli  = (mpl3115a2_sample_data[ 8] << 8)+mpl3115a2_sample_data[ 9];
	pitchi = (mpl3115a2_sample_data[10] << 8)+mpl3115a2_sample_data[11];
	yawi   = (mpl3115a2_sample_data[12] << 8)+mpl3115a2_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_ALTIMETER);
	log_put_array((char *)mpl3115a2_sample_data, 5);
	putstr("Y");

#if 0
	puthex(mpl3115a2_sample_data[0]);
	puthex(mpl3115a2_sample_data[1]);
	puthex(mpl3115a2_sample_data[2]);
	putstr("\n");
#endif
}