#include "Data_PWR.h"
#include "CAN_Communication_Matrix.h"
#include "Communication_Over_CAN.h"
#include "Common_Interface.h"

uint16_t DrvMtrPwr_ACT;
uint16_t DrvMtrPwr_DISP;
uint8_t  DrvMtrPwr_Valid;
uint16_t DrvMtrPwr_CurDis;

uint8_t IstDrvMtrPwr_Value;
uint8_t IstDrvMtrPwr_Valid;
uint8_t IstDrvMtrPwr_Sign;

uint8_t SelfTest_Status;

void DrvMtrPwr_Reset(void)
{
    DrvMtrPwr_ACT    = 0u;
    DrvMtrPwr_DISP   = 0u;
    DrvMtrPwr_Valid  = 0u;
    DrvMtrPwr_CurDis = 0u;

    IstDrvMtrPwr_Value = 0u;
    IstDrvMtrPwr_Valid = 0u;
    IstDrvMtrPwr_Sign  = 0u;
}

void DrvMtrPwr_KL30_Init(void)
{
    DrvMtrPwr_Reset();
}

void DrvMtrPwr_KL15_ON_Init(void)
{
    DrvMtrPwr_Reset();
}

void DrvMtrPwr_Processing_Service(void)
{
    if (Common_Get_IG_Sts() == 1u)
    {
        uint32_t u32Result = Get_ID_18F502F0_Sig_VCU_DrvMtrPwr();
        uint16_t u16Result = Get_ID_18F502F0_Sig_VCU_DrvSysMaxPwr();
        
        if ((u32Result >= 0xFFFE) || (CAN_MSG_Status(ID_VCU_MCUSts2_Msg_Count) != CAN_SIG_RECEIVE))
        {
            IstDrvMtrPwr_Valid = 0u;
            IstDrvMtrPwr_Value = 0u;
            IstDrvMtrPwr_Sign  = 0u;
        }
        else
        {
            IstDrvMtrPwr_Valid = 1u;
            
            if (u32Result <= 400ul) //有效值-200~200
            {
                if (u32Result >= 200ul)
                {
                    IstDrvMtrPwr_Value = (uint8_t)(u32Result - 200ul);
                    IstDrvMtrPwr_Sign  = 0u;
                }
                else
                {
                    IstDrvMtrPwr_Value = (uint8_t)(200ul - u32Result);
                    IstDrvMtrPwr_Sign  = 1u;
                }
            }
            else
            {
                IstDrvMtrPwr_Value = 0u;
                IstDrvMtrPwr_Sign  = 0u;
            }
        }
        
        uint8_t  dist   = 7u;
        uint32_t OnTime = Common_GetIgnOnTime(); 
                   
        if (OnTime <= 4000u)
        {
            SelfTest_Status = 0u;
            dist = 14u;

            DrvMtrPwr_ACT   = 0u;
            DrvMtrPwr_Valid = 0u;
            
            if (OnTime <= 2000u)
            {
                DrvMtrPwr_DISP = 1000u;
            }
            else
            {
                DrvMtrPwr_DISP = 0u;
            }
        }
        else
        {
            if (OnTime <= 6000u)
            {
                dist = 28u;
            }

            SelfTest_Status = 1u;

            if ((u32Result >= 0xFFFE) || (u16Result >= 0xFFFE) || (CAN_MSG_Status(ID_VCU_MCUSts2_Msg_Count) != CAN_SIG_RECEIVE))
            {
                DrvMtrPwr_Valid = 0u;
            }
            else
            {
                DrvMtrPwr_Valid = 1u;
            }
            
            u32Result = IstDrvMtrPwr_Value;
            
            if (u16Result > 1000)
            {
                u16Result = 1000;
            }

            if (DrvMtrPwr_Valid && u16Result)
            {
                u32Result *= 1000ul;
                u32Result /= u16Result;
            }
            else
            {
                u32Result = 0ul;
            } 
            
            if (u32Result > 1000ul) 
            {
                u32Result = 1000ul;
            }
            
            DrvMtrPwr_ACT  = (uint16_t)u32Result;
            DrvMtrPwr_DISP = DrvMtrPwr_ACT;
        }
        //阻尼
        if (DrvMtrPwr_CurDis < DrvMtrPwr_DISP)
        {
            DrvMtrPwr_CurDis += dist;

            if (DrvMtrPwr_CurDis > DrvMtrPwr_DISP)
            {
                DrvMtrPwr_CurDis = DrvMtrPwr_DISP;
            }
        }
        else if (DrvMtrPwr_CurDis != DrvMtrPwr_DISP)
        {
            if (DrvMtrPwr_CurDis >= dist)
            {
                DrvMtrPwr_CurDis -= dist;
            }
            else
            {
                DrvMtrPwr_CurDis = DrvMtrPwr_DISP;
            }
        }
    }
    else
    {
        DrvMtrPwr_Reset();
    }
}

uint16_t DrvMtrPwr_Get_ACT(void)
{
    return DrvMtrPwr_ACT;
}

uint16_t DrvMtrPwr_Get_DISP(void)
{
    return DrvMtrPwr_DISP;
}

uint16_t DrvMtrPwr_Get_CurDis(void)
{
    return DrvMtrPwr_CurDis;
}

uint8_t IstDrvMtrPwr_Get_Value(void)
{
    return IstDrvMtrPwr_Value;
}

uint8_t IstDrvMtrPwr_Get_Valid(void)
{
    return IstDrvMtrPwr_Valid;
}

uint8_t IstDrvMtrPwr_Get_Sign(void)
{
    return IstDrvMtrPwr_Sign;
}

uint8_t SelfTest_Status_Get(void)
{
    return SelfTest_Status;
}
