/**
 * @file cmdgen.c
 * @author Zhang, Kai(zhangkai71@midea.com)
 * @brief System FSM
 * @version 0.1
 * @date 2021-09-27
 *
 * @copyright Copyright (c) 2021
 *
 */


#include "cmdgen.h"

/******************************
 *
 *  Parameter
 *
 ******************************/

SLONG cmd_pvt_slAccDeltaInPu;    // Q31, Acceleration before acceleration limit
SLONG cmd_pvt_slAccDeltaOutPu;   // Q31, Acceleration after acceleration limit
SLONG cmd_pvt_slAccDeltaOutZ1Pu; // Q31, Old Acceleration
SLONG cmd_pvt_slSpdRefZ1Pu;

void cmd_voCmdGenInit(CMD_CONTROL *s)
{
    s->Out.swSpdCmdRefPu = 0;
    s->Out.swSpdRefPu = 0;
    s->Out.slSpdRefPu = 0;
    s->Out.swSpdCmdDir = 1;
    s->Out.swSpdRefDirNow = 1;
    cmd_pvt_slAccDeltaInPu = 33408; // when ACC value is 1500
    cmd_pvt_slAccDeltaOutPu = 33408;
    cmd_pvt_slAccDeltaOutZ1Pu = 33408;
    s->Out.enSpdStatus = Constant;
    cmd_pvt_slSpdRefZ1Pu = 0;
}

