/**
 * @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
 *
 */
#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 "queue.h"
#include "spdctrmode.h"
#include "gd32f30x.h"
#include "string.h"
#include "syspar.h"
#include "uart_appl.h"
#include "uart_driver.h"
#include "user.h"
#include "STLmain.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
_op_ Op[proc_cnt] = {{Event_5ms, EVE1MSCNT_5ms, EVE1MSCNT_5ms},
                     {Event_10ms, EVE1MSCNT_20ms, EVE1MSCNT_20ms},
                     {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;
UWORD  AssistCNT1=0; 
UWORD socTest = 100;
void  Event_1ms(void)
{
    AssistCNT1++;
    // 1st FSM control
    FSM_1st_Main();
    FSM1st_Sys_state.Event_hook();
    

    // Power control
    //power_voPowerManagement(cp_stFlg.ParaHistorySaveEEFinishFlg, cp_stFlg.ParaSaveEEFlg);
    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();

    if (cadence_stFreGetOut.uwForwardCnt > 0)
    {
        torsensor_voCadenceCnt();
        cadence_stFreGetOut.uwForwardCnt = 0;
        maf_torque.value = torsensor_stTorSensorOut.uwTorquePu;
        MoveAverageFilter(&maf_torque);
    }
    
    torsensor_voDynamicOffset();
    torsensor_voTorADCwithTemp();

    // Tor assist cal
        // ass_CalIn.swFlxIqLimit = MC_RunInfo.SOC;
        ass_CalIn.SOCValue = socTest;
        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 = maf_torque.AverValue; // maf_torque.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;
        Assist();
        
        uart_swTorqRefNm = Assist_torqueper;
//        if(cp_stFlg.RunModelSelect == CityBIKE )
//        {
//            uart_swTorqRefNm = -Assist_torqueper;
//        }
//        else if(cp_stFlg.RunModelSelect == MountainBIKE )
//        {
//            uart_swTorqRefNm = Assist_torqueper;
//        }
//        else
//        {
//
//        }

    // 3rd FSM flag judge and set
    // torque assist model flg
    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;
    }

    // spd assist model flg
    if((cp_stFlg.RunModelSelect == CityBIKE) || (cp_stFlg.RunModelSelect == MountainBIKE))
    {
        #if 0  // By throttle ADC signal
        if(bikethrottle_stBikeThrottleOut.uwThrottlePercent < 500)
        {
            AssistCNT ++;
            if(AssistCNT > 1000 && cp_stFlg.RunPermitFlg == TRUE)
            {
                signal_state.Assist = TRUE;
                AssistCNT = 1000;
            }
        }
        else
        {   
            AssistCNT = 0;
            signal_state.Assist = FALSE;
        }
        #elif 0 // By walk assist GPIO signal
        
        #elif 1 //By can bus command code
        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;
        }
        #endif

        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;
        }
    }

    // user code end here

       
}

void Event_5ms(void)
{
    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;
        }
     
        /* bikespeed closeloop control  */
//         bikemotorspdref();
//         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)
{
    stl_voDoRunTimeChecks();
}

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

}

UWORD ReadorWrite = 2;
void  Event_200ms(void)
{
  
 // SendData(ID_MC_BC, MODE_REPORT, 0x1020, (uint8_t *)&MC_RunInfo.BikeSpeed);
    if (ReadorWrite == 3)
    {
        stErrorLog.NotesInfo1 = 1;
        stErrorLog.NotesInfo2 = 2;
        stErrorLog.NotesInfo3 = 3;
        que_ubPushIn(&stFlashErrorLog, &stErrorLog, sizeof(stErrorLog));
        ReadorWrite = 2;
    }

    Can_voMC_Run_200ms();

    bikelight_voGetBikeLightError(adc_stUpOut.uwU6VPu);
    display_voGetDisplayError(adc_stUpOut.uwU12VPu);
    bikespeed_voGetBikeSpeedPwrError(adc_stUpOut.uwU5VPu);

    /* Bike faults 200ms 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;
   // SWORD sign=1;/*V2 -1  V3 1*/
//      uart_slSpdRefRpm = - ((SLONG)MC_MotorSPD_rpm_Percent*5500)/100;
//      if(abs(uart_slSpdRefRpm) < 300)
//      {
//        uart_slSpdRefRpm = 0;
//      }
    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 *3088;
      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 *3500;
      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;
    }
}