/**
 * @file xmc_math.c
 * @date 2019-12-16
 *
 * @cond
 *****************************************************************************
 * XMClib v2.2.0 - XMC Peripheral Driver Library
 *
 * Copyright (c) 2015-2020, Infineon Technologies AG
 * All rights reserved.
 *
 * Boost Software License - Version 1.0 - August 17th, 2003
 *
 * Permission is hereby granted, free of charge, to any person or organization
 * obtaining a copy of the software and accompanying documentation covered by
 * this license (the "Software") to use, reproduce, display, distribute,
 * execute, and transmit the Software, and to prepare derivative works of the
 * Software, and to permit third-parties to whom the Software is furnished to
 * do so, all subject to the following:
 *
 * The copyright notices in the Software and this entire statement, including
 * the above license grant, this restriction and the following disclaimer,
 * must be included in all copies of the Software, in whole or in part, and
 * all derivative works of the Software, unless such copies or derivative
 * works are solely in the form of machine-executable object code generated by
 * a source language processor.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
 * SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
 * FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 *
 * To improve the quality of the software, users are encouraged to share
 * modifications, enhancements or bug fixes with Infineon Technologies AG
 * at XMCSupport@infineon.com.
 *****************************************************************************
 *
 * Change History
 * --------------
 *
 * 2015-06-20:
 *     - Removed version macros and declaration of GetDriverVersion API <br>
 *     - Updated copyright and change history section.
 *
 * 2015-09-23:
 *     - Added SQRT functions
 *
 * 2015-10-08:
 *     - Return values for sin(), cos(), sinh(), cosh(), arctan() are corrected.
 *
 * 2017-04-20:
 *     - Foward declaration of __aeabi_* to fix link time optimization (-flto) compilation errors
 *
 * 2017-11-30:
 *     - Fixed reentrancy problems when using __aeabi_* functions (division ‘/’ and modulo ‘%’ C operators) which are active by default
 *       as long as XMC_MATH_DISABLE_DIV_ABI is not defined
 *
 * 2018-08-08:
 *     - Fixed compiler warnings for compiler others than GCC
 *
 * 2019-12-16:
 *     - Fix including files following the convention: angle brackets are used for standard includes and double quotes for everything else.
 *
 * @endcond
 *
 */

/**
 *
 * @brief MATH driver - API implementation for XMC13/14 family MATH libraries. <br>
 *
 * <b>Detailed description of file</b> <br>
 * APIs provided in this file cover the following functional blocks of MATH: <br>
 *
 */

/*********************************************************************************************************************
 * HEADER FILES
 ********************************************************************************************************************/
#include "xmc_math.h"

#if defined (MATH)
/*********************************************************************************************************************
 * MACROS
 ********************************************************************************************************************/
/* Reciprocal of Circular gain in XMC_MATH_Q0_23_t format ((2^23)/1.646760258121) */
#define XMC_MATH_RECIPROC_CIRCULAR_GAIN_IN_Q023       (0x4DBA76U)
/* Reciprocal of Hyperbolic gain in XMC_MATH_Q1_22_t format ((2^22)/0.828159360960) */
#define XMC_MATH_RECIPROC_HYPERBOLIC_GAIN_IN_Q1_22    (0x4D47A1U)
/* Signed division is selected */
#define XMC_MATH_SIGNED_DIVISION                      ((uint32_t) 0 << MATH_DIVCON_USIGN_Pos)
/* Unsigned division is selected */
#define XMC_MATH_UNSIGNED_DIVISION                    ((uint32_t) 1 << MATH_DIVCON_USIGN_Pos)

/*********************************************************************************************************************
 * ENUMS
 ********************************************************************************************************************/

/*********************************************************************************************************************
 * GLOBAL DATA
 ********************************************************************************************************************/

/*********************************************************************************************************************
 * DATA STRUCTURES
 ********************************************************************************************************************/

/*********************************************************************************************************************
 * LOCAL ROUTINES
 ********************************************************************************************************************/

/*********************************************************************************************************************
 * API IMPLEMENTATION - Utility functions
 ********************************************************************************************************************/
__attribute__((always_inline)) __STATIC_INLINE uint32_t critical_section_enter(void)
{
  uint32_t status;
  status = __get_PRIMASK();
  __disable_irq ();
  return status;
}

__attribute__((always_inline)) __STATIC_INLINE void critical_section_exit(uint32_t status)
{
  __set_PRIMASK(status);
}