void cmd_voCmdGenCoef(CMD_GEN_COFIN *in, CMD_CONTROL *s)
{
    SLONG slAccLim1Tmp = 0;
    SLONG slAccLim2Tmp = 0;
    SLONG slAccLim3Tmp = 0;
    SLONG slDecLim1Tmp = 0;
    SLONG slDecLim2Tmp = 0;
    SLONG slDecLim3Tmp = 0;

    if (in->uwBaseRpm < 1000)
    {
        in->uwBaseRpm = 1000;
    }
    s->Coef.uwBaseRpm = in->uwBaseRpm;
    s->Coef.uwBaseRpmInv = ((ULONG)1 << 25) / in->uwBaseRpm; // Q25

    if (in->uwFTbsHz < 600)
    {
        in->uwFTbsHz = 600;
    }
    s->Coef.uwFTbsHzInv = ((ULONG)1 << 25) / in->uwFTbsHz; // Q25

    if (in->uwRefMaxRpm < 20000)
    {
        in->uwRefMaxRpm = 20000;
    }

    s->Coef.ulSpdRefMaxPu = ((ULONG)in->uwRefMaxRpm << 15) / in->uwBaseRpm; // Q15

    if (in->uwAccSw1Rpm < 10)
    {
        in->uwAccSw1Rpm = 10;
    }
    if (in->uwAccSw2Rpm < 20)
    {
        in->uwAccSw2Rpm = 20;
    }
    if (in->uwDecSw1Rpm < 10)
    {
        in->uwDecSw1Rpm = 10;
    }
    if (in->uwDecSw2Rpm < 20)
    {
        in->uwDecSw2Rpm = 20;
    }

    if (in->swAccLim1Rpmps > 5000)
    {
        in->swAccLim1Rpmps = 5000;
    }
    else if (in->swAccLim1Rpmps <= 0)
    {
        in->swAccLim1Rpmps = 1;
    }

    if (in->swAccLim2Rpmps > 5000)
    {
        in->swAccLim2Rpmps = 5000;
    }
    else if (in->swAccLim2Rpmps <= 0)
    {
        in->swAccLim2Rpmps = 1;
    }

    if (in->swAccLim3Rpmps > 5000)
    {
        in->swAccLim3Rpmps = 5000;
    }
    else if (in->swAccLim3Rpmps <= 0)
    {
        in->swAccLim3Rpmps = 1;
    }

    if (in->swDecLim1Rpmps > 5000)
    {
        in->swDecLim1Rpmps = 5000;
    }
    else if (in->swDecLim1Rpmps <= 0)
    {
        in->swDecLim1Rpmps = 1;
    }

    if (in->swDecLim2Rpmps > 5000)
    {
        in->swDecLim2Rpmps = 5000;
    }
    else if (in->swDecLim2Rpmps <= 0)
    {
        in->swDecLim2Rpmps = 1;
    }

    if (in->swDecLim3Rpmps > 5000)
    {
        in->swDecLim3Rpmps = 5000;
    }
    else if (in->swDecLim3Rpmps <= 0)
    {
        in->swDecLim3Rpmps = 1;
    }

    s->Coef.ulAccSpdSw1Pu = (((ULONG)in->uwAccSw1Rpm << 15) / in->uwBaseRpm); // Q15
    s->Coef.ulAccSpdSw2Pu = (((ULONG)in->uwAccSw2Rpm << 15) / in->uwBaseRpm); // Q15
    s->Coef.ulDecSpdSw1Pu = (((ULONG)in->uwDecSw1Rpm << 15) / in->uwBaseRpm); // Q15
    s->Coef.ulDecSpdSw2Pu = (((ULONG)in->uwDecSw2Rpm << 15) / in->uwBaseRpm); // Q15

    slAccLim1Tmp = ((SLONG)in->swAccLim1Rpmps << 15) / in->uwBaseRpm;
    if (slAccLim1Tmp > 32767)
    {
        slAccLim1Tmp = 32767;
    }
    slAccLim2Tmp = ((SLONG)in->swAccLim2Rpmps << 15) / in->uwBaseRpm;
    if (slAccLim2Tmp > 32767)
    {
        slAccLim2Tmp = 32767;
    }
    slAccLim3Tmp = ((SLONG)in->swAccLim3Rpmps << 15) / in->uwBaseRpm;
    if (slAccLim3Tmp > 32767)
    {
        slAccLim3Tmp = 32767;
    }
    slDecLim1Tmp = ((SLONG)in->swDecLim1Rpmps << 15) / in->uwBaseRpm;
    if (slDecLim1Tmp > 32767)
    {
        slDecLim1Tmp = 32767;
    }
    slDecLim2Tmp = ((SLONG)in->swDecLim2Rpmps << 15) / in->uwBaseRpm;
    if (slDecLim2Tmp > 32767)
    {
        slDecLim2Tmp = 32767;
    }
    slDecLim3Tmp = ((SLONG)in->swDecLim3Rpmps << 15) / in->uwBaseRpm;
    if (slDecLim3Tmp > 32767)
    {
        slDecLim3Tmp = 32767;
    }

    s->Coef.slAccLim1Pu = (slAccLim1Tmp << 16) / in->uwFTbsHz; // Q31
    s->Coef.slAccLim2Pu = (slAccLim2Tmp << 16) / in->uwFTbsHz; // Q31
    s->Coef.slAccLim3Pu = (slAccLim3Tmp << 16) / in->uwFTbsHz; // Q31
    s->Coef.slDecLim1Pu = (slDecLim1Tmp << 16) / in->uwFTbsHz; // Q31
    s->Coef.slDecLim2Pu = (slDecLim2Tmp << 16) / in->uwFTbsHz; // Q31
    s->Coef.slDecLim3Pu = (slDecLim3Tmp << 16) / in->uwFTbsHz; // Q31
}

