#include "TLE5012B_spi.h"
#include "math.h"
#include "math_utils.h"

//++++++++++++++++++++++++++++++ For TEST - Should be Delete After DEBUGING +++++++++
#include "OBS_MOT.h"

#define _2PI 6.28318530718f

uint16_t cpr = 32768;
float full_rotation_offset = 0.0f;
int16_t angle_data_prev = 0;

// static float staticAngleOffset = 2.8445f  ;

#if TLE5012B_BSP_SPI_USE_IQ == TLE5012B_SPI_CONFIG_ENABLE
_iq14 full_rotation_offset_iq = 0;
const _iq14 zero_8_M_32768 = _IQ14(26214.4f);
const _iq14 zero_8_M_cpr = _IQ14(32768);
const _iq14 pi_2_iq = _IQ14(_2PI);
_iq14 angle_data_prev_iq = 0;
#endif

uint16_t velocity_calc_timestamp = 0;
float angle_prev = 0.0f;

//============== Low Lay API ===============
static uint16_t SPI_Read_Write_Byte_LL(uint16_t TxData);
static uint16_t ReadTLE5012B(unsigned short Comm);
static uint16_t WriteTLE5012B(uint16_t Comm, uint16_t data);

static uint16_t tle5012b_set_predictionMode(void);

void tle5012b_spi_init(void)
{

	cpr = 32768; // for TLE5012B
	full_rotation_offset = 0;
	angle_data_prev = ReadTLE5012B(TLE5012_CMD_READ_ANGLE_VALUE) & 0x7FFF;

#if TLE5012B_SPI_USE_IQ
	angle_data_prev_iq = _IQ14(angle_data_prev);
#endif

	tle5012b_set_predictionMode();

	velocity_calc_timestamp = 0;
}
/***************************************************************************/

static uint16_t SPI_Read_Write_Byte_LL(uint16_t TxData)
{
	uint16_t retry = 0;
	// while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE) == RESET) // 检查指定的SPI标志位设置与否:发送缓存空标志位
	while (LL_SPI_IsActiveFlag_TXE(TLE5012_SPIx_BASE) == RESET)
	{
		retry++;
		if (retry > 5000)
			return 0;
	}
	LL_SPI_TransmitData16(TLE5012_SPIx_BASE, TxData);
	retry = 0;

	while (LL_SPI_IsActiveFlag_RXNE(TLE5012_SPIx_BASE) == RESET)
	{
		retry++;
		if (retry > 5000)
			return 0;
	}
	return LL_SPI_ReceiveData16(TLE5012_SPIx_BASE);
}

/***************************************************************************/
static uint16_t ReadTLE5012B(uint16_t Comm)
{
	uint16_t u16Data;

	// taskENTER_CRITICAL();

	TLE5012_CS1_Enable;
	SPI_Read_Write_Byte_LL(Comm);
	TLE5012_TX_OFF;
#if TLE5012B_BSP_PLATFORM_SELECT == TLE5012_SUPPORT_PLATFORM_F1
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP(); // Twr_delay=130ns min
#endif
#if TLE5012B_BSP_PLATFORM_SELECT == TLE5012_SUPPORT_PLATFORM_F4
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP(); // Twr_delay=130ns min
#endif

	u16Data = SPI_Read_Write_Byte_LL(0xffff);
	SPI_Read_Write_Byte_LL(0xffff); // safety_word
	TLE5012_CS1_Disable;
	TLE5012_TX_ON;

	// taskEXIT_CRITICAL();

	return (u16Data);
}

static uint16_t WriteTLE5012B(uint16_t Comm, uint16_t data)
{
	uint16_t u16Data;

	// taskENTER_CRITICAL();

	TLE5012_CS1_Enable;
	SPI_Read_Write_Byte_LL(Comm);
	SPI_Read_Write_Byte_LL(data);
	TLE5012_TX_OFF;
#if TLE5012B_BSP_PLATFORM_SELECT == TLE5012_SUPPORT_PLATFORM_F1
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP(); // Twr_delay=130ns min
#endif
#if TLE5012B_BSP_PLATFORM_SELECT == TLE5012_SUPPORT_PLATFORM_F4
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP();
	__NOP(); // Twr_delay=130ns min
#endif
	SPI_Read_Write_Byte_LL(0xffff); // safety_word
	TLE5012_CS1_Disable;
	TLE5012_TX_ON;

	// taskEXIT_CRITICAL();

	return (u16Data);
}

