#include "Accelerometer.h"
#include "GPIO.h"
#include "RCC.h"

#include "stm32f4xx.h"

static uint8_t ReadByte(uint8_t address);
static void ReadBytes(uint8_t *buffer, uint8_t address, int numbytes);
static void WriteByte(uint8_t byte, uint8_t address);
static void WriteBytes(uint8_t *bytes, uint8_t address, int numbytes);

static inline uint8_t LowerCS();
static inline uint8_t RaiseCS();
static uint8_t TransferByte(uint8_t byte);

void InitializeAccelerometer() {
	// Enable peripherals.
	EnableAHB1PeripheralClock(RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOEEN);
	EnableAPB2PeripheralClock(RCC_APB2ENR_SPI1EN);

	// Configure SPI pins as alternate function.
	SelectAlternateFunctionForGPIOPin(GPIOA, 5, 5);
	SelectAlternateFunctionForGPIOPin(GPIOA, 6, 5);
	SelectAlternateFunctionForGPIOPin(GPIOA, 7, 5);
	SetGPIOAlternateFunctionMode(GPIOA, (1 << 5) | (1 << 6) | (1 << 7));
	SetGPIOPushPullOutput(GPIOA, (1 << 5) | (1 << 6) | (1 << 7));
	SetGPIOSpeed50MHz(GPIOA, (1 << 5) | (1 << 6) | (1 << 7));
	SetGPIOPullDownResistor(GPIOA, (1 << 5) | (1 << 6) | (1 << 7));

	// Configure and enable SPI.
	SetAPB2PeripheralReset(RCC_APB2RSTR_SPI1RST);
	ClearAPB2PeripheralReset(RCC_APB2RSTR_SPI1RST);

	SPI1->I2SCFGR &= ~SPI_I2SCFGR_I2SMOD; // Disable I2S mode.
	SPI1->CR1 = SPI_CR1_SSM | (1 * SPI_CR1_BR_0) | SPI_CR1_SSI | SPI_CR1_MSTR | SPI_CR1_SPE;

	// Configure CS pin as input to be compatible with VGA driver.
	// Set the output register to 0 anyway, and reconfigure later in LowerCS().
	SetGPIOInputMode(GPIOE, (1 << 3));
	SetGPIOPushPullOutput(GPIOE, (1 << 3));
	SetGPIOSpeed50MHz(GPIOE, (1 << 3));
	SetGPIOPullUpResistor(GPIOE, (1 << 3));
	GPIOE->BSRRH = 1 << 3;

	// Configure interrupt pins.
	SetGPIOInputMode(GPIOE, (1 << 0) | (1 << 1));
	SetGPIONoPullResistor(GPIOE, (1 << 0) | (1 << 1));
}

bool PingAccelerometer() {
	uint8_t byte = ReadByte(LIS302DL_WHO_AM_I_ADDR);
	return byte == 0x3b;
}

void SetAccelerometerMainConfig(uint8_t config) {
	WriteByte(config, LIS302DL_CTRL_REG1_ADDR);
}

void SetAccelerometerFilterConfig(uint8_t config) {
	WriteByte(config, LIS302DL_CTRL_REG2_ADDR);
}

void SetAccelerometerInterruptConfig(uint8_t config) {
	WriteByte(config, LIS302DL_CLICK_CFG_REG_ADDR);
}

void ResetAccelerometer() {
	uint8_t val = ReadByte(LIS302DL_CTRL_REG2_ADDR);
	WriteByte(val | LIS302DL_BOOT_REBOOTMEMORY, LIS302DL_CTRL_REG2_ADDR);
}

void ResetAccelerometerFilter() {
	ReadByte(LIS302DL_HP_FILTER_RESET_REG_ADDR);
}

void ReadRawAccelerometerData(int8_t *values) {
	uint8_t buffer[5];
	ReadBytes(buffer, LIS302DL_OUT_X_ADDR, 5);
	values[0] = (int8_t)buffer[0];
	values[1] = (int8_t)buffer[2];
	values[2] = (int8_t)buffer[4];
}

#define ReadCommand 0x80 
#define MultiByteCommand 0x40

static uint8_t ReadByte(uint8_t address) {  
	LowerCS();
	TransferByte(address | ReadCommand);
	uint8_t byte = TransferByte(0);
	RaiseCS();
	return byte;
}

static void ReadBytes(uint8_t *buffer, uint8_t address, int numbytes)
{  
	if (numbytes > 1) 
    address |= ReadCommand | MultiByteCommand;
	else
    address |= ReadCommand;

	LowerCS();
	TransferByte(address);
	for (int i = 0; i < numbytes; i++)
    buffer[i] = TransferByte(0);
	RaiseCS();
}

static void WriteByte(uint8_t byte, uint8_t address) {
	LowerCS();
	TransferByte(address);
	TransferByte(byte);
	RaiseCS();
}

static void WriteBytes(uint8_t *bytes, uint8_t address, int numbytes) {
	if (numbytes > 1)
    address |= MultiByteCommand;

	LowerCS();
	TransferByte(address);
	for (int i = 0; i < numbytes; i++)
    TransferByte(bytes[i]);
	RaiseCS();
}

static inline uint8_t LowerCS() {
	SetGPIOOutputMode(GPIOE, (1 << 3));
}

static inline uint8_t RaiseCS() {
	SetGPIOInputMode(GPIOE, (1 << 3));
}

#define Timeout 0x1000

static uint8_t TransferByte(uint8_t byte) {
	uint32_t timer = Timeout;
	while (!(SPI1->SR & SPI_SR_TXE))
    if (timer-- == 0)
      return 0;

	SPI1->DR = byte;
	timer = Timeout;
	while (!(SPI1->SR & SPI_SR_RXNE))
    if (timer-- == 0) 
      return 0;

	return SPI1->DR;
}

