/*
 * 常用数学函数实现
 */

#include "math_utils.h"

#include "isoCalculate.h"

// normalizing radian angle to [0, 2pi]
float _normalizeAngle(float angle)
{
    float a = fmod(angle, _2PI); // fmod()函数用于浮点数的取余运算
    return a >= 0.0f ? a : (a + _2PI);
}

//======================================== LowPassFilter SimpleVer ==============================

// 低通滤波
void LowPassFilter(float *value)
{
    static float valueLast = 0.0f;
    *value = 0.9f * valueLast + 0.1f * (*value);
    valueLast = *value;
}

// 低通滤波
void LowPassFilter1(float *value)
{
    static float valueLast = 0.0f;
    *value = 0.9f * valueLast + 0.1f * (*value);
    valueLast = *value;
}

//======================================== LowPassFilter STD Ver ==============================

void LowPassFilterSTD_Init(LowPassFSTD_s *config, float Tf)
{
    config->Tf = Tf;
    TimeFlash(&(config->timeS));
}

float LowPassFilterSTD_Process(LowPassFSTD_s *config, float x)
{
    float timePass = 0;
    float Tf = config->Tf;
    float y_prev = config->y_prev;
    timePass = TimeFlash(&(config->timeS));

    // unsigned long timestamp = _micros();

    // if (dt < 0.0f ) dt = 1e-3f;
    // else if(dt > 0.3f) {
    //     y_prev = x;
    //     timestamp_prev = timestamp;
    //     return x;
    // }

    float alpha = Tf / (Tf + timePass);
    y_prev = alpha * y_prev + (1.0f - alpha) * x;

    config->y_prev = y_prev;
    return y_prev;
}

float LowPassFilterSTD_Process_EX(LowPassFSTD_s *config, float x, float y_prev)
{
    float timePass = 0;
    float Tf = config->Tf;
    // float y_prev = config->y_prev;
    timePass = TimeFlash(&(config->timeS));

    // unsigned long timestamp = _micros();

    // if (dt < 0.0f ) dt = 1e-3f;
    // else if(dt > 0.3f) {
    //     y_prev = x;
    //     timestamp_prev = timestamp;
    //     return x;
    // }

    float alpha = Tf / (Tf + timePass);
    y_prev = alpha * y_prev + (1.0f - alpha) * x;

    config->y_prev = y_prev;
    return y_prev;
}

//======================================== LowPassFilter Ring Ver ==============================

void LowPassFilterSTDRing_Init(LowPassFSTDRing_s *config, float Tf, float ringRange)
{
    config->Tf = Tf;
    config->ringSet = fabsf(ringRange);
    TimeFlash(&(config->timeS));
}

float LowPassFilterSTDRing_Process_EX(LowPassFSTDRing_s *config, float x, float y_prev)
{
    float timePass = 0;
    float Tf = config->Tf;
    // float y_prev = config->y_prev;
    timePass = TimeFlash(&(config->timeS));

    float err = shortARCCalculate(y_prev, x);

    float x_New = y_prev + err;

    float alpha = Tf / (Tf + timePass);
    y_prev = alpha * y_prev + (1.0f - alpha) * x_New;

    y_prev = keepIn180UpDown(y_prev);

    // config->y_prev = y_prev;
    return y_prev;
}

//======================================== Interval Filter Utils ==============================

#define INTERFILTER_OFFSET 3
int32_t interF_PreData = 0;

uint16_t intervalFilter(uint16_t input)
{
    if ((input > interF_PreData) && (input < (uint16_t)(interF_PreData + INTERFILTER_OFFSET))) // Overflow UP
    {
        interF_PreData = input;
    }
    else
    {
        goto FLASH_INTERF_PREDATA;
    }

    if ((input < interF_PreData) && (input > (uint16_t)(interF_PreData + INTERFILTER_OFFSET)))
    {
        interF_PreData = input;
    }
    else
    {
        goto FLASH_INTERF_PREDATA;
    }

    return interF_PreData;

FLASH_INTERF_PREDATA:
    interF_PreData = input;
    return interF_PreData;
}

uint16_t Inter1_Rec = 0; // 区间滤波的中值
uint16_t Inter1_Range = 2;

uint16_t inter_filter(uint16_t x)
{

    if (x > (Inter1_Rec + Inter1_Range))
    {
        Inter1_Rec = x - Inter1_Range; // 把区间顶上去
    }
    else if (x < (Inter1_Rec - Inter1_Range))
    {
        Inter1_Rec = x + Inter1_Range;
    }
    return Inter1_Rec;
}

uint16_t Inter2_Rec = 0; // 区间滤波的中值
uint16_t Inter2_Range = 3;

