#include "spi.h"
#include "APPDEF.H"
#include "math.h"
#include "string.h"
#include "stdio.h"
// AD7190寄存器地址定义
#define AD7190_REG_COMM 0x00 << 3
#define AD7190_REG_STAT 0x00 << 3
#define AD7190_REG_MODE 0x01 << 3
#define AD7190_REG_CONF 0x02 << 3
#define AD7190_REG_DATA 0x03 << 3
#define AD7190_REG_GPOC 0x05 << 3

// AD7190命令定义
#define AD7190_CMD_NOP 0xFF
#define AD7190_CMD_WR  0x00
#define AD7190_CMD_RD  0x40

// AD7190配置寄存器位掩码
#define AD7190_CONF_CHOP 0x800000
#define AD7190_CONF_REFSEL2    0x100000
// AD7190配置寄存器通道选择
#define AD7190_CONF_CH7		 		0x008000
#define AD7190_CONF_CH6   		0x004000
#define AD7190_CONF_CH5   		0x002000
#define AD7190_CONF_CH4   		0x001000
#define AD7190_CONF_CH3   		0x000800
#define AD7190_CONF_CH2   		0x000400
#define AD7190_CONF_CH1   		0x000200
#define AD7190_CONF_CH0   		0x000100
// AD7190配置寄存器
#define AD7190_CONF_BURNOUT   0x000080
#define AD7190_CONF_REFDET    0x000040
#define AD7190_CONF_BUF  		 	0x000010
#define AD7190_CONF_UB   		 	0x000008
// AD7190配置寄存器增益定义
#define AD7190_CONF_GAIN_1   0x000000
#define AD7190_CONF_GAIN_8   0x000003
#define AD7190_CONF_GAIN_16  0x000004
#define AD7190_CONF_GAIN_32  0x000005
#define AD7190_CONF_GAIN_64  0x000006
#define AD7190_CONF_GAIN_128 0x000007

// AD7190配置寄存器工作模式定义
#define AD7190_CONF_MODE_CONT    0x000000
#define AD7190_CONF_MODE_ZEROSEL 0x800000
#define AD7190_CONF_MODE_FullSEL 0xA00000
#define AD7190_CONF_MODE_FS9_0   0x0003FF	//	4.7Hz
#define AD7190_CONF_MODE_FS9_02   0x0001E0//10Hz
#define AD7190_CONF_MODE_FS9_03   0x000096//50Hz

#define AD7190_CONF_MODE_InCLK   0x080000// 4.92 MHz内部时钟 MCLK2引脚为三态
// AD7190 电桥开关
#define AD7190_GPOCON_BDPSW 0x40

void AD7190_WriteRegister(uint8_t regAddr, uint32_t regValue)
{
    SPI1_ReadWrite(AD7190_CMD_WR | regAddr);
    SPI1_ReadWrite((regValue >> 16) & 0xFF);
    SPI1_ReadWrite((regValue >> 8) & 0xFF);
    SPI1_ReadWrite(regValue & 0xFF);
}
uint32_t AD7190_ReadRegister(uint8_t regAddr)
{
    uint32_t regValue = 0;

    SPI1_ReadWrite(AD7190_CMD_RD | regAddr);
    regValue = SPI1_ReadWrite(0xFF);
    regValue = (regValue << 8) + SPI1_ReadWrite(0xFF);
    regValue = (regValue << 8) + SPI1_ReadWrite(0xFF);

    return regValue;
}
void AD7190_WriteRegisterOnce(uint8_t regAddr, uint8_t regValue)
{
    SPI1_ReadWrite(AD7190_CMD_WR | regAddr);
    SPI1_ReadWrite(regValue);
}
uint8_t AD7190_ReadRegisterOnce(uint8_t regAddr)
{
    uint32_t regValue = 0;
    SPI1_ReadWrite(AD7190_CMD_RD | regAddr);
    regValue = SPI1_ReadWrite(0xFF);
    return regValue;
}
bool AD7190_WaitForReady(uint32_t timeout)
{
    uint32_t count = 0;

    while (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_6) && (count < timeout))
    {
        delay_us(1);
        count++;
    }

    if (count >= timeout)
    {
        return false;
    }
    else
    {
        return true;
    }
}
//extern uint16_t Read_Internal_Temperature();
//// AD7190函数：读取AD转换结果
//float V25 = 1.43; // 25 度时的传感器电压(mV)
//  float Avg_Slope = 4.3; // 传感器电压与温度之间的平均斜率(mV/°C)
//  // 获取 ADC 参考电压值（一般为 3.3V）
//  float Vref = 3.3; // 参考电压值(V)
//float TemperatureIn;

