/*
 * Copyright (c) 2022, Shenzhen CVA Innovation CO.,LTD
 * All rights reserved.
 *
 * Shenzhen CVA Innovation CO.,LTD (CVA chip) is supplying this file for use
 * exclusively with CVA's microcontroller products. This file can be freely
 * distributed within development tools that are supporting such microcontroller
 * products.
 *
 * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
 * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
 * CVA SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL,
 * OR CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
 */

/*******************************************************************************
 * the includes
 ******************************************************************************/

#include "foc.h"

/*******************************************************************************
 * the defines
 ******************************************************************************/

#define FULL_WAVE_VS (_IQ(0.66666)) /* The Vs used in full wave mode */

/*******************************************************************************
 * the typedefs
 ******************************************************************************/

/*******************************************************************************
 * the globals
 ******************************************************************************/

/*******************************************************************************
 * the functions
 ******************************************************************************/

void Foc_Init(FocType *obj, const Foc_ParamsType *pParams)
{
    obj->ctrlType = pParams->ctrlType;

    obj->minVdc_pu  = _IQ(pParams->minVdc_V / pParams->VBase_V);
    obj->maxVdc_pu  = _IQ(pParams->maxVdc_V / pParams->VBase_V);
    obj->factorKpKi = 2 * MATH_PI * pParams->IBase_A / pParams->VBase_V / pParams->bandWidthScale;

    /* Calculate PID parameters */
    _iq Ki_parallel = _IQ(2 * MATH_PI * pParams->Rs_Ohm / pParams->bandWidthScale
                          * pParams->IBase_A / pParams->ratedVdc_V);
    _iq Kp_parallel = _IQ(2 * MATH_PI * pParams->Ld_mH * pParams->runFreq_kHz / pParams->bandWidthScale
                          * pParams->IBase_A / pParams->ratedVdc_V);

    obj->VsMax = _IQ(pParams->VsMax_pu);
    obj->VdMax = _IQ(pParams->VsMax_pu * pParams->VdMaxScale);

    /* Apply to current PID, note that the pidIq limits will be set dynamically, so just set 0 when init */
    Pid_Init(&obj->pidId, Kp_parallel, Ki_parallel, 0, -obj->VdMax, obj->VdMax);

    Ki_parallel = _IQ(2 * MATH_PI * pParams->Rs_Ohm / pParams->bandWidthScale
                      * pParams->IBase_A / pParams->ratedVdc_V);
    Kp_parallel = _IQ(2 * MATH_PI * pParams->Lq_mH * pParams->runFreq_kHz / pParams->bandWidthScale
                      * pParams->IBase_A / pParams->ratedVdc_V);
    Pid_Init(&obj->pidIq, Kp_parallel, Ki_parallel, 0, 0, 0);

    /* Full wave control */
    FullWave_ParamsType fullwaveParams;
    fullwaveParams.Rs_Ohm         = pParams->Rs_Ohm;
    fullwaveParams.Ld_mH          = pParams->Ld_mH;
    fullwaveParams.runFreq_kHz    = pParams->runFreq_kHz;
    fullwaveParams.ratedVdc_V     = pParams->ratedVdc_V;
    fullwaveParams.IBase_A        = pParams->IBase_A;
    fullwaveParams.VsMax          = pParams->VsMax_pu;
    fullwaveParams.VdMaxScale     = pParams->VdMaxScale;
    fullwaveParams.VsRampStep     = FULL_WAVE_VS / (pParams->switchTime_ms * pParams->runFreq_kHz);
    fullwaveParams.bandWidthScale = pParams->fullwaveBWScale;
    FullWave_Init(&obj->fullwave, &fullwaveParams);

    obj->svgen.mode = SVGEN_MODE_NORMAL;

    obj->IdRef = 0;
    obj->IqRef = 0;
    obj->Vref  = 0;
    obj->Vdff  = 0;
    obj->Vqff  = 0;
}

