/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            sysfsm.c
 Partner Filename:    sysfsm.h
 Description:         System finite state machine
 Complier:            IAR Embedded Workbench for ARM 7.80.4
 CPU TYPE :           GD32F3x0
*************************************************************************
 Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
 All rights reserved.
*************************************************************************
*************************************************************************
 Revising History (ECL of this file):

************************************************************************/

/************************************************************************
 Beginning of File, do not put anything above here except notes
 Compiler Directives:
************************************************************************/
#ifndef _STARTFSM_C_
#define _STARTFSM_C_
#endif
/************************************************************************
Include File
************************************************************************/
#include "syspar.h"
#include "user.h"
#include "FSM_1st.h"
#include "FSM_2nd.h"
#include "spdctrFSM.h"
#include "switchhall.h"
#include "spi_master.h"
#include "cmdgennew.h"
#include "power.h"
#include "sys_ctrl.h"
/************************************************************************
 Constant Table:
************************************************************************/

/************************************************************************
 Exported Functions:
************************************************************************/
void InitPosDet_TbcupHook(void)
{
    scm_ulStatCt++;

    align_stIn.ulStatCt = scm_ulStatCt;
    align_voInitPos(&align_stIn, &align_stCoef, &align_stOut);
    scm_slIdRefPu = align_stOut.slIdRefPu;   // Q29
    scm_swIdRefPu = align_stOut.swIdRefPu;   // Q14
    scm_swIqRefPu = align_stOut.swIqRefPu;   // Q14
    scm_uwAngRefPu = align_stOut.uwAngRefPu; // Q15
}

void ParDet_TbcupHook(void)
{}
void StartUp_TbcupHook(void)
{
    flx_stCtrlOut.swIqLimPu = (SWORD)cof_uwCurMaxPu;
    scm_ulStatCt++;

    align_stIn.ulStatCt = scm_ulStatCt;
    align_stIn.swRotateDir = scm_swRotateDir;
    align_voStartUp(&align_stIn, &align_stCoef, &align_stOut);

    scm_slDragSpdRefPu = align_stOut.slDragSpdRefPu;
    scm_slDragSpdPu = align_stOut.slDragSpdPu;
    scm_swIdRefPu = align_stOut.swIdRefPu;
    scm_swIqRefPu = align_stOut.swIqRefPu;
    scm_slAngManuPu = align_stOut.slAngManuPu;
    scm_uwAngRefPu = align_stOut.uwAngRefPu;
    scm_StartUpOvrFlg = align_stOut.blStartUpOvrFlg;
}

void Open2Clz_TbcupHook(void)
{

    flx_stCtrlOut.swIqLimPu = (SWORD)cof_uwCurMaxPu;
    
    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        align_stIn.uwObsElecThetaPu = obs_stObsOutPu.uwElecThetaPu;
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        align_stIn.uwObsElecThetaPu = spi_stResolverOut.uwSpiThetaPu;
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        align_stIn.uwObsElecThetaPu = switchhall_stOut.uwLowThetaPu;
    }
    else
    {
        //do nothing
    }

    align_stIn.swCurRefrompu = swCurRefrompu; // swCurRefrompu;
    align_voOpen2Clz(&align_stIn, &align_stCoef, &align_stOut);

    scm_swIdRefPu = align_stOut.swIdRefPu;
    scm_swIqRefPu = align_stOut.swIqRefPu;
    scm_blCurSwitchOvrFlg = align_stOut.blCurSwitchOvrFlg;
    scm_blAngSwitchOvrFlg = align_stOut.blAngSwitchOvrFlg;
    scm_slAngManuPu = align_stOut.slAngManuPu;
    scm_uwAngManuPu = align_stOut.uwAngManuPu;
    scm_uwAngRefPu = align_stOut.uwAngRefPu;
}

