#include "Data_VehicleSpeed.h"
#include "TimerB.h"
#include "Gauges.h"
#include "GaugesInterface.h"
#include "Common_Interface.h"
#include "GUI.h"
#include "kwp2000_service.h"
#include "GPIO.h"

static uint32_t SPEED_RADIO = 637u;

uint16_t Vehicle_ACT_Speed;
uint16_t Vehicle_DISP_Speed;
uint8_t  VehicleSpeedValid;
uint8_t  Vehicle_CurSeg;
uint8_t  DataOverSpeed;
uint8_t  DataOverSpeedK_Line;
uint32_t SPEED_PPK;

_ACCSUBSPEED   AccSubSpeed;
uint16_t       Speed_Old  ;
uint16_t       GslYblSpeedTime  ;

volatile uint32_t VehFreBuf[VehFreNum];

DriveInfoSpeedPlusConverterStruct  DriveInfoSpeed1PlusConverter;
DriveInfoSpeedPlusConverterStruct  DriveInfoSpeed2PlusConverter;

void Speed_KL30_Init(void)
{
    uint8_t i = 0u;
    Vehicle_ACT_Speed = 0u;
    Vehicle_DISP_Speed = 0u;
    VehicleSpeedValid = 0u;
    DataOverSpeedK_Line = 0u;

    for (i = 0; i < VehFreNum; i++)
    {
        VehFreBuf[i] = 0u;
    }
}

void Speed_KL15_ON_Init(void)
{
    uint8_t i = 0u;
    Vehicle_ACT_Speed = 0u;
    Vehicle_DISP_Speed = 0u;
    VehicleSpeedValid = 0u;
    Vehicle_CurSeg = 0u;
    DataOverSpeedK_Line = 0u;

    SPEED_PPK = 15925u;

    Veh_Fre_Init(SPEED_PPK);
    for (i = 0; i < VehFreNum; i++)
    {
        VehFreBuf[i] = 0u;
    }

    Speed_Old = 0u;
    GslYblSpeedTime = 0u;


    DriveInfoSpeed1PlusConverter.Enable    = 0;
    DriveInfoSpeed1PlusConverter.Level     = 1;
    DriveInfoSpeed1PlusConverter.Timer     = 0xFFFFFFFE;
    DriveInfoSpeed1PlusConverter.Period    = 0xFFFFFFFF;
    DriveInfoSpeed1PlusConverter.ActPeriod = 0xFFFFFFFF;

    DriveInfoSpeed2PlusConverter.Enable    = 0;
    DriveInfoSpeed2PlusConverter.Level     = 1;
    DriveInfoSpeed2PlusConverter.Timer     = 0xFFFFFFFE;
    DriveInfoSpeed2PlusConverter.Period    = 0xFFFFFFFF;
    DriveInfoSpeed2PlusConverter.ActPeriod = 0xFFFFFFFF;
}

void Speed_KL15_OFF_Init(void)
{
    Vehicle_ACT_Speed = 0u;
    Vehicle_DISP_Speed = 0u;
    VehicleSpeedValid = 0u;
    Vehicle_CurSeg = 0u;
}

void Speed_Wakeup_Init(void)
{
    Vehicle_ACT_Speed = 0u;
    Vehicle_DISP_Speed = 0u;
    VehicleSpeedValid = 0u;
    Vehicle_CurSeg = 0u;
}

void Speed_Sleep_Init(void)
{
    Vehicle_ACT_Speed = 0u;
    Vehicle_DISP_Speed = 0u;
    VehicleSpeedValid = 0u;
    Vehicle_CurSeg = 0u;
}

