/*
 * adxl.c
 *
 *  Created on: Jul 18, 2023
 *      Author: true
 */


#include "adxl.h"
#include "i8atan2.h"

#include <string.h>
#include <stdlib.h>


adxl345_axes adxl;
adxl345_axes adxl_last[8];
adxl345_axes adxl_smoothing;
adxl345_axes adxl_summing;

uint8_t buf[7];

uint8_t  last_idx = 0;
int16_t  movement = 0;
uint16_t movement_worst = 0;

static const uint8_t adxl_dfmt = ADXL345_DATA_FORMAT_RANGE_2G; // | ADXL345_DATA_FORMAT_FULL_RES;



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;	// BW_RATE
	buf[2] = ADXL345_POWER_CTL_MEASURE;								// POWER_CTL
	buf[3] = 0;														// INT_ENABLE
	buf[4] = 0xff;													// INT_MAP
	adxl_spi_xfer(0, buf, 5);

	buf[0] = ADXL345_REG_DATA_FORMAT | ADXL345_MODE_WR;
	buf[1] = adxl_dfmt;
	adxl_spi_xfer(0, buf, 2);
}

void adxl345_tick()
{
	uint8_t i;

	adxl_last[last_idx].x = adxl.x;
	adxl_last[last_idx].y = adxl.y;
	adxl_last[last_idx++].z = adxl.z;
	last_idx &= 0x7;

	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;

	if (!last_idx) {
		adxl_summing.x = adxl_summing.y = adxl_summing.z = 0;
		for (i = 0; i < 8; i++) {
			adxl_summing.x += adxl_last[i].x;
			adxl_summing.y += adxl_last[i].y;
			adxl_summing.z += adxl_last[i].z;
		}

		movement = 0;
		for (uint8_t i = 0; i < 4; i++) {
			movement += (adxl_last[i].x - adxl.x) + (adxl_last[i].y - adxl.y) + (adxl_last[i].z - adxl.z);
		}
		if (abs(movement) > movement_worst) {
			movement_worst = abs(movement);
		}
	}
}

void adxl345_get_axes(struct adxl345_axes *adxl)
{
	buf[0] = ADXL345_REG_DATAX0 | ADXL345_MODE_RD | ADXL345_MODE_MB;
	memset(&buf[1], 0x00, 6);
	adxl_spi_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_axes *adxl)
{
	int8_t nx, ny, ret;

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

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

	return ret;
}

int16_t adxl345_movement()
{
	return movement;
}

void adxl345_set_reg8(uint8_t reg, uint8_t val)
{
	buf[0] = reg | ADXL345_MODE_WR;
	buf[1] = val;
	adxl_spi_xfer(0, buf, 2);
}

void adxl345_set_intr(uint8_t int2_map, uint8_t interrupt_flags)
{
	buf[0] = ADXL345_REG_INT_ENABLE | ADXL345_MODE_WR | ADXL345_MODE_MB;
	buf[1] = interrupt_flags;			// set interrupts which are enabled
	buf[2] = int2_map;					// which interrupts map to INT2
	adxl_spi_xfer(0, buf, 3);
}

void adxl345_set_intr_polarity(uint8_t active_polarity)
{
	buf[0] = ADXL345_REG_DATA_FORMAT | ADXL345_MODE_WR;
	buf[1] = adxl_dfmt | (active_polarity ? 0 : ADXL345_DFMT_INT_INVERT);
	adxl_spi_xfer(buf, buf, 2);
}

uint8_t adxl345_get_intr_flag()
{
	buf[0] = ADXL345_REG_INT_SOURCE | ADXL345_MODE_RD;
	buf[1] = 0x00;
	adxl_spi_xfer(buf, buf, 2);

	return buf[1];
}
