#include "Data_VSpeed.h"
#include "Components.h"
#include  "CAN_CH0_CAN_Communication_Matrix.h"

 uint16_t DataVSpeedActual;
 uint16_t DataVSpeedDisp;
 uint16_t DataVSpeedHysteresis;
 uint8_t  DataVSpeedValid;
// static uint16_t DataVSpeedDisp_Mile;
static DataVSpeedSamplerStruct DataVSpeedSampler;
static DataVSPeedDampingStruct DataVSPeedDamping;


#define DATA_APPR_DIR_INC 0x01
#define DATA_APPR_DIR_DEC 0x02

/******************************************************************************
Data_Vehicle_Speed_KL30_Wakeup_Init
功  能：车速数据KL30_Wakeup_Init
参  数：无
返回值：无
******************************************************************************
注  意：该函数KL30_Wakeup_Init被调用一次
******************************************************************************/
void Data_Vehicle_Speed_KL30_Wakeup_Init(void)
{
    DataVSpeedActual         = 0;
    DataVSpeedDisp           = 0;
    DataVSpeedHysteresis     = 0;
    DataVSpeedValid          = 1;
    DataVSpeedSampler.Cnt    = 0;
    DataVSPeedDamping.Speed  = 0;
    DataVSPeedDamping.Delta  = 0;
    DataVSPeedDamping.Dir    = DATA_APPR_DIR_INC;
    DataVSpeedSampler.Timer1 = 0;
    DataVSpeedSampler.Timer  = 0;
}

/******************************************************************************
函数名：Data_Vehicle_Speed_Wakeup_Init
功  能：车速数据唤醒初始化
参  数：无
返回值：无
******************************************************************************
注  意：该函数唤醒初始化被调用一次
******************************************************************************/
void Data_Vehicle_Speed_Wakeup_Init(void)
{
    DataVSpeedDisp           = 0;
    DataVSpeedHysteresis     = 0;
    DataVSPeedDamping.Speed  = 0;
    DataVSPeedDamping.Delta  = 0;
    DataVSPeedDamping.Dir    = DATA_APPR_DIR_INC;
    DataVSpeedSampler.Timer1 = 0;
    DataVSpeedSampler.Timer  = 0;
}

