//
// Created by hu123456 on 2024/1/16.
//

#include "tle5012b.h"
long  cpr =32767;
float motory_full_rotation_offset=0,motorx_full_rotation_offset=0;
long  motory_angle_data_prev =0,motorx_angle_data_prev = 0;


//void tle5012b_init(void)
//{
//    // 初始化TLE5012B传感器
//    GPIO_InitTypeDef GPIO_InitStruct = {0};
//    __HAL_RCC_GPIOA_CLK_ENABLE();
//
//    GPIO_InitStruct.Pin = GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5;
//    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
//    GPIO_InitStruct.Pull = GPIO_NOPULL;
//    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
//    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
//    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3 | GPIO_PIN_4 | GPIO_PIN_5, GPIO_PIN_RESET);
//
//    HAL_Delay(10);
//    getAngle(motory);
//    getAngle(motorx);
//}
//
//void mosi()
//{
//    GPIO_InitTypeDef GPIO_InitStruct = {0};
//    __HAL_RCC_GPIOA_CLK_ENABLE();
//
//    GPIO_InitStruct.Pin = GPIO_PIN_7;
//    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
//    GPIO_InitStruct.Pull = GPIO_NOPULL;
//    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
//    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
//
//    /*Configure GPIO pins : PBPin PB14 */
//    GPIO_InitStruct.Pin = GPIO_PIN_6;
//    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
//    GPIO_InitStruct.Pull = GPIO_NOPULL;
//    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
//}
//
//void  miso()
//{
//    GPIO_InitTypeDef GPIO_InitStruct = {0};
//    __HAL_RCC_GPIOA_CLK_ENABLE();
//
//    GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7;
//    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
//    GPIO_InitStruct.Pull = GPIO_NOPULL;
//    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
//}
//
//static void Write_16bit(unsigned short dat)
//{
//    mosi();
//    for(uint8_t i = 0; i < 16; i++)
//    {
//        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,1);
//
//        if(dat&0x8000) HAL_GPIO_WritePin(GPIOA,GPIO_PIN_7,1);
//        else HAL_GPIO_WritePin(GPIOA,GPIO_PIN_7,0);
//        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,0);
//        dat <<= 1;
//    }
//}
//
//static void Read_16bit(unsigned short *dat)
//{
//    miso();
//    *dat = 0;
//    for(uint8_t i = 0; i < 16; i++) {
//        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,1);
//        *dat <<= 1;
//        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,0);
//        *dat |=  HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_6) ? 1 : 0;
//    }
//}
//
//uint16_t ReadTLE5012B_1(uint16_t u16RegValue)
//{
//    uint16_t u16Data;
//    Write_16bit(u16RegValue);
//    Read_16bit(&u16Data);
//    return u16Data;
//}
//
//double getAngle(unsigned char motor)
//{
//    float angle_data,d_angle;
//    if(motor == motory)
//    {
//        TLE_CS1_ENABLE;
//        angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE)&0x7FFF;
//        d_angle = angle_data - motory_angle_data_prev;
//        if(fabs(d_angle) > (0.8*cpr) ) motory_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
//        motory_angle_data_prev = angle_data;
//        TLE_CS1_DISABLE;
//        return  (motory_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
//    }
//    else
//    {
//        TLE_CS2_ENABLE;
//        angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE)&0x7FFF;
//        d_angle = angle_data - motorx_angle_data_prev;
//        if(fabs(d_angle) > (0.8*cpr) ) motorx_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
//        motorx_angle_data_prev = angle_data;
//        TLE_CS2_DISABLE;
//        return  (motorx_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
//    }
//}
/******************************************************************************/



///////硬件


void tle5012b_init(void)
{
    // 初始化TLE5012B传感器
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    __HAL_RCC_GPIOA_CLK_ENABLE();

    GPIO_InitStruct.Pin = GPIO_PIN_3 | GPIO_PIN_4;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3 | GPIO_PIN_4, GPIO_PIN_RESET);

    HAL_Delay(10);
    getAngle(motory);
    getAngle(motorx);
    FOCStruct_X.Angle_Prev = getAngle(motorx);
    FOCStruct_Y.Angle_Prev = getAngle(motory);
}

void SPI2_TX_OFF()
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    __HAL_RCC_GPIOA_CLK_ENABLE();
/**SPI1 GPIO Configuration
PA5     ------> SPI1_SCK
PA6     ------> SPI1_MISO
PA7     ------> SPI1_MOSI
*/
    GPIO_InitStruct.Pin = GPIO_PIN_7;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}

void  SPI2_TX_ON()
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    __HAL_RCC_GPIOA_CLK_ENABLE();
    GPIO_InitStruct.Pin = GPIO_PIN_7;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    GPIO_InitStruct.Alternate = GPIO_AF5_SPI1;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}


double getAngle(unsigned char motor)
{
    long angle_data,d_angle;
    if(motor == motory)
    {
        TLE_CS1_ENABLE;
        angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE);
//        printf("angle_data:%d\n",angle_data);
        d_angle = angle_data - motory_angle_data_prev;
        if(fabs(d_angle) > (0.8*cpr) ) motory_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
        motory_angle_data_prev = angle_data;
        TLE_CS1_DISABLE;
        return  (motory_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) /2.f;
    }
    else
    {
        TLE_CS2_ENABLE;
        angle_data = ReadTLE5012B_1(READ_ANGLE_VALUE);
        d_angle = angle_data - motorx_angle_data_prev;
        if(fabs(d_angle) > (0.8*cpr) ) motorx_full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
        motorx_angle_data_prev = angle_data;
        TLE_CS2_DISABLE;
        return  (motorx_full_rotation_offset + ( angle_data / (float)cpr) * _2PI) /2.f;
    }
}


uint16_t ReadTLE5012B_1(uint16_t u16RegValue)
{
    uint16_t u16Data;
    HAL_SPI_Transmit( &hspi1, (uint8_t *)(&u16RegValue), sizeof(u16RegValue)/sizeof(uint16_t), 0xff );
    SPI2_TX_OFF();
    HAL_SPI_Receive( &hspi1,(uint8_t *)(&u16Data), sizeof(u16Data)/sizeof(uint16_t), 0xff );
    SPI2_TX_ON();
    return (u16Data & 0x7FFF);
}





//uint16_t ReadSpeed(void)
//{
//    return ReadTLE5012B_1(READ_SPEED_VALUE);
//}