#include "fast_trig.h"
#include "num/num.h"

static libnum::num cosTable[360];
static libnum::num sinTable[360];
static bool is_fast_trig_initialised = false;

void Fast_Trig_Init( void )
{
    for(int u=0; u<360; u++)
    {
        cosTable[u] = libnum::num( cos( u * PI / 180 ) );
        sinTable[u] = libnum::num( sin( u * PI / 180 ) );
    }
    is_fast_trig_initialised = true;
}


libnum::num FastCosInt( int16_t angle )
{
    if (!is_fast_trig_initialised) Fast_Trig_Init();

    if (angle>=0 and angle<360) return cosTable[ angle ];
    else
    {
        int16_t input = angle;
        if (input<0)
        {
            while (input<0) input+=360;
            return cosTable[ input ];
        }
        else
        {
            while (input>=360) input-=360;
            return cosTable[ input ];
        }
    }
}

libnum::num FastSinInt( int16_t angle )
{
    if (!is_fast_trig_initialised) Fast_Trig_Init();

    if (angle>=0 and angle<360) return sinTable[ angle ];
    else
    {
        int16_t input = angle;
        if (input<0)
        {
            while (input<0) input+=360;
            return sinTable[ input ];
        }
        else
        {
            while (input>=360) input-=360;
            return sinTable[ input ];
        }
    }
}

libnum::num FastTanInt( int16_t angle )
{
    if (!is_fast_trig_initialised) Fast_Trig_Init();

    int16_t input = angle;

    if (input<0)
    {
        while (input<0) input+=360;
    }
    else if (input>=360)
    {
        while (input>=360) input-=360;
    }

    libnum::num value;

    if (input==90)
    {
        value.v = INT32_MAX;
        return value;
    }
    else if (input==270)
    {
        value.v = INT32_MIN;
        return value;
    }
    else
    {
        value = FastSinInt(input) / FastCosInt(input);
        return value;
    }
}



libnum::num32 sqrt_num32(libnum::num32 v) {
  uint32_t t, q, b, r;
  r = v.v;
  b = 0x40000000;
  q = 0;
  while (b > 0x40) {
    t = q + b;
    if (r >= t) {
      r -= t;
      q = t + b;
    }
    r <<= 1;
    b >>= 1;
  }
  q >>= 8;
  libnum::num32 ret;
  ret.v = q;
  return ret;
}

/* TO DO : rework these functions for sine and cosine calculation */

libnum::num32 cos_num32(libnum::num32 angle) {
  //  Taylor serie for cos(x) = 1 - x²/2! + x⁴/4! + x⁶/6! + x⁸/8! + ...

  // Cosine function is even
  if (angle < libnum::num32(0))
    return cos_num32(-angle);

  // Look for an angle in the range [0, 2*pi [
  libnum::num32 anglereduced = angle;
  while (anglereduced >= libnum::num32(2 * PI))
    anglereduced -= libnum::num32(2 * PI);

  // Exploit the symetry for angle and angle+Pi to reduce the order of the
  // limited developpement
  if (anglereduced >= libnum::num(PI))
    return -cos_num32(anglereduced - libnum::num(PI));

  libnum::num32 sum = libnum::num32(1);
  libnum::num32 angle2 = anglereduced * anglereduced;

  // set first value of the Taylor serie : x⁰/0! = 1/1
  libnum::num32 numerator = libnum::num32(1);
  libnum::num32 denominator = libnum::num32(1);

  for (int i = 2; i <= 8; i += 2) {
    numerator *= (-angle2);
    denominator *= libnum::num32(i - 1) * libnum::num32(i);
    sum += (numerator / denominator);
  }

  return sum;
}

libnum::num32 sin_num32(libnum::num32 angle) {
  //  Taylor serie for cos(x) = x/1! - x³/3! + x⁵/5! - x⁷/7! + x⁹/9! + ...

  // Sine function is odd
  if (angle < libnum::num32(0))
    return -sin_num32(-angle);

  // Look for an angle in the range [0, 2*pi [
  libnum::num32 anglereduced = angle;
  while (anglereduced >= libnum::num32(2 * PI))
    anglereduced -= libnum::num32(2 * PI);

  // Exploit the symetry for angle and angle+Pi to reduce the order of the
  // limited developpement
  if (anglereduced >= libnum::num(PI))
    return -sin_num32(anglereduced - libnum::num(PI));

  libnum::num32 sum = anglereduced;
  libnum::num32 angle2 = anglereduced * anglereduced;

  // set first value of the Taylor serie : x¹/1! = x/1
  libnum::num32 numerator = anglereduced;
  libnum::num32 denominator = libnum::num32(1);

  for (int i = 2; i <= 8; i += 2) {
    numerator *= (-angle2);
    denominator *= libnum::num32(i) * libnum::num32(i + 1);
    sum += (numerator / denominator);
  }

  return sum;
}