//
// Created by ZK on 2023/3/14.
//

#ifndef BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H
#define BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H

//#include "APP_Main.h"
#include "arm_math.h"
#include "main.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include "adc.h"
#include "dma.h"
#include "i2c.h"
#include "opamp.h"
#include "spi.h"
#include "tim.h"
#include "usb_device.h"
#include "gpio.h"



typedef enum {
    MT6816,
    AS5600
} tEncoderModel;

typedef struct {
    tEncoderModel EncoderModel;

    float32_t (*getAngle)();
} tAngle;

typedef struct {
    tAngle Angle;

    float32_t (*Angle_Init)();
} tData;

uint16_t MT_ReadAngle(void);
float32_t GetSpeed(uint16_t angleVal);

float32_t getAngle_MT6816();

float32_t getAngle_AS5600();

bool Data_Init(tData *data);


typedef struct sMT6825 {
    bool no_mag;
    bool over_speed;
    uint32_t angle;

    uint8_t rx_err_count;
    uint8_t check_err_count;
} tMT6825;

typedef struct sEncoder {
    int raw;
    int count_in_cpr;
    int count_in_cpr_prev;

    int64_t shadow_count;

    // pll use
    float pos_cpr_counts;
    float vel_estimate_counts;

    float pos;
    float vel;

    float phase;
    float phase_vel;

    float pll_kp;
    float pll_ki;
    float interpolation;
    float snap_threshold;
} tEncoder;

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

    uint16_t sendBuf[2];
    uint16_t recvBuf[2];

    uint16_t offset;
    uint16_t angleVal, lastAngle;

    int32_t cycleNum;

    float32_t angle_360;
    float32_t eAngle_360;

//    int32_t angleArr[32][2];
//    uint8_t cnt;

    float32_t speed;
    float32_t filt;
    float32_t speedFilt;
};
#endif //BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H