/**
 * @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 "user.h"
#include "TimeTask_Event.h"
#include "AssistCurve.h"
#include "FSM_1st.h"
#include "can.h"
#include "canAppl.h"
#include "syspar.h"
#include "torquesensor.h"
#include "bikelight.h"
#include "bikethrottle.h"
#include "STLmain.h"
#include "api.h"
#include "power.h"
#include "bikebrake.h"
#include "display.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  Event_pvt_uwAssistCnt = 0;
static BOOL Event_pvt_blMafClrFlg = FALSE;
static UWORD Event_pvt_uwBikeSpdRefTarget = 0, Event_pvt_uwBikeSpdRef = 0, Event_pvt_uwBikeSpdRefTargetZ1 = 0;
static SWORD Event_pvt_swIqRefTarget = 0, Event_pvt_swIqRef = 0, Event_pvt_swIqRefTargetZ1 = 0;
static BOOL Event_pvt_blBikeThroFlg = FALSE, Event_pvt_blBikeThroFlgZ1 = FALSE;
/******************************
 *
 *  Functions
 *
 ******************************/
void  Event_1ms(void)
{
  /* Timing of time slices */
    TimingTaskTimerServer();
    
    // 1st FSM control
    FSM_1st_Main();
    FSM1st_Sys_state.Event_hook();

    // Power Management
    power_voPowerManagement(ass_stParaCong.uwAutoPowerOffTime, 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)
    {        
        // Torq Maf
        if (cadence_stFreGetOut.uwForwardCnt > 0)
        {
            //torsensor_voCadenceCnt();
            cadence_stFreGetOut.uwForwardCnt = 0;
            ass_stTorqMafValue.swValue = (SWORD)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);
                Event_pvt_blMafClrFlg = FALSE;
            }
            else if((!ass_stCalOut.blTorqPIFlg) && (Event_pvt_blMafClrFlg == FALSE))
            {
                ass_voMoveAverageFilterClear(&ass_stUqLimMafValue);
                Event_pvt_blMafClrFlg = TRUE;
            }
            else
            {
            	//do nothing
            }
        }
        
        // Torque info update
        torsensor_voTorADC();                 
        // Torq Sensor Offset Update
        torsensor_voOffsetUpdate();
        
        /* Bike brake info update */
        bikebrake_voBikeBrakeDetect();
        
        // Tor assist cal

        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 = (SWORD)ABS(flx_stCtrlOut.swIqLimPu);
        ass_stCalIn.swPwrIqLimit = (SWORD)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 = (UWORD)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();

        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 Torq */
            if(Event_pvt_blBikeThroFlgZ1 == TRUE)
            {
                /* Initial Value of Torq Assit Output */
                ass_stCalOut.swAssitCurRef = scm_swIqFdbLpfPu;    
                ass_pvt_stCurLpf.slY.sw.hi = scm_swIqFdbLpfPu;
                ass_stCalOut.swTorRefEnd = (SWORD)ABS(scm_swIqFdbLpfPu);
            }
                     
            uart_swTorqRefNm = ass_stCalOut.swAssitCurRef;
        }
        else
        {
            signal_state.Sensor = TRUE;
            ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;

            /* Torq to Throttle */
            if(Event_pvt_blBikeThroFlgZ1 == FALSE)
            {
                /* Initial Value of Throttle Assit Output */
                Event_pvt_swIqRef = scm_swIqFdbLpfPu;
                Event_pvt_swIqRefTarget = scm_swIqFdbLpfPu;
                bikespeed_stPIOut.slIqRefPu = scm_swIqFdbLpfPu << 16;    
            }

            /* Bike Throttle Assist Iqref Ramp */
            if(Event_pvt_swIqRef< Event_pvt_swIqRefTarget - 100)
            {
                if(Event_pvt_swIqRefTarget >= Event_pvt_swIqRefTargetZ1)
                {
                    Event_pvt_swIqRef += 100;
                }               
            }
            else
            {
                Event_pvt_swIqRef = Event_pvt_swIqRefTarget;
            }
            Event_pvt_swIqRefTargetZ1 = Event_pvt_swIqRefTarget;
            uart_swTorqRefNm = Event_pvt_swIqRef * ass_stCalIn.swDirection;
        }
        Event_pvt_blBikeThroFlgZ1 = Event_pvt_blBikeThroFlg;
        
        // spd assist model flg
        if((cp_stFlg.RunModelSelect == CityBIKE) || (cp_stFlg.RunModelSelect == MountainBIKE))
        {
            if(cp_stBikeRunInfoPara.uwBikeGear == 0x22)
            {
                Event_pvt_uwAssistCnt ++;
                if(Event_pvt_uwAssistCnt > 200 && cp_stFlg.RunPermitFlg == TRUE)
                {
                    signal_state.Assist = TRUE;
                    Event_pvt_uwAssistCnt = 200;
                }
            }
            else
            {
                Event_pvt_uwAssistCnt = 0;
                signal_state.Assist = FALSE;
            }

            if(signal_state.Assist == TRUE)
            {
                //6km/H = 100m/min  1.67m/s
                if(cp_stFlg.RunModelSelect == CityBIKE)
                {
                    if(MC_WorkMode == 1)
                      uart_slSpdRefRpm = -((SLONG)MC_MotorSPD_rpm_Percent * 5000) / 100;
                    else
                      uart_slSpdRefRpm = -(10000/(ass_stParaCong.uwWheelPerimeter))*ass_stParaCong.uwNmBackChainring*ass_stParaCong.uwMechRationMotor/ass_stParaCong.uwNmFrontChainring;
                }
                else if(cp_stFlg.RunModelSelect == MountainBIKE)
                {
                    if(MC_WorkMode == 1)
                      uart_slSpdRefRpm = ((SLONG)MC_MotorSPD_rpm_Percent * 5000) / 100;
                    else
                      uart_slSpdRefRpm = (10000/(ass_stParaCong.uwWheelPerimeter))*ass_stParaCong.uwNmBackChainring*ass_stParaCong.uwMechRationMotor/ass_stParaCong.uwNmFrontChainring;
                }
                else
                {
                    //do nothing
                }
                
                //Limit max speed
//                if(cp_stBikeRunInfoPara.BikeSpeedKmH > (ass_stParaCong.uwThrottleMaxSpdKmH * 10))
//                {
//                    uart_slSpdRefRpm = 0;
//                }
            }  
            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)
{
    Can_voMC_Run_5ms();
}