/// WARN: 未测试,不可用
/// @brief 设置TLE5012B 预测模式,未测试,不可用
/// @param
/// @return
static uint16_t tle5012b_set_predictionMode(void)
{
	uint16_t regData = 0, regData1 = 0;

	// regData = ReadTLE5012B(TLE5012_CMD_READ_MOD_2) & 0x7FFF;
	// regData = regData | TLE5012_CMD_MOD_2_PREDIECTION_MASK;
	regData = 0x0801;
	WriteTLE5012B(TLE5012_CMD_WRITE_MOD_2, regData);
	HAL_Delay(300);
	regData1 = ReadTLE5012B(TLE5012_CMD_READ_MOD_2) & 0x7FFF;
	if (regData == regData1)
	{
		return 0;
	}
	else
	{
		return 1;
	}
}

uint16_t tle5012b_read_angleRAW(void)
{
	uint16_t angle_data = 0;

	angle_data = ReadTLE5012B(TLE5012_CMD_READ_ANGLE_VALUE) & 0x7FFF;

	return angle_data;
}

#if TLE5012B_BSP_SPI_USE_IQ == TLE5012B_SPI_CONFIG_ENABLE

float tle5012b_read_angle_iq(void)
{
	_iq14 d_angle;
	_iq14 angle_data = 0;
	float motor_roate_angle = 0.0f;

	angle_data = _IQ14(ReadTLE5012B(TLE5012_CMD_READ_ANGLE_VALUE) & 0x7FFF);

	// // printf("Angle:%d\n", angle_data);

	// d_angle = angle_data - angle_data_prev_iq;

	// if (_IQ14abs(d_angle) > zero_8_M_32768)
	// 	full_rotation_offset_iq += d_angle > 0 ? -pi_2_iq : pi_2_iq;
	// angle_data_prev_iq = angle_data;

	// motor_roate_angle = _IQ14toF(full_rotation_offset_iq + (angle_data * pi_2_iq / zero_8_M_cpr));
	motor_roate_angle = _IQ14toF(full_rotation_offset_iq + (_IQ14div(_IQ14mpy(angle_data, pi_2_iq), zero_8_M_cpr)));
	// printf("Angle:%f\n", motor_roate_angle);

	return motor_roate_angle;
}

_iq14 tle5012b_read_angle_iqOut(void)
{
	_iq14 d_angle;
	_iq14 angle_data = 0;
	float motor_roate_angle = 0.0f;

	angle_data = _IQ14(ReadTLE5012B(TLE5012_CMD_READ_ANGLE_VALUE) & 0x7FFF);

	// // printf("Angle:%d\n", angle_data);

	d_angle = angle_data - angle_data_prev_iq;

	// if (fabs(d_angle) > (0.8 * cpr))
	if (_IQ14abs(d_angle) > zero_8_M_32768)
		full_rotation_offset_iq += d_angle > 0 ? -pi_2_iq : pi_2_iq;
	angle_data_prev_iq = angle_data;

	motor_roate_angle = full_rotation_offset_iq + (angle_data * pi_2_iq / zero_8_M_cpr);
	// printf("Angle:%f\n", motor_roate_angle);

	return motor_roate_angle;
}

#endif

float tle5012b_read_angle_f(void)
{
	float d_angle;
	uint16_t angle_data = 0;
	float motor_roate_angle = 0.0f;

	angle_data = ReadTLE5012B(TLE5012_CMD_READ_ANGLE_VALUE) & 0x7FFF;

	d_angle = angle_data - angle_data_prev;

	if (fabs(d_angle) > (0.8 * cpr))
		full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
	angle_data_prev = angle_data;

	motor_roate_angle = (full_rotation_offset + (angle_data * _2PI / cpr));
	// printf("Angle:%f\n", motor_roate_angle);

	return motor_roate_angle;
}

