#include "mathUtils.h"

bool initializeAngles ()
{
  //populates all lookup tables with values.

  //OK, cycle from 0 to 256. Convert to radians.
  uint16_t bd;

  //sine
  for (bd = 0; bd < 256; bd++)
    {
      sineAngles.push_back(sin(byteDegreesToRadians(bd)));
    }
  //filling in special values
  sineAngles[0]=0;
  sineAngles[64]=1;
  sineAngles[128]=0;
  sineAngles[192]=-1;

  //cosine
  for (bd = 0; bd < 256; bd++)
    {
      cosineAngles.push_back(cos(byteDegreesToRadians(bd)));
    }
  //filling in special values
  cosineAngles[0]=1;
  cosineAngles[64]=0;
  cosineAngles[128]=-1;
  cosineAngles[192]=0;

  for (bd = 0; bd < 256; bd++)
    {
      tangentAngles.push_back(tan(byteDegreesToRadians(bd)));
    }
  //filling in special values
  tangentAngles[0]=0;
  tangentAngles[32]=1;
  tangentAngles[64]=0xFFFF;
  tangentAngles[96]=-1;
  tangentAngles[128]=0;
  tangentAngles[192]=-1 * 0xFFFF;
  tangentAngles[224]=-1;

  return true;
}

double byteDegreesToRadians (uint8_t bds)
{
  return double(bds) * NUMBER_PI / 128.0;
}

uint8_t radiansToByteDegrees (double radians)
{
  //note: I'm abusing integer casting here so it takes care of negative numbers for me.

  double temp = radians * 128.0 / NUMBER_PI;
  double frac, integ;

  frac = modf(temp, &integ);

  if (frac > 0.5)
    return uint8_t(temp + 1.0);

  return uint8_t(temp);
}

float byteDegreesToDegrees(uint8_t bt)
{
  return (bt * 360.0 / 256.0);
}

FixedInt sine (uint8_t bds)
{
  return sineAngles[bds];
}

FixedInt cosine (uint8_t bds)
{
  return cosineAngles[bds];
}

FixedInt tangent (uint8_t bds)
{
  return tangentAngles[bds];
}

uint8_t arctangent (int run, int rise, uint8_t defaultAngle)
{
  //calculates the arctangent of the rise/run, in byte degrees (out of 256)
  //This uses cmath's atan2 function and is thus slower.

  //if distance is 0, angle is undefined. Send a default angle to return in case of error.
  if (run == 0 && rise == 0)
    return defaultAngle;

  //So arctan doesn't work if the angle is 64 or 192. Check if X is 0 and send 64/192 based on Y direction.
  if (run == 0 && rise > 0)
    return 64;
  else if (run == 0)
    return 192;

  if (rise == 0 && run > 0)
    return 0;
  else if (rise == 0 && run < 0)
    return 128;

  //OK now do arctan. atan2 figures out what quadrant it is in as well, w00t.
  double temp = atan2 (double(rise), double(run));

  //temp is in radians, convert to byte degrees
  return radiansToByteDegrees(temp);
}

bool pointRectangleCollision (int pointX, int pointY, int leftSide, int rightSide, int topSide, int bottomSide)
{
  //returns true if Point is within Side.
  if (pointX < leftSide)
    return false;

  if (pointX > rightSide)
    return false;

  if (pointY < topSide)
    return false;

  if (pointY > bottomSide)
    return false;

  return true;
}

//returns true if point is within the circle.
bool pointCircleCollision (int pointX, int pointY, int centerX, int centerY, int radius)
{
  int dist = ((pointX - centerX) * (pointX - centerX)) + ((pointY - centerY) * (pointY - centerY));
  if (dist > radius * radius)
    return false;

  return true;
}

int intAbs (int a)
{
  if (a < 0)
    a *= -1;

  return a;
}

/*
 *      Square root by abacus algorithm, Martin Guy @ UKC, June 1985.
 *      From a book on programming abaci by Mr C. Woo.
 *      Argument is a positive integer, as is result.
 *
 *      I have formally proved that on exit:
 *                 2               2               2
 *              res  <= x < (res+1)     and     res  + op == x
 *
 *      This is also nine times faster than the library routine (-lm).

http://freaknet.org/martin/tape/gos/misc/personal/msc/sqrt/sqrt.html
 */

