/*
 * gyro.c
 *
 *  Created on: 16 sep. 2016
 *      Author: rsd12002
 */

#include <drivers/accel_gyro.h>
#include "drivers/spi.h"

spi_profile mpu6000_spi_profile;
uint8_t num_failed_receive = 0;

/***********************************************************************
 * BRIEF: SPI1_Init initializes the SPI1 instance with predefined values*
 * INFORMATION															*
 ***********************************************************************/
bool spi1_init()
{
	return spi_init(SPI1, &mpu6000_spi_profile, MPU6000_NSS_PIN, MPU6000_NSS_PORT);
}

/***********************************************************************
 * BRIEF: mpu6000_Transmit transmits the specified register and data    *
 * to the mpu6000                                                       *
 * INFORMATION:                                                         *
 * data[0] = register                                                   *
 * data[1] = command                                                    *
 ***********************************************************************/
bool mpu6000_transmit(uint8_t* data, uint8_t length)
{
	return spi_transmit(&mpu6000_spi_profile, data, length, 10);
}

/***********************************************************************
 * BRIEF: mpu6000_TransmitReceive is used to request data from the      *
 * mpu6000 which it stores in data                                      *
 * INFORMATION:                                                         *
 ***********************************************************************/
bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32_t timeout_ms)
{
	return spi_receive_reg_value(&mpu6000_spi_profile, reg, data, length, timeout_ms);
}

/***********************************************************************
 * BRIEF: mpu6000_read_offset reads and returns the offset of the       *
 * gyroscope and accelerometer                                          *
 * INFORMATION:                                                         *
 * Is automatically called when mpu6000_init is called                  *
 * The flight controller needs to be stationary when this function is   *
 * called                                                               *
 * When the UAV is finished this data could be saved so that the        *
 * offset doesn't need to be read every time                            *
 ***********************************************************************/
HAL_StatusTypeDef mpu6000_read_offset(gyro_t* gyro, accel_t* accel)
{
	uint8_t dataG[6]; /* Temporary data variable used to receive gyroscope data */
	uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */

	if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10))
		return HAL_ERROR;

	if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10))
		return HAL_ERROR;

#ifdef YAW_ROT_0
	gyro->offsetX = -(((int16_t)dataG[0] << 8) | dataG[1]);
	gyro->offsetY = -(((int16_t)dataG[2] << 8) | dataG[3]);
	gyro->offsetZ =  (((int16_t)dataG[4] << 8) | dataG[5]);

	accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1];
	accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3];
	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
#elif defined(YAW_ROT_90)
	gyro->offsetX = -(((int16_t)dataG[2] << 8) | dataG[3]);
	gyro->offsetY =  (((int16_t)dataG[0] << 8) | dataG[1]);
	gyro->offsetZ =  (((int16_t)dataG[4] << 8) | dataG[5]);

	accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3];
	accel->offsetY =  ((int16_t)dataA[0] << 8) | dataA[1];
	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
#elif defined(YAW_ROT_180)
	gyro->offsetX = (((int16_t)dataG[0] << 8) | dataG[1]);
	gyro->offsetY = (((int16_t)dataG[2] << 8) | dataG[3]);
	gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]);

	accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1];
	accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3];
	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
#elif defined(YAW_ROT_270)
	gyro->offsetX =  (((int16_t)dataG[2] << 8) | dataG[3]);
	gyro->offsetY = -(((int16_t)dataG[0] << 8) | dataG[1]);
	gyro->offsetZ =  (((int16_t)dataG[4] << 8) | dataG[5]);

	accel->offsetX =  ((int16_t)dataA[2] << 8) | dataA[3];
	accel->offsetY = -((int16_t)dataA[0] << 8) | dataA[1];
	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
#endif

	return HAL_OK;
}

/***********************************************************************
 * BRIEF: mpu6000_Init initializes the gyroscope and accelerometer      *
 * INFORMATION:                                                         *
 * Utilizing the GyroZ clock                                            *
 * I2C bus disabled                                                     *
 * Sample rate division = 0											    *
 * 256Hz Digital Low Pass Filter (DLPF)                                 *
 * Full scale range of the gyroscope = ± 2000°/s						*
 ***********************************************************************/