/* Utility function to check if the DIV unit is busy */
bool XMC_MATH_DIV_IsBusy(void)
{
  bool status;
  if (MATH->DIVST & MATH_DIVST_BSY_Msk)
  {
    status = true;  /* DIV unit is busy running a division operation */
  }
  else
  {
    status = false; /* DIV unit is idle */
  }

  return (status);
}

/* Utility function to check if the CORDIC unit is busy */
bool XMC_MATH_CORDIC_IsBusy(void)
{
  bool status;
  if (MATH->STATC & MATH_STATC_BSY_Msk)
  {
    status = true;  /* CORDIC unit is busy running an operation */
  }
  else
  {
    status = false; /* CORDIC unit is idle */
  }

  return (status);
}

/* This functions returns the status of a requested event */
bool XMC_MATH_GetEventStatus(const XMC_MATH_EVENT_t event)
{
  bool status;
  if (MATH->EVFR & (uint32_t) event)
  {
    status = true; /* Requested event has been detected */
  }
  else
  {
    status = false; /* Requested event has not been detected */
  }
  return (status);
}

#ifndef XMC_MATH_DISABLE_DIV_ABI

/* Forward prototypes.  */
#if defined ( __GNUC__ )
uint32_t __aeabi_uidiv(uint32_t dividend, uint32_t divisor) __attribute__((externally_visible));
int32_t __aeabi_idiv(int32_t dividend, int32_t divisor) __attribute__((externally_visible));
uint64_t __aeabi_uidivmod(uint32_t dividend, uint32_t divisor) __attribute__((externally_visible));
int64_t __aeabi_idivmod(int32_t dividend, int32_t divisor) __attribute__((externally_visible));
#else
uint32_t __aeabi_uidiv(uint32_t dividend, uint32_t divisor);
int32_t __aeabi_idiv(int32_t dividend, int32_t divisor);
uint64_t __aeabi_uidivmod(uint32_t dividend, uint32_t divisor);
int64_t __aeabi_idivmod(int32_t dividend, int32_t divisor);
#endif

/***********************************************************************************************************************
 * API IMPLEMENTATION - aeabi routines
 **********************************************************************************************************************/
/* This function performs unsigned integer division */
uint32_t __aeabi_uidiv(uint32_t dividend, uint32_t divisor)
{
  uint32_t result;
  uint32_t ics;
  ics = critical_section_enter();

  MATH->DIVCON  = XMC_MATH_UNSIGNED_DIVISION;
  MATH->DVD     = dividend;
  MATH->DVS     = divisor;

  result = MATH->QUOT;

  critical_section_exit(ics);

  return result;
}

/* This function performs signed integer division */
int32_t __aeabi_idiv(int32_t dividend, int32_t divisor)
{
  uint32_t result;
  uint32_t ics;
  ics = critical_section_enter();

  MATH->DIVCON  = XMC_MATH_SIGNED_DIVISION;
  MATH->DVD     = dividend;
  MATH->DVS     = divisor;

  result = MATH->QUOT;

  critical_section_exit(ics);

  return result;
}

/* This function performs unsigned integer division modulo */
uint64_t __aeabi_uidivmod(uint32_t dividend, uint32_t divisor)
{
  uint64_t remainder;
  uint64_t quot;
  uint32_t ics;
  ics = critical_section_enter();

  MATH->DIVCON  = XMC_MATH_UNSIGNED_DIVISION;
  MATH->DVD     = dividend;
  MATH->DVS     = divisor;

  remainder = (uint64_t)MATH->RMD;
  quot = (uint64_t)MATH->QUOT;

  critical_section_exit(ics);

  return ((remainder << 32) | quot);
}

/* This function performs signed integer division modulo */
int64_t __aeabi_idivmod(int32_t dividend, int32_t divisor)
{
  uint64_t remainder;
  uint64_t quot;
  uint32_t ics;
  ics = critical_section_enter();

  MATH->DIVCON  = XMC_MATH_SIGNED_DIVISION;
  MATH->DVD     = dividend;
  MATH->DVS     = divisor;

  remainder = (uint64_t)MATH->RMD;;
  quot    = (uint64_t)MATH->QUOT;

  critical_section_exit(ics);

  return ((int64_t)((remainder << 32) | quot));
}
#endif

/***********************************************************************************************************************
 * API IMPLEMENTATION - Blocking functions
 **********************************************************************************************************************/