/******************************************************************************
函数名：Data_Vehicle_Speed_Processing_Service
功  能：车速数据处理函数
参  数：无
返回值：无
******************************************************************************
注  意：该函数必须每20ms被调用一次
******************************************************************************/
void Data_Vehicle_Speed_Processing_Service(void)
{
    uint8_t  i;
    uint16_t Delta;
    uint32_t VSpeed;
    uint32_t VSpeedCal;
    // uint8_t  ESC_VehicleSpeedvalid;
    uint32_t Vehicle_Speed;

    Vehicle_Speed = Get_CAN_CH0_ID_101_Sig_ECU_Vehicle_Speed();
    //ESC_VehicleSpeedvalid = Get_CAN_CH0_ID_330_Sig_ESC_Vehicle_speed_valid( );

    // 第1步：获取实际车速值及车速有效性
    if( Common_Get_IG_Sts( ) == COMMON_POWER_ON )
    {
        if ((CAN_MSG_Status(&CAN_CH0_CanMsgOp, CAN_CH0_ID_CH0_101_Msg_Count) == CAN_SIG_LOST) || (Vehicle_Speed > 0XFF))
        {
            /* 先判断掉线 */
            if (CAN_MSG_Status(&CAN_CH0_CanMsgOp, CAN_CH0_ID_CH0_101_Msg_Count) == CAN_SIG_LOST)
            {
                DataVSpeedValid          = 0;
                DataVSpeedActual         = 0;
                DataVSpeedSampler.Backup = 0;
            }
            #if 0
            else if ( ESC_VehicleSpeedvalid == 1 )//发送can无效标志位
            {
                if ( DataVSpeedSampler.Timer1 <= 10 ) /* 10帧无效 */
                {
                    if ( DataVSpeedValid )//判断车速是否有效,有效采集10次,车速等于上次值
                    {
                        DataVSpeedSampler.Timer1++;
                        DataVSpeedActual = DataVSpeedSampler.Backup;
                    }
                    else//无效,不采集,车速=0
                    {
                        DataVSpeedSampler.Timer1 = 10;
                        DataVSpeedActual         = 0;
                        DataVSpeedSampler.Backup = 0;
                    }
                }
                else//不采集就,车速=0
                {
                    DataVSpeedValid          = 0;
                    DataVSpeedActual         = 0;
                    DataVSpeedSampler.Backup = 0;
                }
            }
            #endif
            else /* 收到无效值 */
            {
                if ( DataVSpeedSampler.Timer <= 100 ) /* 100帧无效值 */
                {
                    if ( DataVSpeedValid )
                    {
                        DataVSpeedSampler.Timer++;
                        DataVSpeedActual = DataVSpeedSampler.Backup;
                    }
                    else
                    {
                        DataVSpeedSampler.Timer  = 100;
                        DataVSpeedActual         = 0;
                        DataVSpeedSampler.Backup = 0;
                    }
                }
                else    // 接收到100帧无效值
                {
                    DataVSpeedValid          = 0;
                    DataVSpeedActual         = 0;
                    DataVSpeedSampler.Backup = 0;
                }
            }  
        }
        else //车速在线且有效
        {
            if ( DataVSpeedValid == 0 )
            {
                DataVSpeedValid = 1;
                VSpeed = Vehicle_Speed;
                // VSpeed *= 9;
                // VSpeed >>= 4; 
                DataVSpeedActual = VSpeed * 10;
            }
            else
            {
                DataVSpeedSampler.Buffer [ DataVSpeedSampler.Cnt ] = Vehicle_Speed;

                i = DataVSpeedSampler.Cnt;
                while ( (i > 0) && (DataVSpeedSampler.Buffer [ i ] < DataVSpeedSampler.Buffer [ i - 1 ]) )
                {
                    VSpeed                             = DataVSpeedSampler.Buffer [ i ];
                    DataVSpeedSampler.Buffer [ i ]     = DataVSpeedSampler.Buffer [ i - 1 ];
                    DataVSpeedSampler.Buffer [ i - 1 ] = VSpeed;
                    i--;
                }

                DataVSpeedSampler.Cnt++;
                if ( DataVSpeedSampler.Cnt >= 3 )
                {
                    DataVSpeedSampler.Cnt = 0;
                    VSpeed                = DataVSpeedSampler.Buffer [ 1 ];
                    //VSpeed *= 9;
                    //VSpeed >>= 4;
                    DataVSpeedActual = VSpeed * 10;
                }
                DataVSpeedSampler.Timer  = 0;
                DataVSpeedSampler.Timer1 = 0;
                DataVSpeedSampler.Backup = DataVSpeedActual;
            }
        }
    }
    else
    {
        DataVSpeedValid          = 0;
        DataVSpeedActual         = 0;
        DataVSpeedSampler.Timer  = 0;
        DataVSpeedSampler.Timer1 = 0;
        DataVSpeedSampler.Cnt    = 0;
        DataVSpeedSampler.Backup = 0;
    }

    // 第2步：车速的阻尼处理
    VSpeed = DataVSpeedActual;
    if ( VSpeed > DataVSPeedDamping.Speed )    // 实际值比当前显示值大时
    {
        Delta = ( uint16_t )VSpeed - DataVSPeedDamping.Speed;

        if ( DataVSPeedDamping.Dir == DATA_APPR_DIR_INC )    // 显示值正在向实际值增加,则维持当前方向调节增加速度
        {
            if ( DataVSPeedDamping.Delta < Delta )           // 当前增量小于显示值与实际值的差值,则提升增加的速度
            {
                DataVSPeedDamping.Delta += DATA_VSPEED_INC_STEP;
                if ( DataVSPeedDamping.Delta > Delta )
                    DataVSPeedDamping.Delta = Delta;
            }
            else    // 当前增量大于等于显示值与实际值的差值,保持增量与当前差值同步,增加的速度即逐步减小
                DataVSPeedDamping.Delta = Delta;

            Delta = DataVSPeedDamping.Delta;
            Delta /= DATA_VSPEED_DAMPING_FACTOR;

            if ( Delta < DATA_VSPEED_APPR_SPEED_MIN )
                Delta = DATA_VSPEED_APPR_SPEED_MIN;

            DataVSPeedDamping.Speed += Delta;
            if ( DataVSPeedDamping.Speed > VSpeed )
                DataVSPeedDamping.Speed = ( uint16_t )VSpeed;
        }
        else    // 显示值正在减小,则尽快减速至速度为最小时更换方向
        {
            if ( DataVSPeedDamping.Delta > DATA_VSPEED_DEC_STEP )
                DataVSPeedDamping.Delta -= DATA_VSPEED_DEC_STEP;
            else
                DataVSPeedDamping.Delta = 0;

            Delta = DataVSPeedDamping.Delta;
            Delta /= DATA_VSPEED_DAMPING_FACTOR;

            if ( Delta < DATA_VSPEED_APPR_SPEED_MIN )         // 已减速至最小速度
                DataVSPeedDamping.Dir = DATA_APPR_DIR_INC;    // 更换方向
            else
            {
                if ( DataVSPeedDamping.Speed > Delta )
                    DataVSPeedDamping.Speed -= Delta;
                else
                    DataVSPeedDamping.Speed = 0;
            }
        }
    }
    else if ( VSpeed < DataVSPeedDamping.Speed )    // 实际值比当前显示值小时
    {
        Delta = DataVSPeedDamping.Speed - ( uint16_t )VSpeed;

        if ( DataVSPeedDamping.Dir == DATA_APPR_DIR_INC )    // 显示值仍在增加,则尽快减速至速度为最小时更换方向
        {
            if ( DataVSPeedDamping.Delta > DATA_VSPEED_DEC_STEP )
                DataVSPeedDamping.Delta -= DATA_VSPEED_DEC_STEP;
            else
                DataVSPeedDamping.Delta = 0;

            Delta = DataVSPeedDamping.Delta;
            Delta /= DATA_VSPEED_DAMPING_FACTOR;

            if ( Delta < DATA_VSPEED_APPR_SPEED_MIN )         // 已减速至最小速度
                DataVSPeedDamping.Dir = DATA_APPR_DIR_DEC;    // 更换方向
            else
            {
                DataVSPeedDamping.Speed += Delta;
                if ( DataVSPeedDamping.Speed > 1990 )
                     DataVSPeedDamping.Speed = 1990;
            }
        }
        else                                          // 显示值正在向实际值减小,则维持当前方向调节增加速度
        {
            if ( DataVSPeedDamping.Delta < Delta )    // 当前(负)增量小于显示值与实际值的差值,则提升减小的速度
            {
                DataVSPeedDamping.Delta += DATA_VSPEED_INC_STEP;
                if ( DataVSPeedDamping.Delta > Delta )
                    DataVSPeedDamping.Delta = Delta;
            }
            else    // 当前(负)增量大于等于显示值与实际值的差值,保持(负)增量与当前差值同步,减小的速度即逐步减小
                DataVSPeedDamping.Delta = Delta;

            Delta = DataVSPeedDamping.Delta;
            Delta /= DATA_VSPEED_DAMPING_FACTOR;

            if ( Delta < DATA_VSPEED_APPR_SPEED_MIN )
                Delta = DATA_VSPEED_APPR_SPEED_MIN;

            if ( DataVSPeedDamping.Speed < VSpeed + Delta )
                DataVSPeedDamping.Speed = ( uint16_t )VSpeed;
            else
                DataVSPeedDamping.Speed -= Delta;
        }
    }
    else    // 实际值与当前显示值相等时
    {
        Delta = DataVSPeedDamping.Delta;
        Delta /= DATA_VSPEED_DAMPING_FACTOR;

        if ( Delta > DATA_VSPEED_APPR_SPEED_MIN )                    // 当前的速度不是最小,说明数值正在增加/减小中,则继续原过程
        {
            if ( DataVSPeedDamping.Delta > DATA_VSPEED_DEC_STEP )    // 显示值越过了实际值,必然要先减速至最小速度,再改变方向返回实际值
                DataVSPeedDamping.Delta -= DATA_VSPEED_DEC_STEP;
            else
                DataVSPeedDamping.Delta = 0;

            Delta = DataVSPeedDamping.Delta;
            Delta /= DATA_VSPEED_DAMPING_FACTOR;

            if ( DataVSPeedDamping.Dir == DATA_APPR_DIR_INC )     // 显示值当前是增加方向
            {
                if ( Delta < DATA_VSPEED_APPR_SPEED_MIN )         // 已减速至最小速度
                    DataVSPeedDamping.Dir = DATA_APPR_DIR_DEC;    // 更换方向
                else
                {
                    DataVSPeedDamping.Speed += Delta;
                    if ( DataVSPeedDamping.Speed > 1990 )
                         DataVSPeedDamping.Speed = 1990;
                }
            }
            else                                                  // 显示值当前是减小方向
            {
                if ( Delta < DATA_VSPEED_APPR_SPEED_MIN )         // 已减速至最小速度
                    DataVSPeedDamping.Dir = DATA_APPR_DIR_INC;    // 更换方向
                else
                {
                    if ( DataVSPeedDamping.Speed > Delta )
                        DataVSPeedDamping.Speed -= Delta;
                    else
                        DataVSPeedDamping.Speed = 0;
                }
            }
        }
    }

    // 第3步：生成显示车速
     if ( (DataVSPeedDamping.Speed >= DataVSpeedHysteresis) || (DataVSPeedDamping.Speed < DATA_VSPEED_HYSTERESIS) )
    {
        DataVSpeedHysteresis = DataVSPeedDamping.Speed;
    }
    else
    {
        if ( DataVSpeedHysteresis - DataVSPeedDamping.Speed >= DATA_VSPEED_HYSTERESIS )
        {
            DataVSpeedHysteresis = DataVSPeedDamping.Speed;
        }
    } 

    VSpeedCal = ( uint32_t )DataVSpeedHysteresis;

    if ( DataVSpeedValid )
    {
        VSpeedCal *= 105; 
        VSpeedCal /= 100;
        if((VSpeedCal % 10) >= 5)
        {
            VSpeedCal += 5; /* 四舍五入 */
        }
        DataVSpeedDisp = ( uint16_t )VSpeedCal;
    }
    else
    {
        DataVSpeedDisp = 0;
    }
}

/*车速有效位：有效=1，无效=0*/
uint8_t Get_VechileSpeedValid(void)
{
    return DataVSpeedValid;
}
/*真实的车速，精度10倍,*/
uint16_t Get_ActualVechileSpeed(void)
{
    return DataVSpeedActual;
}
/*显示车速，精度10倍，取整，舍去小数点*/
uint16_t Get_DispVechileSpeed(void)
{
    uint16_t Vspeed = 0;
    if(Get_Dis_Unit() == 0) /* KM */
    {
        if ( DataVSpeedDisp >= 1990 )
        {
            DataVSpeedDisp = 1990;
        }
        Vspeed = DataVSpeedDisp;
    }
    else //if (Get_Dis_Unit() == 1)   /* MILE */
    {
        Vspeed = Data_Km_To_Mile(DataVSpeedDisp);
        if ( Vspeed >= 1230 )
        {
            Vspeed = 1230;
        }
    }
    // else    
    // {
    //     ;
    // }
    return Vspeed;
}