bool mpu6000_init(gyro_t* gyro, accel_t* accel)
{
	spi1_init();

	uint8_t reg[2]; /* Register address and bit selection */

	// Reset device
	reg[0] = MPU_RA_PWR_MGMT_1;
	reg[1] = BIT_H_RESET;
	if(!mpu6000_transmit(reg, 2))
		return false;
	HAL_Delay(10);

	// Reset Signal Path
	reg[0] = MPU_RA_SIGNAL_PATH_RESET;
	reg[1] = BIT_GYRO | BIT_ACC;
	HAL_Delay(150);
	if(!mpu6000_transmit(reg, 2))
		return false;

	// Wake up device and select GyroZ clock (better performance)
	reg[0] = MPU_RA_PWR_MGMT_1;
	reg[1] = 0b00001000 | MPU_CLK_SEL_PLLGYROZ;
	if(!mpu6000_transmit(reg, 2))
		return false;

	// Disable I2C bus
	reg[0] = MPU_RA_USER_CTRL;
	reg[1] = 0x50;
	if(!mpu6000_transmit(reg, 2))
		return false;

	// No standby mode
	reg[0] = MPU_RA_PWR_MGMT_2;
	reg[1] = 0x00;
	if(!mpu6000_transmit(reg, 2))
		return false;

	// Sample rate
	reg[0] = MPU_RA_SMPLRT_DIV;
	reg[1] = 0x00;
	if(!mpu6000_transmit(reg, 2))
		return false;

	// Digital Low Pass Filter (DLPF)
	reg[0] = MPU_RA_CONFIG;
	reg[1] = BITS_DLPF_CFG_256HZ;
	if(!mpu6000_transmit(reg, 2))
		return false;

	// FIFO
	reg[0] = MPU_RA_FIFO_EN;
	//   Temperature  GyroX    GyroY    GyroZ    Accel    Slave    Slave    Slave
	reg[1] = 0 << 7 | 1 << 6 | 1 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0;
	if(!mpu6000_transmit(reg, 2))
		return false;

	// Gyroscope 2000DPS
	reg[0] = MPU_RA_GYRO_CONFIG;
	reg[1] = BITS_FS_2000DPS;
	gyro->scale = (1.0f / 16.4f);
	if(!mpu6000_transmit(reg, 2))
		return false;

	// Accelerometer 16±G
	reg[0] = MPU_RA_ACCEL_CONFIG;
	reg[1] = BITS_FS_16G;
	accel->accel1G = 2048; // (32768/16)/G
	if(!mpu6000_transmit(reg, 2))
		return false;

	mpu6000_read_offset(gyro, accel);

	return true;
}

/***********************************************************************
 * BRIEF: mpu6000_ReadGyro reads the three axis of the gyroscope and    *
 * stores the data, in °/s format, in the gyro struct                   *
 * INFORMATION:                                                         *
 ***********************************************************************/
bool mpu6000_read_gyro(gyro_t* gyro)
{
	uint8_t data[6]; /* Temporary data variable used to receive gyroscope data */
	HAL_StatusTypeDef err; /* SPI transmission status variable */

	if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, data, 6, 10))
		return false;

#ifdef YAW_ROT_0
	gyro->gyroX = -(((int16_t)data[0] << 8) | data[1]);
	gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale;
	gyro->gyroY = -(((int16_t)data[2] << 8) | data[3]);
	gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale;
	gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]);
	gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale;
#elif defined(YAW_ROT_90)
	gyro->gyroX =  -(((int16_t)data[2] << 8) | data[3]);
	gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale;
	gyro->gyroY = (((int16_t)data[0] << 8) | data[1]);
	gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale;
	gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]);
	gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale;
#elif defined(YAW_ROT_180)
	gyro->gyroX = (((int16_t)data[0] << 8) | data[1]);
	gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale;
	gyro->gyroY = (((int16_t)data[2] << 8) | data[3]);
	gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale;
	gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]);
	gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale;
#elif defined(YAW_ROT_270)
	gyro->gyroX = (((int16_t)data[2] << 8) | data[3]);
	gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale;
	gyro->gyroY = -(((int16_t)data[0] << 8) | data[1]);
	gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale;
	gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]);
	gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale;
#else
	No Yaw Direction Defined
#endif

	return true;
}

