#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;

/*************<prototype>**************/
void SPI0_Callback(void *device, uint32_t wpara, uint32_t lpara);
ERROR_Type SPI_TransmitReceivePoll_V2(SPI_Type *SPIx, uint8_t *rxBuffer, const uint8_t *txBuffer, uint32_t length, uint32_t timeout);
uint8_t SPI_CRC8(uint8_t *message, uint8_t Bytelength );

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参数,波特率 = 0.8Mbps = (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	= ENABLE;//CS有SPI硬件控制
	spiConfig.continuousCSEn= ENABLE;//片选连续模式
	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;
}

float AngleSensor_GetAngleInt(void)
{
	uint16_t angle_value;
	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;
	
	SPI_TransmitReceiveInt(SPI0, g_spiRxBuff1, g_spiTxBuff1, 6);
	while(!(SPI_GetTransmitReceiveStatus(SPI0) & SPI_STATUS_RX_FINISH_MASK))//等待读取完毕
	{
	
	};
	SPI_MasterReleaseCS(SPI0);

	printf("g_spiRxBuff1[0]:0x%x, g_spiRxBuff1[1]:0x%x \r\n", 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]);
//	
	angle_value = g_spiRxBuff1[3]&0x7F;
	angle_value = (angle_value<<8)|g_spiRxBuff1[2];
	angle = (360.0*angle_value)/32768;

	if((g_spiRxBuff1[5]&0x30) != 0x30){
		angle = -180.0;
		printf("angle_raw 111 : %f \r\n", angle);
	}else{
		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;
		}
		
	}
	
	return angle;
	
}

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_V2(SPI0, g_spiRxBuff1, g_spiTxBuff1, 6, 2000);
	//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;
	}
	
	return angle;
}

float AngleSensor_GetAngle(void)
{
	//return AngleSensor_GetAngleInt();
	return AngleSensor_GetAnglePolling();
}

float AngleSensor_GetAngleRaw(void)
{
		return angle_raw;
}

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

}

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

/**
* @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)
{
	if (wpara & SPI_STATUS_TXUF_Msk)
	{
		//TX下溢处理
	}
	else if (wpara & SPI_STATUS_RXOF_Msk)
	{
		//RX溢出处理
	}
	
}

/*!
* @brief Clear SPI Tx under flow and Rx over flow status
*
* @param[in] SPIx: SPI type pointer,x can be 0 to 1
* @return Function status
*            0: ERROR, occour Tx under flow or Rx over flow flag
*            1: SUCCESS, no Tx under flow and Rx over flow flag
*/
static ERROR_Type MY_SPI_ClearTxUFRxOF(SPI_Type *SPIx)
{
    ERROR_Type ret = SUCCESS;

    /* Clear Tx under flow flag */
    if (SPI_IsTxUF(SPIx))
    {
        SPI_ClearTxUF(SPIx);
        ret = ERROR;
    }
    else
    {
        /* Do nothing */
    }

    /* Clear Rx over flow flag */
    if (SPI_IsRxOF(SPIx))
    {
        SPI_ClearRxOF(SPIx);
        ret = ERROR;
    }
    else
    {
        /* Do nothing */
    }

    return ret;
}


