/*
 * $Id: adxl.c 464 2019-08-01 02:15:54Z true $
 * begin 20190527 true
 */

#include "sensor/adxl.h"
#include "math/i8atan2.h"

#include <string.h>


adxl345_data xdata adxl;
adxl345_data xdata adxl_smoothing;

uint8_t xdata buf[7];


void adxl345_init()
{
	buf[0] = ADXL345_REG_BW_RATE | ADXL345_MODE_WR | ADXL345_MODE_MB;
	buf[1] = ADXL345_BW_RATE_200; // | ADXL345_BW_RATE_LOW_POWER;
	buf[2] = ADXL345_POWER_CTL_MEASURE;
	spi0_xfer(0, buf, 3);

	buf[0] = ADXL345_REG_DATA_FORMAT | ADXL345_MODE_WR;
	buf[1] = ADXL345_DATA_FORMAT_RANGE_2G; // | ADXL345_DATA_FORMAT_FULL_RES;
	spi0_xfer(0, buf, 2);
}

void adxl345_tick()
{
	adxl345_get_axes(&adxl_smoothing);
	adxl.x += adxl_smoothing.x; adxl.x >>= 1;
	adxl.y += adxl_smoothing.y; adxl.y >>= 1;
	adxl.z += adxl_smoothing.z; adxl.z >>= 1;
}

void adxl345_get_axes(struct adxl345_data *adxl)
{
	buf[0] = ADXL345_REG_DATAX0 | ADXL345_MODE_RD | ADXL345_MODE_MB;
	memset(&buf[1], 0x00, 6);
	spi0_xfer(buf, buf, 7);

	adxl->x = buf[1] | (buf[2] << 8);
	adxl->y = buf[3] | (buf[4] << 8);
	adxl->z = buf[5] | (buf[6] << 8);
}

int8_t adxl345_get_rotation(struct adxl345_data *adxl)
{
	int8_t nx, ny, ret;

	nx = -adxl->x;
	ny = adxl->y;

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

	return ret;
}

void adxl345_set_interrupt(uint8_t interrupt_flags)
{
	UNUSED(interrupt_flags);
	// unimplemented
}

void adxl345_get_interrupt()
{
	buf[0] = ADXL345_REG_INT_SOURCE | ADXL345_MODE_RD;
	buf[1] = 0x00;
	spi0_xfer(buf, buf, 2);
}
