/**
 * @file ctrlmdfsm1.c
 * @author Zhang, Kai(zhangkai71@midea.com)
 * @brief Control mode FSM
 * @version 0.1
 * @date 2021-09-27
 *
 * @copyright Copyright (c) 2021
 *
 */
/******************************
 *
 *  Included File
 *
 ******************************/
#include "user.h"
#include "FSM_1st.h"
#include "FSM_2nd.h"
#include "sys_ctrl.h"
#include "cmdgennew.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
FSM_RUN_HOOK    FSM2nd_Run_state;
FSM_RUN_HOOK    Exit_state = {Exit, Exit_hook, Exit_tbcuphook, Exit_tbcdownhook, Exit_tbshook};
static FSM_RUN_HOOK    Assistance_state = {Assistance, Assistance_hook, Assistance_tbcuphook, Assistance_tbcdownhook, Assistance_tbshook};
static FSM_RUN_HOOK    Boost_state = {Boost, Boost_hook, Boost_tbcuphook, Boost_tbcdownhook, Boost_tbshook};
CTRLMD_FSM_FLG1 cmfsm_stFlg = CTRLMD_FSM_FLG_DEFAULT;
/******************************
 *
 *  Function
 *
 ******************************/
void RUN_FSM_voInit(void)
{
    FSM2nd_Run_state = Exit_state;
    cmfsm_stFlg.blMotorStopFlg = TRUE;
}

void FSM_2nd_Main(void)
{
    switch (FSM2nd_Run_state.state)
    {
    case Exit:
        /* code */
        if (switch_flg.SysRun_Flag == 1)
        {
            if (signal_state.Sensor == 1)
            {

                /* PWM on */
                pwm_stGenOut.uwNewTIM1COMPR[0] = HW_INIT_HHPWM_PERIOD;
                pwm_stGenOut.uwNewTIM1COMPR[1] = HW_INIT_HHPWM_PERIOD;
                pwm_stGenOut.uwNewTIM1COMPR[2] = HW_INIT_HHPWM_PERIOD;
                pwm_stGenOut.uwNewTIM1COMPR[3] = HW_INIT_HHPWM_PERIOD;
                pwm_stGenOut.uwNewTIM1COMPR[4] = HW_INIT_HHPWM_PERIOD;
                pwm_stGenOut.uwNewTIM1COMPR[5] = HW_INIT_HHPWM_PERIOD;

                sysctrl_voPwmOn();

                /* 3rd FSM Init */
                Switch_speed_FSMInit();

                /* switch to Boost FSM */
                Switch_Second_FSM(&Assistance_state);
            }
            if (signal_state.Assist == 1)
            {

                /* PWM on */
                pwm_stGenOut.uwNewTIM1COMPR[0] = HW_INIT_HHPWM_PERIOD;
                pwm_stGenOut.uwNewTIM1COMPR[1] = HW_INIT_HHPWM_PERIOD;
                pwm_stGenOut.uwNewTIM1COMPR[2] = HW_INIT_HHPWM_PERIOD;
                pwm_stGenOut.uwNewTIM1COMPR[3] = HW_INIT_HHPWM_PERIOD;
                pwm_stGenOut.uwNewTIM1COMPR[4] = HW_INIT_HHPWM_PERIOD;
                pwm_stGenOut.uwNewTIM1COMPR[5] = HW_INIT_HHPWM_PERIOD;
                sysctrl_voPwmOn();

                /* 3rd FSM Init */
                Switch_speed_FSMInit();

                /* switch to Boost FSM */
                Switch_Second_FSM(&Boost_state);
            }
        }
        break;

    case Assistance:

        if ((!switch_flg.SysRun_Flag) && cmfsm_stFlg.blMotorStopFlg && (curSpeed_state.state == Stop))
        {
            Switch_Second_FSM(&Exit_state);
        }

        if(signal_state.Assist)
        {
            /* Initial Value of ASR Output */
            asr_stSpdPIOut.slIqRefPu = (SLONG)scm_swIqFdbLpfPu << 16;
            /* Initial Value of SpeedCmd Output */
            cmd_stCmdOut.slIntRefPu = (SLONG)scm_stSpdFbkLpf.slY.sw.hi << 14; //Q29
            /* Switch to Boost FSM */
            Switch_Second_FSM(&Boost_state);
        }

        break;

    case Boost:

        if ((!switch_flg.SysRun_Flag) && cmfsm_stFlg.blMotorStopFlg && (curSpeed_state.state == Stop))
        {
            Switch_Second_FSM(&Exit_state);
        }

        if(signal_state.Sensor && (!signal_state.Assist))
        {
            /* Initial Value of Assit Output */
            ass_stCalOut.swAssitCurRef = scm_swIqFdbLpfPu;    // scm_swIqFdbLpfPu need filter？ In case of big ripple？
            ass_pvt_stCurLpf.slY.sw.hi = scm_swIqFdbLpfPu;
            /* SpdRef Clear */
            scm_swSpdRefPu = 0;
            /* Switch to Assist FSM */
            Switch_Second_FSM(&Assistance_state);
        }

        break;

    case Cruise:
        /* code */
        break;

    default:
        break;
    }
}

void Assistance_hook(void)
{}
void Assistance_tbcuphook(void)
{
    scm_voSpdCtrMdUpTbc();
}
void Assistance_tbcdownhook(void)
{
    scm_voSpdCtrMdDownTbc();
}
void Assistance_tbshook(void)
{
    scm_voTorqCtrMdTbs();
}

void Boost_hook(void)
{}
void Boost_tbcuphook(void)
{
    scm_voSpdCtrMdUpTbc();
}
void Boost_tbcdownhook(void)
{
    scm_voSpdCtrMdDownTbc();
}
void Boost_tbshook(void)
{
    scm_voSpdCtrMdTbs();
}

void Exit_hook(void)
{}
void Exit_tbcuphook(void)
{}
void Exit_tbcdownhook(void)
{}
void Exit_tbshook(void)
{}

void Switch_Second_FSM(const FSM_RUN_HOOK *in)
{
    FSM2nd_Run_state = *in;
}

/************************************************************************
 Local Functions: N/A
************************************************************************/

/************************************************************************
 Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
 All rights reserved.
************************************************************************/
#ifdef _CTRLMDFSM1_C_
#undef _CTRLMDFSM1_C_
#endif
/************************************************************************
 End of this File (EOF)!
 Do not put anything after this part!
************************************************************************/