void Foc_Run(FocType *obj)
{
    if(obj->ctrlType == FOC_CTRLTYPE_VVVF)
    {
        /* No current control */
        obj->ipark.Ds = obj->Vref;
        obj->ipark.Qs = 0;
    }
    else
    {
        /* Clarke transform */
        obj->clarkeI.As = obj->Ia_pu;
        obj->clarkeI.Bs = obj->Ib_pu;
        obj->clarkeI.Cs = obj->Ic_pu;
        Clarke_Run(&obj->clarkeI);

        /* Park transform */
        obj->park.alpha = obj->clarkeI.alpha;
        obj->park.beta  = obj->clarkeI.beta;
        obj->park.sinTh = obj->parkSinTh;
        obj->park.cosTh = obj->parkCosTh;
        Park_Run(&obj->park);

        if(obj->ctrlType == FOC_CTRLTYPE_CURRENTLOOP)
        {
            /* Id PID */
            obj->pidId.ref = obj->IdRef;
            obj->pidId.fdb = obj->park.Ds;
            Pid_Run(&obj->pidId);
            obj->Vd = _IQsat(obj->pidId.output + obj->Vdff, obj->VdMax, -obj->VdMax);

            /* Vq limit calculation */
            _iq VsMaxSqr = _IQmpy(obj->VsMax, obj->VsMax);
            _iq VdSqr    = _IQmpy(obj->Vd, obj->Vd);
            _iq VqMaxSqr = VdSqr < VsMaxSqr ? VsMaxSqr - VdSqr : 0;
            _iq VqMax    = _IQsqrt(VqMaxSqr);

            /* Iq PID */
            obj->pidIq.outMax = VqMax;
            obj->pidIq.outMin = -VqMax;
            obj->pidIq.ref    = obj->IqRef;
            obj->pidIq.fdb    = obj->park.Qs;
            Pid_Run(&obj->pidIq);
            obj->Vq = _IQsat(obj->pidIq.output + obj->Vqff, VqMax, -VqMax);

            /* IPARK */
            obj->ipark.Ds = obj->Vd;
            obj->ipark.Qs = obj->Vq;
        }
        else if(obj->ctrlType == FOC_CTRLTYPE_FULLWAVE)
        {
            /* Full-wave control */
            obj->fullwave.IqRef_pu = obj->IqRef;
            obj->fullwave.Iq_pu    = obj->park.Qs;
            obj->fullwave.speed_pu = obj->speed_pu;
            obj->fullwave.Vdff     = obj->Vdff;
            FullWave_Run(&obj->fullwave);

            /* IPARK */
            obj->ipark.Ds = obj->fullwave.Vd;
            obj->ipark.Qs = obj->fullwave.Vq;
        }
    }

    obj->ipark.sinTh = obj->iparkSinTh;
    obj->ipark.cosTh = obj->iparkCosTh;
    IPark_Run(&obj->ipark);

    /* Calculate Vs */
    obj->Vs = _IQmag(obj->ipark.Ds, obj->ipark.Qs);

    /* SVGEN */
    obj->svgen.Ualpha = obj->ipark.alpha;
    obj->svgen.Ubeta  = obj->ipark.beta;
    Svgen_Run(&obj->svgen);
}

void Foc_GetPwmCompareValue(FocType *obj, _iq *Ta, _iq *Tb, _iq *Tc)
{
    *Ta = obj->svgen.Ta;
    *Tb = obj->svgen.Tb;
    *Tc = obj->svgen.Tc;
}

void Foc_Reset(FocType *obj)
{
    obj->IqRef        = 0; /* Clear IqRef first just to be safe */
    obj->IdRef        = 0;
    obj->pidId.Ui     = 0;
    obj->pidId.output = 0;
    obj->pidIq.Ui     = 0;
    obj->pidIq.output = 0;
    obj->Vd           = 0;
    obj->Vq           = 0;
    obj->ipark.Ds     = 0;
    obj->ipark.Qs     = 0;
    obj->Vdff         = 0;
    obj->Vqff         = 0;
}

void Foc_UpdateParams(FocType *obj, const Foc_DynamicParamsType *pParams)
{
    if(pParams->Vdc_pu >= obj->minVdc_pu && pParams->Vdc_pu <= obj->maxVdc_pu)
    {
        if(obj->ctrlType != FOC_CTRLTYPE_FULLWAVE)
        {
            obj->pidId.Kp = _IQdiv(_IQ(obj->factorKpKi * pParams->Ld_mH * pParams->runFreq_kHz), pParams->Vdc_pu);
            obj->pidId.Ki = _IQdiv(_IQ(obj->factorKpKi * pParams->Rs_Ohm), pParams->Vdc_pu);

            obj->pidIq.Kp = _IQdiv(_IQ(obj->factorKpKi * pParams->Lq_mH * pParams->runFreq_kHz), pParams->Vdc_pu);
            obj->pidIq.Ki = obj->pidId.Ki;
        }
        else
        {
            FullWave_DynamicParamsType fullwaveDynamicParams;
            fullwaveDynamicParams.Rs_Ohm      = pParams->Rs_Ohm;
            fullwaveDynamicParams.Ld_mH       = pParams->Ld_mH;
            fullwaveDynamicParams.runFreq_kHz = pParams->runFreq_kHz;
            fullwaveDynamicParams.Vdc_pu      = pParams->Vdc_pu;
            FullWave_UpdateParams(&obj->fullwave, &fullwaveDynamicParams);
        }
    }
}

void Foc_ChangeCtrlType(FocType *obj, Foc_CtrlModeType ctrlType)
{
    if(obj->ctrlType != FOC_CTRLTYPE_FULLWAVE
       && ctrlType == FOC_CTRLTYPE_FULLWAVE)
    { /* Switch to full wave control */
        /* Remember the current Vd, Vq */
        obj->fullwave.Vd           = obj->ipark.Ds;
        obj->fullwave.Vq           = obj->ipark.Qs;
        obj->fullwave.pidIq.output = -obj->fullwave.Vd;
        obj->fullwave.pidIq.Ui     = obj->fullwave.pidIq.output;
        /* Change the target Vs */
        FullWave_SetVsMax(&obj->fullwave, FULL_WAVE_VS);
        obj->svgen.mode = SVGEN_MODE_SQUAREWAVE;
    }
    else if(obj->ctrlType == FOC_CTRLTYPE_FULLWAVE
            && ctrlType == FOC_CTRLTYPE_CURRENTLOOP)
    { /* Switch back to current loop */
        /* Remember the current Vd, Vq */
        obj->pidId.output = obj->ipark.Ds;
        obj->pidId.Ui     = obj->pidId.output;
        obj->pidIq.output = obj->ipark.Qs;
        obj->pidIq.Ui     = obj->pidIq.output;
        /* Change the target Vs */
        FullWave_SetVsMax(&obj->fullwave, obj->VsMax);
        obj->svgen.mode = SVGEN_MODE_NORMAL;
    }

    obj->ctrlType = ctrlType;
}
