#include "Data_VSpeed\Data_VSpeed.h"
#include "Application.h"

static uint16_t DataVSpeedActual;
static uint16_t DataVSpeedDisp;
static uint32_t DataVSpeedTarget;
static uint16_t DataVSpeedHysteresis;
static 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_Init
功  能：车速数据KL30初始化
参  数：无
返回值：无
******************************************************************************
注  意：该函数KL30初始化被调用一次
******************************************************************************/
void Data_Vehicle_Speed_KL30_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;
    DataVSpeedTarget         = 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;
    DataVSpeedTarget         = 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_VehicleSpeedState;
    uint32_t Vehicle_Speed;

    Vehicle_Speed = Get_CAN_CH0_ID_101_Sig_ECU_Vehicle_Speed( );
    ESC_VehicleSpeedState = Get_CAN_CH0_ID_101_Sig_ECU_Vehicle_Speed_State( );

    // 第1步：获取实际车速值及车速有效性
    if( Common_Get_IG_Sts( ) == COMMON_POWER_ON )
    {        
        if (CAN_MSG_Status(&CAN_CH0_CanMsgOp, CAN_CH0_ID_CAN_0x101_Msg_Count) == CAN_SIG_LOST)
        {
            DataVSpeedValid          = 0;
            DataVSpeedActual         = 0;
            DataVSpeedSampler.Backup = 0;
        }
        else
        {
            if (ESC_VehicleSpeedState == 1)
            {
                DataVSpeedValid          = 0;
                DataVSpeedActual         = 0;
                DataVSpeedSampler.Backup = 0;
            }
            else
            {
                if ( DataVSpeedValid == 0 )
                {
                    DataVSpeedValid = 1;
                    VSpeed = Vehicle_Speed;                    
                    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 ];                        
                        DataVSpeedActual = VSpeed * 10;
                    }
                    //DataVSpeedSampler.Timer  = 0;
                    //DataVSpeedSampler.Timer1 = 0;
                    DataVSpeedSampler.Backup = DataVSpeedActual;
                    DataVSpeedValid = 1;
                }
            }
        }
    }
    else
    {
        DataVSpeedValid          = 0;
        DataVSpeedActual         = 0;
        //DataVSpeedSampler.Timer  = 0;
        //DataVSpeedSampler.Timer1 = 0;
        DataVSpeedSampler.Cnt    = 0;
        DataVSpeedSampler.Backup = 0;
    }
    
        

    DataVSpeedTarget = DataVSpeedActual;       
    
    if ( DataVSpeedValid )
    {
        DataVSpeedTarget *= 108; 
        DataVSpeedTarget /= 100;
    
        if(DataVSpeedTarget > 1990)
        {
           DataVSpeedTarget = 1990;
        }       
    
    }
    else
    {
        DataVSpeedTarget = 0;
    }

    if ( (DataVSpeedTarget >= DataVSpeedHysteresis) || (DataVSpeedTarget < DATA_VSPEED_HYSTERESIS) )
    {
        DataVSpeedHysteresis = DataVSpeedTarget;
    }
    else
    {
        if ( DataVSpeedHysteresis - DataVSpeedTarget >= DATA_VSPEED_HYSTERESIS )
        {
            DataVSpeedHysteresis = DataVSpeedTarget;
        }
    } 

    DataVSpeedSampler.Timer ++;
    if(DataVSpeedSampler.Timer >= 10)
    {
        DataVSpeedSampler.Timer = 0;
        if(DataVSpeedDisp < DataVSpeedHysteresis) 
        {
            DataVSPeedDamping.Dir = DATA_APPR_DIR_INC;
        }
        else if (DataVSpeedDisp > DataVSpeedHysteresis)
        {
            DataVSPeedDamping.Dir = DATA_APPR_DIR_DEC;
        }
        else
        {
            ;
        }
        if ( DataVSPeedDamping.Dir == DATA_APPR_DIR_INC)
        {
            if (DataVSpeedHysteresis >= DataVSpeedDisp) 
            {
                if (DataVSpeedHysteresis > (DataVSpeedDisp + 400))
                {
                    DataVSpeedDisp += 370;
                }
                else if (DataVSpeedHysteresis > (DataVSpeedDisp + 150))
                {
                    DataVSpeedDisp += 120;
                }
                else if (DataVSpeedHysteresis > (DataVSpeedDisp + 50))
                {
                    DataVSpeedDisp += 30;
                }
                else if (DataVSpeedHysteresis > (DataVSpeedDisp + 10))
                {
                    DataVSpeedDisp += 10;
                }
                else 
                {
                    DataVSpeedDisp = DataVSpeedHysteresis;
                }
            }
        }
        else if (DataVSPeedDamping.Dir == DATA_APPR_DIR_DEC)
        {        
            if (DataVSpeedHysteresis < DataVSpeedDisp)
            {
                if ((DataVSpeedHysteresis + 400) < DataVSpeedDisp) 
                {
                    DataVSpeedDisp -= 370;
                }
                else if ((DataVSpeedHysteresis + 150) < DataVSpeedDisp) 
                {
                    DataVSpeedDisp -= 120;
                }
                else if ((DataVSpeedHysteresis + 50) < DataVSpeedDisp) 
                {
                    DataVSpeedDisp -= 30;
                }
                else if ((DataVSpeedHysteresis + 10) < DataVSpeedDisp) 
                {
                    DataVSpeedDisp -= 10;
                }
                else 
                {
                    DataVSpeedDisp = DataVSpeedHysteresis;
                }
            }
            if (DataVSpeedDisp >= 1990) 
            {
                DataVSpeedDisp = 1990;
            }
            
        }
    }
    
#if(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 *= 108; 
        VSpeedCal /= 100;
        
        if(VSpeedCal > 1990)
        {
            VSpeedCal = 1990;
        }
        
        DataVSpeedDisp = ( uint16_t )VSpeedCal;
    }
    else
    {
        DataVSpeedDisp = 0;
    }
    #endif
}

/*车速有效位：有效=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_KM_Unit() == 0) /* KM */
    {
        if ( DataVSpeedDisp >= 1990 )
        {
            DataVSpeedDisp = 1990;
        }
        Vspeed = DataVSpeedDisp;
    }
    else    /* MILE */
    {
        Vspeed = Data_Km_To_Mile(DataVSpeedDisp);
        if ( Vspeed >= 1230 )
        {
            Vspeed = 1230;
        }
    }
    
    return Vspeed;
}
/*外发显示车速，精度10倍，取整，舍去小数点*/
uint16_t Get_DispVechileSpeed_TX(void)
{
    uint16_t Vspeed = 0;
    if ( DataVSpeedDisp >= 1990 )
    {
        DataVSpeedDisp = 1990;
    }
    Vspeed = DataVSpeedDisp;    
    
    return Vspeed;
}