#include "MapUnit.h"
#include "../util/mathUtils.h"

using namespace std;

MapUnit :: MapUnit ()
{
  clear ();
}

MapUnit :: ~MapUnit ()
{
}

void MapUnit :: clear ()
{
  resourceName="NULL_INDEX";
  setMapPosition (0, 0);
  mapMoving=false;
  mapMovingSpeed=30;
  as.clear();
}

void MapUnit :: setMapPosition (uint16_t posX, uint16_t posY)
{
  mapPositionX = posX;
  mapPositionY = posY;
}

uint16_t MapUnit :: getMapPositionX ()
{
  return mapPositionX;
}

uint16_t MapUnit :: getMapPositionY ()
{
  return mapPositionY;
}

void MapUnit :: moveToDestination (uint16_t destX, uint16_t destY, uint32_t startTime)
{
  mapDestinationX=destX;
  mapDestinationY=destY;
  mapStartX=getMapPositionX();
  mapStartY=getMapPositionY();
  mapMovingStartTime=startTime;

  mapMoving=true;

  int distX = destX-mapStartX;
  int distY = destY-mapStartY;

  int temp;

  mapMovingAngle=arctangent(distX, distY, 0);

  //ANYTHING INVOLVING TRIG IS BROKE; I'M USING FIXED INT NOW

  if (distX == 0 && distY == 0)
    {
      mapMovingAngle = 0;
      mapMovingTravelTime = 0;
    }
  else if (!(mapMovingAngle == 64 || mapMovingAngle==192))
    {
      temp=(distX*1000*1000)/(mapMovingSpeed * cosine(mapMovingAngle));

      if (temp < 0)
        mapMovingTravelTime=temp*-1;
      else
        mapMovingTravelTime=temp;
    }
  else
    {
      temp=(distY*1000*1000)/(mapMovingSpeed * sine(mapMovingAngle));

      if (temp < 0)
        mapMovingTravelTime=temp*-1;
      else
        mapMovingTravelTime=temp;
    }
}

void MapUnit :: mapAction (uint32_t currentTime)
{
  if (mapMoving)
    {
      if (currentTime-mapMovingStartTime>mapMovingTravelTime)
        mapMoving=false;
      else
        {
          moveWithTime (currentTime-mapMovingStartTime);
        }
    }
}

uint8_t MapUnit :: backpedalHitRectangle (int leftSide, int rightSide, int topSide, int bottomSide, uint32_t currentTime, bool lockAngle)
{
  //This is called when the MapUnit is inside of something (a wall most likely) it needs to back out of.
  //The plan is to reverse direction and move until MapUnit is just outside of the box.
  //If lockAngle is true, the direction is not reversed.
  //returns 0 if reversing sends it somewhere illegal (off the board is most likely)
  //returns 1 if it retreats out of range.
  //returns 2 if not inside the given hitbox.

  //First of all, is MapUnit actually inside Side?
  if (!pointRectangleCollision (getMapPositionX(), getMapPositionY(), leftSide, rightSide, topSide, bottomSide))
    return 2; //we're not inside, go home.

  bool done = false;
  uint32_t travelTime = 0;
  uint16_t tempPosX, tempPosY;
  //uint32_t timeToRetreatTenPixels=10*1000/mapMovingSpeed;
  uint32_t maxRetreatTime = 25000;

  if (!lockAngle)
    mapMovingAngle+=128; //turn around unless lockAngle is true.

  //resets the movement to this position.
  stopMoving(currentTime);

  while (!done)
    {
      calculateMoveWithTime (tempPosX, tempPosY, travelTime);

      //if you're outside the box, you're done.
      if (!pointRectangleCollision (tempPosX, tempPosY, leftSide, rightSide, topSide, bottomSide))
        done = true;
      else
        {
          //if not, increase the time spent traveling and try again.
          travelTime ++;
          //maybe put in a cutoff time here?
          if (travelTime > maxRetreatTime)
            done=true;
        }

    }

  if (travelTime > maxRetreatTime)
    return 0;

  setMapPosition(tempPosX, tempPosY);
  stopMoving (currentTime);

  return 1;
}

void MapUnit :: moveWithTime (uint32_t travelTime)
{
  uint16_t posX, posY;
  calculateMoveWithTime (posX, posY, travelTime);
  setMapPosition (posX, posY);
}

void MapUnit :: calculateMoveWithTime (uint16_t & posX, uint16_t & posY, uint32_t travelTime)
{
  posX = uint16_t(mapStartX + (int(mapMovingSpeed) * int(cosine(mapMovingAngle)) * int(travelTime))/(1000 * 1000));
  posY = uint16_t(mapStartY + (int(mapMovingSpeed) * int(sine(mapMovingAngle)) * int(travelTime))/(1000 * 1000));
}

void MapUnit :: stopMoving (uint32_t currentTime)
{
  //MapUnit stops moving at its current position. If you try to move it again it will start from its current position.
  mapStartX=getMapPositionX();
  mapStartY=getMapPositionY();
  mapMovingTravelTime=0;
  mapMovingStartTime=currentTime;
  mapMoving=false;
}

bool MapUnit::operator==(const MapUnit &other) const {
  // Compare the values, and return a bool result.
  if ((mapPositionY == other.mapPositionY) &&
      (mapPositionX == other.mapPositionX) &&
      (mapDestinationY == other.mapDestinationY) &&
      (mapDestinationX == other.mapDestinationX) &&
      (mapStartX == other.mapStartX) &&
      (mapStartY == other.mapStartY) &&
      (mapMovingStartTime == other.mapMovingStartTime) &&
      (mapMovingTravelTime == other.mapMovingTravelTime) &&
      (mapMoving == other.mapMoving) &&
      (mapMovingAngle == other.mapMovingAngle) &&
      (mapMovingSpeed == other.mapMovingSpeed))
    return true;
  return false;
}

const string & MapUnit::getResourceName()
{
  return resourceName;
}

void MapUnit::setResourceName(const string & nam)
{
  resourceName = nam;
}

//You can modify the resource name.
string & MapUnit::getModifiableResourceName()
{
  return resourceName;
}

AnimatedState & MapUnit :: getAnimatedState ()
{
  return as;
}
