/**
 * @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;
SWORD tstBikeSpdRefTarget = 0, tstBikeSpdRef = 0, tstBikeSpdRefTargetZ1 = 0;
SWORD tstIqRefTarget = 0, tstIqRef,tstIqRefTargetZ1 = 0;
BOOL Event_pvt_blBikeThroFlg = FALSE, Event_pvt_blBikeThroFlgZ1 = FALSE;
/******************************
 *
 *  Function
 *
 ******************************/
void  Event_1ms(void)
{
    /* 1st FSM control */
    FSM_1st_Main();
    FSM1st_Sys_state.Event_hook();
    
    /* Power control */
    power_voPowerManagement(ass_stParaCong.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();

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

        /* Torque info update */
        torsensor_voTorADC();
        torsensor_voOffsetUpdate();
        
        /* Torque assist calculation*/
        //ass_stCalIn.SOCValue = 100;
        ass_stCalIn.SOCValue = MC_RunInfo.SOC;
        if(cp_stFlg.RunModelSelect == CityBIKE )
        {
            ass_stCalIn.swDirection = -1;
        }
        else if(cp_stFlg.RunModelSelect == MountainBIKE)
        {
            ass_stCalIn.swDirection = 1;
        }
        else
        {
            ass_stCalIn.swDirection = 1;
        }
        ass_stCalIn.swFlxIqLimit = abs(flx_stCtrlOut.swIqLimPu);
        ass_stCalIn.swPwrIqLimit = abs(pwr_stPwrLimOut2.swIqLimPu);
        ass_stCalIn.uwbikespeed = bikespeed_stFreGetOut.uwLPFFrequencyPu;
        ass_stCalIn.uwcadancelast = ass_stCalIn.uwcadance;
        ass_stCalIn.uwcadance = cadence_stFreGetOut.uwLPFFrequencyPu;
        ass_stCalIn.uwcadancePer = cadence_stFreGetOut.uwFreqPercent;
        ass_stCalIn.uwcadanceFWCnt = cadence_stFreGetOut.uwForwardCnt;
        ass_stCalIn.uwGearSt = (cp_stBikeRunInfoPara.uwBikeGear <= 6) ? cp_stBikeRunInfoPara.uwBikeGear : 0;
        ass_stCalIn.uwSpdFbkAbsPu = scm_uwSpdFbkLpfAbsPu;
        ass_stCalIn.swSpdFbkPu = scm_stSpdFbkLpf.slY.sw.hi;
        ass_stCalIn.uwBaseSpdrpm = cof_uwVbRpm;
        ass_stCalIn.uwtorque = ass_stTorqMafValue.slAverValue; //torsensor_stTorSensorOut.uwTorqueLPFPu;
        ass_stCalIn.uwtorquelpf = torsensor_stTorSensorOut.uwTorqueLPFPu;
        ass_stCalIn.uwtorquePer = torsensor_stTorSensorOut.uwTorquePu;
        ass_stCalIn.swCurFdbPu = scm_swIqFdbLpfPu;
        ass_stCalIn.swCurRefPu = scm_swIqRefPu;
        ass_voAssist();

        /* Select Bike Torque or Throttle Assist */
        if(Event_pvt_blBikeThroFlg == FALSE)
        {
            if (ass_stCalCoef.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;  //for Spi Theta Offset 
            }
            else
            {
                signal_state.Sensor = FALSE;
            }

            /* Throttle to Torque */
            if(Event_pvt_blBikeThroFlgZ1 == TRUE)
            {
                /* Initial Value of Torque Assit Output */
                ass_stCalOut.swAssitCurRef = scm_swIqFdbLpfPu;    
                ass_pvt_stCurLpf.slY.sw.hi = scm_swIqFdbLpfPu;
                ass_stCalOut.swTorRefEnd = abs(scm_swIqFdbLpfPu);
            }

            uart_swTorqRefNm = ass_stCalOut.swAssitCurRef;
        }
        else
        {
            signal_state.Sensor = TRUE;
            ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
              
            /* Torque to Throttle */
            if(Event_pvt_blBikeThroFlgZ1 == FALSE)
            {
                /* Initial Value of Throttle Assit Output */
                tstIqRef = scm_swIqFdbLpfPu;
                tstIqRefTarget = scm_swIqFdbLpfPu;
                bikespeed_stPIOut.slIqRefPu = scm_swIqFdbLpfPu << 16;    
            }

            /* Bike Throttle Assist Iqref Ramp */
            if(tstIqRef< tstIqRefTarget - 100)
            {
                if(tstIqRefTarget >= tstIqRefTargetZ1)
                {
                    tstIqRef += 100;
                }               
            }
            else
            {
                tstIqRef = tstIqRefTarget;
            }
            tstIqRefTargetZ1 = tstIqRefTarget;
            uart_swTorqRefNm = tstIqRef * ass_stCalIn.swDirection;
        }
        Event_pvt_blBikeThroFlgZ1 = Event_pvt_blBikeThroFlg;

        /* 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_stParaCong.uwWheelPerimeter))*ass_stParaCong.uwNmBackChainring*ass_stParaCong.uwMechRationMotor/ass_stParaCong.uwNmFrontChainring;
    //                cp_stBikeRunInfoPara.BikeSpeedKmH = 60;      //constant display of 6km/h
                }
                else if(cp_stFlg.RunModelSelect == MountainBIKE)
                {
                    uart_slSpdRefRpm = (10000/(ass_stParaCong.uwWheelPerimeter))*ass_stParaCong.uwNmBackChainring*ass_stParaCong.uwMechRationMotor/ass_stParaCong.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)
{
    if(switch_flg.SysCoef_Flag == TRUE)
    {
        /* 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;
            }
      
             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)
{
    SWORD swIqLowerPu;
    if(switch_flg.SysCoef_Flag == TRUE)
    {
        /* Bike Speed LPF */
        bikespeed_stFreGetOut.uwLPFFrequencyPu = (bikespeed_stFreGetOut.uwLPFFrequencyPu * bikespeed_stFreGetCof.uwBikeSpeedLPFGain +
                                                  bikespeed_stFreGetOut.uwFrequencyPu * (100 - bikespeed_stFreGetCof.uwBikeSpeedLPFGain)) /
                                                 100;
        /* Bike Throttle Assist */
        if((bikethrottle_stBikeThrottleOut.uwThrottlePercent > 200)  && (cp_stBikeRunInfoPara.uwBikeGear > 0) && (cp_stFlg.RunPermitFlg == TRUE))
        {
            // signal_state.Sensor = TRUE;
            Event_pvt_blBikeThroFlg = TRUE;

            /* Bike Speed Ref, 200-890Percent: 4-25km/h */
            tstBikeSpdRefTarget = ((ULONG)(25 - 4) *(bikethrottle_stBikeThrottleOut.uwThrottlePercent - 200)/690 + 4) * BIKESPEED_KMPERH2FREQPU; // Q20
            
            /* Bike Speed Ref Ramp */
            if(tstBikeSpdRef < tstBikeSpdRefTarget - 80)
            {   
                if(tstBikeSpdRefTarget >= tstBikeSpdRefTargetZ1)
                {
                    tstBikeSpdRef += 80;
                }             
            }
            else if(tstBikeSpdRef > tstBikeSpdRefTarget + 160)
            {
                tstBikeSpdRef -= 160;
            }               
            else
            {
                tstBikeSpdRef = tstBikeSpdRefTarget;
            }           
            tstBikeSpdRefTargetZ1 = tstBikeSpdRefTarget;
            
            /* Bike Speed Closed Loop */
            swIqLowerPu = (flx_stCtrlOut.swIqLimPu < abs(pwr_stPwrLimOut2.swIqLimPu)) ? flx_stCtrlOut.swIqLimPu : abs(pwr_stPwrLimOut2.swIqLimPu);  
            bikespeed_stPIIn.slSpdRefPu = tstBikeSpdRef;
            bikespeed_stPIIn.slSpdFdkPu = bikespeed_stFreGetOut.uwLPFFrequencyPu; //bikespeed_stFreGetOut.uwFrequencyPu; 
            bikespeed_stPIIn.swIqMaxPu = swIqLowerPu; // ((SLONG)55 << 14)/60;
            bikespeed_stPIIn.swIqMinPu = 0;           
            bikespeed_voPI(&bikespeed_stPIIn, &bikespeed_stPIOut);
            
            tstIqRefTarget = bikespeed_stPIOut.swIqRefPu;
        }
        else
        {      
            // signal_state.Sensor = FALSE;
            Event_pvt_blBikeThroFlg = FALSE;
            bikespeed_voPIInit();

            tstIqRef = 0;
            tstIqRefTarget = 0;
            tstIqRefTargetZ1 = 0;
            tstBikeSpdRef = 0;
            tstBikeSpdRefTarget = 0;
            tstBikeSpdRefTargetZ1 = 0;
            
            // bikespeed_stPIIn.slSpdRefPu = 0; // Q20
            // bikespeed_stPIIn.slSpdFdkPu = bikespeed_stFreGetOut.uwLPFFrequencyPu; //bikespeed_stFreGetOut.uwFrequencyPu; 
        }  
    }

}

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

    if(switch_flg.SysCoef_Flag == TRUE)
    {
        /* 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_stCalIn.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(cp_stFlg.RotateDirectionSelect == ForwardRotate)
    {
        sign = 1;
    }
    else if(cp_stFlg.RotateDirectionSelect == BackwardRotate)
    {
        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;
    }
}