void Event_10ms(void)
{
    if(switch_flg.SysCoef_Flag == TRUE)
    {
        // throttle vol ADC update
        bikethrottle_voBikeThrottleADC();

        // spd cmd set
        if(cp_stFlg.RunModelSelect != CityBIKE && cp_stFlg.RunModelSelect != MountainBIKE )
        {
//            Signal_detect();
            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;
            }
            else
            {
            	//do nothing
            }
            
             if(ABS((int32_t)uart_slSpdRefRpm) < 300)
             {
                 uart_slSpdRefRpm = 0;
             }
        }     
      
        // Bike light control
        Can_Light_switch();
        bikelight_voBikeLightControl(cp_stBikeRunInfoPara.uwLightSwitch, BikeBrake_blGetstate(), ass_stParaCong.uwLightVoltage);

        // 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)
    {
//        if(cp_stBikeRunInfoPara.uwBikeGear == 0x22)
//        {
//            Event_pvt_uwAssistCnt ++;
//            if(Event_pvt_uwAssistCnt >= 2 && cp_stFlg.RunPermitFlg == TRUE)
//            {
//                Event_pvt_uwAssistCnt = 2;
//            }
//        }
//        else
//        {
//            Event_pvt_uwAssistCnt = 0;
//        }
          
        // 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) || Event_pvt_uwAssistCnt == 2)&& (cp_stBikeRunInfoPara.uwBikeGear > 0) && (cp_stFlg.RunPermitFlg == TRUE) && (BikeBrake_blGetstate() == FALSE))
        if((bikethrottle_stBikeThrottleOut.uwThrottlePercent > 200)  && (cp_stBikeRunInfoPara.uwBikeGear > 0) && (cp_stFlg.RunPermitFlg == TRUE))
        {
            Event_pvt_blBikeThroFlg = TRUE;

            /* Bike Speed Ref, 200-890Percent: 4-25km/h */
//            if(Event_pvt_uwAssistCnt == 2)
//            {
//                Event_pvt_uwBikeSpdRefTarget = (UWORD)BIKESPEED_KMPERH2FREQPU * 6; // Q20
//            }
//            else
            {
                Event_pvt_uwBikeSpdRefTarget = (UWORD)((((ULONG)25 - (ULONG)4) *(bikethrottle_stBikeThrottleOut.uwThrottlePercent - 200)/690 + 4) * BIKESPEED_KMPERH2FREQPU); // Q20   
            }
            
            /* Bike Speed Ref Ramp */
            if(Event_pvt_uwBikeSpdRef < Event_pvt_uwBikeSpdRefTarget - 80)
            {   
                if(Event_pvt_uwBikeSpdRefTarget >= Event_pvt_uwBikeSpdRefTargetZ1)
                {
                    Event_pvt_uwBikeSpdRef += 80;
                }             
            }
            else if(Event_pvt_uwBikeSpdRef > Event_pvt_uwBikeSpdRefTarget + 160)
            {
                Event_pvt_uwBikeSpdRef -= 160;
            }               
            else
            {
                Event_pvt_uwBikeSpdRef = Event_pvt_uwBikeSpdRefTarget;
            }           
            Event_pvt_uwBikeSpdRefTargetZ1 = Event_pvt_uwBikeSpdRefTarget;
            
            /* Bike Speed Closed Loop */
            swIqLowerPu = (SWORD)((flx_stCtrlOut.swIqLimPu < ABS(pwr_stPwrLimOut2.swIqLimPu)) ? flx_stCtrlOut.swIqLimPu : ABS(pwr_stPwrLimOut2.swIqLimPu));  
            bikespeed_stPIIn.slSpdRefPu = Event_pvt_uwBikeSpdRef;
            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);
            
            Event_pvt_swIqRefTarget = bikespeed_stPIOut.swIqRefPu;
        }
        else
        {   
            Event_pvt_blBikeThroFlg = FALSE;
            bikespeed_voPIInit();

            Event_pvt_swIqRef = 0;
            Event_pvt_swIqRefTarget = 0;
            Event_pvt_swIqRefTargetZ1 = 0;
            Event_pvt_uwBikeSpdRef = 0;
            Event_pvt_uwBikeSpdRefTarget = 0;
            Event_pvt_uwBikeSpdRefTargetZ1 = 0;
        }  
    }  
}

void  Event_200ms(void)
{
    Can_voMC_Run_200ms();
    
    if(switch_flg.SysCoef_Flag == TRUE)
    {
        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.PCBTempReg;
            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 -= (SLONG)TimingTaskTimerTickPassed;
            if (Op[cnt].tick <= 0)
            {
                Op[cnt].tick += Op[cnt].timespan;
                Op[cnt].proc();
            }
        }
        LoopServerExecutedFlag = 1;
    }
}