#include "BSP.H"

#define MAX6675_SCK_GPIO GPIOB
#define MAX6675_SCK_PIN  GPIO_Pin_5

#define MAX6675_CS_GPIO  GPIOB
#define MAX6675_CS_PIN   GPIO_Pin_4

#define MAX6675_SO_GPIO  GPIOB
#define MAX6675_SO_PIN   GPIO_Pin_3

uint16_t MAX6675_ReadData(void)
{
    uint16_t data = 0;
    int8_t i      = 0;
    // 选中传感器
    GPIO_ResetBits(MAX6675_CS_GPIO, MAX6675_CS_PIN);

    // 从SO引脚读取数据
    for (i = 15; i >= 0; i--) {
        // 将SCK引脚拉低
        GPIO_ResetBits(MAX6675_SCK_GPIO, MAX6675_SCK_PIN);

        // 延时一段时间
        delay_us(1);

        // 读取SO引脚状态
        if (GPIO_ReadInputDataBit(MAX6675_SO_GPIO, MAX6675_SO_PIN)) {
            data |= (1 << i); // 设置对应位为1
        }

        // 将SCK引脚拉高
        GPIO_SetBits(MAX6675_SCK_GPIO, MAX6675_SCK_PIN);

        // 延时一段时间
        delay_us(1);
    }

    // 解除传感器选中状态
    GPIO_SetBits(MAX6675_CS_GPIO, MAX6675_CS_PIN);

    return data;
}
float Temperature;
// 定义卡尔曼滤波器的参数结构体
typedef struct {
    float x; // 状态估计值
    float P; // 状态协方差
    float Q; // 过程噪声协方差
    float R; // 观测噪声协方差
    float K; // 卡尔曼增益
} KalmanFilter;
KalmanFilter kf;

// 初始化卡尔曼滤波器
void kalman_filter_init(KalmanFilter *kf, float initial_x, float initial_P, float process_noise, float measurement_noise)
{
    kf->x = initial_x;
    kf->P = initial_P;
    kf->Q = process_noise;
    kf->R = measurement_noise;
}

// 更新卡尔曼滤波器状态
void kalman_filter_update(KalmanFilter *kf, float measurement)
{
    // 预测步骤
    float x_pred = kf->x;         // 预测的状态值
    float P_pred = kf->P + kf->Q; // 预测的状态协方差

    // 更新步骤
    kf->K = P_pred / (P_pred + kf->R);               // 计算卡尔曼增益
    kf->x = x_pred + kf->K * (measurement - x_pred); // 更新状态估计值
    kf->P = (1 - kf->K) * P_pred;                    // 更新状态协方差
}
#include "APPDEF.H"
void MAX6675_ConvertTemperature(const void *P_arg)
{
    kalman_filter_init(&kf, 0.0, 1.0, 0.01, 0.3);

    for (;;) {
        osDelay(250);
        uint16_t data = MAX6675_ReadData();
        // 提取温度数据部分（高位14位）
        uint16_t temperatureData = data >> 3;

        // 判断正负号位
        uint8_t sign = (data & 0x04) >> 2;

        // 温度转换为摄氏度
        float temperature = (float)temperatureData * 0.25f;

        // 如果为负温度，取补码并添加负号
        if (sign == 1) {
            temperature = -((~temperatureData + 1) * 0.25f);
        }
        kalman_filter_update(&kf, temperature); // 更新步骤
        Temperature = kf.x + Set.TempDelta;
        /* code */
    }
}
osThreadDef(MAX6675_ConvertTemperature, osPriorityNormal, 1, 0);

void MAX6675_Init(void)
{
    // 初始化CS引脚为高电平（不选中传感器）
    GPIO_SetBits(MAX6675_CS_GPIO, MAX6675_CS_PIN);
    // 初始化GPIO引脚和延时函数
    GPIO_InitTypeDef GPIO_InitStruct;
    GPIO_InitStruct.GPIO_Pin   = MAX6675_SCK_PIN;
    GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_Out_PP;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(MAX6675_SCK_GPIO, &GPIO_InitStruct);

    GPIO_InitStruct.GPIO_Pin   = MAX6675_CS_PIN;
    GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_Out_PP;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(MAX6675_CS_GPIO, &GPIO_InitStruct);

    GPIO_InitStruct.GPIO_Pin   = MAX6675_SO_PIN;
    GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_IN_FLOATING;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(MAX6675_SO_GPIO, &GPIO_InitStruct);
    osThreadCreate(osThread(MAX6675_ConvertTemperature), NULL);
}