uint8_t ErrCount;
uint16_t DifErrCount;
uint32_t lastdata = 0;
uint32_t UseuLBuf[20];
uint32_t UseuLBuf2[20];
uint16_t BufIndex = 0;
// 计算一组数字的平均值
float calculateAverage(uint32_t arr[], int size)
{
    double sum = 0.0;

    // 计算总和
    int i;
    for (i = 0; i < size; i++)
    {
        sum += arr[i];
    }

    // 计算平均值
    double average = sum / size;

    return average;
}

void bubbleSort(uint32_t *arr, int n) {
    int i, j, temp;
    for (i = 0; i < n-1; i++) {
        for (j = 0; j < n-i-1; j++) {
            if (arr[j] > arr[j+1]) {
                // 交换 arr[j] 和 arr[j+1]
                temp = arr[j];
                arr[j] = arr[j+1];
                arr[j+1] = temp;
            }
        }
    }
}


/**
***********************************************************
* @brief printf函数默认打印输出到显示器，如果要输出到串口，
		 必须重新实现fputc函数，将输出指向串口，称为重定向
* @param
* @return
***********************************************************
*/
int fputc(int ch, FILE *f)
{
    while (!(READ_BIT(USART1->SR, USART_SR_TXE))) {
        delay_us(100);
    }
    USART1->DR = ch;
    return ch;
}


float average;
float center;
int ErrcountAdd = 0;
int DifErrCountAdd = 0;
uint32_t AD7190_ReadData( void )
{
    uint32_t data = 0;

    if(AD7190_WaitForReady(50000))
    {
        data = AD7190_ReadRegister(AD7190_REG_DATA);
        if(fabs((int)data - (int)lastdata) > 100000 )
        {
            DifErrCount ++;
					DifErrCountAdd++;
        }
        else
        {
            DifErrCount = 0;
        }
        lastdata = data;
        if((data > 8388608) || (data < 100 ))
        {
            ErrCount++;
					ErrcountAdd++;
        }
        else
        {
            ErrCount = 0;
        }
    }
    else
    {
        ErrCount++;
			ErrcountAdd++;
    }
    if(ErrCount > 3 || DifErrCount > 20)
    {
        uint32_t modeReg;

        SPI1_Configuration();
        SPI1_ReadWrite(0xFF);
        SPI1_ReadWrite(0xFF);
        SPI1_ReadWrite(0xFF);
        SPI1_ReadWrite(0xFF);
        SPI1_ReadWrite(0xFF); // 40个脉冲复位ADC
        osDelay(50);
        // 配置寄存器：通道选择、参考电压源、增益和工作模式
        //	打开PSW
        uint8_t GPOC = AD7190_ReadRegisterOnce(AD7190_REG_GPOC);
        GPOC |= AD7190_GPOCON_BDPSW;
        AD7190_WriteRegisterOnce(AD7190_REG_GPOC, GPOC);
        osDelay(5);
        uint32_t config = AD7190_CONF_GAIN_128 | AD7190_CONF_UB | AD7190_CONF_BUF | AD7190_CONF_CH0;// 0x0000011F;
        AD7190_WriteRegister(AD7190_REG_CONF, config);
        osDelay(5);

				//	连续转换模式
        modeReg = AD7190_CONF_MODE_FS9_0 | AD7190_CONF_MODE_InCLK;
        AD7190_WriteRegister(AD7190_REG_MODE, modeReg);


        ErrCount = 0;
        osDelay(500);
        if(AD7190_WaitForReady(50000))
        {
            data = AD7190_ReadRegister(AD7190_REG_DATA);
        }
								
    }
    return data;
}

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

