#include "stdio.h"
#include "string.h"

#include "gpio.h"
#include "ac780x_spi_reg.h"
#include "AngleSensor.h"

#define DATA_SIZE 6U

/*! SPI发送和接收数组*/
uint8_t g_spiTxBuff1[DATA_SIZE] = {0};
uint8_t g_spiRxBuff1[DATA_SIZE] = {0};

static float angle_offset = 0.0;
static float angle_raw = 0.0;
static float enc_angle = 0.0;

/*************<prototype>**************/
static void SPI0_Callback(void *device, uint32_t wpara, uint32_t lpara);
static uint8_t SPI_CRC8(uint8_t *message, uint8_t Bytelength );
static float Parse_EncAngle(void);

extern void Process_MotorControl(float angle);

void AngleSensor_Init(void)
{
	SPI_ConfigType spiConfig;
	
	
	/*初始化SPI引脚,功能复用选择.*/
	//GPIO_SetFunc(GPIOB, GPIO_PIN15, GPIO_FUN3);//SCK
	//GPIO_SetFunc(GPIOB, GPIO_PIN14, GPIO_FUN3);//MOSI
	//GPIO_SetFunc(GPIOC, GPIO_PIN0, GPIO_FUN3);//MISO
	//GPIO_SetFunc(GPIOC, GPIO_PIN1, GPIO_FUN3);//CS
	
	/*清零配置结构体变量.*/
	memset(&spiConfig, 0x00, sizeof(spiConfig));
	
	/*初始化SPI参数,波特率 = 2Mbps = (F_BCLK / (SCK_LOW+1 + SCK_HIGH+1)).*/
	spiConfig.csSetup = 4;/*片选建立时间  = (CS_SETUP + 1) * CLK_PERIOD.*/
	spiConfig.csHold  = 4;/*片选保持时间  = (CS_HOLD + 1) * CLK_PERIOD.*/
	spiConfig.sckHigh = 5;/*SCK高电平时间 = (SCK_HIGH + 1) * CLK_PERIOD.*/
	spiConfig.sckLow  = 5;/*SCK低电平时间 = (SCK_LOW + 1) * CLK_PERIOD.*/
	spiConfig.csIdle  = 3;/*两条数据间最短时间间隔 = (CS_IDLE + 1) * CLK_PERIOD.*/
	spiConfig.mode   		= SPI_MASTER;//设置为主机模式
	spiConfig.cpha	 		= SPI_CPHA_2EDGE;//设置数据采样相位,第2个边沿采样数据
	spiConfig.cpol	 		= SPI_CPOL_LOW;//设置SCK空闲时极性,空闲时SCK为低
	spiConfig.frmSize 		= SPI_FRAME_SIZE_16BITS;
	spiConfig.rxMsbFirstEn	= ENABLE;//选择从最高位开始接收
	spiConfig.txMsbFirstEn	= ENABLE;//选择从最高位开始发送
	spiConfig.csOutputEn	= DISABLE;//CS有SPI硬件控制
	spiConfig.continuousCSEn= DISABLE;//片选连续模式
	spiConfig.dmaRxEn		= DISABLE;//禁止使用DMA接收数据
	spiConfig.dmaTxEn		= DISABLE;//禁止使用DMA发送数据
	spiConfig.modeFaultEn	= DISABLE;//模式故障禁止
	spiConfig.wakeUpEn		= DISABLE;//主机模式不支持唤醒功能
	spiConfig.spiEn 		= ENABLE;
	spiConfig.callBack		= SPI0_Callback;
	spiConfig.interruptEn		= ENABLE;//使能NVIC中断
	spiConfig.txUFInterruptEn 	= ENABLE;//打开TXUF中断,可禁止
	spiConfig.rxOFInterruptEn 	= ENABLE;//打开RXOF中断,可禁止
	spiConfig.modeFaultInterruptEn = DISABLE;//模式故障中断
	SPI_Init(SPI0, &spiConfig);
	
	NVIC_SetPriority(SPI0_IRQn, 1);
  NVIC_ClearPendingIRQ(SPI0_IRQn);
  NVIC_EnableIRQ(SPI0_IRQn);
	

}

void AngleSensor_Setoffset(float offset)
{
	angle_offset = offset;
}


