/*
 *
 */


#include "adxl345.h"

#include "math/i8atan2.h"


ADXL345 adxl = ADXL345();
ADXLXYZ accel;

ADXLXYZ accelbuf[32];


void adxl_init()
{
	adxl.powerOn();
	adxl.setRate(400);
	adxl.setLowPower(0);
	adxl.setFullResBit(1);
}

void adxl_update()
{
	uint8_t i;

	for (i = 0; i < 31; i++) {
		accelbuf[i].x = accelbuf[i + 1].x;
		accelbuf[i].y = accelbuf[i + 1].y;
		accelbuf[i].z = accelbuf[i + 1].z;
	}

	adxl.readAccel(&accelbuf[15].x, &accelbuf[15].y, &accelbuf[15].z);

	for (i = 0; i < 32; i++) {
		accel.x += accelbuf[i].x;
		accel.y += accelbuf[i].y;
		accel.z += accelbuf[i].z;
	}

	accel.x >>= 5;
	accel.y >>= 5;
	accel.z >>= 5;
}

int8_t adxl_get_rotation(struct ADXLXYZ *a)
{
	int8_t nx, ny, ret;

	nx = -a->x >> 1;
	ny = a->y >> 1;

	ret = i8atan2(ny, nx) >> 1;
	if (ret < 0) {
		ret += 128;
	}

	return ret;
}