void ClzLoop_TbcupHook(void)
{
    /*=======================================================================
                                  Flux weakening
      =======================================================================*/
    flx_stCtrlIn.swUalphaPu = crd_stVltIParkOut.swAlphaPu; // Q14
    flx_stCtrlIn.swUbetaPu = crd_stVltIParkOut.swBetaPu;   // Q14
    flx_stCtrlIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;         // Q14
    flx_stCtrlIn.swIqRefPu = swCurRefrompu;                // Q14
    flx_stCtrlIn.swUqRefPu = scm_swUqRefPu;                // Q14
    flx_voCtrl(&flx_stCtrlIn, &flx_stCtrlCoef, &flx_stCtrlOut);
    
    /*=======================================================================
                                     Power Limit
      =======================================================================*/
    // Q15
    pwr_stPwrLimIn.swMotorPwrPu = scm_stMotoPwrInLpf.slY.sw.hi; // Q15
    pwr_stPwrLimIn.swIqrefPu = swCurRefrompu;                   // asr_stSpdPIOut.swIqRefPu;
    pwr_stPwrLimIn.PCBTemp = adc_stUpOut.PCBTemp;
    pwr_voPwrLim2(&pwr_stPwrLimIn, &pwr_stPwrLimCof, &pwr_stPwrLimOut2); // Q14

    /*======================================================================= 
             Set Iq and Id reference for Constant Voltage Break
    =======================================================================*/
    cvb_stBrakeIn.uwVdcLpfPu = adc_stUpOut.uwVdcLpfPu;
    cvb_stBrakeIn.swIdRefPu = scm_swIdRefPu;
    cvb_stBrakeIn.swIqRefPu = swCurRefrompu;             
    cvb_stBrakeIn.swSpdPu = scm_stSpdFbkLpf.slY.sw.hi;
    cvb_stBrakeIn.uwAngelPu = scm_uwAngRefPu;
    cvb_stBrakeIn.uwSpdLpfAbsPu = scm_uwSpdFbkLpfAbsPu;
    cvb_voBrake(&cvb_stBrakeIn,&cvb_stBrakeCoef,&cvb_stBrakeOut);
 
    /** Idref可变范围-3A~0A且未弱磁，暂不做过多处理；Angle在down中更新，暂不改为改为恒压制动角度 **/
    if(cvb_stBrakeIn.uwVdcLpfPu >= cvb_stBrakeCoef.uwVdcStartCvbPu)
    {
        scm_swIdRefPu = cvb_stBrakeOut.swIdRefPu;
    }
    else
    {
        scm_swIdRefPu = 0;  //flx_stCtrlOut.swIdRefPu;  
    }  

    scm_swIqRefPu = cvb_stBrakeOut.swIqRefPu;
}

void Stop_TbcupHook(void)
{}

