#include "Mezz_read_SPI.h"


// CTRL1_XL (10h)address
#define  range2g (uint8_t)0
#define  range4g (uint8_t)8
#define  range8g (uint8_t)12
#define  range16g (uint8_t)4
#define  antiAliXL400 (uint8_t)0
#define  antiAliXL200 (uint8_t)1
#define  antiAliXL100 (uint8_t)2
#define  antiAliXL50 (uint8_t)3
#define  updateXL104Hz (uint8_t)0x40
#define  updateXL416Hz (uint8_t)0x60
#define  updateXL1_66Hz (uint8_t)0x80
#define  updateXL6_66Hz (uint8_t)0xA0

// CTRL2_G (11h)address
#define range245dps (uint8_t) 0
#define  range1000dps (uint8_t) 8
#define  range2000dps (uint8_t)12
#define  range500dps (uint8_t)4
#define  range125dps (uint8_t)2
#define  updateG104Hz (uint8_t)0x40
#define  updateG416Hz (uint8_t)0x60
#define  updateG833Hz (uint8_t)0x70
#define  updateG1_66Hz (uint8_t)0x80

//----Accelerometers set-up----
const uint8_t setUpXL = range4g + updateXL6_66Hz;
const uint8_t setUpG = range500dps + updateG1_66Hz;

int16_t roll = 0;
int16_t pitch = 0;
int16_t yaw = 0;
int16_t lon = 0;
int16_t lat = 0;
int16_t ver = 0;

uint8_t initIMU_flag = 1;

/*	Partly adopted from
 *   Title:  Indirect Speed Measurement for All-wheel-drive Racing Vehicles / ECUG_SPI
*    Author: Lukas Hostacny
*    Date:   2016
*    Code version: 3.0
*    Availability: https://dspace.cvut.cz/handle/10467/64682
*
*/

int16_t  Mezz_read_SPI(int16_t * pData1, int16_t * pData2, int16_t * pData3)
{
	int16_t xlData[3];
	int16_t gData[3];
#ifdef MACRON
	if(initIMU_flag == 1)
	{
		GYRO_Init();
		HAL_Delay(200);
		initIMU_flag = 0;
	}

	GYRO_read_axis_XL_int16(xlData);
	lon = xlData[0];
	lat = xlData[1];
	ver = xlData[2];

	GYRO_read_axis_G_int16(gData);
	roll = gData[0];
	pitch = gData[1];
	yaw = gData[2];

#endif
}


#ifdef MACRON
int16_t GYRO_read_axis_XL_int16(int16_t * data)
{
	uint8_t tempReg[2] = {0, 0};
	int16_t dataReady = 0;

	if(GYRO_dataReady_XL())
	{
		tempReg[0] = GYRO_read_register(0x28);// OUTX_L_XL 0x28
		tempReg[1]= GYRO_read_register(0x29);//OUTX_H_XL 0x29
		data[0] = ((((int16_t)tempReg[1]) << 8) + ((int16_t)tempReg[0]));

		tempReg[0] = GYRO_read_register(0x2A);// OUTY_L_XL 0x2A
		tempReg[1] = GYRO_read_register(0x2B);//OUTY_H_XL 0x2B
		data[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);

		tempReg[0] = GYRO_read_register(0x2C);// OUTZ_L_XL 0x2C
		tempReg[1] = GYRO_read_register(0x2D);//OUTZ_H_XL 0x2D
		data[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);

		dataReady = 1;
	}
	return dataReady;
}

int16_t  GYRO_read_axis_G_int16(int16_t * data)
{
	uint8_t tempReg[2] = {0, 0};
	int16_t dataReady = 0;
	if(GYRO_dataReady_G())
	{
		tempReg[0] = GYRO_read_register(0x22);// OUTX_L_G 0x22
		tempReg[1] = GYRO_read_register(0x23);//OUTX_H_G 0x23
		data[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);

		tempReg[0] = GYRO_read_register(0x24);// OUTX_L_G 0x22
		tempReg[1] = GYRO_read_register(0x25);//OUTX_H_G 0x23
		data[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);

		tempReg[0] = GYRO_read_register(0x26);// OUTX_L_G 0x22
		tempReg[1] = GYRO_read_register(0x27);//OUTX_H_G 0x23
		data[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);

		dataReady = 1;
	}
	return dataReady;
}

uint8_t GYRO_dataReady_XL(void)
{
	uint8_t temp;
	temp = GYRO_read_register(0x1E); //STATUS_REG  (- - - - EV_BOOT TDA GDA XLDA) available data

	if((temp&0x01) == 0x01) // XLDA
	{return 1;}
	else
	{return 0;}
}

uint8_t GYRO_dataReady_G(void)
{
	uint8_t temp;
	temp = GYRO_read_register(0x1E); //STATUS_REG  (- - - - EV_BOOT TDA GDA XLDA) available data

	if((temp&0x02) == 0x02) // GDA
	{return 1;}
	else
	{return 0;}
}

void GYRO_Init(void)
{
	GYRO_write_register(0x10,setUpXL);

	GYRO_write_register(0x11,setUpG);
}

uint8_t GYRO_read_register(uint8_t addressOfRegister)
{
	uint8_t gyroData = 0;
	uint8_t AOR = 0x80|addressOfRegister;
	GPIOB->BSRR = GPIO_PIN_9<<16;	//BSRRH = GND
	//HAL_SPI_TransmitReceive_IT(&hspi2,&AOR,&gyroData,1);
	HAL_SPI_TransmitReceive(&hspi2,&AOR,&gyroData,1,10);
	while( !(SPI2->SR & SPI_FLAG_TXE) ); // wait until transmit complete
	while( SPI2->SR & SPI_FLAG_BSY ); // wait until SPI is not busy anymore
	//HAL_SPI_TransmitReceive_IT(&hspi2,&AOR,&gyroData,1);
	HAL_SPI_TransmitReceive(&hspi2,&AOR,&gyroData,1,10);
	while( !(SPI2->SR & SPI_FLAG_TXE) ); // wait until transmit complete
	while( SPI2->SR & SPI_FLAG_BSY ); // wait until SPI is not busy anymore
	GPIOB->BSRR = GPIO_PIN_9; // BSRRL = 3V3
	return gyroData;
}

void GYRO_write_register(uint8_t addressOfRegister, uint8_t dataToWrite )
{
	uint8_t gyroData = 0;
	uint8_t AOR = addressOfRegister;
	GPIOB->BSRR = GPIO_PIN_9<<16;  // BSRRH
	//HAL_SPI_TransmitReceive_IT(&hspi2,&AOR,&gyroData,1);
	HAL_SPI_TransmitReceive(&hspi2,&AOR,&gyroData,1,10);
	while( !(SPI2->SR & SPI_FLAG_TXE) ); // wait until transmit complete
	while( SPI2->SR & SPI_FLAG_BSY ); // wait until SPI is not busy anymore
	//HAL_SPI_TransmitReceive_IT(&hspi2,&AOR,&gyroData,1);
	HAL_SPI_TransmitReceive(&hspi2,&dataToWrite,&gyroData,1,10);
	while( !(SPI2->SR & SPI_FLAG_TXE) ); // wait until transmit complete
	while( SPI2->SR & SPI_FLAG_BSY ); // wait until SPI is not busy anymore
	GPIOB->BSRR = GPIO_PIN_9; // BSRRL
}
#endif