void Speed_Processing_Service(void)
{
    static uint8_t i = 0u;
    uint32_t VehValTmp = 0u;
    uint32_t VehValDisp = 0u;
    uint16_t VehValCup = 0u;
    uint32_t PPK = 0u;
    uint16_t  DataOverSpeedValue ;

    COMMON_PowerStatus_t IG_Sta;

    IG_Sta = Common_Get_IG_Sts_Valid();

    if ( IG_Sta == COMMON_POWER_ON)
    {
        /**/
        DataOverSpeedValue = K_Line_Set.K_Line_LID44;
        DataOverSpeedValue *= 10u;

        PPK = Get_Speed_PPK();

        VehValTmp = Cal_Veh_Frequency();
        VehValTmp *= 36000u;
        VehValTmp /= PPK;
        VehFreBuf[i] = VehValTmp;

        i++;
        if (i > VehFreNum)
        {
            Fre_SortShort(VehFreBuf, VehFreNum);
            i = 0u;
        }
        VehValCup = (uint16_t) VehFreBuf[1u];

        if (VehValCup > 2250)
        {
            VehicleSpeedValid = 0u;

            Vehicle_ACT_Speed = 0u;
        }
        else
        {
            VehicleSpeedValid = 1u;

            if (VehValCup >= 1400u)
            {
                Vehicle_ACT_Speed = 1400u;
            }
            else  if (VehValCup <= 40u)
            {
                Vehicle_ACT_Speed = 0u;
            }
            else
            {
                Vehicle_ACT_Speed = VehValCup;
            }
        }

        //传入表头  计算放大系数
        VehValDisp = VehSpeedAmplification((uint16_t)Vehicle_ACT_Speed);
        SetGaugesPara(VehGauges, (uint16_t)VehValDisp);

        //获得显示格和显示车速
        Vehicle_CurSeg = GetGaugesCurrentPos(VehGauges);
        Vehicle_DISP_Speed = VehValDisp;//GetVehDisVal(Vehicle_CurSeg, &GaugesInfo_Init_Table[VehGauges]);


        if (K_Line_Set.K_Line_LID44 != 0xFF)
        {
            if (DataOverSpeedK_Line)
            {
                if (Vehicle_DISP_Speed <= DataOverSpeedValue)
                    DataOverSpeedK_Line = 0;
            }
            else
            {
                if (Vehicle_DISP_Speed >= (DataOverSpeedValue + 20))
                    DataOverSpeedK_Line = 1;
            }
        }
    }
    else
    {
        i = 0u;
    }
}

/*-------------------------------------------------------------------------
 * Function Name  : Set_Speed_PPK
 * Description    : 设置PPK值,初始化 或需要改变时调用
 * Input          :
 * Output         : None
 * Return         : None
 * onther         : PPK = 频率 * 3600 / 车速
 --------------------------------------------------------------------------*/
void Set_Speed_PPK(uint32_t PPK)
{
    SPEED_PPK = PPK;
}
/*-------------------------------------------------------------------------
 * Function Name  : Set_Speed_PPK
 * Description    : 设置速比值,初始化 或需要改变时调用
 * Input          : 速比
 * Output         : None
 * Return         : None
 * onther         : 速比 = PPK / 脉冲
 --------------------------------------------------------------------------*/
void Set_Speed_Ratio(uint32_t Ratio)
{
    SPEED_RADIO = Ratio;
}
/*-------------------------------------------------------------------------
 * Function Name  : Get_Speed_PPK
 * Description    :
 * Input          : None
 * Output         : None
 * Return         : 10倍PPK值
 * onther         :
 --------------------------------------------------------------------------*/
uint32_t Get_Speed_PPK(void)
{
    uint32_t u32Result = 0u;
    u32Result = SPEED_PPK;
    return u32Result;
}

uint16_t Speed_Get_ActualValue(void)
{
    uint16_t u16Result = 0u;
    u16Result = Vehicle_ACT_Speed;
    return u16Result;
}
uint16_t Speed_Get_Display_Value(void)
{
    uint16_t u16Result = 0u;
    u16Result = Vehicle_DISP_Speed;
    return u16Result;
}
/*-------------------------------------------------------------------------
 * Function Name  : Speed_Get_CurSeg
 * Description    : 车速当前格
 * Input          : None
 * Output         : None
 * Return         : None
 * onther         : 应用于断码
 --------------------------------------------------------------------------*/
uint8_t Speed_Get_CurSeg(void)
{
    return Vehicle_CurSeg;
}
/*-------------------------------------------------------------------------
 * Function Name  : Speed_Get_Valid
 * Description    : 车速有效？
 * Input          : None
 * Output         : None
 * Return         : None
 * onther         : None
 --------------------------------------------------------------------------*/
uint8_t Speed_Get_Valid(void)
{
    return VehicleSpeedValid;
}