void InitPosDet_TbcdownHook(void)
{
    /* Get angle for park transformation */
    scm_uwAngParkPu = scm_uwAngRefPu; // Q15
    scm_uwAngIParkPu = scm_uwAngParkPu;

    /*=======================================================================
                        Current decoupling
    =======================================================================*/
    acr_stUdqDcpOut.swUdPu = 0;
    acr_stUdqDcpOut.swUqPu = 0;
}
void ParDet_TbcdownHook(void)
{
    /* Get angle for park transformation */
    scm_uwAngParkPu = scm_uwAngRefPu; // Q15
    scm_uwAngIParkPu = scm_uwAngParkPu;

    /*=======================================================================
                        Current decoupling
    =======================================================================*/
    acr_stUdqDcpOut.swUdPu = 0;
    acr_stUdqDcpOut.swUqPu = 0;
}
void StartUp_TbcdownHook(void)
{
    /* Speed feedback LPF */
    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf);
        obs_stObsCalcIn.swUalphaPu = scm_swUalphaPu;              // Q14
        obs_stObsCalcIn.swUbetaPu = scm_swUbetaPu;                // Q14
        obs_stObsCalcIn.swIalphaPu = crd_stCurClarkOut.swAlphaPu; // Q14
        obs_stObsCalcIn.swIbetaPu = crd_stCurClarkOut.swBetaPu;   // Q14
        obs_stObsCalcIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;
        obs_voObsCalc(&obs_stObsCalcIn, &obs_stObsCoefPu, &obs_stObsOutPu);
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        mth_voLPFilter(spi_stResolverOut.swSpdFbkPu, &scm_stSpdFbkLpf);
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        scm_stSpdFbkLpf.slY.sw.hi = switchhall_stOut.swLowSpdLpfPu;
    }
    else
    {
        //do nothing
    }

    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = (UWORD)ABS(scm_stSpdFbkLpf.slY.sw.hi); // Q15

    /* Get angle for park transformation */
    scm_uwAngParkPu = scm_uwAngRefPu; // Q15
    scm_uwAngIParkPu = scm_uwAngParkPu;

    /*=======================================================================
                        Current decoupling
    =======================================================================*/
    acr_stUdqDcpOut.swUdPu = 0;
    acr_stUdqDcpOut.swUqPu = 0;
}
void Open2Clz_TbcdownHook(void)
{
    /* Speed feedback LPF */
    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        obs_stObsCalcIn.swUalphaPu = scm_swUalphaPu;              // Q14
        obs_stObsCalcIn.swUbetaPu = scm_swUbetaPu;                // Q14
        obs_stObsCalcIn.swIalphaPu = crd_stCurClarkOut.swAlphaPu; // Q14
        obs_stObsCalcIn.swIbetaPu = crd_stCurClarkOut.swBetaPu;   // Q14
        obs_stObsCalcIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;
        obs_voObsCalc(&obs_stObsCalcIn, &obs_stObsCoefPu, &obs_stObsOutPu);
        mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf);
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        mth_voLPFilter(spi_stResolverOut.swSpdFbkPu, &scm_stSpdFbkLpf);
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        scm_stSpdFbkLpf.slY.sw.hi = switchhall_stOut.swLowSpdLpfPu;
    }
    else
    {
        //do nothing
    }

    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = (UWORD)ABS(scm_stSpdFbkLpf.slY.sw.hi); // Q15

    /* Get angle for park transformation */
    scm_uwAngParkPu = scm_uwAngRefPu; // Q15
    scm_uwAngIParkPu = scm_uwAngParkPu;

    /*=======================================================================
                        Current decoupling
    =======================================================================*/
    acr_stUdqDcpOut.swUdPu = 0;
    acr_stUdqDcpOut.swUqPu = 0;
}

void ClzLoop_TbcdownHook(void)
{
    ULONG ulTmp1;
    SWORD swAngCompPu = 0; // Q15

/* Speed feedback LPF */

    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        obs_stObsCalcIn.swUalphaPu = scm_swUalphaPu;              // Q14
        obs_stObsCalcIn.swUbetaPu = scm_swUbetaPu;                // Q14
        obs_stObsCalcIn.swIalphaPu = crd_stCurClarkOut.swAlphaPu; // Q14
        obs_stObsCalcIn.swIbetaPu = crd_stCurClarkOut.swBetaPu;   // Q14
        obs_stObsCalcIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;
        obs_voObsCalc(&obs_stObsCalcIn, &obs_stObsCoefPu, &obs_stObsOutPu);
        mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf);
        scm_uwAngRefPu = obs_stObsOutPu.uwElecThetaPu;
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        mth_voLPFilter(spi_stResolverOut.swSpdFbkPu, &scm_stSpdFbkLpf);
        scm_uwAngRefPu = spi_stResolverOut.uwSpiThetaPu;
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        mth_voLPFilter(switchhall_stOut.swLowSpdPu, &scm_stSpdFbkLpf);
        scm_uwAngRefPu = switchhall_stOut.uwLowThetaPu;
    }
    else
    {
        //do nothing
    }

    /* Speed feedback Absolute */
    scm_uwSpdFbkLpfAbsPu = (UWORD)ABS(scm_stSpdFbkLpf.slY.sw.hi); // Q15

    /* Get angle for park transformation */
    scm_uwAngParkPu = scm_uwAngRefPu; // Q15

    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        swAngCompPu = (SWORD)(((SLONG)obs_stObsOutPu.swElecFreqPu * TBC_TM) >> 10); // Q15
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        swAngCompPu = (SWORD)(((SLONG)spi_stResolverOut.swSpdFbkPu * TBC_TM) >> 10); // Q15
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL)
    {
        swAngCompPu = (SWORD)(((SLONG)switchhall_stOut.swLowSpdPu * TBC_TM) >> 10); // Q15
    }
    else
    {
        //do nothing
    }

    ulTmp1 = scm_uwAngParkPu + ((swAngCompPu * 3) >> 1) + cof_sl720DegreePu; 
    scm_uwAngIParkPu = (UWORD)(ulTmp1 & 0x7FFF);
    /*=======================================================================
                        Current decoupling
    =======================================================================*/ 
    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
    {
        acr_stUdqDcpIn.swWsPu =  scm_stSpdFbkLpf.slY.sw.hi; // Q15
    }
    else if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
    {
        acr_stUdqDcpIn.swWsPu = spi_stResolverOut.swSpdFbkPu; // Q15  swSpdFbkLpfPu spi_stResolverOut.swSpdFbkPu
    }
    else
    {
        //do nothing
    }
    acr_stUdqDcpIn.swIdRefPu = scm_swIdFdbLpfPu;//scm_swIdRefPu  scm_swIdFdbLpfPu          // Q14
    acr_stUdqDcpIn.swIqRefPu = scm_swIqFdbLpfPu; //scm_swIqRefPu  scm_swIqFdbLpfPu         // Q14  
    acr_stUdqDcpIn.swUdqLimPu = scm_swVsDcpLimPu;      // Q14
    acr_voUdqDcp(&acr_stUdqDcpIn, &acr_stUdqDcpCoef, &acr_stUdqDcpOut);
}

