#ifndef MT6816_ENCODER_H
#define MT6816_ENCODER_H

#include "general_def.h"
#include "gpio.h"
#include "spi.h"
#include "BLDCMotor.h"
#include <stm32g431xx.h>

// ʱض
#define MT6816_MAX_DELAY (10) // ʱֵλ룩

// SPIƬѡźſƺ궨
//#define MT6816_IN_OUT

#ifdef MT6816_IN_OUT // 
#define MT6816_SPI_Get_HSPI (hspi1) // ȡSPI
#define MT6816_SPI_CS_L() HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_RESET)
#define MT6816_SPI_CS_H() HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_SET)
#else // 
#define MT6816_SPI_Get_HSPI (hspi3) // ȡSPI
#define MT6816_SPI_CS_L() HAL_GPIO_WritePin(SPI3_CS_GPIO_Port, SPI3_CS_Pin, GPIO_PIN_RESET)
#define MT6816_SPI_CS_H() HAL_GPIO_WritePin(SPI3_CS_GPIO_Port, SPI3_CS_Pin, GPIO_PIN_SET)
#endif

// MT6816Ĵַ
#define MT6816_Init_Reg (0x00)              // ʼĴַ
#define MT6816_Angle_Reg (0x80 | 0x03)      // ȡǶϢĴַ
#define MT6816_Warning_Reg (0x80 | 0x04)    // ȡϢĴַżУλ
#define MT6816_Over_Speed_Reg (0x80 | 0x05) // ȡϢĴַ

// 
#define ENCODER_PLL_BANDWIDTH 2000.0f
#define ENCODER_CPR 16384u     // ֱu
#define ENCODER_CPR_F 16384.0f // ֱf
#define ENCODER_CPR_DIV (ENCODER_CPR >> 1)
#define MAX_ANGLE 360.0f      // 360ΪһȦ
#define MAX_ANGLE_HALF 180.0f // 180

// öͶ
typedef enum
{
    CW = 1,     // ˳ʱ뷽
    CCW = -1,   // ʱ뷽
    UNKNOWN = 0 // δ֪Ч״̬
} Direction;

typedef struct
{
    SPI_HandleTypeDef *hspi;
    GPIO_TypeDef *CS_Port;
    uint16_t CS_Pin;

    uint32_t angle;
    uint8_t rx_err_count;
    uint8_t check_err_count;

    int32_t offset_lut[128]; // ڴűԻĲұ
    int32_t count;           // űĵǰֵһڵļֵ

    // ǶϢ
    uint8_t pole_pairs; // ڼǶ
    int cnt;
    int raw; // űԭʼֵ
    int dir; // űת+1 ʾ˳ʱ룬-1 ʾʱ
    int pos_abs_;
    int count_in_cpr_;
    int count_in_cpr_prev;
    int shadow_count_;
    int EncoderInit_Flag;
    float pos_estimate_counts_;
    float vel_estimate_counts_;
    float pos_cpr_counts_;
    float pos_estimate_;
    float vel_estimate_;
    float pos_cpr_;
    float phase_;
    float interpolation_;
    int calib_valid; // (Auto)
    float mec_angle;
    float elec_angle;     // ǶȣӦڻеǶȵĵתλãݵļõ
    float encoder_offset; // offset
    float speed;          // ٶ
    float last_angle;     // һεĽǶ

    // ٶϢ
    float theta_acc; // ǶĿ
} ENCODER_DATA;

extern ENCODER_DATA encoder_data;

float low_pass_filter(float input);                    // ͨ˲
float normalize_angle(float angle);                    // Ƕȹһ
void GetMotor_Angle(ENCODER_DATA *encoder);            // ±ݺ
void Theta_ADD(ENCODER_DATA *encoder);                 // Ƕ㺯
#endif                                                 // MT6816_ENCODER_H
