/**
 * @file TimeTask_Event.c
 * @author Zhang, Kai(zhangkai71@midea.com)
 * @brief Time task control
 * @version 0.1
 * @date 2021-09-27
 *
 * @copyright Copyright (c) 2021
 *
 */
/******************************
 *
 *  Included File
 *
 ******************************/
#include "TimeTask_Event.h"
#include "AssistCurve.h"
#include "FSM_1st.h"
#include "FSM_2nd.h"
#include "FuncLayerAPI.h"
#include "can.h"
#include "canAppl.h"
#include "flash_master.h"
#include "gd32f30x.h"
#include "string.h"
#include "syspar.h"
#include "user.h"
#include "STLmain.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
_op_ Op[proc_cnt] = {{Event_5ms, EVE1MSCNT_5ms, EVE1MSCNT_5ms},
                     {Event_10ms, EVE1MSCNT_10ms, EVE1MSCNT_10ms},
                     {Event_20ms, EVE1MSCNT_20ms, EVE1MSCNT_20ms},
                     {Event_100ms, EVE1MSCNT_100ms, EVE1MSCNT_100ms},
                     {Event_200ms, EVE1MSCNT_200ms, EVE1MSCNT_200ms}};

static SQWORD TimingTaskTimerTick = 0;
static SQWORD TimingTaskTimerTickTemp = 0;
static SQWORD TimingTaskTimerTickTempOld = 0;
static SQWORD TimingTaskTimerTickPassed = 0;
static UWORD  LoopServerExecutedFlag = 0;
static UWORD  AssistCNT = 0;
static BOOL tstMafClrFlg = FALSE;
/******************************
 *
 *  Function
 *
 ******************************/
