/*
 * 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 "fw.h"

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

/*! \brief Defines the Field Weakening (FW) increment delta correction
 */
#define FW_INC_DELTA (0.00002)

/*! \brief Defines the Field Weakening (FW) decrement delta correction
 */
#define FW_DEC_DELTA (0.00002)

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

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

void Fw_Init(FwType *obj, _iq maxNegIdRef)
{
    obj->outMax   = 0;
    obj->outMin   = maxNegIdRef;
    obj->deltaInc = _IQ(FW_INC_DELTA);
    obj->deltaDec = _IQ(FW_DEC_DELTA);

    obj->output = 0;
}

void Fw_Run(FwType *obj)
{
    if(obj->refValue > obj->fbackValue)
    {
        obj->output += obj->deltaInc;
    }
    else if(obj->refValue < obj->fbackValue)
    {
        obj->output -= obj->deltaDec;
    }

    obj->output = _IQsat(obj->output, obj->outMax, obj->outMin); /* Saturate the output */
}
