#include "spi.h"
void Write_Register(unsigned char COMMAND, unsigned char H_DATA, unsigned char M_DATA, unsigned char L_DATA, unsigned char S_DATA)
{
    SPI2_ReadWrite(COMMAND);
    SPI2_ReadWrite(H_DATA);
    SPI2_ReadWrite(M_DATA);
    SPI2_ReadWrite(L_DATA);
    SPI2_ReadWrite(S_DATA);
}
uint32_t Read_Register(unsigned char COMMAND)
{
    SPI2_ReadWrite(COMMAND);
    delay_us(1000);
    uint8_t H_DATA = SPI2_ReadWrite(0xFF);
    uint8_t M_DATA = SPI2_ReadWrite(0xFF);
    uint8_t L_DATA = SPI2_ReadWrite(0xFF);
    uint8_t S_DATA = SPI2_ReadWrite(0xFF);
    return H_DATA << 24 | M_DATA << 16 | L_DATA << 8 | S_DATA;
}
bool WaitForReady(uint32_t timeout)
{
    uint32_t count = 0;

    while (GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_14) && (count < timeout)) {
        delay_us(1);
        count++;
    }

    if (count >= timeout) {
        return false;
    } else {
        return true;
    }
}
void RESET_SPI(void)
{
    SPI2_ReadWrite(0x00);
    SPI2_ReadWrite(0xA5);
    SPI2_ReadWrite(0xFF);
    SPI2_ReadWrite(0x5A);
}
#include "APPDEF.H"

static void bubbleSort(float *num)
{
    float temp;
    int i, j;
    for (i = 0; i < 5; i++) {
        for (j = 0; j < 5 - i; j++) {
            if (num[j] > num[j + 1]) {
                // 交换相邻元素的值
                temp       = num[j];
                num[j]     = num[j + 1];
                num[j + 1] = temp;
            }
        }
    }
}

struct uCalibrateWeight CalibrateWeight;
float WeightData       = 0.0f;
float WeightDatafilter = 0.0f;
static float bufferdata[5];
static float dataout[5];
int32_t ADvalue = 0;
uint8_t ic      = 0;
int32_t data    = 0;

void GET_ADC_CH0(const void *p_arg)
{
    for (;;) {
        osDelay(100);
        //        if (WaitForReady(50000)) {
        delay_us(1000);
        ADvalue          = Read_Register(0x5A);
        bufferdata[ic++] = ADvalue;
        if (ic >= 3) // 5个数据取中间值
        {
            ic = 0;
            bubbleSort(bufferdata); // 冒泡排序
                                    // 滑动滤波
                                    //                dataout[4]       = dataout[3];
                                    //                dataout[3]       = dataout[2];
            dataout[2]    = dataout[1];
            dataout[1]    = dataout[0];
            dataout[0]    = bufferdata[3]; // 取中间值
            bufferdata[0] = bufferdata[1] = bufferdata[2] = bufferdata[3] = bufferdata[4] = 0;
            WeightDatafilter                                                              = ((dataout[0] + dataout[1] + dataout[2] /*+ dataout[3] + dataout[4]*/) / 3.0);
            WeightData                                                                    = CalibrateWeight.WeightSlope * ((int32_t)WeightDatafilter - (int32_t)CalibrateWeight.WeightZero);
        }
        // if (ADvalue != -1) {
        //     kalman_filter_update(&kf, ADvalue); // 更新步骤
        // 			WeightDatafilter = kf.x;
        // }
        // 					WeightDatafilter = 	kalman_filter_1(ADvalue);
        // WeightData = CalibrateWeight.WeightSlope * ((int32_t)WeightDatafilter - (int32_t)CalibrateWeight.WeightZero);
        // 		WeightData = (int32_t)(WeightData * 10000) /10000.0f;
        //        }
    }
}
osThreadDef(GET_ADC_CH0, osPriorityNormal, 1, 0);
void INIT_CS555X(void)
{
    SPI2_Configuration();
    osDelay(100);
    RESET_SPI();
    osDelay(100);
    Write_Register(0x30, 0x80, 0x00, 0x00, 0x00); // RESET
    osDelay(100);
    Write_Register(0x00, 0x00, 0x00, 0x00, 0x00); // OS_CH0
    Write_Register(0x09, 0x02, 0x00, 0x00, 0x00); // GAIN_CH0
    Write_Register(0x21, 0x00, 0x00, 0x00, 0xC1); // CONV_CONF0 //PGA=128 DATA RATE=6.25Hz
                                                  //    Write_Register(0x28, 0x00, 0x00, 0x00, 0x00); // CONV_CONF1
    Write_Register(0x30, 0x00, 0x01, 0x00, 0x00); // SYS_CONF0 //****************HBF_EN=0;
    Write_Register(0x39, 0x00, 0x11, 0x02, 0x00); // SYS_CONF1 //****************FR_SEL=0;
    Write_Register(0x41, 0x00, 0x02, 0x00, 0x00); // SYS_CONF2
    osDelay(5);
    SPI2_ReadWrite(0x82); // 转换命令帧 正常连续转换
    osDelay(1);
    CalibrateWeight.WeightSlope = 0.0000013f;
    CalibrateWeight.WeightZero  = 1425000;
    osThreadCreate(osThread(GET_ADC_CH0), NULL);
    return;
}