#include "mma8653.h"
#include <util/twi.h>

#define AXL_ADDR 0x1D
#define AXL_ERR_RET(e) if(1){err = e; goto err;}

#define TRY(c) if(1){ret = c; if (ret) return c;}
static int mma8653_i2c_transfer(int write, uint8_t reg, uint8_t *data, uint8_t length)
{
	int err = 0;
	
	// WRITE TARGET REGISTER
	// Send start
	TWCR = (1<<TWINT) | (1<<TWSTA) | (1 << TWEN);
	while (!(TWCR & (1<<TWINT)));
	if ((TWSR & 0xF8) != TW_START)
		AXL_ERR_RET(1);
	// Send device address
	TWDR = (AXL_ADDR << 1);
	TWCR = (1<<TWINT) | (1<<TWEN);
	while (!(TWCR & (1<<TWINT)));
	if ((TWSR & 0xF8) != TW_MT_SLA_ACK)
		AXL_ERR_RET(2);
	// Write register address
	TWDR = reg;
	TWCR = (1<<TWINT) | (1<<TWEN);
	while (!(TWCR & (1<<TWINT)));
	if ((TWSR & 0xF8) != TW_MT_DATA_ACK)
		AXL_ERR_RET(3);

	if (!write) {
		// Get data
		// Send re-start
		TWCR = (1<<TWINT) | (1<<TWSTA) | (1 << TWEN);
		while (!(TWCR & (1<<TWINT)));
		if ((TWSR & 0xF8) != TW_REP_START)
			AXL_ERR_RET(4);
		// Command a read cycle
		TWDR = (AXL_ADDR << 1) | (!write);
		TWCR = (1<<TWINT) | (1<<TWEN);
		while (!(TWCR & (1<<TWINT)));
		if ((TWSR & 0xF8) != (write ? TW_MT_SLA_ACK : TW_MR_SLA_ACK))
			AXL_ERR_RET(5);
	}
	while (length--) {
		if (write) {
			TWDR = *(data++);
			TWCR = (1<<TWINT) | (1<<TWEN);
			while (!(TWCR & (1<<TWINT)));
			if ((TWSR & 0xF8) != TW_MT_DATA_ACK)
				AXL_ERR_RET(6);
		} else {
			// Read byte
			TWCR = (1<<TWINT) | (1<<TWEN) | ( (length != 0) << TWEA);
			while (!(TWCR & (1<<TWINT)));
			if ((TWSR & 0xF8) != (length != 0 ? TW_MR_DATA_ACK : TW_MR_DATA_NACK))
				AXL_ERR_RET(6);
			*(data++) = TWDR;
		}
	}
	// Send stop
	TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO);
	while (TWCR & (1<<TWSTO));
	
	return 0;
	
	err:
		TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO); // Send stop to abort
		return err;
}

static inline int mma8653_i2c_write(uint8_t reg, uint8_t value)
{
	return mma8653_i2c_transfer(1, reg, &value, 1);
}

int mma8653_init() {
	int ret;

	TWBR = 0xff;
	//TWSR = 3;

	TRY(mma8653_i2c_write(0x2A, (1 << 1) | (1 << 0)));
	TRY(mma8653_i2c_write(0x2B, (1 << 7))); // Test mode en
	return 0;
}

int mma8653_get_measurements(struct axl_result *measurements) {
	int ret;
	int8_t buffer[4];

	TRY(mma8653_i2c_transfer(0, 0x0, (uint8_t*) buffer, 4));
	measurements->x = buffer[1];
	measurements->y = buffer[2];
	measurements->z = buffer[3];
/*
	led[0].r = buffer[1] > 0;
	led[1].r = buffer[2] > 0;
	led[2].r = buffer[3] > 0;
	
	led[5].r = !!(buffer[0] & 1); // 0
	led[5].g = !!(buffer[0] & 2); // 1
	led[5].b = !!(buffer[0] & 4); // 2
	led[4].r = !!(buffer[0] & 8); // 3
	led[4].g = !!(buffer[0] & 16); // 4
	led[4].b = !!(buffer[0] & 32); // 5
	led[3].r = !!(buffer[0] & 64); // 6
	led[3].g = !!(buffer[0] & 128); // 7
*/
	return 0;
}