/* This function computes the cosine of a given angle in radians */
XMC_MATH_Q0_23_t XMC_MATH_CORDIC_Cos(XMC_MATH_Q0_23_t angle_in_radians)
{
  int32_t result;
  MATH->STATC = 0U; /* Clear register */
  MATH->CON   = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_CIRCULAR + \
                (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ = ((uint32_t) angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY = 0U;  /* Clear register */
  MATH->CORDX = XMC_MATH_RECIPROC_CIRCULAR_GAIN_IN_Q023 << MATH_CORDX_DATA_Pos;
  result      = ((int32_t)MATH->CORRX) >> MATH_CORRX_RESULT_Pos;
  return ((XMC_MATH_Q0_23_t) result);
}

/* This function computes the sine of a given angle in radians */
XMC_MATH_Q0_23_t XMC_MATH_CORDIC_Sin(XMC_MATH_Q0_23_t angle_in_radians)
{
  int32_t result;
  MATH->STATC = 0U; /* Clear register */
  MATH->CON   = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_CIRCULAR + \
                (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ = ((uint32_t)angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY = 0U; /* Clear register */
  MATH->CORDX = XMC_MATH_RECIPROC_CIRCULAR_GAIN_IN_Q023 << MATH_CORDX_DATA_Pos;
  result      = ((int32_t)MATH->CORRY) >> MATH_CORRY_RESULT_Pos;
  return ((XMC_MATH_Q0_23_t) result);
}

/* This function computes the tangent of a given angle in radians */
XMC_MATH_Q0_11_t XMC_MATH_CORDIC_Tan(XMC_MATH_Q0_23_t angle_in_radians)
{
  uint32_t result;
  MATH->GLBCON = (uint32_t) XMC_MATH_DIV_DVDRC_CORRY_IS_SOURCE + \
                 (uint32_t) XMC_MATH_DIV_DVSRC_CORRX_IS_SOURCE;      /* Chain the results of CORDIC operation to DIV unit */
  MATH->DIVCON = (uint32_t) 11 << MATH_DIVCON_DVSSRC_Pos;            /* Right Shifts Divisor by 11 places prior to division */
  MATH->STATC  = 0U; /* Clear register */
  MATH->CON    = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_CIRCULAR + \
                 (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ  = ((uint32_t)angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY  = 0U; /* Clear register */
  MATH->CORDX  = XMC_MATH_RECIPROC_CIRCULAR_GAIN_IN_Q023 << MATH_CORDX_DATA_Pos;
  result       = MATH->QUOT;
  return ((XMC_MATH_Q0_11_t) result);
}

/* This function computes the arc tangent of a given angle in radians */
XMC_MATH_Q0_23_t XMC_MATH_CORDIC_ArcTan(XMC_MATH_Q8_15_t x, XMC_MATH_Q8_15_t y)
{
  uint32_t result;
  MATH->STATC = 0U; /* Clear register */
  MATH->CON   = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_CIRCULAR;
  MATH->CORDZ = 0U; /* Clear register */
  MATH->CORDY = ((uint32_t) y) << MATH_CORDY_DATA_Pos;
  MATH->CORDX = ((uint32_t) x) << MATH_CORDX_DATA_Pos;
  result      = ((int32_t)MATH->CORRZ) >> MATH_CORRZ_RESULT_Pos;
  return ((XMC_MATH_Q0_23_t) result);
}

/* This function computes the hyperbolic cosine of a given angle in radians */
XMC_MATH_Q1_22_t XMC_MATH_CORDIC_Cosh(XMC_MATH_Q0_23_t angle_in_radians)
{
  int32_t result;
  MATH->STATC = 0U; /* Clear register */
  MATH->CON   = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_HYPERBOLIC + \
                (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ = ((uint32_t) angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY = 0U; /* Clear register */
  MATH->CORDX = XMC_MATH_RECIPROC_HYPERBOLIC_GAIN_IN_Q1_22 << MATH_CORDX_DATA_Pos;
  result      = ((int32_t)MATH->CORRX) >> MATH_CORRX_RESULT_Pos;
  return ((XMC_MATH_Q1_22_t) result);
}

/* This function computes the hyperbolic sine of a given angle in radians */
XMC_MATH_Q1_22_t XMC_MATH_CORDIC_Sinh(XMC_MATH_Q0_23_t angle_in_radians)
{
  int32_t result;
  MATH->STATC = 0U; /* Clear register */
  MATH->CON   = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_HYPERBOLIC + \
                (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ = ((uint32_t)angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY = 0U; /* Clear register */
  MATH->CORDX = XMC_MATH_RECIPROC_HYPERBOLIC_GAIN_IN_Q1_22 << MATH_CORDX_DATA_Pos;
  result      = ((int32_t)MATH->CORRY) >> MATH_CORRY_RESULT_Pos;
  return ((XMC_MATH_Q1_22_t) result);
}

/* This function computes the hyperbolic tangent of a given angle in radians */
XMC_MATH_Q0_11_t XMC_MATH_CORDIC_Tanh(XMC_MATH_Q0_23_t angle_in_radians)
{
  uint32_t result;
  MATH->GLBCON = (uint32_t) XMC_MATH_DIV_DVDRC_CORRY_IS_SOURCE + \
                 (uint32_t) XMC_MATH_DIV_DVSRC_CORRX_IS_SOURCE;      /* Chain the results of CORDIC operation to DIV unit */
  MATH->DIVCON = (uint32_t) 11 << MATH_DIVCON_DVSSRC_Pos;            /* Right Shifts Divisor by 11 places prior to division */
  MATH->STATC  = 0U; /* Clear register */
  MATH->CON    = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_HYPERBOLIC + \
                 (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ  = ((uint32_t) angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY  = 0U; /* Clear register */
  MATH->CORDX  = XMC_MATH_RECIPROC_HYPERBOLIC_GAIN_IN_Q1_22 << MATH_CORDX_DATA_Pos;
  result       = MATH->QUOT;
  return ((XMC_MATH_Q0_11_t) result);
}

/***********************************************************************************************************************
 * API IMPLEMENTATION - Non blocking functions
 **********************************************************************************************************************/
/* This function computes the cosine of a given angle in radians */
void XMC_MATH_CORDIC_CosNB(XMC_MATH_Q0_23_t angle_in_radians)
{
  MATH->STATC = 0U; /* Clear register */
  MATH->CON   = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_CIRCULAR + \
                (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ = ((uint32_t) angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY = 0U;  /* Clear register */
  MATH->CORDX = XMC_MATH_RECIPROC_CIRCULAR_GAIN_IN_Q023 << MATH_CORDX_DATA_Pos;
}

/* This function computes the sine of a given angle in radians */
void XMC_MATH_CORDIC_SinNB(XMC_MATH_Q0_23_t angle_in_radians)
{
  MATH->STATC = 0U; /* Clear register */
  MATH->CON   = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_CIRCULAR + \
                (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ = ((uint32_t)angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY = 0U; /* Clear register */
  MATH->CORDX = XMC_MATH_RECIPROC_CIRCULAR_GAIN_IN_Q023 << MATH_CORDX_DATA_Pos;
}

/* This function computes the tangent of a given angle in radians */
void XMC_MATH_CORDIC_TanNB(XMC_MATH_Q0_23_t angle_in_radians)
{
  MATH->GLBCON = (uint32_t) XMC_MATH_DIV_DVDRC_CORRY_IS_SOURCE + \
                 (uint32_t) XMC_MATH_DIV_DVSRC_CORRX_IS_SOURCE;      /* Chain the results of CORDIC operation to DIV unit */
  MATH->DIVCON = (uint32_t) 11 << MATH_DIVCON_DVSSRC_Pos;            /* Right Shifts Divisor by 11 places prior to division */

  MATH->STATC  = 0U; /* Clear register */
  MATH->CON    = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_CIRCULAR + \
                 (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ  = ((uint32_t)angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY  = 0U; /* Clear register */
  MATH->CORDX  = XMC_MATH_RECIPROC_CIRCULAR_GAIN_IN_Q023 << MATH_CORDX_DATA_Pos;
}

/* This function computes the arc tangent of a given value */
void XMC_MATH_CORDIC_ArcTanNB(XMC_MATH_Q8_15_t x, XMC_MATH_Q8_15_t y)
{
  MATH->STATC = 0U; /* Clear register */
  MATH->CON   = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_CIRCULAR;
  MATH->CORDZ = 0U; /* Clear register */
  MATH->CORDY = ((uint32_t) y) << MATH_CORDY_DATA_Pos;
  MATH->CORDX = ((uint32_t) x) << MATH_CORDX_DATA_Pos;
}

/* This function computes the hyperbolic cosine of a given angle in radians */
void XMC_MATH_CORDIC_CoshNB(XMC_MATH_Q0_23_t angle_in_radians)
{
  MATH->STATC = 0U; /* Clear register */
  MATH->CON   = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_HYPERBOLIC + \
                (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ = ((uint32_t)angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY = 0U; /* Clear register */
  MATH->CORDX = XMC_MATH_RECIPROC_HYPERBOLIC_GAIN_IN_Q1_22 << MATH_CORDX_DATA_Pos;
}

/* This function computes the hyperbolic sine of a given angle in radians */
void XMC_MATH_CORDIC_SinhNB(XMC_MATH_Q0_23_t angle_in_radians)
{
  MATH->STATC = 0U; /* Clear register */
  MATH->CON   = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_HYPERBOLIC + \
                (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ = ((uint32_t)angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY = 0U; /* Clear register */
  MATH->CORDX = XMC_MATH_RECIPROC_HYPERBOLIC_GAIN_IN_Q1_22 << MATH_CORDX_DATA_Pos;
}

/* This function computes the hyperbolic tangent of a given angle in radians */
void XMC_MATH_CORDIC_TanhNB(XMC_MATH_Q0_23_t angle_in_radians)
{
  MATH->GLBCON = (uint32_t) XMC_MATH_DIV_DVDRC_CORRY_IS_SOURCE + \
                 (uint32_t) XMC_MATH_DIV_DVSRC_CORRX_IS_SOURCE;      /* Chain the results of CORDIC operation to DIV unit */
  MATH->DIVCON = (uint32_t) 11 << MATH_DIVCON_DVSSRC_Pos;            /* Right Shifts Divisor by 11 places prior to division */
  MATH->STATC  = 0U; /* Clear register */
  MATH->CON    = (uint32_t) XMC_MATH_CORDIC_OPERATING_MODE_HYPERBOLIC + \
                 (uint32_t) XMC_MATH_CORDIC_ROTVEC_MODE_ROTATION;
  MATH->CORDZ  = ((uint32_t) angle_in_radians) << MATH_CORDZ_DATA_Pos;
  MATH->CORDY  = 0U; /* Clear register */
  MATH->CORDX  = XMC_MATH_RECIPROC_HYPERBOLIC_GAIN_IN_Q1_22 << MATH_CORDX_DATA_Pos;
}

/* This function performs division for given two unsigned arguments */
void XMC_MATH_DIV_UnsignedDivNB(uint32_t dividend, uint32_t divisor)
{
  MATH->DIVCON = XMC_MATH_UNSIGNED_DIVISION;
  MATH->DVD    = dividend;
  MATH->DVS    = divisor;
}

/* This function performs division for given two signed arguments */
void XMC_MATH_DIV_SignedDivNB(int32_t dividend, int32_t divisor)
{
  MATH->DIVCON = XMC_MATH_SIGNED_DIVISION;
  MATH->DVD    = dividend;
  MATH->DVS    = divisor;
}

/* This function performs modulo operation for given two unsigned arguments */
void XMC_MATH_DIV_UnsignedModNB(uint32_t dividend, uint32_t divisor)
{
  MATH->DIVCON = XMC_MATH_UNSIGNED_DIVISION;
  MATH->DVD    = dividend;
  MATH->DVS    = divisor;
}

/* This function performs modulo operation for given two signed arguments */
void XMC_MATH_DIV_SignedModNB(int32_t dividend, int32_t divisor)
{
  MATH->DIVCON = XMC_MATH_SIGNED_DIVISION;
  MATH->DVD    = dividend;
  MATH->DVS    = divisor;
}

int16_t XMC_MATH_CORDIC_Q15_Sqrt(int16_t x)
{
  int32_t temp;
  MATH->STATC = 0U; /* Clear register */

  MATH->CON   = (uint32_t)XMC_MATH_CORDIC_OPERATING_MODE_HYPERBOLIC |
                (uint32_t)XMC_MATH_CORDIC_ROTVEC_MODE_VECTORING;

  temp = (int32_t)x << 15; /* Q30 to handle numbers > 1.0 */

  MATH->CORDY = (temp - 0x10000000U); /* x - 0.25 */
  MATH->CORDX = (temp + 0x10000000U); /* x + 0.25 */

  return (int16_t)(((MATH->CORRX >> 14) * 39568) >> 16); /* Q16 * Q15 */
}

int32_t XMC_MATH_CORDIC_Q31_Sqrt(int32_t x)
{
  MATH->STATC = 0U; /* Clear register */

  MATH->CON   = (uint32_t)XMC_MATH_CORDIC_OPERATING_MODE_HYPERBOLIC |
                (uint32_t)XMC_MATH_CORDIC_ROTVEC_MODE_VECTORING;

  x >>= 1;  /* Q30 to handle numbers > 1.0 */

  MATH->CORDY = (x - 0x10000000U); /* x - 0.25 */
  MATH->CORDX = (x + 0x10000000U); /* x + 0.25 */

  return ((MATH->CORRX >> 14) * 39568); /* Q16 * Q15 */
}

#endif /* end of #if defined (MATH) */