/**
 * @brief Direct Angle Output Range From 0 To 2pi
 * @param
 * @return dir angle output of tle5012b
 */
float tle5012b_read_angleAbs_f(void)
{
	uint16_t angle_data = 0;
	float motor_roate_angle = 0.0f;

	angle_data = ReadTLE5012B(TLE5012_CMD_READ_ANGLE_VALUE) & 0x7FFF;

	motor_roate_angle = ((float)angle_data * _2PI / cpr);
	// printf("Angle:%f\n", motor_roate_angle);

	return motor_roate_angle;
}

/**
 * @brief Direct Angle Output Range From 0 To 2pi
 * @param
 * @return dir angle output of tle5012b
 */
float tle5012b_read_angleAbs_AvgF_f(void)
{
	uint16_t angle_data = 0;
	uint32_t angle_dataSum = 0;
	float motor_roate_angle;

	for (int i = 0; i < 10; i++)
	{
		angle_data = ReadTLE5012B(TLE5012_CMD_READ_ANGLE_VALUE) & 0x7FFF;
#if TLE5012B_BSP_PLATFORM_SELECT == TLE5012_SUPPORT_PLATFORM_F1
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP(); // Twr_delay=130ns min
#endif
#if TLE5012B_BSP_PLATFORM_SELECT == TLE5012_SUPPORT_PLATFORM_F4
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP();
		__NOP(); // Twr_delay=130ns min
#endif
		angle_dataSum += angle_data;
	}

	angle_data = (uint16_t)(angle_dataSum / 10);

	motor_roate_angle = (angle_data * _2PI / cpr);

	return motor_roate_angle;
}

//NOTE: 旧版本中带有滤波切换,用于快速实验
// float tle5012b_read_angleAbs_InterF_f(uint32_t filterSelect)
float tle5012b_read_angleAbs_InterF_f()
{
	uint16_t angle_data = 0;
	float motor_roate_angle = 0.0f;

	angle_data = ReadTLE5012B(TLE5012_CMD_READ_ANGLE_VALUE) & 0x7FFF;

	motor_obs.mge.raw_angle = angle_data;

	// switch(filterSelect){
	// 	case 1: angle_data = inter_filterC1(angle_data); break;
	// 	default : angle_data = inter_filter(angle_data); break;
	// }
	angle_data = inter_filter(angle_data);

	motor_roate_angle = (angle_data * _2PI / cpr);
	// printf("Angle:%f\n", motor_roate_angle);

	// Debug Area
	motor_obs.mge.raw_angle_F = motor_roate_angle;

	return motor_roate_angle;
}

static uint16_t tle5012b_RecAVGDatalist_P = 0;
uint16_t tle5012b_RecAVGDatalist[TLE5012B_RECAVG_BUFFLEN] = {0};
float tle5012b_read_angle_RecAVGF_f(void)
{
	uint16_t temp, temp1;
	int i, j;
	uint32_t dataSUM = 0;
	float motorAngle = 0;
	temp = tle5012b_read_angleRAW();
	tle5012b_RecAVGDatalist[tle5012b_RecAVGDatalist_P] = temp;
	tle5012b_RecAVGDatalist_P++;
	if (tle5012b_RecAVGDatalist_P > TLE5012B_RECAVG_BUFFLEN - 1)
	{
		tle5012b_RecAVGDatalist_P = 0; // ReSet the point
	}

	for (i = 0; i < TLE5012B_RECAVG_BUFFLEN; i++)
	{
		dataSUM += tle5012b_RecAVGDatalist[tle5012b_RecAVGDatalist_P];
	}
	dataSUM /= TLE5012B_RECAVG_BUFFLEN;

	motorAngle = (dataSUM * _2PI / cpr);

	return motorAngle;
}