uint16_t inter_filterC1(uint16_t x)
{

    if (x > (Inter2_Rec + Inter2_Range))
    {
        Inter2_Rec = x - Inter2_Range; // 把区间顶上去
    }
    else if (x < (Inter2_Rec - Inter2_Range))
    {
        Inter2_Rec = x + Inter2_Range;
    }
    return Inter2_Rec;
}

uint16_t inter_filter_GetRange(){
    return Inter1_Range;
}
int inter_filter_SetRange(uint16_t range){
    Inter1_Range = range;
    return 0;
}

uint16_t inter_filterC1_GetRange(){
    return Inter2_Range;
}
int inter_filterC1_SetRange(uint16_t range){
    Inter2_Range = range;
    return 0;
}

//======================================== Other Utils ==============================

float calculateArcMidpointAngle(float angle1, float angle2)
{
    float a, b, arc1, arc2, c;
    if (angle1 > angle2)
    {
        a = angle1;
        b = angle2;
    }
    else
    {
        b = angle1;
        a = angle2;
    }
    arc1 = _2PI - a + b;
    arc2 = a - b;
    if (arc1 < arc2)
    {
        c = a + arc1 * 0.5f;
        if (c > _2PI)
        {
            return c - _2PI;
        }
        else
        {
            return c;
        }
    }
    else
    {
        return arc2 * 0.5f + b;
    }
}

//======================================== KalmanFilter Utils ==============================

int kalmanFilter_Init(KFP *kfp, KFP_CONFIG config)
{
    memset(kfp, 0x00, sizeof(KFP));
    kfp->config = config;
    return 0;
}

/**
 *卡尔曼滤波器
 *@param KFP *kfp 卡尔曼结构体参数
 *   float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
float kalmanFilter(KFP *kfp, float input)
{
    // 预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    kfp->Now_P = kfp->LastP + kfp->config.Q;
    // 卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->config.R);
    // 更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kfp->out = kfp->out + kfp->Kg * (input - kfp->out); // 因为这一次的预测值就是上一次的输出值
    // 更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
    kfp->LastP = (1 - kfp->Kg) * kfp->Now_P;
    return kfp->out;
}

//======================================== KalmanFilter Utils (old) ==============================

// static float p_last = 0;
// static float x_last = 0;

// //过程噪音   买的破温度计有多破。。
// #define P_Q 0.1
// static float kalman_P_Q = 0.002f;
// //测量噪声
// #define M_R 0.01
// static float kalman_M_R = 0.6f;
// /*
//         Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
//         R:测量噪声，R增大，动态响应变慢，收敛稳定性变好

//         其中p的初值可以随便取，但是不能为0（为0的话卡尔曼滤波器就认为已经是最优滤波器了）
// q,r的值需要我们试出来，讲白了就是(买的破温度计有多破，以及你的超人力有多强)

// r参数调整滤波后的曲线与实测曲线的相近程度，r越小越接近。

// q参数调滤波后的曲线平滑程度，q越小越平滑。

// */

// // 2. 以高度为例 定义卡尔曼结构体并初始化参数
// // KFP KFP_height = {0.02, 0, 0, 0, 0.001, 0.543};

// void kalmanFilter_Set_R(float input){
//     kalman_M_R = input;
// }

// void kalmanFilter_Set_Q(float input){
//     kalman_P_Q = input;
// }

// float KalmanFilter_A(float ResrcData)
// {
//     float R = kalman_M_R;
//     float Q = kalman_P_Q;

//     float x_mid = x_last;
//     float x_now;

//     float p_mid ;
//     float p_now;

//     float kg;

//     //这里p_last 等于 kalmanFilter_A 的p直接取0
//     x_mid=x_last;                       //x_last=x(k-1|k-1),x_mid=x(k|k-1)
//     p_mid=p_last+Q;                     //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声

//     /*
//       *  卡尔曼滤波的五个重要公式
//       */
//     kg=p_mid/(p_mid+R);                 //kg为kalman filter，R 为噪声
//     x_now=x_mid+kg*(ResrcData-x_mid);   //估计出的最优值
//     p_now=(1-kg)*p_mid;                 //最优值对应的covariance
//     p_last = p_now;                     //更新covariance 值
//     x_last = x_now;                     //更新系统状态值

//     return x_now;
// }

// float kalmanFilter_B(float inData)
// {
//   static float prevData=0;
//   //其中p的初值可以随便取，但是不能为0（为0的话卡尔曼滤波器就认为已经是最优滤波器了）
//   static float p=0.01, q=P_Q, r=M_R, kGain=0;
//     p = p+q;
//     kGain = p/(p+r);

//     inData = prevData+(kGain*(inData-prevData));
//     p = (1-kGain)*p;

//     prevData = inData;

//     return inData;
// }