void Stop_TbcdownHook(void)
{
    scm_swIdRefPu = 0;
    scm_swIqRefPu = 0;

    sysctrl_voPwmInit();
    cmfsm_stFlg.blMotorStopFlg = TRUE;
}

void InitPosDet_TbsHook(void)
{}
void ParDet_TbsHook(void)
{}
void StartUp_TbsHook(void)
{
    /*=======================================================================
                          Speed PI output limit in "OpenDrg"
      =======================================================================*/
    if ((curSpeed_state.state == StartUp) && (scm_uwStartMd == START_ALIGN))
    {
        if (scm_swRotateDir * asr_stSpdPIOut.slIqRefPu < 0)
        {
            asr_stSpdPIOut.slIqRefPu = 0; // Q30
            asr_stSpdPIOut.slIqSumPu = 0; // Q30
            asr_stSpdPIOut.slIqiPu = 0;
        }
        else
        {
            if (scm_swRotateDir * asr_stSpdPIOut.slIqRefPu > ((SLONG)mn_uwDragCurPu << 16)) // Q30
            {
                asr_stSpdPIOut.slIqRefPu = scm_swRotateDir * ((SLONG)mn_uwDragCurPu << 16); // Q30
                asr_stSpdPIOut.slIqiPu = scm_swRotateDir * ((SLONG)mn_uwDragCurPu << 16);   // Q14+Q16=Q30
            }
        }
    }
}
void Open2Clz_TbsHook(void)
{}
void ClzLoop_TbsHook(void)
{}

void Stop_TbsHook(void)
{}