void  Event_1ms(void)
{
    /* 1st FSM control */
    FSM_1st_Main();
    FSM1st_Sys_state.Event_hook();
    
    /* Power control */
    power_voPowerManagement(ass_ParaCong.uwAutoPowerOffTime * 60, cp_ulSystickCnt, OBC_ButtonStatus.ulButtonSetTimeCnt, \
                            MC_RunInfo.Torque, MC_RunInfo.Cadence, MC_RunInfo.BikeSpeed, \
                            cp_stFlg.ParaHistorySaveEEFinishFlg, cp_stFlg.ParaSaveEEFlg);
    
    /* cp_history info update */
    Can_voMC_Run_1ms();

    /* Torque move average filter */
    if (cadence_stFreGetOut.uwForwardCnt > 0)
    {
        cadence_stFreGetOut.uwForwardCnt = 0;
        ass_stTorqMafValue.value = torsensor_stTorSensorOut.uwTorquePu;
        ass_voMoveAverageFilter(&ass_stTorqMafValue);
        
        /* Iqref maf test, dont add torq obs */
        if(ass_CalOut.blTorqPIFlg)
        {
            ass_stUqLimMafValue.value = ass_stTorqPIOut.swIRefPu;
            ass_voMoveAverageFilter(&ass_stUqLimMafValue);
            tstMafClrFlg = FALSE;
        }
        else if((!ass_CalOut.blTorqPIFlg) && (tstMafClrFlg == FALSE))
        {
            ass_voMoveAverageFilterClear(&ass_stUqLimMafValue);
            tstMafClrFlg = TRUE;
        }    
        
    }

    /* Torque info update */
    torsensor_voTorADC();

    /* Torque assist calculation*/
    //ass_CalIn.SOCValue = 100;
    ass_CalIn.swFlxIqLimit = MC_RunInfo.SOC;
    if(cp_stFlg.RunModelSelect == CityBIKE )
    {
        ass_CalIn.swDirection = -1;
    }
    else if(cp_stFlg.RunModelSelect == MountainBIKE)
    {
        ass_CalIn.swDirection = 1;
    }
    else
    {
        ass_CalIn.swDirection = 1;
    }
    ass_CalIn.swFlxIqLimit = abs(flx_stCtrlOut.swIqLimPu);
    ass_CalIn.swPwrIqLimit = abs(pwr_stPwrLimOut2.swIqLimPu);
    ass_CalIn.uwbikespeed = bikespeed_stFreGetOut.uwLPFFrequencyPu;
    ass_CalIn.uwcadancelast = ass_CalIn.uwcadance;
    ass_CalIn.uwcadance = cadence_stFreGetOut.uwLPFFrequencyPu;
    ass_CalIn.uwcadancePer = cadence_stFreGetOut.uwFreqPercent;
    ass_CalIn.uwcadanceFWCnt = cadence_stFreGetOut.uwForwardCnt;
    ass_CalIn.uwGearSt = (cp_stBikeRunInfoPara.uwBikeGear <= 6) ? cp_stBikeRunInfoPara.uwBikeGear : 0;
    ass_CalIn.uwSpdFbkAbsPu = scm_uwSpdFbkLpfAbsPu;
    ass_CalIn.swSpdFbkPu = scm_stSpdFbkLpf.slY.sw.hi;
    ass_CalIn.uwBaseSpdrpm = cof_uwVbRpm;
    ass_CalIn.uwtorque = ass_stTorqMafValue.AverValue; //torsensor_stTorSensorOut.uwTorqueLPFPu;
    ass_CalIn.uwtorquelpf = torsensor_stTorSensorOut.uwTorqueLPFPu;
    ass_CalIn.uwtorquePer = torsensor_stTorSensorOut.uwTorquePu;
    ass_CalIn.swCurFdbPu = scm_swIqFdbLpfPu;
    ass_CalIn.swCurRefPu = scm_swIqRefPu;
    ass_voAssist();
    
    uart_swTorqRefNm = ass_CalOut.swAssitCurRef;
    
    /////////Constant Current Control Test///////////
//    if((cp_stFlg.RunModelSelect == CityBIKE) || (cp_stFlg.RunModelSelect == MountainBIKE))
//    {
//            
//      ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu;
//      uart_swTorqRefNm = ass_CalIn.swDirection * ass_ParaSet.uwSpeedAssistIMaxA;
//      
//      if(uart_swTorqRefNm != 0)
//      {
//         ass_CalCoef.blAssistflag = TRUE;
//      }
//      else
//      {
//         ass_CalCoef.blAssistflag = FALSE;
//      }
//          
//    }
    
    /* Torque assist mode flag */
    if (ass_CalCoef.blAssistflag == TRUE && cp_stFlg.RunPermitFlg == TRUE && cp_stFlg.SpiOffsetFirstSetFlg ==1)
    {
        signal_state.Sensor = TRUE;
    }
    else if( cp_stFlg.SpiOffsetFirstSetFlg == 0 && cp_stFlg.RunPermitFlg == TRUE && cp_stFlg.SpiOffsetFirstSetFinishFlg == FALSE)
    {
        signal_state.Sensor = TRUE;
    }
    else
    {
        signal_state.Sensor = FALSE;
    }

    /* Speed assist mode flag */
    if((cp_stFlg.RunModelSelect == CityBIKE) || (cp_stFlg.RunModelSelect == MountainBIKE))
    {
        if(cp_stBikeRunInfoPara.uwBikeGear == 0x22)
        {
            AssistCNT ++;
            if(AssistCNT > 200 && cp_stFlg.RunPermitFlg == TRUE)
            {
                signal_state.Assist = TRUE;
                AssistCNT = 200;
            }
        }
        else
        {
            AssistCNT = 0;
            signal_state.Assist = FALSE;
        }

        if(signal_state.Assist == TRUE)
        {
            //6km/H = 100m/min = 1.67m/s
            if(cp_stFlg.RunModelSelect == CityBIKE)
            {
                uart_slSpdRefRpm = -(10000/(ass_ParaCong.uwWheelDiameter))*ass_ParaCong.uwNmBackChainring*ass_ParaCong.uwMechRationMotor/ass_ParaCong.uwNmFrontChainring;
//                cp_stBikeRunInfoPara.BikeSpeedKmH = 60;      //constant display of 6km/h
            }
            else if(cp_stFlg.RunModelSelect == MountainBIKE)
            {
                uart_slSpdRefRpm = (10000/(ass_ParaCong.uwWheelDiameter))*ass_ParaCong.uwNmBackChainring*ass_ParaCong.uwMechRationMotor/ass_ParaCong.uwNmFrontChainring;
//                cp_stBikeRunInfoPara.BikeSpeedKmH = 60;      //constant display of 6km/h
            }
            else
            {
            }

        }
        else
        {
            uart_slSpdRefRpm = 0;
        }
    }
    else
    {          
        if ((uart_slSpdRefRpm >= 10 || uart_slSpdRefRpm <= -10) && cp_stFlg.RunPermitFlg == TRUE )
        {
            signal_state.Assist = TRUE;
        }
        else
        {
            signal_state.Assist = FALSE;
        }
    }  
}

