// ****************************************************************************
//
// (C) Automotive-Lighting Reutlingen GmbH  -  ALRT/EEG2
//     Tuebinger Strasse 123, 72703 Reutlingen, Germany
//
// ****************************************************************************

// ----------------------------------------------------------------------------
///  \file    UtilMath.c
///
///  \author  Ralf Sondershaus        Ralf.Sondershaus@al-lighting.com
///  \author  Christopher Kormanyos   Christopher.Kormanyos@al-lighting.com
///
///  \owner   Ralf Sondershaus        Ralf.Sondershaus@al-lighting.com
///
///  \date    12.09.2007
///
///  \brief   General math routines like filters, 16 bit code
///
// ----------------------------------------------------------------------------
// Avoid MISRA-/QAC-Warnings:
// warning - QAC(Prio4) Msg: 1505(The function '...' is only referenced in the translation unit where it is defined.)
// Needed warning suppression introduced for global, but not always needed, functions.
// PRQA S 1505 EOF

//=============================================================================
// includes
//=============================================================================

#include <UtilMath.h>
#include <UtilUtility.h>

//=============================================================================
// defines
//=============================================================================

//=============================================================================
// prototypes
//=============================================================================

// *****************************************************************************
// Function    : uint16 UtilMath_LinearInterpolateU16(const uint16* punX, const uint16* punY, sint32 lLinPairs, uint16 unX)
//
// Description : Interpolate a table of points using linear interpolation rule.
//               Find the appropriate pair of points interpolate the corresponding Y-value.
//               Important: It is assumed that the X-values increase in the order of increasing
//               table entries.
//
// *****************************************************************************
uint16 UtilMath_LinearInterpolateU16(const uint16* punX, const uint16* punY, sint32 lLinPairs, uint16 unX)
{
  uint32 ulRetVal;

  if((unX <= punX[0]) || (lLinPairs < 2))
  {
    ulRetVal = punY[0];
  }
  else if(unX >= punX[lLinPairs - 1])
  {
    ulRetVal = punY[lLinPairs - 1];
  }
  else
  {
    // Find pair of interpolation points
    sint32 n;

    for(n = 0; n < (lLinPairs - 2); n++)
    {
      if(punX[n + 1] > unX)
      {
        break;
      }
    }

    // Interpolate pair of points using linear interpolation rule.
    //
    //                               (nX - X0) * (Y1 - Y0) + (X1 - X0)/2
    //                    nY = Y0 +  -----------------------------------  ( for Y1 > Y0 )
    //                                           (X1 - X0)
    //
    //                               (nX - X0) * (Y1 - Y0) - (X1 - X0)/2
    //                    nY = Y0 +  -----------------------------------  ( for Y1 < Y0 )
    //                                           (X1 - X0)

    {
      const sint32 lStepY     = (sint32) punY[n + 1] - (sint32) punY[n];
      const sint32 lStepX     = (sint32) punX[n + 1] - (sint32) punX[n];
      const sint32 lDeltaX    = (sint32) unX         - (sint32) punX[n];
      sint32       lHalfStepX = (sint32) (((uint32) lStepX) / 2);

      if(lStepY < 0)
      {
        lHalfStepX = -lHalfStepX;
      }

      ulRetVal = (uint32) (punY[n] + (((lDeltaX * lStepY) + lHalfStepX) / lStepX));
    }
  }

  return (uint16) ulRetVal;
}

//-----------------------------------------------------------------------------
/// \brief  Lowpass 1st order
///
/// \descr  Y(n) = (1 - Beta) * X(n) + Beta * Y(n-1) ,   0 <= Beta < 1
///         unBeta is normalized to 1024 (0...1023)
///         filter time constant = - (sampling time) / LN(Beta)
///         filter storage is scaled by factor 64
///
///         ideal:
///         Y =  Y * Beta / 1024 + X * 64 * (1024 - Beta)         / 1024
///         Y = (Y * Beta        + X * 64 * (1024 - Beta))        / 1024
///
///         rounding:
///         Y = (Y * Beta        + X * 64 * (1024 - Beta) + 512) >> 10
///
/// \param  pFilter  - pointer to filter structure
/// \param  unBeta   - filter coefficient
/// \param  nValue   - input value
///
/// \return none
//-----------------------------------------------------------------------------
void Order1Filter(ORDER1_Filter* const pFilter, const sint16 nValue, const uint16 unBeta)
{
  //PRQA S 502 1 // Compiler can do right shift
  *pFilter = ((*pFilter * unBeta) + ((sint32) nValue * 64 * (1024 - unBeta)) + 512) >> 10;
}

//-----------------------------------------------------------------------------
/// \brief  Initialization of lowpass 1st order
///
/// \descr  See Order1Filter description
///
/// \param  pFilter  - pointer to filter structure
/// \param  nValue   - input value
///
/// \return void
//-----------------------------------------------------------------------------
void Order1FilterInit(ORDER1_Filter* const pFilter, const sint16 nValue)
{
  //PRQA S 502 1 // Compiler can do right shift
  *pFilter = 64 * (sint32) nValue;
}

//-----------------------------------------------------------------------------
/// \brief  Get return value of lowpass 1st order
///
/// \descr  See Order1Filter description
///
/// \param  pFilter  - pointer to filter structure
///
/// \return sint16
//-----------------------------------------------------------------------------
sint16 Order1FilterValue(const ORDER1_Filter* const pFilter)
{
  //PRQA S 502 1 // Compiler can do right shift
  return (sint16) ((*pFilter + 32) >> 6);
}

//-----------------------------------------------------------------------------
/// \brief  Calculate filter coefficient for lowpass 1st order
///
/// \descr  Calculate filter coef for filter time constant <= 250 * sampling time
///         See also Order1Filter description
///
/// \param  unSamplimgTime  - sampling time in ms
/// \param  unFilterTime    - filter time   in ms
///
/// \return uint16  - filter coef for Order1Filter (which is the parameter unBeta)
//-----------------------------------------------------------------------------

uint16 Order1FilterGetCoef(const uint16 unSamplimgTime, const uint16 unFilterTime)
{
  static const uint16 UtilMathFiltCoefTableX[] =
  {
    (uint16) ((256U *    14U) / 100U),   // linear interpolation
    (uint16) ((256U *    68U) / 100U),   // leads to error <10%
    (uint16) ((256U *   209U) / 100U),   // or error < 1/10 sampling time
    (uint16) ((256U *   540U) / 100U),
    (uint16) ((256U *  1300U) / 100U),
    (uint16) ((256U *  2940U) / 100U),
    (uint16) ((256U *  6000U) / 100U),
    (uint16) ((256U * 11000U) / 100U),
    (uint16) ((256U * 17017U) / 100U),
    (uint16) ((256U * 20430U) / 100U),
    (uint16) ((256U * 25550U) / 100U)
  };

  static const uint16 UtilMathFiltCoefTableY[UTIL_COUNTOF(UtilMathFiltCoefTableX)] =
  {
       0U,  270U,  664U,  865U,  954U,
     992U, 1008U, 1015U, 1018U, 1019U,
    1020U
  };

  uint16 unRelFilterTime = (uint16) ((256U * (uint32) unFilterTime) / unSamplimgTime);

  return UtilMath_LinearInterpolateU16(UtilMathFiltCoefTableX,               UtilMathFiltCoefTableY,
                                       UTIL_COUNTOF(UtilMathFiltCoefTableX), unRelFilterTime);
}