void cmd_voCmdGen(CMD_GEN_IN *in, CMD_CONTROL *s)
{
    SLONG slSpdCmdRefPu = 0; // Q31
    SLONG slAccDeltaInPuTmp1 = 0;
    SLONG slAccDeltaInPuTmp2 = 0;
    UWORD uwSpdRefZ1AbsPu = 0;
    ULONG ulAccDeltaInAbsPu = 33408; // when ACC value is 1500

    s->Out.swSpdRefPu = s->Out.slSpdRefPu >> 16;         // Q31-Q16=Q15
    s->Out.swSpdCmdRefPu = in->slRefRpm;                 // Q15
    slSpdCmdRefPu = ((SLONG)s->Out.swSpdCmdRefPu << 16); // Q31
    if (s->Out.swSpdCmdRefPu >= 0)
    {
        s->Out.swSpdCmdDir = 1;
    }
    else
    {
        s->Out.swSpdCmdDir = -1;
    }

    slAccDeltaInPuTmp1 = ((SLONG)in->swAccRpmps * s->Coef.uwBaseRpmInv) >> 13; // Q0+Q25-Q13=Q12
    if (slAccDeltaInPuTmp1 > 131071)                                           // Q17 = 131072
    {
        slAccDeltaInPuTmp1 = 131071;
    }
    slAccDeltaInPuTmp2 = (slAccDeltaInPuTmp1 * s->Coef.uwFTbsHzInv) >> 11; // Q12+Q25-Q11=Q26
    if (slAccDeltaInPuTmp2 > 67108863)                                     // Q26 =  67108864
    {
        slAccDeltaInPuTmp2 = 67108863;
    }
    cmd_pvt_slAccDeltaInPu = (slAccDeltaInPuTmp2 << 5);

    /* Accelaration & decelaration limit */
    //    uwSpdRefZ1AbsPu = abs(s->Out.swSpdRefPu);
    //    ulAccDeltaInAbsPu = abs(cmd_pvt_slAccDeltaInPu);
    if ((s->Out.swSpdRefPu) >= 0)
    {
        uwSpdRefZ1AbsPu = (UWORD)s->Out.swSpdRefPu;
    }
    else
    {
        uwSpdRefZ1AbsPu = (UWORD)(-(s->Out.swSpdRefPu));
    }
    if ((cmd_pvt_slAccDeltaInPu) >= 0)
    {
        ulAccDeltaInAbsPu = (UWORD)cmd_pvt_slAccDeltaInPu;
    }
    else
    {
        ulAccDeltaInAbsPu = (UWORD)(-cmd_pvt_slAccDeltaInPu);
    }

    if (cmd_pvt_slAccDeltaInPu > 0)
    {
        if ((s->Out.swSpdRefPu - s->Out.swSpdCmdRefPu) < 0)
        {
            if (s->Out.swSpdRefPu >= 0) // in acceleration
            {
                s->Out.enSpdStatus = Accelerate;
                if (uwSpdRefZ1AbsPu < s->Coef.ulAccSpdSw1Pu) // Q15
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slAccLim1Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = s->Coef.slAccLim1Pu; // Q31
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
                else if (uwSpdRefZ1AbsPu < s->Coef.ulAccSpdSw2Pu)
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slAccLim2Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = s->Coef.slAccLim2Pu;
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
                else
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slAccLim3Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = s->Coef.slAccLim3Pu; // Q31
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
            }
            else // in deceleration
            {
                s->Out.enSpdStatus = Decelerate;
                if (uwSpdRefZ1AbsPu < s->Coef.ulDecSpdSw1Pu)
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slDecLim1Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = s->Coef.slDecLim1Pu; // Q31
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
                else if (uwSpdRefZ1AbsPu < s->Coef.ulDecSpdSw2Pu)
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slDecLim2Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = s->Coef.slDecLim2Pu; // Q31
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
                else
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slDecLim3Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = s->Coef.slDecLim3Pu; // Q31
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
            }
        }
        else if ((s->Out.swSpdRefPu - s->Out.swSpdCmdRefPu) > 0)
        {
            if (s->Out.swSpdRefPu > 0) // in deceleration
            {
                s->Out.enSpdStatus = Decelerate;
            }
            else // in acceleration
            {
                s->Out.enSpdStatus = Accelerate;
            }

            if (cmd_pvt_slAccDeltaOutZ1Pu >= 0)
            {
                cmd_pvt_slAccDeltaOutPu = -cmd_pvt_slAccDeltaOutZ1Pu;
            }
            else
            {
                cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaOutZ1Pu;
            }
            // cmd_pvt_slAccDeltaOutPu = -abs(cmd_pvt_slAccDeltaOutZ1Pu);
        }
        else
        {}
    }
    else if (cmd_pvt_slAccDeltaInPu < 0)
    {
        if ((s->Out.swSpdRefPu - s->Out.swSpdCmdRefPu) > 0)
        {
            if (s->Out.swSpdRefPu > 0) // in deceleration
            {
                s->Out.enSpdStatus = Decelerate;
                if (uwSpdRefZ1AbsPu < s->Coef.ulDecSpdSw1Pu)
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slDecLim1Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = s->Coef.slDecLim1Pu; // Q31
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
                else if (uwSpdRefZ1AbsPu < s->Coef.ulDecSpdSw2Pu)
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slDecLim2Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = -s->Coef.slDecLim2Pu; // Q31
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
                else
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slDecLim3Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = -s->Coef.slDecLim3Pu; // Q31
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
            }
            else // in acceleration
            {
                s->Out.enSpdStatus = Accelerate;

                if (uwSpdRefZ1AbsPu < s->Coef.ulAccSpdSw1Pu)
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slAccLim1Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = -s->Coef.slAccLim1Pu; // Q31
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
                else if (uwSpdRefZ1AbsPu < s->Coef.ulAccSpdSw2Pu)
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slAccLim2Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = -s->Coef.slAccLim2Pu; // Q31
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
                else
                {
                    if (ulAccDeltaInAbsPu > s->Coef.slAccLim3Pu)
                    {
                        cmd_pvt_slAccDeltaOutPu = -s->Coef.slAccLim3Pu; // Q31
                    }
                    else
                    {
                        cmd_pvt_slAccDeltaOutPu = cmd_pvt_slAccDeltaInPu;
                    }
                }
            }
        }
        else if ((s->Out.swSpdRefPu - s->Out.swSpdCmdRefPu) < 0)
        {
            if (s->Out.swSpdRefPu < 0) // in deceleration
            {
                s->Out.enSpdStatus = Decelerate;
            }
            else // in acceleration
            {
                s->Out.enSpdStatus = Accelerate;
            }
            if (cmd_pvt_slAccDeltaOutZ1Pu >= 0)
            {
                cmd_pvt_slAccDeltaOutZ1Pu = cmd_pvt_slAccDeltaOutZ1Pu;
            }
            else
            {
                cmd_pvt_slAccDeltaOutPu = -cmd_pvt_slAccDeltaOutZ1Pu;
            }
            // cmd_pvt_slAccDeltaOutPu = abs(cmd_pvt_slAccDeltaOutZ1Pu);
        }
        else
        {}
    }
    else
    {}

    cmd_pvt_slAccDeltaOutZ1Pu = cmd_pvt_slAccDeltaOutPu;

    if (s->Out.swSpdRefPu > s->Out.swSpdCmdRefPu) // if swSpdRefPu > 0 deceleration, else acceleration
    {
        s->Out.slSpdRefPu += cmd_pvt_slAccDeltaOutPu; // Q31
        if (s->Out.slSpdRefPu <= slSpdCmdRefPu)
        {
            s->Out.slSpdRefPu = slSpdCmdRefPu;
        }
    }
    else if (s->Out.swSpdRefPu < s->Out.swSpdCmdRefPu) // if swSpdRefPu > 0 acceleration, else deceleration
    {
        s->Out.slSpdRefPu += cmd_pvt_slAccDeltaOutPu; // Q31
        if (s->Out.slSpdRefPu >= slSpdCmdRefPu)
        {
            s->Out.slSpdRefPu = slSpdCmdRefPu;
        }
    }
    else
    {
        s->Out.slSpdRefPu = slSpdCmdRefPu;
    }
    s->Out.swSpdRefPu = s->Out.slSpdRefPu >> 16; // Q31-Q16=Q15

    if (cmd_pvt_slSpdRefZ1Pu == s->Out.slSpdRefPu)
    {
        s->Out.enSpdStatus = Constant;
    }
    cmd_pvt_slSpdRefZ1Pu = s->Out.slSpdRefPu;

    if (s->Out.swSpdRefPu > (SLONG)s->Coef.ulSpdRefMaxPu)
    {
        s->Out.swSpdRefPu = (SLONG)s->Coef.ulSpdRefMaxPu;
    }
    else if (s->Out.swSpdRefPu < -(SLONG)s->Coef.ulSpdRefMaxPu)
    {
        s->Out.swSpdRefPu = -(SLONG)s->Coef.ulSpdRefMaxPu;
    }

    if (s->Out.swSpdRefPu >= 0)
    {
        s->Out.swSpdRefDirNow = 1;
    }
    else
    {
        s->Out.swSpdRefDirNow = -1;
    }
}

void cmd_voCmdDirGen(CMD_GEN_IN *in, CMD_CONTROL *s)
{
    if (in->slRefRpm >= 0)
    {
        s->Out.swSpdCmdDir = 1;
    }
    else
    {
        s->Out.swSpdCmdDir = -1;
    }
}