void Event_5ms(void)
{
    /* Upper Computer Info Update */
    Can_voMC_Run_5ms();
}

void Event_10ms(void)
{
    /* Throttle ADC voltage update */
    bikethrottle_voBikeThrottleADC();

    /*  Speed command set */
    if(cp_stFlg.RunModelSelect != CityBIKE && cp_stFlg.RunModelSelect != MountainBIKE )
    {
        /* Use instrument */
//        Signal_detect();
      
        /* Use upper computer */
        if(cp_stFlg.RotateDirectionSelect == ForwardRotate)
        {
            uart_slSpdRefRpm = ((SLONG)MC_MotorSPD_rpm_Percent*5000)/100;
        }
        else if(cp_stFlg.RotateDirectionSelect == BackwardRotate)
        {
            uart_slSpdRefRpm = -((SLONG)MC_MotorSPD_rpm_Percent*5000)/100;
        }
     
        /*  Use Bike Speed PI */
//         bikespeed_voPuCal();
//         bikespeed_voPI(&bikespeed_stPIIn, &bikeSpdPIOut);
//         ass_CalIn.swDirection =-1;       
//         uart_slSpdRefRpm = bikeSpdPIOut.swSpdRefRpm * ass_CalIn.swDirection;

         if(abs(uart_slSpdRefRpm) < 300)
         {
             uart_slSpdRefRpm = 0;
         }
    }
    
    /* Bike light control */
    Can_Light_switch();
    bikelight_voBikeLightControl(cp_stBikeRunInfoPara.uwLightSwitch);

    /* Trip cal when open */
    bikespeed_votempTripCal();
}


void  Event_20ms(void)
{
    /* MCU Self Check */
    stl_voDoRunTimeChecks();
}

void Event_100ms(void)
{
    /* Bike Speed LPF */
    bikespeed_stFreGetOut.uwLPFFrequencyPu = (bikespeed_stFreGetOut.uwLPFFrequencyPu * bikespeed_stFreGetCof.uwBikeSpeedLPFGain +
                                              bikespeed_stFreGetOut.uwFrequencyPu * (100 - bikespeed_stFreGetCof.uwBikeSpeedLPFGain)) /
                                             100;
}

