/* qtrig.c */

#define SIN_VALUES 256
#define SIN_BUF_VALUES 257

float restrict_angle(float angle)
{
  if (angle >= 2.0*PI)
    return angle-2.0*PI*(float)((int)(angle*0.5*(1.0/PI)));
  else if (angle < 0.0)
    return restrict_angle(angle-2.0*PI*(float)(-1+(int)(angle*0.5*(1.0/PI))));
  else return angle;
}

float restrict_diff_angle(float angle)
{
  angle = restrict_angle(angle);
  if (angle <= PI)
    return angle;
  else
    return -_2PI+angle;
} 

#ifdef QUICK_TRIG
float sin_buf[SIN_BUF_VALUES];

void init_qtrig()
{
  int i;

  printf("init_qtrig...");

  for (i=0; i<SIN_BUF_VALUES;i++)
    sin_buf[i] = sin(((float)i+0.5)*PI_2*(1.0/(float)SIN_VALUES));

  printf("done\n");
}

float qsin(float angle)
{
  if (angle < 0.0 || angle >= 2.0*PI)
    angle = restrict_angle(angle);

  if (angle <= PI_2)
    return sin_buf[(int)(angle*(float)SIN_VALUES*(1.0/PI_2)-0.5)];
  else if (angle <= PI)
    return sin_buf[(int)((PI-angle)*(float)SIN_VALUES*(1.0/PI_2)-0.5)];
  else if (angle <= 3.0*PI_2)
    return -sin_buf[(int)((angle-PI)*(float)SIN_VALUES*(1.0/PI_2)-0.5)];
  else
    return -sin_buf[(int)((2.0*PI-angle)*(float)SIN_VALUES*(1.0/PI_2)-0.5)];
}

float qcos(float angle)
{
  if (angle < 0.0 || angle >= 2.0*PI)
    angle = restrict_angle(angle);

  if (angle <= PI_2)
    return sin_buf[(int)((PI_2-angle)*(float)SIN_VALUES*(1.0/PI_2)-0.5)];
  else if (angle <= PI)
    return -sin_buf[(int)((angle-PI_2)*(float)SIN_VALUES*(1.0/PI_2)-0.5)];
  else if (angle <= 3.0*PI_2)
    return -sin_buf[(int)((3.0*PI_2-angle)*(float)SIN_VALUES*(1.0/PI_2)-0.5)];
  else
    return sin_buf[(int)((angle-3.0*PI_2)*(float)SIN_VALUES*(1.0/PI_2)-0.5)];
}
#endif