void AngleSensor_GetAngleInt(void)
{	
	g_spiTxBuff1[0] = 0x21;
	g_spiTxBuff1[1] = 0x80;
	g_spiTxBuff1[2] = 0xFF;
	g_spiTxBuff1[3] = 0xFF;
	g_spiTxBuff1[4] = 0xFF;
	g_spiTxBuff1[5] = 0xFF;
	
	memset(g_spiRxBuff1, 0x00, 6);
	
	ENC_CS_L;
	SPI_TransmitReceiveInt(SPI0, g_spiRxBuff1, g_spiTxBuff1, 6);
//	while(!(SPI_GetTransmitReceiveStatus(SPI0) & SPI_STATUS_RX_FINISH_MASK));//等待读取完毕
//	SPI_MasterReleaseCS(SPI0);
//	ENC_CS_H;
	
//	printf("22g_spiRxBuff1[0]:0x%x, g_spiRxBuff1[1]:0x%x \r\n", g_spiRxBuff1[0], g_spiRxBuff1[1]);
//	printf("22g_spiRxBuff1[2]:0x%x, g_spiRxBuff1[3]:0x%x \r\n", g_spiRxBuff1[2], g_spiRxBuff1[3]);
//	printf("22g_spiRxBuff1[4]:0x%x, g_spiRxBuff1[5]:0x%x \r\n", g_spiRxBuff1[4], g_spiRxBuff1[5]);

}


float AngleSensor_GetAnglePolling(void)
{
	uint16_t angle_value;
	ERROR_Type ret = SUCCESS;
	//uint8_t  safeword =0;
	uint8_t crc;
	float angle = 0.0;
			
	g_spiTxBuff1[0] = 0x21;
	g_spiTxBuff1[1] = 0x80;
	g_spiTxBuff1[2] = 0xFF;
	g_spiTxBuff1[3] = 0xFF;
	g_spiTxBuff1[4] = 0xFF;
	g_spiTxBuff1[5] = 0xFF;
	
	memset(g_spiRxBuff1, 0x00, 6);
	
	ret = SPI_TransmitReceivePoll(SPI0, g_spiRxBuff1, g_spiTxBuff1, 6);
	
	if(SUCCESS == ret){
		
			//复用txbuff
			g_spiTxBuff1[0] = 0x80;
			g_spiTxBuff1[1] = 0x21;
			g_spiTxBuff1[2] = g_spiRxBuff1[3];
			g_spiTxBuff1[3] = g_spiRxBuff1[2];
		
			crc = SPI_CRC8(g_spiTxBuff1, 4);
			if(g_spiRxBuff1[4] == crc){
				
//				printf("crc22 %x: g_spiRxBuff1[0]:0x%x, g_spiRxBuff1[1]:0x%x \r\n", crc, g_spiRxBuff1[0], g_spiRxBuff1[1]);
//				printf("g_spiRxBuff1[2]:0x%x, g_spiRxBuff1[3]:0x%x \r\n", g_spiRxBuff1[2], g_spiRxBuff1[3]);
//				printf("g_spiRxBuff1[4]:0x%x, g_spiRxBuff1[5]:0x%x \r\n", g_spiRxBuff1[4], g_spiRxBuff1[5]);
//				
				if((g_spiRxBuff1[5]&0x30) != 0x30){
//					printf("angle_raw 111 : %f \r\n", angle);
					angle = -180.0;
				}else{
					
					angle_value = g_spiRxBuff1[3]&0x7F;
					angle_value = (angle_value<<8)|g_spiRxBuff1[2];
					angle = (360.0*angle_value)/32768;
					
					angle_raw = angle;
//					printf("angle_raw 222 : %f \r\n", angle);
					
					angle += angle_offset;
					if(angle > 360.0){
						angle -= 360.0;
					}
					
					if(angle < 0.0){
						angle += 360.0;
					}
				}
			
			}else{
				angle = -180.0;
			}
		
	}else{
		angle = -180.0;
	}
	
	return angle;
}

void AngleSensor_Read(void)
{
	g_spiTxBuff1[0] = 0x21;
	g_spiTxBuff1[1] = 0x80;
	g_spiTxBuff1[2] = 0xFF;
	g_spiTxBuff1[3] = 0xFF;
	g_spiTxBuff1[4] = 0xFF;
	g_spiTxBuff1[5] = 0xFF;
	
	memset(g_spiRxBuff1, 0x00, 6);
	
	ENC_CS_L;
	SPI_TransmitReceiveInt(SPI0, g_spiRxBuff1, g_spiTxBuff1, 6);
	
}

float AngleSensor_GetAngle(void)
{
	return enc_angle;
}

float AngleSensor_GetAngleRaw(void)
{
		return angle_raw;
}

void AngleSensor_PrintInfo(void)
{
	printf("AngleSensor angle:%f \r\n", AngleSensor_GetAngle());

}

void AngleSensor_DeInit(void)
{
	SPI_DeInit(SPI0);
}

void Angle_Correct(float angle)
{
	if(angle < 0){
		angle_raw = angle;
		enc_angle = angle;
		
	}else{
		
		angle_raw = angle;
		enc_angle = angle + angle_offset;
		if(enc_angle > 360.0){
			enc_angle -= 360.0;
		}
		
		if(enc_angle < 0.0){
			enc_angle += 360.0;
		}
	
	}
	
}