/***********************************************************************
 * BRIEF: mpu6000_ReadGyro reads the three axis of the accelerometer    *
 * INFORMATION:                                                         *
 * The data is both saved in raw format and in converted into the       *
 * number of G (9.82 m/s^2) the accelerometer is sensing		        *
 ***********************************************************************/
bool mpu6000_read_accel(accel_t* accel)
{
	uint8_t data[6]; /* Temporary data variable used to receive accelerometer data */

	if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, data, 6, 10))
		return false;

#ifdef YAW_ROT_0
	accel->accelXraw = -((int16_t)data[0] << 8) | data[1];
	accel->accelXraw = accel->accelXraw - accel->offsetX;
	accel->accelYraw = -((int16_t)data[2] << 8) | data[3];
	accel->accelYraw = accel->accelYraw - accel->offsetY;
	accel->accelZraw = ((int16_t)data[4] << 8) | data[5];
	accel->accelZraw = accel->accelZraw + accel->offsetZ;

	accel->accelXconv = ((float)accel->accelXraw / accel->accel1G);
	accel->accelYconv = ((float)accel->accelYraw / accel->accel1G);
	accel->accelZconv = ((float)accel->accelZraw / accel->accel1G);
#elif defined(YAW_ROT_90)
	accel->accelXraw = -((int16_t)data[2] << 8) | data[3];
	accel->accelXraw = accel->accelXraw - accel->offsetX;
	accel->accelYraw =  ((int16_t)data[0] << 8) | data[1];
	accel->accelYraw = accel->accelYraw - accel->offsetY;
	accel->accelZraw =  ((int16_t)data[4] << 8) | data[5];
	accel->accelZraw = accel->accelZraw + accel->offsetZ;

	accel->accelXconv = (float)(accel->accelYraw / accel->accel1G);
	accel->accelYconv = (float)(accel->accelXraw / accel->accel1G);
	accel->accelZconv = (float)(accel->accelZraw / accel->accel1G);
#elif defined(YAW_ROT_180)
	accel->accelXraw = ((int16_t)data[0] << 8) | data[1];
	accel->accelXraw = accel->accelXraw - accel->offsetX;
	accel->accelYraw = ((int16_t)data[2] << 8) | data[3];
	accel->accelYraw = accel->accelYraw - accel->offsetY;
	accel->accelZraw = ((int16_t)data[4] << 8) | data[5];
	accel->accelZraw = accel->accelZraw + accel->offsetZ;

	accel->accelXconv = ((float)accel->accelXraw / accel->accel1G);
	accel->accelYconv = ((float)accel->accelYraw / accel->accel1G);
	accel->accelZconv = ((float)accel->accelZraw / accel->accel1G);
#elif defined(YAW_ROT_270)
	accel->accelXraw =  ((int16_t)data[2] << 8) | data[3];
	accel->accelXraw = accel->accelXraw - accel->offsetX;
	accel->accelYraw = -((int16_t)data[0] << 8) | data[1];
	accel->accelYraw = accel->accelYraw - accel->offsetY;
	accel->accelZraw =  ((int16_t)data[4] << 8) | data[5];
	accel->accelZraw = accel->accelZraw + accel->offsetZ;

	accel->accelXconv = ((float)accel->accelYraw / accel->accel1G);
	accel->accelYconv = ((float)accel->accelXraw / accel->accel1G);
	accel->accelZconv = ((float)accel->accelZraw / accel->accel1G);
#endif

	return true;
}

/***********************************************************************
 * BRIEF: mpu6000_ReadFIFO read the X, Y, and Z gyroscope axis from the *
 * FIFO queue													        *
 * INFORMATION:                                                         *
 * Reads every complete set of gyro data (6 bytes) from the queue and   *
 * stores it it data_out											    *
 * returns:															    *
 * -1 if SPI transmission error occurs								    *
 * -2 if FIFO queue overflow											*
 * -3 if FIFO queue doesn't contain any complete set of gyro data       *
 * else the number of bytes read from the FIFO queue                    *
 ***********************************************************************/