/*!
* @brief SPI transmission,reception by polling
*
* @param[in] SPIx: SPI type pointer,x can be 0 to 1
* @param[in] rxBuffer: point to the receive data
* @param[in] txBuffer: point to the send data
* @param[in] length: transfer data length
* @param[in] timeout: timeout us 
* @return Function status
*            0: ERROR, length is 0 or rdbuff is NULL or txBuffer is NULL
*            1: SUCCESS
*/
ERROR_Type SPI_TransmitReceivePoll_V2(SPI_Type *SPIx, uint8_t *rxBuffer, const uint8_t *txBuffer, uint32_t length, uint32_t timeout)
{
    uint32_t  i = 0;
		uint32_t  _time_us = 0;
    ERROR_Type ret = SUCCESS;

    DEVICE_ASSERT(IS_SPI_PERIPH(SPIx));

    if ((length == 0) || (rxBuffer == NULL) || (txBuffer == NULL))
    {
        ret = ERROR;
    }
    else
    {
        /* Disable Tx/Rx only mode */
        SPI_SetTxOnly(SPIx, DISABLE);
        SPI_SetRxOnly(SPIx, DISABLE);

        if ((SPI_FrameSizeType)SPI_GetFRMSize(SPIx) > SPI_FRAME_SIZE_8BITS)  /* FrameSize is 9 bits ~ 16 bits */
        {
            if (((uint32_t)txBuffer & 0x01) || ((uint32_t)rxBuffer & 0x01)) /* txBuffer or rxBufer is not half-word alignment */
            {
                ret = ERROR;
                return ret;
            }

            length = length >> 0x01u;
            /* transmit and receive data */
            for (i = 0; i < length; i++)
            {
                while (!SPI_IsTxEF(SPIx) &&(_time_us < timeout))
								{
										udelay(1);
										_time_us++;
								}
                SPI_WriteDataReg(SPIx, ((uint16_t *)txBuffer)[i]);
                while (!SPI_IsRxFF(SPIx) &&(_time_us < timeout))
								{
										udelay(1);
										_time_us++;
								}
                ((uint16_t *)rxBuffer)[i] = SPI_ReadDataReg(SPIx);
            }
        }
        else  /* FrameSize is 4 bits ~ 8 bits */
        {
            /* transmit and receive data */
            for (i = 0; i < length; i++)
            {
                while (!SPI_IsTxEF(SPIx) &&(_time_us < timeout))
								{
										udelay(1);
										_time_us++;
								}
                SPI_WriteDataReg(SPIx, txBuffer[i]);
                while (!SPI_IsRxFF(SPIx) &&(_time_us < timeout))
								{
										udelay(1);
										_time_us++;
								}
                rxBuffer[i] = (uint8_t)SPI_ReadDataReg(SPIx);
            }
        }
        while ((SPI_IsBusy(SPIx)));
        SPI_CSRelease(SPIx);
    }

    /* Check and Clear Tx under flow/ Rx over flow flag */
    if (MY_SPI_ClearTxUFRxOF(SPIx) == ERROR)
    {
        ret = ERROR;
    }
    else
    {
			if((_time_us >= timeout)){
				ret = ERROR;
			}
        /* Do nothing */
    }

    return ret;
}

//CRC对照表
const uint8_t SPI_TableCRC[256] = 
{
	//The ?°crc?± of the position [1] (result from operation [crc ^*(message+Byteidx)])
	//is 0x00 -> 0x00 XOR 0x11D = 0x00 (1 byte).
	0x00,
	//The ?°crc?± of the position [2] is 0x1D -> 0x01 XOR 0x11D = 0x1D (1 byte).
	0x1D,
	//The ?°crc?± of the position [3] is 0x3A -> 0x02 XOR 0x11D = 0x3A (1 byte).
	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 )
{
	//?°crc?± defined as the 8-bits that will be generated through the message till the
	//final crc is generated. In the example above this are the blue lines out of the
	//XOR operation.
	uint8_t crc;
	//?°Byteidx?± is a counter to compare the bytes used for the CRC calculation and
	//?°Bytelength?±.
	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?± is the value in the look-up table TableCRC[x] at the position ?°x?±.
		//The position ?°x?± is determined as the XOR operation between the previous ?°crc?± and
		//the next byte of the ?°message?±.
		//?°^?± is the XOR operator.
		crc = SPI_TableCRC[crc ^ *(message+Byteidx)];
	}
	//Return the inverted ?°crc?± remainder(?°~?± is the invertion operator). An alternative
	//to the ?°~?± operator would be a XOR operation between ?°crc?± and a 0xFF polynomial.
	return(~crc);
}