/******************************************************************************
  函数名：Data_Acc_Speed_Processing
  功  能: 加速度计算
  参  数: 无
  返回值：无
 ******************************************************************************/
void Data_Acc_Speed_Processing(void)
{
    uint16_t Speed_New = 0;
    uint16_t AccSpeed  = 0;
    Speed_New = Common_Get_Act_V_Speed_Valid();

    AccSubSpeed.ACCSpeed_Time++;
    if (AccSubSpeed.ACCSpeed_Time >= 10)
    {
        AccSubSpeed.ACCSpeed_Time = 0;
        if (Speed_Old <= Speed_New)
        {
            AccSpeed      = Speed_New - Speed_Old;
            Speed_Old     = Speed_New ;

            if (AccSpeed > User_App.User_JADD)
            {
                AccSubSpeed.AccSpeed_Flag = 1 ;
            }
            else
            {
                AccSubSpeed.AccSpeed_Flag = 0 ;
            }
            AccSubSpeed.SubSpeed_Flag = 0 ;
        }
        else
        {
            AccSpeed      = Speed_Old - Speed_New;
            Speed_Old     = Speed_New ;
            if (AccSpeed > User_App.User_JSUB)
            {
                AccSubSpeed.SubSpeed_Flag = 1 ;
            }
            else
            {
                AccSubSpeed.SubSpeed_Flag = 0 ;
            }
            AccSubSpeed.AccSpeed_Flag = 0 ;
        }
    }
}
/******************************************************************************
  函数名：Data_GsLYbL_Speed_Processing
  功  能: 高速路， 一般路判断
  参  数: 无
  返回值：无
 ******************************************************************************/
void Data_GsLYbL_Speed_Processing(void)
{
    uint16_t User_Gsl = 0;
    uint16_t User_Ybl = 0;
    uint16_t Veh_Speed = 0u;

    Veh_Speed = Common_Get_Act_V_Speed_Valid();

    User_Gsl = (uint16_t)User_App.User_Gsl * 10 ;
    User_Ybl = (uint16_t)User_App.User_Ybl * 10 ;

    if ( Veh_Speed >= 600 )
    {
        if (GslYblSpeedTime <= 6000 )
            GslYblSpeedTime++;
    }
    else if ( Veh_Speed <= 590 )
    {
        GslYblSpeedTime = 0;
    }

    if (GslYblSpeedTime >= 6000)
    {
        AccSubSpeed.GslYbl_Flag = 1;
    }
    else
    {
        AccSubSpeed.GslYbl_Flag = 0;
    }

    if (AccSubSpeed.GslYbl_Flag == 1)
    {
        if (Veh_Speed > User_Gsl)
        {
            AccSubSpeed.GslSpeed_Flag = 1;
        }
        else if (Veh_Speed <= (User_Gsl - 10))
        {
            AccSubSpeed.GslSpeed_Flag = 0;
        }
    }
    else
    {
        if (Veh_Speed > User_Ybl)
        {
            AccSubSpeed.GslSpeed_Flag = 1;
        }
        else if (Veh_Speed <= (User_Ybl - 10))
        {
            AccSubSpeed.GslSpeed_Flag = 0;
        }

    }
}
//15925
//5096