// 初始化卡尔曼滤波器
void kalman_filter_init2(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_update2(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;                    // 更新状态协方差
}


uint8_t FollowCount				= 0;		//	零点跟踪计数
uint8_t TimeCount					= 0;		//	蠕变时间计数
uint32_t FliterCount			= 0;		//	长期抑制计数
uint16_t SetZeroCount 		= 0;		//	卸载归零计数

int32_t ADvalue2					= 0;		//	ADC采样原始值

double ADvalue2filter 		= 0.0f;	//	ADC采样卡尔曼滤波值
double ADvalue2filterOld	= 0;		//	上次ADC采样卡尔曼滤波值

double WeightData2Temp 		= 0.0f;	//	本次ADvalue2filter计算重量值
double WeightData2TempOld;				//	上次ADvalue2filter计算重量值

double WeightData2Finally	= 0.0f;	//	最终输出前重量值

double UninstiallRefWeight= 0.0f;	//	卸载重物时的重量值

double WeightZeroOld 			= 0;		//	上次稳定状态时零点值
struct uCalibrateWeight CalibrateWeight2;	//传感器	斜率 零点

double WeightData2    = 0.0f;			//	最终输出重量值
bool   weightChanging = false;		//	传感器稳定标志
bool   weight11g = false;		//	传感器稳定标志
float ZeroWeightOld;
float ZeroWeightChange;

float LowFecAlpha = 0.95f;				//	超低频波动抑制系数，发生突变时偏向跟踪新值，相对稳定时跟踪旧值
float WeightRefStart = 0;

uint8_t i;

#define ChangeMax 10
#define ZeroTrackMax 0.050f
#define FollowMax 0.030f
#define FliterCountMax 200
#define FollowDlteaMax 0.001f
#define FollowAlpha 0.9999f				//	长期数据跟踪系数
void AD7190_Run(const void *p_arg)
{
    uint8_t FilterQOld, SensorQOld, ZeroTrackOld, RuBianLiangOld, ruBianTimeOld;
    int8_t RubianLiang, RubianTime;
    float ZeroTrackTime, ZeroTrackRange;

    osDelay(600);
    ADvalue2 = AD7190_ReadData();
    osDelay(200);
    ADvalue2          = AD7190_ReadData();
    ADvalue2filterOld = ADvalue2filter = ADvalue2;
    kalman_filter_init2(&kf2, ADvalue2, 1.0, 0.001, 0.1);
    WeightZeroOld = CalibrateWeight2.WeightZero;
    for (;;)
    {
        if (FilterQOld != Set.FilterQ)
        {
            FilterQOld = Set.FilterQ;
            switch (Set.FilterQ)    						// 滤波器信任度 信任度越高 反应越慢数据显示越稳定
            {
            case 0:
                kf2.Q = 0.1;
                break;
            case 1:
                kf2.Q = 0.08;
                break;
            case 2:
                kf2.Q = 0.06;
                break;
            case 3:
                kf2.Q = 0.04;
                break;
            case 4:
                kf2.Q = 0.02;
                break;
            case 5:
                kf2.Q = 0.009;
                break;
            case 6:
                kf2.Q = 0.007;
                break;
            case 7:
                kf2.Q = 0.005;
                break;
            case 8:
                kf2.Q = 0.003;
                break;
            case 9:
                kf2.Q = 0.001;
                break;
            default:
                break;
            }
        }
        if (SensorQOld != Set.SensorQ)
        {
            SensorQOld = Set.SensorQ;
            switch (Set.SensorQ)   							// 传感器信任度 信任度越高 反应越迅速，数据波动越大 显示越不稳定
            {
            case 0:
                kf2.R = 0.5;
                break;
            case 1:
                kf2.R = 0.3;
                break;
            case 2:
                kf2.R = 0.1;
                break;
            case 3:
                kf2.R = 0.08;
                break;
            case 4:
                kf2.R = 0.06;
                break;
            case 5:
                kf2.R = 0.04;
                break;
            case 6:
                kf2.R = 0.02;
                break;
            case 7:
                kf2.R = 0.009;
                break;
            case 8:
                kf2.R = 0.005;
                break;
            case 9:
                kf2.R = 0.001;
                break;
            default:
                break;
            }
        }
        if (ZeroTrackOld != Set.ZeroTrack)   		// 零点跟踪 跟踪级别越高 原始数据发生缓慢变化时，最终数据不会发生变化的可能性越大。例如 当跟踪级别设置为9时，那么如果原始数据在4秒内变化小于2.5mg，那么最终数据不会发生变化。原理是cpu会将这4秒内变化的数据叠加到零点上，相当于动态调零了。
        {
            ZeroTrackOld = Set.ZeroTrack;
            switch (ZeroTrackOld)
            {
            case 0:
                ZeroTrackTime  = 20;	//	连续20秒
                ZeroTrackRange = 0;		//	相邻两次采样数据小于0.000g
                break;
            case 1:
                ZeroTrackTime  = 15;
                ZeroTrackRange = 0.0005;
                break;
            case 2:
                ZeroTrackTime  = 10;
                ZeroTrackRange = 0.0005;
                break;
            case 3:
                ZeroTrackTime  = 7;
                ZeroTrackRange = 0.0005;
                break;
            case 4:
                ZeroTrackTime  = 4;
                ZeroTrackRange = 0.0005;
                break;
            case 5:
                ZeroTrackTime  = 4;
                ZeroTrackRange = 0.0008;
                break;
            case 6:
                ZeroTrackTime  = 4;
                ZeroTrackRange = 0.001;
                break;
            case 7:
                ZeroTrackTime  = 4;
                ZeroTrackRange = 0.0015;
                break;
            case 8:
                ZeroTrackTime  = 4;
                ZeroTrackRange = 0.002;
                break;
            case 9:
                ZeroTrackTime  = 4;
                ZeroTrackRange = 0.0025;
                break;
            default:
                break;
            }
        }
        if (ruBianTimeOld != Set.RuBianTime)		// 蠕变时间 级别越高 蠕变时间越短，蠕变越快 零为不蠕变
        {
            ruBianTimeOld = Set.RuBianTime;
            switch (Set.RuBianTime)
            {
            case 0:
                RubianTime = 100;
                break;
            case 1:
                RubianTime = 60;
                break;
            case 2:
                RubianTime = 30;
                break;
            case 3:
                RubianTime = 20;
                break;
            case 4:
                RubianTime = 15;
                break;
            case 5:
                RubianTime = 10;
                break;
            case 6:
                RubianTime = 8;
                break;
            case 7:
                RubianTime = 6;
                break;
            case 8:
                RubianTime = 4;
                break;
            case 9:
                RubianTime = 2;
                break;
            default:
                break;
            }
        }
        if (RuBianLiangOld != Set.RuBianLiang)	//蠕变量  可分正向蠕变和反向蠕变  零为不蠕变  当零点跟踪为0时 调零后观察一段时间的数据变化 如果数据持续变化为负数 则为负蠕变。反之为正蠕变。 负蠕变调整蠕变量为负数级别（谨慎调整 建议不调整）
        {
            RuBianLiangOld = Set.RuBianLiang;
            switch (Set.RuBianLiang)
            {
            case 0:
                RubianLiang = -5;
                break;
            case 1:
                RubianLiang = -4;
                break;
            case 2:
                RubianLiang = -3;
                break;
            case 3:
                RubianLiang = -2;
                break;
            case 4:
                RubianLiang = -1;
                break;
            case 5:
                RubianLiang = 0;
                break;
            case 6:
                RubianLiang = 1;
                break;
            case 7:
                RubianLiang = 2;
                break;
            case 8:
                RubianLiang = 3;
                break;
            case 9:
                RubianLiang = 4;
                break;
            case 10:
                RubianLiang = 5;
                break;
            default:
                break;
            }
        }


        osDelay(200);
        ADvalue2 = AD7190_ReadData();						//4.7Hz  213ms
        kalman_filter_update2(&kf2, ADvalue2); 	// 更新步骤
				if(fabs(ADvalue2filter - kf2.x) < ChangeMax )
				{
						LowFecAlpha = 0.95;			//	数据处于稳定状态 未有重量突变或小幅度连续单向改变时信任上次值
				}
				else
				{
						LowFecAlpha = 0.5;			//	数据处于较大波动状态 有重量突变或小幅度连续单向改变时信任本次值
				}
				ADvalue2filter = kf2.x *(1-LowFecAlpha)+ADvalue2filter*LowFecAlpha;
//        if(pageNum != 1)
//        {

//            if(WeightData2TempOld - WeightData2Temp > 0.500f)  // 卸载重物
//            {
//                if(UninstiallRefWeight < 0.005f)		//	如果未触发归零条件
//                {
//                    UninstiallRefWeight = WeightData2TempOld;
//                    WeightZeroOld = CalibrateWeight2.WeightZero;		//	备份零点原始值，以备不稳定状态下重新加载重物导致数据不准确
//                }
//            }
//            else if(WeightData2TempOld - WeightData2Temp < -0.500f)//加载重物
//            {
//                UninstiallRefWeight = 0.0f;
//                if( WeightZeroOld != CalibrateWeight2.WeightZero)
//                {
//                    CalibrateWeight2.WeightZero = WeightZeroOld;	//	 置零未完成或零点跟踪未完成，恢复至改变前零点值，防止数据不准确
//                }
//                SetZeroCount = 0;
//            }
//            if(UninstiallRefWeight > 0.005f)		//	卸载重物后接近归零时进行归零操作
//            {
//                if(SetZeroCount++ > 50 )		//	归零后重置触发归零条件
//                {
//										
//                    UninstiallRefWeight = 0.0f;
//										WeightZeroOld = CalibrateWeight2.WeightZero;
//                    SetZeroCount = 0;
//                }
//                if(fabs(WeightData2Temp) < 0.100f && fabs(WeightData2Temp) > 0.004f)		//	符合归零区间范围则缓慢归零
//                {
//                    SetZeroCount = 0;
//                    CalibrateWeight2.WeightZero += WeightData2Temp / CalibrateWeight2.WeightSlope / 10;
//										ZeroWeightChange = CalibrateWeight2.WeightSlope * (ADvalue2filter -  CalibrateWeight2.WeightZero);
//                }
//								ZeroWeightOld = CalibrateWeight2.WeightSlope * (ADvalue2filter -  WeightZeroOld);
//								if( fabs(ZeroWeightOld) <= fabs(ZeroWeightChange) )
//								{
//									weight11g=0;
//									CalibrateWeight2.WeightZero = WeightZeroOld;
//								}
//								else
//								{
//									weight11g=1;
//								}

//            }
//        }

        WeightData2TempOld = WeightData2Temp;
        WeightData2Temp = CalibrateWeight2.WeightSlope * (ADvalue2filter - CalibrateWeight2.WeightZero);

//        printf("%f,%f,%f,%.4f\n",Temperature,ADvalue2filter,WeightData2Temp,WeightData2Finally);

        if( RubianLiang != 0 )			//	蠕变
        {
            if (TimeCount++ > 4 * RubianTime) // 动态称重时蠕变跟踪
            {
                TimeCount = 0;
                if (CalibrateWeight2.WeightZero == WeightZeroOld)
                {
                    CalibrateWeight2.WeightZero += RubianLiang;
                }
                WeightZeroOld = CalibrateWeight2.WeightZero;
            }
        }

        if (fabs(WeightData2TempOld - WeightData2Temp) < ZeroTrackRange  &&  fabs(WeightData2Temp) < ZeroTrackMax && ( pageNum != 1))		//	符合跟踪范围  型评 当示值为零或相当于毛重为0时 负的净重值，且未运行，且水分测定仪处于平衡稳定状态  才允许运行
        {
            if (FollowCount++ > 4 * ZeroTrackTime) // 零点动态跟踪
            {
                FollowCount = 0;
                TimeCount   = 0;
                CalibrateWeight2.WeightZero += ADvalue2filter - ADvalue2filterOld;
                WeightZeroOld = CalibrateWeight2.WeightZero;
                ADvalue2filterOld = ADvalue2filter;
            }
        }
        else
        {
            FollowCount       = 0;
            ADvalue2filterOld = ADvalue2filter;
        }

        if ( fabs(WeightData2Temp) > ZeroTrackMax && fabs(WeightData2TempOld - WeightData2Temp) < FollowDlteaMax  &&  fabs(WeightData2Temp - WeightRefStart) < FollowMax && ( pageNum != 1))		//重量不变且大于零点跟踪范围未加热进行长期数据跟踪
        {
            if(FliterCount < FliterCountMax)
                FliterCount++;
            else
                FliterCount = FliterCountMax;
            WeightData2Finally = WeightData2Finally + (WeightData2Temp - WeightData2Finally) * ( 1.00000f - FollowAlpha * FliterCount / FliterCountMax);
        }
        else
        {
            WeightData2Finally = WeightRefStart = WeightData2Temp;
            FliterCount = 0;
        }


        if (fabs(WeightData2 - WeightData2Finally) > 0.006f) // 变化大于50mg/s  置位传感器不稳定标志
        {
            weightChanging = 1;
        }
        else
        {
            weightChanging = 0;
        }


        if (fabs(WeightData2 - WeightData2Finally) > 0.0002f) // 数据滤波
        {
            WeightData2  = (int32_t)(WeightData2Finally * 10000) / 10000.0f;
        }
        else if(fabs(WeightData2Finally) < 0.001f)
        {
            WeightData2  = (int32_t)(WeightData2Finally * 10000) / 10000.0f;
        }

    }
}

osThreadDef(AD7190_Run, osPriorityNormal, 1, 0);
// AD7190函数：初始化AD7190
void AD7190_Init()
{
    uint32_t modeReg;

    SPI1_Configuration();
    SPI1_ReadWrite(0xFF);
    SPI1_ReadWrite(0xFF);
    SPI1_ReadWrite(0xFF);
    SPI1_ReadWrite(0xFF);
    SPI1_ReadWrite(0xFF); // 40个脉冲复位ADC
    osDelay(50);
    // 配置寄存器：通道选择、参考电压源、增益和工作模式
    //	打开PSW
    uint8_t GPOC = AD7190_ReadRegisterOnce(AD7190_REG_GPOC);
    GPOC |= AD7190_GPOCON_BDPSW;
    AD7190_WriteRegisterOnce(AD7190_REG_GPOC, GPOC);
    osDelay(5);
    uint32_t config = AD7190_CONF_GAIN_128 | AD7190_CONF_UB | AD7190_CONF_BUF | AD7190_CONF_CH0;// 0x0000011F;
    AD7190_WriteRegister(AD7190_REG_CONF, config);
    osDelay(5);
    modeReg = AD7190_CONF_MODE_FS9_0 ;// 滤波器最低速率输出 执行上电校准
    AD7190_WriteRegister(AD7190_REG_MODE, modeReg);
    osDelay(5);
    //连续转换模式
    modeReg = AD7190_CONF_MODE_FS9_0 | AD7190_CONF_MODE_InCLK;
    AD7190_WriteRegister(AD7190_REG_MODE, modeReg);

    CalibrateWeight2.WeightSlope = 0.0001530345f;
//    CalibrateWeight2.WeightZero = 745430;

    osThreadCreate(osThread(AD7190_Run), NULL);
}