#include "adxl345.h"
#include "./USART/USART.h"
#include "./I2C/I2C.h"


/***************************************************************************//**
 * @brief  First, the content of Tx1_Buffer is written to the EEPROM_WriteAddress1
 *         and the written data are read. The written and the read buffers data are
 *         then compared. Following the read operation, the program wait that the
 *         EEPROM reverts to its Standby state. A second write operation is, then,
 *         performed and this time, Tx2_Buffer is written to EEPROM_WriteAddress2,
 *         which represents the address just after the last written one in the first
 *         write. After completion of the second write operation, the written data
 *         are read. The contents of the written and the read buffers are compared.
 ******************************************************************************/
void ADXL345_Init(ADXL345_InitTypeDef* xl345)
{
    USART_SendStr(USART2, "Initializing ADXL345...");
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x31, 0x0B);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x2D, 0x08);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x2E, 0x80);

    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x1D, xl345->ADXL345_TapThreshold);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x1E, xl345->ADXL345_OFSX_UserOffsetAdjustement);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x1F, xl345->ADXL345_OFSY_UserOffsetAdjustement);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x20, xl345->ADXL345_OFSZ_UserOffsetAdjustement);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x21, xl345->ADXL345_EventMaximumDuration);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x22, xl345->ADXL345_DoubleTapLatency);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x23, xl345->ADXL345_DoubleTapWindow);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x24, xl345->ADXL345_ActivityDetectThreshold);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x25, xl345->ADXL345_InactivityDetectThreshold);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x27, xl345->ADXL345_ActInactSetup);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x28, xl345->ADXL345_FreeFallThreshold);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x29, xl345->ADXL345_FreeFallMinTime);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x2A, xl345->ADXL345_TapAxesSelect);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x2C, xl345->ADXL345_BandwidthRateSelect);

    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x2E, xl345->ADXL345_IrqEnable);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x2F, xl345->ADXL345_RemapInterruptsToINT2);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x31, xl345->ADXL345_DataFormat);

    uint8_t fifo_mode_temp;
    fifo_mode_temp = (uint8_t)(xl345->ADXL345_FIFO_Mode << 6) |
    		(uint8_t)((xl345->ADXL345_TriggerMappingBit & 1) << 5) |
    		(uint8_t)(xl345->ADXL345_NumOfSamples & 0x0F);
    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x38, 0);

    ADXL345_WriteRegister(XL345_ALT_WRITE, 0x2D, xl345->ADXL345_PowerControl);

    USART_SendStr(USART2, "done\r\n");
}

uint8_t ADXL345_ReadDevID(uint8_t dev_addr)
{
	uint8_t result = 0;
	result = ADXL345_ReadRegister(dev_addr, 0x00);
	return result;
}

uint8_t ADXL345_ReadRegister(uint8_t dev_addr, uint8_t reg)
{
	uint8_t result = 0;
	I2C_OpenCommunication (I2C_I2C, dev_addr, I2C_Direction_Transmitter);
	I2C_SendRegister (I2C_I2C, reg);
	I2C_SendRestart (I2C_I2C, dev_addr, I2C_Direction_Receiver);
	result = I2C_GetByte(I2C_I2C);
	return result;
}

void ADXL345_GetMeasurements(uint8_t dev_addr, float *values)
{
	uint8_t temp[6] = {};
	int16_t _x, _y, _z;
	I2C_OpenCommunication (I2C_I2C, dev_addr, I2C_Direction_Transmitter);
	I2C_SendRegister (I2C_I2C, XL345_DATAX0);
	I2C_SendRestart (I2C_I2C, dev_addr, I2C_Direction_Receiver);
	I2C_MultiRead(I2C_I2C, temp, 6);
	_x = temp[0] + (temp[1] * 256);
	_y = temp[2] + (temp[3] * 256);
	_z = temp[4] + (temp[5] * 256);
	values[0] = (float)_x;
	values[1] = (float)_y;
	values[2] = (float)_z;
}

void ADXL345_WriteRegister(uint8_t dev_addr, uint8_t reg, uint8_t value)
{
	I2C_OpenCommunication (I2C_I2C, dev_addr, I2C_Direction_Transmitter);
	I2C_SendRegister (I2C_I2C, reg);
	I2C_SendByte (I2C_I2C, value);
	I2C_GenerateSTOP(I2C_I2C, ENABLE);
}