void Drive_Info_Speed1_Plus_Conversion_ISR ( void )
{
    if ( DriveInfoSpeed1PlusConverter.Enable )
    {
        DriveInfoSpeed1PlusConverter.Timer++;

        if ( DriveInfoSpeed1PlusConverter.ActPeriod >= 10000u )
            DriveInfoSpeed1PlusConverter.ActPeriod = DriveInfoSpeed1PlusConverter.Period;

        if ( DriveInfoSpeed1PlusConverter.Timer >= DriveInfoSpeed1PlusConverter.ActPeriod )
        {
            DriveInfoSpeed1PlusConverter.Timer = 0;

            if ( DriveInfoSpeed1PlusConverter.Level )
            {
                DriveInfoSpeed1PlusConverter.Level = 0;
                DriveInfoSpeed1PlusConverter.ActPeriod = DriveInfoSpeed1PlusConverter.Period;
            }
            else
                DriveInfoSpeed1PlusConverter.Level = 1;
        }

        SPEED1_PWM_MCU_OUT = DriveInfoSpeed1PlusConverter.Level;
    }
}
void Drive_Info_Speed1_Plus_Conversion ( void )
{
    uint32_t Speed;

    if ( ( Speed_Get_Valid() == 1u ) && ( Common_Get_Disp_V_Speed() >= 0u ) )
    {
        Speed = ( uint32_t ) Common_Get_Disp_V_Speed();
        DriveInfoSpeed1PlusConverter.Period = ( uint32_t ) ( 360000000 / 15925) / Speed;
        DriveInfoSpeed1PlusConverter.Enable = 1;
    }
    else
    {
        if ( DriveInfoSpeed1PlusConverter.Enable )
        {
            DriveInfoSpeed1PlusConverter.Enable    = 0;
            DriveInfoSpeed1PlusConverter.Level     = 0;
            DriveInfoSpeed1PlusConverter.Timer     = 0;
            DriveInfoSpeed1PlusConverter.Period    = 0xFFFFFFFF;
            DriveInfoSpeed1PlusConverter.ActPeriod = 0xFFFFFFFF;
        }

        SPEED1_PWM_MCU_OUT = 1;
    }
}
void Drive_Info_Speed2_Plus_Conversion_ISR ( void )
{
    if ( DriveInfoSpeed2PlusConverter.Enable )
    {
        DriveInfoSpeed2PlusConverter.Timer++;

        if ( DriveInfoSpeed2PlusConverter.ActPeriod >= 10000u )
            DriveInfoSpeed2PlusConverter.ActPeriod = DriveInfoSpeed2PlusConverter.Period;

        if ( DriveInfoSpeed2PlusConverter.Timer >= DriveInfoSpeed2PlusConverter.ActPeriod )
        {
            DriveInfoSpeed2PlusConverter.Timer = 0;

            if ( DriveInfoSpeed2PlusConverter.Level )
            {
                DriveInfoSpeed2PlusConverter.Level = 0;
                DriveInfoSpeed2PlusConverter.ActPeriod = DriveInfoSpeed2PlusConverter.Period;
            }
            else
                DriveInfoSpeed2PlusConverter.Level = 1;
        }

        SPEED2_PWM_MCU_OUT = DriveInfoSpeed2PlusConverter.Level;
        SPEED3_PWM_MCU_OUT = DriveInfoSpeed2PlusConverter.Level;
    }
}
void Drive_Info_Speed2_Plus_Conversion ( void )
{
    uint32_t Speed;

    if ( ( Speed_Get_Valid() == 1u ) && ( Common_Get_Disp_V_Speed() >= 0u ) )
    {
        Speed = ( uint32_t ) Common_Get_Disp_V_Speed();
        DriveInfoSpeed2PlusConverter.Period = ( uint32_t ) ( 360000000 / 5096) / Speed;
        DriveInfoSpeed2PlusConverter.Enable = 1;
    }
    else
    {
        if ( DriveInfoSpeed2PlusConverter.Enable )
        {
            DriveInfoSpeed2PlusConverter.Enable    = 0;
            DriveInfoSpeed2PlusConverter.Level     = 0;
            DriveInfoSpeed2PlusConverter.Timer     = 0;
            DriveInfoSpeed2PlusConverter.Period    = 0xFFFFFFFF;
            DriveInfoSpeed2PlusConverter.ActPeriod = 0xFFFFFFFF;
        }

        SPEED2_PWM_MCU_OUT = 1;
        SPEED3_PWM_MCU_OUT = 1;
    }
}
//void Drive_Info_Speed2_Plus_Conversion(void)
//{
//    uint32_t Veh_Frequenc = 0u;
//
//    Veh_Frequenc = Cal_Veh_Frequency();
//    Veh_Frequenc *= 10u;
//    TimerB_PWM_Channel_Fre_Set(TIMERB_2_CH10, Veh_Frequenc, 500u);/*车速1 25*/
//
//    Veh_Frequenc = Veh_Frequenc * 5096 / 15925;
//    TimerB_PWM_Channel_Fre_Set(TIMERB_2_CH13, Veh_Frequenc, 500u);/*车速2 8*/
//
//    TimerB_PWM_Channel_Fre_Set(TIMERB_2_CH15, Veh_Frequenc, 500u);/*车速3 8*/
//}