/*
 * 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.
 */

#ifndef _INTERP1D_H_
#define _INTERP1D_H_

/*! \brief  Contains public interface to various functions related
 *         to the 1-D Linear Interpolation
 */

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

#include <stdint.h>
#include "common/iqmath/iqmath.h"

#ifdef __cplusplus
extern "C" {
#endif

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

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

typedef struct _Interp1d_PointType_
{
    _iq x; /*!< x. */
    _iq y; /*!< y. */
} Interp1d_PointType;

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

/*******************************************************************************
 * the function prototypes
 ******************************************************************************/

/*! \brief This is an linear interpolate calculation function using 2 x and the scale as input
 *         The result x should satisfy: scale = (x-x1) / (x2-x1)
 *
 * \param[in] x1 : x value of point 1
 * \param[in] x2 : x value of point 2
 * \param[in] scale : the scale = (x-x1) / (x2-x1)
 * \return the value x in the scale equation
 */
static inline _iq Interp1d(_iq x1, _iq x2, _iq scale)
{
    return (_IQmpy(scale, x2 - x1) + x1);
}

/*! \brief This is an linear interpolate calculation function using 2 points as input
 *          Calculate y for the given x, based on two points - (x1, y1) and (x2, y2)
 *
 * \note If x1 is equal to x2, which is not actually allowed, it will return
 *       (y1+y2)/2 instead. User should prevent this situation.
 *
 * \param[in] x1 : x value of point 1
 * \param[in] y1 : y value of point 1
 * \param[in] x2 : x value of point 2
 * \param[in] y2 : y value of point 2
 * \param[in] x : x value of the target point
 * \return the y value of the target point
 */
static inline _iq Interp1dPoints(_iq x1, _iq y1, _iq x2, _iq y2, _iq x)
{
    return (x2 != x1) ? (_IQmpy(_IQdiv(y2 - y1, x2 - x1), x - x1) + y1) : _IQdiv2(y2 + y1);
}

/*! \brief This is an linear interpolate calculation function using the whole table as input
 *
 * \note The values in x[] must be from smallest to biggest
 *
 * \param[in] u0 : x value of the target point
 * \param[in] x[] : the x value array
 * \param[in] y[] : the y value array
 * \param[in] maxIndex : max index of the array
 * \return the y value of the target point
 */
static inline _iq Interp1dTable(_iq u0, const _iq x[], const _iq y[], uint32_t maxIndex)
{
    _iq      frac;
    uint32_t right;
    uint32_t left;
    uint32_t bpIdx;

    if(u0 <= x[0U])
    {
        left = 0U;
        frac = (x[1U] == x[0U]) ? _IQ(0.5) : _IQdiv(u0 - x[0U], x[1U] - x[0U]);
    }
    else if(u0 < x[maxIndex])
    {
        /* Binary Search */
        bpIdx = maxIndex >> 1U;
        left  = 0U;
        right = maxIndex;
        while(right - left > 1U)
        {
            if(u0 < x[bpIdx])
            {
                right = bpIdx;
            }
            else
            {
                left = bpIdx;
            }

            bpIdx = (right + left) >> 1U;
        }

        frac = (x[left + 1U] != x[left]) ? _IQdiv(u0 - x[left], x[left + 1U] - x[left]) : _IQ(0.5);
    }
    else
    {
        left = maxIndex - 1U;
        frac = (x[maxIndex] != x[maxIndex - 1U]) ? _IQdiv(u0 - x[maxIndex - 1U], x[maxIndex] - x[maxIndex - 1U]) : _IQ(0.5);
    }

    return (_IQmpy(_IQ(1.0) - frac, y[left]) + _IQmpy(y[left + 1U], frac));
}

/*! \brief This is an linear interpolate calculation function using the whole table as input
 *
 * \note The values in pPt[].x must be from smallest to biggest
 *
 * \param[in] u0 : x value of the target point
 * \param[in] pPt : the pointer to the points array
 * \param[in] maxIndex : max index of the array
 * \return the y value of the target point
 */
static inline _iq Interp1dPtArray(_iq u0, const Interp1d_PointType *pPt, uint32_t maxIndex)
{
    _iq      frac;
    uint32_t right;
    uint32_t left;
    uint32_t bpIdx;

    if(u0 <= pPt[0U].x)
    {
        left = 0U;
        frac = (pPt[1U].x == pPt[0U].x) ? _IQ(0.5) : _IQdiv(u0 - pPt[0U].x, pPt[1U].x - pPt[0U].x);
    }
    else if(u0 < pPt[maxIndex].x)
    {
        /* Binary Search */
        bpIdx = maxIndex >> 1U;
        left  = 0U;
        right = maxIndex;
        while(right - left > 1U)
        {
            if(u0 < pPt[bpIdx].x)
            {
                right = bpIdx;
            }
            else
            {
                left = bpIdx;
            }

            bpIdx = (right + left) >> 1U;
        }

        frac = (pPt[left + 1U].x != pPt[left].x) ? _IQdiv(u0 - pPt[left].x, pPt[left + 1U].x - pPt[left].x) : _IQ(0.5);
    }
    else
    {
        left = maxIndex - 1U;
        frac = (pPt[maxIndex].x != pPt[maxIndex - 1U].x) ? _IQdiv(u0 - pPt[maxIndex - 1U].x, pPt[maxIndex].x - pPt[maxIndex - 1U].x) : _IQ(0.5);
    }

    return (_IQmpy(_IQ(1.0) - frac, pPt[left].y) + _IQmpy(pPt[left + 1U].y, frac));
}

#ifdef __cplusplus
}
#endif /* extern "C" */

#endif /* _INTERP1D_H_ */