int mpu6000_read_fifo(gyro_t* gyro, int16_t* data_out)
{
	uint16_t fifoCount = 0; /* Number of bytes in the FIFO queue */
	uint8_t countH = 0; /* Bits 8-16 of the number of bytes in the FIFO queue */
	uint8_t countL = 0; /* Bits 0-7 of the number of bytes in the FIFO queue */
	uint16_t bytesRead = 0; /* Number of bytes actually read from the FIFO queue */
	uint8_t reg[2]; /* Register address and bit selection */

	if(!mpu6000_transmit_receive(MPU_RA_FIFO_COUNTH, &countH, 1, 10))
		return -1;

	if(!mpu6000_transmit_receive(MPU_RA_FIFO_COUNTL, &countL, 1, 10))
		return -1;


	fifoCount = (uint16_t)((countH << 8) | countL);
	if (fifoCount == 1024 || num_failed_receive > 20)
	{
		reg[0] = MPU_RA_USER_CTRL;
		reg[1] = BIT_FIFO_RESET;

		mpu6000_transmit(reg, 2);

		reg[0] = MPU_RA_USER_CTRL;
		reg[1] = 1<<6;

		mpu6000_transmit(reg, 2);
		return -2;
	}

	if (fifoCount < 6)
	{
		num_failed_receive++;
		return -3;
	}
	else
		num_failed_receive = 0;

	/* Make sure that only complete sets of gyro data are read */
	bytesRead = (fifoCount/6)*6;

	uint8_t fifobuffer[bytesRead+1];


	if(!mpu6000_transmit_receive(MPU_RA_FIFO_R_W, fifobuffer, bytesRead, 20))
		return false;


	uint8_t xL, xH, yL, yH, zL, zH;
	for(int j = 0; 6+((j-1)*6) < bytesRead; j++)
	{
		xH = fifobuffer[0+((j-1)*6)];
		xL = fifobuffer[1+((j-1)*6)];
		yH = fifobuffer[2+((j-1)*6)];
		yL = fifobuffer[3+((j-1)*6)];
		zH = fifobuffer[4+((j-1)*6)];
		zL = fifobuffer[5+((j-1)*6)];

#ifdef YAW_ROT_0


		data_out[0+((j-1)*3)] = -(((int16_t)xH << 8) | xL);								// X
		data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale;

		data_out[1+((j-1)*3)] = -(((int16_t)yH << 8) | yL);								// Y
		data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale;

		data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL);								// Z
		data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale;

#elif defined(YAW_ROT_90)
		data_out[0+((j-1)*3)] = -(((int16_t)yH << 8) | yL);								// X
		data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale;

		data_out[1+((j-1)*3)] = (((int16_t)xH << 8) | xL);								// Y
		data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale;

		data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL);								// Z
		data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale;
#elif defined(YAW_ROT_180)
		data_out[0+((j-1)*3)] = (((int16_t)xH << 8) | xL);								// X
		data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale;

		data_out[1+((j-1)*3)] = (((int16_t)yH << 8) | yL);								// Y
		data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale;

		data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL);								// Z
		data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale;
#elif defined(YAW_ROT_270)
		data_out[0+((j-1)*3)] = (((int16_t)yH << 8) | yL);								// X
		data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale;

		data_out[1+((j-1)*3)] = -(((int16_t)xH << 8) | xL);								// Y
		data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale;

		data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL);								// Z
		data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale;
#endif
	}
	return bytesRead;
}

/***********************************************************************
 * BRIEF: mpu6000_WhoAmI requests the product ID of the mpu6000 to      *
 * confirm device                                                       *
 * INFORMATION: 														*
 * returns true if correct device and revision if found                 *
 ***********************************************************************/
bool mpu6000_who_am_i()
{
	uint8_t data = 0; /* Received data is placed in this variable */

	if(!mpu6000_transmit_receive(MPU_RA_WHO_AM_I, &data, 1, 10))
		return false;

	if (data != MPU6000_WHO_AM_I_CONST)
	{
		return false;
	}


	/* look for a product ID we recognize */
	if(!mpu6000_transmit_receive(MPU_RA_PRODUCT_ID, &data, 1, 10))
		return false;

	// verify product revision
	switch (data)
	{
	case MPU6000ES_REV_C4:
	case MPU6000ES_REV_C5:
	case MPU6000_REV_C4:
	case MPU6000_REV_C5:
	case MPU6000ES_REV_D6:
	case MPU6000ES_REV_D7:
	case MPU6000ES_REV_D8:
	case MPU6000_REV_D6:
	case MPU6000_REV_D7:
	case MPU6000_REV_D8:
	case MPU6000_REV_D9:
	case MPU6000_REV_D10:
		return true;
	}

	return false;
}