void  Event_200ms(void)
{
    /* Upper Computer Info Update */
    Can_voMC_Run_200ms();

    /* Bike Sesor Suply Voltage Fault Detect */
    bikelight_voGetBikeLightError(adc_stUpOut.uwU6VPu);
    display_voGetDisplayError(adc_stUpOut.uwU12VPu);
    bikespeed_voGetBikeSpeedPwrError(adc_stUpOut.uwU5VPu);

    /* Bike Sensor Faults Detect */
//    if((cp_stFlg.RunModelSelect == MountainBIKE) || (cp_stFlg.RunModelSelect == CityBIKE))
//    {
//        alm_stBikeIn.uwTroqReg = torsensor_stTorSensorOut.uwTorqueReg;
//        alm_stBikeIn.uwTroqPu = torsensor_stTorSensorOut.uwTorqueLPFPu;//torsensor_stTorSensorOut.uwTorquePu;
//        alm_stBikeIn.blBikeSpdOvrFlg = bikespeed_stFreGetOut.blBikeSpeedSensorPwrErrorFlg;
//        alm_stBikeIn.blCadenceFreqOvrFlg = cadence_stFreGetOut.blCadenceSensorErrorFlg;
//        alm_stBikeIn.swMotorSpdDir = ass_CalIn.swDirection;
//        alm_stBikeIn.swMotorSpdPu = scm_stSpdFbkLpf.slY.sw.hi;
//        alm_stBikeIn.uwBikeSpdPu = bikespeed_stFreGetOut.uwFrequencyPu;
//        alm_stBikeIn.uwCadenceFreqPu = cadence_stFreGetOut.uwFrequencyPu;
//        alm_stBikeIn.uwMotorNTCReg = adc_stUpOut.MotorTempReg;
//        alm_stBikeIn.uwPCBNTCReg = adc_stUpOut.PCBTempReg;
//        alm_stBikeIn.uwThrottleReg = adc_stUpOut.uwThrottleReg;
//        alm_stBikeIn.blThrottleExistFlg = FALSE;
//        alm_stBikeIn.blMotorNTCExistFlg = FALSE;
//        alm_voDetec200MS(&alm_stBikeIn, &alm_stDetect200MSCoef);   
//    }
    
    if (switch_flg.SysFault_Flag == TRUE)
    {
        SendData(ID_MC_BC, MODE_REPORT, 0x1104, (uint8_t *)&MC_ErrorCode.Code);
    }
}

 void Signal_detect(void)
{
    SWORD sign;

    if (MOTOR_ID_SEL == MOTOR_WELLING_MTB)
    {
        sign = 1;
    }
    else if(MOTOR_ID_SEL == MOTOR_WELLING_CITY)
    {
        sign = -1;
    }
    else
    {
        sign=1;
    }         
         
    if(MC_ControlCode.GearSt == 0x01)
    {
        uart_slSpdRefRpm = sign *785;
    }
    else if(MC_ControlCode.GearSt == 0x02)
    {
        uart_slSpdRefRpm = sign *1000;
    }
    else if(MC_ControlCode.GearSt == 0x03)
    {
        uart_slSpdRefRpm = sign *3088;      
    }
    else if(MC_ControlCode.GearSt == 0x04)
    {
        uart_slSpdRefRpm = sign*3603;      
    }
    else if(MC_ControlCode.GearSt == 0x33 || MC_ControlCode.GearSt == 0x05)
    {
        uart_slSpdRefRpm = sign*4500;
    }
    else
    {
        uart_slSpdRefRpm = 0;
    }

}

void TimingTaskTimerServer(void)
{
    TimingTaskTimerTick++;
    LoopServerExecutedFlag = 0;
}

void TimingTaskLoopServer(void)
{
    SLONG cnt;
    TimingTaskTimerTickTempOld = TimingTaskTimerTickTemp;
    TimingTaskTimerTickTemp = TimingTaskTimerTick;
    TimingTaskTimerTickPassed = TimingTaskTimerTickTemp - TimingTaskTimerTickTempOld;
    if (LoopServerExecutedFlag == 0)
    {
        for (cnt = 0; cnt < proc_cnt; cnt++)
        {
            Op[cnt].tick -= TimingTaskTimerTickPassed;
            if (Op[cnt].tick <= 0)
            {
                Op[cnt].tick += Op[cnt].timespan;
                Op[cnt].proc();
            }
        }
        LoopServerExecutedFlag = 1;
    }
}