#include "Data_ESpeed\Data_ESpeed.h"
#include "Application.h"

/*** 发动机转速 ***/
uint16_t                DataESpeedActual;
uint16_t                DataESpeedDisp;
uint8_t                 DataESpeedValid;
DataESpeedSamplerStruct DataESpeedSampler;
DataESPeedDampingStruct DataESPeedDamping;
DataESpeedIdleStruct    DataESpeedIdle;

#define DATA_APPR_DIR_INC 0x01
#define DATA_APPR_DIR_DEC 0x02

/******************************************************************************
函数名：Data_Engine_Speed_KL30_Wakeup_Init
功  能：发动机转速初始化函数
参  数：无
返回值：无
******************************************************************************
注  意：该函数KL30初始化/Wakeup初始化被调用一次
******************************************************************************/
void Data_Engine_Speed_KL30_Wakeup_Init (void)
{
    DataESpeedIdle.Timer      = 0;
    DataESpeedActual          = 0;
    DataESpeedDisp            = 0;
    DataESpeedValid           = 0;
    DataESpeedSampler.Cnt     = 0;
    DataESPeedDamping.Speed   = 0;
    DataESPeedDamping.Delta   = 0;
    DataESPeedDamping.Dir     = DATA_APPR_DIR_INC;
}

/******************************************************************************
函数名：Data_Engine_Speed_Processing_Service
功  能：发动机转速数据处理函数
参  数：无
返回值：无
******************************************************************************
注  意：该函数必须每10ms被调用一次
******************************************************************************/
void Data_Engine_Speed_Processing_Service ( void )
{
    uint8_t   i;
    uint16_t  ESpeed;
    uint16_t  Delta;
    uint16_t  Engine_Speed_Value;
    uint8_t   Engine_Speed_State;
    Engine_Speed_Value = Get_CAN_CH0_ID_101_Sig_ECU_Engine_Speed();
    Engine_Speed_State = Get_CAN_CH0_ID_101_Sig_ECU_Engine_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)||(Engine_Speed_State == 1))
        {
            DataESpeedValid          = 0;
            DataESpeedActual         = 0;
            DataESpeedSampler.Cnt    = 0;
        }
        else
        {
            if ( DataESpeedValid == 0 )
            {
                DataESpeedValid = 1;
                ESpeed  = Engine_Speed_Value;
                ESpeed /= 4;
                if ( ESpeed > 12000 )
                     ESpeed = 12000;
                DataESpeedActual = ESpeed;
            }
            else
            {
                DataESpeedSampler.Buffer[DataESpeedSampler.Cnt] = Engine_Speed_Value;

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

                DataESpeedSampler.Cnt++;
                if ( DataESpeedSampler.Cnt >= 5 )
                {
                    DataESpeedSampler.Cnt = 0;
                    ESpeed  = DataESpeedSampler.Buffer[3];
                    ESpeed /= 4;
                    if ( ESpeed > 12000 )
                         ESpeed = 12000;
                    DataESpeedActual = ESpeed;
                }
            }
        }
    }
    else
    {
        DataESpeedValid             = 0;
        DataESpeedActual            = 0;
        DataESpeedSampler.Cnt       = 0;
    }

    //第2步：怠速模式
    ESpeed = DataESpeedActual;
#if 0
    if ( ( ESpeed >= 500 ) && ( ESpeed <= 1000 ) )
    {
        DataESpeedIdle.Timer++;

        if ( ESpeed > DataESpeedIdle.ESpeedBackup )
            Delta = ESpeed - DataESpeedIdle.ESpeedBackup;
        else
            Delta = DataESpeedIdle.ESpeedBackup - ESpeed;

        if ( DataESpeedIdle.Mode == 0 )
        {
            if ( Delta < 150 )
            {
                if ( DataESpeedIdle.Timer >= 200 )
                {
                    DataESpeedIdle.Mode  = 1;
                    DataESpeedIdle.Timer = 0;
                }
            }
            else
                DataESpeedIdle.Timer = 0;

            DataESpeedIdle.ESpeedBackup = ESpeed;
        }
        else
        {
            if ( Delta < 150 )
            {
                if ( DataESpeedIdle.Timer >= 200 )               //怠速模式下每10秒转速数据更新一次
                {
                    DataESpeedIdle.Timer = 0;
                    DataESpeedIdle.ESpeedBackup = ESpeed;
                }
                else
                    ESpeed = DataESpeedIdle.ESpeedBackup;
            }
            else                                                //转速变化大于等于300立即退出怠速模式
            {
                DataESpeedIdle.Mode  = 0;
                DataESpeedIdle.Timer = 0;
                DataESpeedIdle.ESpeedBackup = ESpeed;
            }
        }
    }
    else
    {
        DataESpeedIdle.Mode  = 0;
        DataESpeedIdle.Timer = 0;
        DataESpeedIdle.ESpeedBackup = ESpeed;
    }
#endif
    //第3步：转速的阻尼处理
    if ( ESpeed > DataESPeedDamping.Speed )                //实际值比当前显示值大时
    {
        Delta = ESpeed - DataESPeedDamping.Speed;

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

            Delta  = DataESPeedDamping.Delta;
            Delta /= DATA_ESPEED_DAMPING_FACTOR;

            if ( Delta < DATA_ESPEED_APPR_SPEED_MIN )
                Delta = DATA_ESPEED_APPR_SPEED_MIN;

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

            Delta  = DataESPeedDamping.Delta;
            Delta /= DATA_ESPEED_DAMPING_FACTOR;

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

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

            Delta  = DataESPeedDamping.Delta;
            Delta /= DATA_ESPEED_DAMPING_FACTOR;

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

            Delta  = DataESPeedDamping.Delta;
            Delta /= DATA_ESPEED_DAMPING_FACTOR;

            if ( Delta < DATA_ESPEED_APPR_SPEED_MIN )
                Delta = DATA_ESPEED_APPR_SPEED_MIN;

            if ( DataESPeedDamping.Speed < ESpeed + Delta )
                DataESPeedDamping.Speed  = ESpeed;
            else
                DataESPeedDamping.Speed -= Delta;
        }
    }
    else                                                    //实际值与当前显示值相等时
    {
        Delta  = DataESPeedDamping.Delta;
        Delta /= DATA_ESPEED_DAMPING_FACTOR;

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

            Delta  = DataESPeedDamping.Delta;
            Delta /= DATA_ESPEED_DAMPING_FACTOR;

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

    //第4步 增加回差
    if (DataESpeedDisp <= 80) 
    {  
        DataESpeedDisp = DataESPeedDamping.Speed;
    }
    else
    {
        if ( DataESpeedDisp < DataESPeedDamping.Speed )
        {
            DataESpeedDisp = DataESPeedDamping.Speed;
        }
        else
        {
            Delta = DataESpeedDisp - DataESPeedDamping.Speed;
            if ( Delta >= DATA_ESPEED_HYSTERESIS )
            {
                DataESpeedDisp = DataESPeedDamping.Speed;
            }
        }
    }
}

uint8_t Get_EngineValid(void)
{
  return DataESpeedValid;
}

uint16_t Get_ActualEngineSpeed(void)
{   
    return DataESpeedActual;
}

uint16_t Get_DispEngineSpeed(void)
{
    return DataESpeedDisp;
}