void scm_voSpdCtrMdFSM(void)
{
    switch (curSpeed_state.state)
    {
    case Charge:

        break;

    case InitPosDet:
        /* Command run disable */
        /* Motor run flag set */
        cmfsm_stFlg.blMotorStopFlg = FALSE;
        if (!switch_flg.SysRun_Flag || switch_flg.SysFault_Flag || power_stPowStateOut.powerstate == POWER_OFF)
        {
            Switch_speed_FSM(&Stop_state);
        }

        if (scm_ulStatCt < mn_ulAlignRampTbcCt)
        {}
        else if (scm_ulStatCt >= (mn_ulAlignRampTbcCt + mn_ulAlignHoldTbcCt + 10))
        {
            if(cp_stFlg.SpiOffsetFirstSetFlg == 0)
            {
                cp_stFlg.SpiOffsetFirstSetFinishFlg = 1;
            }
            else
            {
                if(cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER)
                {
                    align_stCoef.uwSPIreadOnceCt = 0;
                }
                else
                {}

                if(cp_stFlg.RunModelSelect == InitPos)
                { 
                }
                else if(cp_stFlg.RunModelSelect == ClZLOOP)
                {
                  if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
                  {
                    Switch_speed_FSM(&StartUp_state);
                  }
                  else
                  {
                    Switch_speed_FSM(&ClzLoop_state);
                  }
                }
                else
                {
                    Switch_speed_FSM(&StartUp_state);
                }
            }
        }
        else
        {
            //do nothing
        }

        break;
    case StartUp:
        if (!switch_flg.SysRun_Flag || switch_flg.SysFault_Flag || power_stPowStateOut.powerstate == POWER_OFF)
        {
            Switch_speed_FSM(&Stop_state);
        }
        if (scm_StartUpOvrFlg == TRUE)
        {
            if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
            {
                if(cp_stFlg.RunModelSelect == ClZLOOP || cp_stFlg.RunModelSelect == CityBIKE || cp_stFlg.RunModelSelect == MountainBIKE)
                {
                    if (FSM2nd_Run_state.state == Boost)
                    {
                        cmd_stCmdOut.slIntRefPu = ((SLONG)scm_stSpdFbkLpf.slY.sw.hi * 5) << 12; // Q29  5/4
                    }
                    else if (FSM2nd_Run_state.state == Assistance)
                    {
                        //do nothing
                    }
                    else
                    {
                        //do nothing
                    }
                    Switch_speed_FSM(&Open2Clz_state);
                }
            }
            else
            {

            }
        }
        /* Command run disable */
        break;
    case Open2Clz:
        /* Command run disable */
        if (!switch_flg.SysRun_Flag || switch_flg.SysFault_Flag || power_stPowStateOut.powerstate == POWER_OFF)
        {
            Switch_speed_FSM(&Stop_state);
        }
        /* Switch over */
        if (scm_blCurSwitchOvrFlg && scm_blAngSwitchOvrFlg)
        {

            Switch_speed_FSM(&ClzLoop_state);

        }

        break;
    case ClzLoop:
        /* Go to stop */
        cmfsm_stFlg.blMotorStopFlg = FALSE;
        if (switch_flg.SysFault_Flag == 1 || power_stPowStateOut.powerstate == POWER_OFF)
        {
            Switch_speed_FSM(&Stop_state);
        }
        if (!switch_flg.SysRun_Flag)
        {
            /* Go to stop */
            if ((ABS(scm_swSpdRefPu) < mn_swStopSpdRefPu) && (scm_uwSpdFbkLpfAbsPu < mn_swStopSpdRefPu))
            {
                scm_swIdRefPu = 0;
                scm_swIqRefPu = 0;

                Switch_speed_FSM(&Stop_state);
            }
        }
        else
        {}

        break;
    case Stop:
        if (switch_flg.SysRun_Flag == TRUE && switch_flg.SysFault_Flag == FALSE) //&& power_stPowStateOut.powerstate == POWER_ON_END
        {
            scm_voSpdCtrMdInit();
            if(cp_stFlg.SpiOffsetFirstSetFlg == 0)
            {
                Switch_speed_FSM(&InitPosDet_state);
            }
            else
            {
                if(cp_stFlg.RunModelSelect == CityBIKE || cp_stFlg.RunModelSelect == MountainBIKE)
                {
                    if(cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER)
                    {
                        Switch_speed_FSM(&InitPosDet_state);
                    }
                    else
                    {   
                        Switch_speed_FSM(&ClzLoop_state);
                    }
                }
                else  
                {
#ifdef RUN_ARCH_SIM
                    Switch_speed_FSM(&ClzLoop_state);                   
#else                    
                    Switch_speed_FSM(&InitPosDet_state);
#endif
                }
            }
        }
        break;
    default:
        break;
    }
}

void Switch_speed_FSM(const SPD_STATE_HOOK *in)
{
    scm_ulStatCt = 0;
    curSpeed_state = *in;
}

void Switch_speed_FSMInit(void)
{
    scm_ulStatCt = 0;
    curSpeed_state = Stop_state;
}

/************************************************************************
 Function: void RUN_FSM_Main(void)
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
************************************************************************/

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

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