/*******************************************************************
  Components for embedded applications builded for
  laboratory and medical instruments firmware

  pxmc_sin_fixed.h - generic multi axis motion controller
             fixed sine approximation

  (C) 2001-2015 by Pavel Pisa pisa@cmp.felk.cvut.cz
  (C) 2002-2015 by PiKRON Ltd. http://www.pikron.com

  This file can be used and copied according to next
  license alternatives
   - GPL - GNU Public License
   - other license provided by project originators

 *******************************************************************/

#include <stdint.h>

#define PXMC_SIN_FIX_TAB_BITS 9
#define PXMC_SIN_FIX_IDX_SLR  23
#define PXMC_SIN_FIX_XD_MASK  0x007fffff
#define PXMC_SIN_FIX_XD_SLR   8
#define PXMC_SIN_FIX_A_MASK   0xffffc000
#define PXMC_SIN_FIX_B_SLL    19
#define PXMC_SIN_FIX_B_SAR    16
#define PXMC_SIN_FIX_B_XD_SAR 6
#define PXMC_SIN_FIX_ZIC_MASK 0x00002000
#define PXMC_SIN_FIX_ZIC_BIT  13

#define PXMC_SIN_FIX_PI2      0x40000000
#define PXMC_SIN_FIX_2PI3     0x55555555

extern const uint32_t pxmc_sin_fixed_table[];

/**
 * pxmc_sin_fixed_inline - fixed math sine computation
 * @x:	The argument value - 2^32 corresponds to 2 * PI or 360 deg
 * @res_unit_bits: number of fraction bits of the result (default 16)
 *
 * Returns sine of value @x argument where unit is 2^16, i.e.
 * return value 0x10000 is equivalent to +1.0 and -0x10000 to -1.0
 */
static inline
int32_t pxmc_sin_fixed_inline(uint32_t x, int res_unit_bits)
{
  uint32_t tabval;
  unsigned int ti;
  int32_t a;
  int b, xd;
  int32_t yl;

  if (!res_unit_bits)
    res_unit_bits = 16;

  ti = x >> PXMC_SIN_FIX_IDX_SLR;
  xd = (x & PXMC_SIN_FIX_XD_MASK) >> PXMC_SIN_FIX_XD_SLR;

  tabval = pxmc_sin_fixed_table[ti];
  a = tabval & PXMC_SIN_FIX_A_MASK;
  b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;

  yl = a;
  yl += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;

  yl += yl & (1 << (29 - res_unit_bits));
  yl >>= 30 - res_unit_bits;

  return yl;
}

/**
 * pxmc_sincos_fixed_inline - fixed math sine computation
 * @pysin: pointer to location where sine value is returned
 * @pycos: pointer to location where cosine value is returned
 * @x:	The argument value - 2^32 corresponds to 2 * PI or 360 deg
 * @res_unit_bits: number of fraction bits of the result (default 16)
 *
 * Returns sine and cosine of value @x argument where unit is 2^16, i.e.
 * return value 0x10000 is equivalent to +1.0 and -0x10000 to -1.0
 */
static inline
void pxmc_sincos_fixed_inline(int32_t *pysin, int32_t *pycos,
                              uint32_t x, int res_unit_bits)
{
  uint32_t tabval;
  unsigned int ti;
  int32_t a;
  int b, xd;
  int32_t ysin;
  int32_t ycos;

  if (!res_unit_bits)
    res_unit_bits = 16;

  ti = x >> PXMC_SIN_FIX_IDX_SLR;
  xd = (x & PXMC_SIN_FIX_XD_MASK) >> PXMC_SIN_FIX_XD_SLR;

  tabval = pxmc_sin_fixed_table[ti];
  a = tabval & PXMC_SIN_FIX_A_MASK;
  b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;

  ysin = a;
  ysin += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;

  ysin += ysin & (1 << (29 - res_unit_bits));
  ysin >>= 30 - res_unit_bits;

  ti = (uint32_t)(x + PXMC_SIN_FIX_PI2) >> PXMC_SIN_FIX_IDX_SLR;

  tabval = pxmc_sin_fixed_table[ti];
  a = tabval & PXMC_SIN_FIX_A_MASK;
  b = (int32_t)(tabval << PXMC_SIN_FIX_B_SLL) >> PXMC_SIN_FIX_B_SAR;

  ycos = a;
  ycos += ((int32_t)b * xd) >> PXMC_SIN_FIX_B_XD_SAR;

  ycos += ycos & (1 << (29 - res_unit_bits));
  ycos >>= 30 - res_unit_bits;

  *pysin = ysin;
  *pycos = ycos;
}

/**
 * pxmc_sin_fixed_zic_inline - indicate range for +/-10 degree zero current correction
 * @x:	The argument value - 2^32 corresponds to 2 * PI or 360 deg
 *
 * Returns value one if input is in the 0 or PI +/- 10 deg range.
 */
static inline
int pxmc_sin_fixed_zic_inline(uint32_t x)
{
  unsigned int ti;

  ti = x >> PXMC_SIN_FIX_IDX_SLR;

  return (pxmc_sin_fixed_table[ti] >> PXMC_SIN_FIX_ZIC_BIT) & 1;
}