float Parse_EncAngle(void)
{
	uint16_t angle_value;
	uint8_t crc;
	float angle = 0.0;
	
		//复用txbuff
		g_spiTxBuff1[0] = 0x80;
		g_spiTxBuff1[1] = 0x21;
		g_spiTxBuff1[2] = g_spiRxBuff1[3];
		g_spiTxBuff1[3] = g_spiRxBuff1[2];
	
		crc = SPI_CRC8(g_spiTxBuff1, 4);
	
//		printf("crc %x: g_spiRxBuff1[0]:0x%x, g_spiRxBuff1[1]:0x%x \r\n", crc, g_spiRxBuff1[0], g_spiRxBuff1[1]);
//		printf("g_spiRxBuff1[2]:0x%x, g_spiRxBuff1[3]:0x%x \r\n", g_spiRxBuff1[2], g_spiRxBuff1[3]);
//		printf("g_spiRxBuff1[4]:0x%x, g_spiRxBuff1[5]:0x%x \r\n", g_spiRxBuff1[4], g_spiRxBuff1[5]);
		
	 if(g_spiRxBuff1[4] == crc){
			
			if((g_spiRxBuff1[5]&0x30) != 0x30){ //safeword  
				angle = -180.0;
			}else{
				
				angle_value = g_spiRxBuff1[3]&0x7F;
				angle_value = (angle_value<<8)|g_spiRxBuff1[2];
				angle = (360.0*angle_value)/32768;
				
			}
		
		}else{
			angle = -180.0;
		}
		
		
		return angle;
		
}

/**
* @prototype SPI0_Callback(void *device, uint32_t wpara, uint32_t lpara)
*
* @param[in] ...
* @return	 void
*
* @brief  	 SPI0中断回调函数.
*/
void SPI0_Callback(void *device, uint32_t wpara, uint32_t lpara)
{
	//printf("SPI0_Callback wpara=0x%x, lpara=0x%x  \r\n", wpara, lpara);
	
	if (wpara & SPI_STATUS_TXUF_Msk)
	{
		//TX下溢处理
	}
	else if (wpara & SPI_STATUS_RXOF_Msk)
	{
		//RX溢出处理
	}
	
	if(SPI_GetTransmitReceiveStatus(SPI0) & SPI_STATUS_RX_FINISH_MASK) //等待读取完毕
	{
		ENC_CS_H;
		Angle_Correct(Parse_EncAngle());
		
		Process_MotorControl(enc_angle);
	}
	
}



//CRC对照表
const uint8_t SPI_TableCRC[256] = 
{
	0x00,
	0x1D,
	0x3A,
	//For all the rest of the cases.
	0x27, 0x74, 0x69, 0x4E, 0x53, 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB, 0xCD,
	0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E, 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B,
	0x76, 0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4, 0x6F, 0x72, 0x55, 0x48, 0x1B,
	0x06, 0x21, 0x3C, 0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19, 0xA2, 0xBF, 0x98,
	0x85, 0xD6, 0xCB, 0xEC, 0xF1, 0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40, 0xFB,
	0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8, 0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90,
	0x8D, 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65, 0x94, 0x89, 0xAE, 0xB3, 0xE0,
	0xFD, 0xDA, 0xC7, 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F, 0x59, 0x44, 0x63,
	0x7E, 0x2D, 0x30, 0x17, 0x0A, 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2, 0x26,
	0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75, 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80,
	0x9D, 0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8, 0x03, 0x1E, 0x39, 0x24, 0x77,
	0x6A, 0x4D, 0x50, 0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2, 0x49, 0x54, 0x73,
	0x6E, 0x3D, 0x20, 0x07, 0x1A, 0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F, 0x84,
	0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7, 0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B,
	0x66, 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E, 0xF8, 0xE5, 0xC2, 0xDF, 0x8C,
	0x91, 0xB6, 0xAB, 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43, 0xB2, 0xAF, 0x88,
	0x95, 0xC6, 0xDB, 0xFC, 0xE1, 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09, 0x7F,
	0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C, 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFe,
	//The ?°crc?± of the position [255] is 0xD9 -> 0xFE XOR 0x11D = 0xD9 (1 byte).
	0xD9,
	//The ?°crc?± of the position [256] is 0xC4 -> 0xFF XOR 0x11D = 0xC4 (1 byte).
	0xC4
};

//配置
uint8_t SPI_CRC8(uint8_t *message, uint8_t Bytelength )
{

	uint8_t crc;
	uint8_t Byteidx;
	//Initially the CRC remainder has to be set with the original seed (0xFF for the TLE5012B).
	crc = 0xFF;
	//For all the bytes of the message.
	for(Byteidx=0; Byteidx<Bytelength; Byteidx++)
	{
		crc = SPI_TableCRC[crc ^ *(message+Byteidx)];
	}

	return(~crc);
}