//This algorithm is based on Martin Guy's code.
int intSqrt (int x)
{
  if (x <= 0)
    return 0;

  int result=0, operand=x, digitToTest=1 << 30;

  //we want to shrink digitToTest until it's 4 bits less than the argument's bits.
  //If it's any bigger there is NO WAY it can be the square root.
  while (digitToTest > operand)
    digitToTest >>= 2;

  while (digitToTest != 0) //stop when the bit is too small.
    {
      //When searching digit by digit, you want to find e, where (r + e) ^ 2 <= x.
      //In binary, e is two bits, or 00 01 10 11.
      //r^2 + 2re + e^2 <= x. e^2 ranges from 0-1001
      //r^2 is what we're looking for.
      //2re is easily handled with bit shifts.

      if (operand >= result + digitToTest)
        {
          operand = operand - (result + digitToTest);
          result = result + (2 * digitToTest);
        }

      result /= 2; //hmm not sure why this is here.
      digitToTest /= 4; //onto the next digit to test.
    }

  return result;
}

void seedRandomization ()
{
  sge_Randomize();
}

int generateRandomNumber (int max)
{
  return sge_Random (0, max);
}

int generateRandomNumber (int min, int max)
{
  return sge_Random (min, max);
}

bool pointAnyRectangleCollision (int ax, int ay, int bx, int by, int cx, int cy, int radius)
{
  FixedInt corigx, corigy;
  AffineTransformationMatrix projectionMatrix;
  int abQuad;

  pointAnyRectangleCollisionHelper2 (ax, ay, bx, by, projectionMatrix, abQuad);
  return pointAnyRectangleCollisionHelper1 (corigx, corigy, projectionMatrix, radius, ax, ay, bx, by, abQuad);
}

//returns true if the point located at (corigx, corigy) is within the rectangle defined by radius distance to AB (ignoring the semi circles past point a and point b.)
//AffineTransformationMatrix atm will transfer point corig onto the line AB
//abQuad is the quadrant the line AB points at.
bool pointAnyRectangleCollisionHelper1 (FixedInt & corigx, FixedInt & corigy, AffineTransformationMatrix & atm, int radius, int ax, int ay, int bx, int by, int abQuad)
{
  FixedInt cx, cy, ccorigx, ccorigy, acx, acy, cbx, cby;
  int acQuad, cbQuad;

  //first, project the mouse pointer onto AB.
  cx=corigx;cy=corigy;
  atm.apply(cx, cy);
  
  //if the projected mouse is too far away, fail
  ccorigx=corigx-cx;ccorigy=corigy-cy;

  if (radius*radius<(ccorigx*ccorigx + ccorigy*ccorigy).toInt())
    return false;
	  
  //Calculate AC CB and get the quadrant they point in. Fail if they don't agree with AB's quadrant.
  acx=FixedInt(cx-ax);acy=FixedInt(cy-ay);
  if (acx>=0 && acy>=0)
    acQuad=0;
  else if (acx<0 && acy>=0)
    acQuad=1;
  else if (acx<0 && acy<0)
    acQuad=2;
  else if (acx>=0 && acy<0)
    acQuad=3;

  cbx=FixedInt(bx)-cx;cby=FixedInt(by)-cy;
  if (cbx>=0 && cby>=0)
    cbQuad=0;
  else if (cbx<0 && cby>=0)
    cbQuad=1;
  else if (cbx<0 && cby<0)
    cbQuad=2;
  else if (cbx>=0 && cby<0)
    cbQuad=3;

  //If they don't agree, fail
  if (abQuad!=cbQuad||abQuad!=acQuad)
    return false;
  
  //This mouse point is close enough and within the boundaries of the segment. The line is still valid.
  return true;
}

//sets up variables
void pointAnyRectangleCollisionHelper2 (int ax, int ay, int bx, int by, AffineTransformationMatrix & projectionMatrix, int & abQuad)
{
  //Now, check the segment between them.
  FixedInt abx, aby;

  abx=FixedInt(bx-ax);aby=FixedInt(by-ay);
  if (abx>=0 && aby>=0)
    abQuad=0;
  else if (abx<0 && aby>=0)
    abQuad=1;
  else if (abx<0 && aby<0)
    abQuad=2;
  else if (abx>=0 && aby<0)
    abQuad=3;

  //Preparing a projection matrix that will map a mousepoint onto the line segment from A to B.
  //We need to shift AB onto the origin first, then project, then shift back.
  projectionMatrix.shift(FixedInt(-1 * ax), FixedInt(-1 * ay));
  projectionMatrix.orthographicProjection(FixedInt(abx), FixedInt(aby));
  projectionMatrix.shift(FixedInt(ax), FixedInt(ay));
}
