/*
 * 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 "common/interp/interp2d.h"
#include "ipm_pte.h"
#include "ipm.h"

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

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

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

void Ipm_Init(IpmType *obj, const Ipm_ParamsType *pParams)
{
    obj->type          = pParams->type;
    obj->maxRows       = pParams->maxRows;
    obj->maxColumns    = pParams->maxColumns;
    obj->defaultRs_Ohm = pParams->defaultRs_Ohm;
    obj->defaultLd_mH  = pParams->defaultLd_mH;
    obj->defaultLq_mH  = pParams->defaultLq_mH;
    obj->defaultPsi_Wb = pParams->defaultPsi_Wb;

    obj->Rs_Ohm = obj->defaultRs_Ohm;
    if(obj->type == IPM_OUTPUTTYPE_FIXED)
    {
        obj->Ld_mH  = obj->defaultLd_mH;
        obj->Lq_mH  = obj->defaultLq_mH;
        obj->Psi_Wb = obj->defaultPsi_Wb;
    }
    else if(obj->type == IPM_OUTPUTTYPE_DYNAMIC)
    {
        Table_Init(&obj->paramTable, pParams->pContainer, obj->maxRows, obj->maxColumns);
        /* Calculate row and column interval, the table must have at least 4 elements */
        IpmPteType elem00;
        IpmPteType elem01;
        IpmPteType elem10;
        Table_GetElement(&obj->paramTable, (void *)&elem00, sizeof(IpmPteType), 0, 0);
        Table_GetElement(&obj->paramTable, (void *)&elem01, sizeof(IpmPteType), 0, 1);
        Table_GetElement(&obj->paramTable, (void *)&elem10, sizeof(IpmPteType), 1, 0);
        /*! \note   The interval between rows and columns must the same for the whole table */
        obj->rowInterval        = _IQabs(elem10.absId_pu - elem00.absId_pu);
        obj->columnInterval     = _IQabs(elem01.absIq_pu - elem00.absIq_pu);
        obj->oneOverRowIntvl    = _IQdiv(_IQ(1.0), obj->rowInterval);
        obj->oneOverColumnIntvl = _IQdiv(_IQ(1.0), obj->columnInterval);
    }
}

void Ipm_Calc(IpmType *obj)
{
    if(obj->type == IPM_OUTPUTTYPE_DYNAMIC)
    {
        /* The row and column index value should all be ABS */
        obj->absId_pu = _IQabs(obj->Id_pu);
        obj->absIq_pu = _IQabs(obj->Iq_pu);
        /* Find row */
        /*! \note   The interval between rows must the same for the whole table */
        uint16_t row = (uint16_t)(obj->absId_pu / obj->rowInterval);
        row          = (row > obj->maxRows - 2 ? obj->maxRows - 2 : row);
        /* Find column */
        /*! \note   The interval between columns must the same for the whole table */
        uint16_t column = (uint16_t)(obj->absIq_pu / obj->columnInterval);
        column          = (column > obj->maxColumns - 2 ? obj->maxColumns - 2 : column);
        /* Find 4 elements */
        IpmPteType topLeft;
        IpmPteType topRight;
        IpmPteType bottomLeft;
        IpmPteType bottomRight;
        Table_GetElement(&obj->paramTable, (void *)&topLeft, sizeof(IpmPteType), row, column);
        Table_GetElement(&obj->paramTable, (void *)&topRight, sizeof(IpmPteType), row, column + 1);
        Table_GetElement(&obj->paramTable, (void *)&bottomLeft, sizeof(IpmPteType), row + 1, column);
        Table_GetElement(&obj->paramTable, (void *)&bottomRight, sizeof(IpmPteType), row + 1, column + 1);
        _iq rowScale    = _IQmpy(obj->absId_pu - topLeft.absId_pu, obj->oneOverRowIntvl);
        rowScale        = (rowScale > _IQ(1.0) ? _IQ(1.0) : (rowScale < 0 ? 0 : rowScale));
        _iq columnScale = _IQmpy(obj->absIq_pu - topLeft.absIq_pu, obj->oneOverColumnIntvl);
        columnScale     = (columnScale > _IQ(1.0) ? _IQ(1.0) : (columnScale < 0 ? 0 : columnScale));
        /* Calculate Ld */
        obj->Ld_mH = Interp2d(topLeft.Ld_mH, topRight.Ld_mH, bottomLeft.Ld_mH, bottomRight.Ld_mH, rowScale, columnScale);
        /* Calculate Lq */
        obj->Lq_mH = Interp2d(topLeft.Lq_mH, topRight.Lq_mH, bottomLeft.Lq_mH, bottomRight.Lq_mH, rowScale, columnScale);

        /* Calculate Psi */
        obj->Psi_Wb = Interp2d(topLeft.Psi_wb, topRight.Psi_wb, bottomLeft.Psi_wb, bottomRight.Psi_wb, rowScale, columnScale);
    }
}
