#include "behaviour_system.h"
#include <queue>
#include <vector>
#include <utility>
#include <random>
#include "game.h"
#include "character_system.h"
#include "direction.h"
#include "transform_system.h"
#include "animation_system.h"

bool pathfindingOptionsContains(PathfindingOptions options, PathfindingOptions value) {
  return (static_cast<int>(options) & static_cast<int>(value)) != 0;
}

void BehaviourSystem::tick(double dt) {
  if (game_.isGameplayPaused()) return;

  timer_.accumulate(dt);
  while (timer_.step()) {
    for (int spriteId : game_.getSprites()) {
      Sprite& sprite = game_.getSprite(spriteId);
      if (!sprite.paused) {
        if (sprite.behaviourType == BehaviourType::Wander) {
          // 75% chance of changing what's happening
          if (std::bernoulli_distribution(0.75)(game_.getRng())) {
            // 50% chance of choosing a direction or stopping
            if (std::bernoulli_distribution(0.5)(game_.getRng())) {
              Direction dir;
              switch (std::uniform_int_distribution(0,3)(game_.getRng())) {
                case 0: dir = Direction::left; break;
                case 1: dir = Direction::up; break;
                case 2: dir = Direction::right; break;
                default: dir = Direction::down; break;
              }
              game_.getSystem<CharacterSystem>().moveInDirection(spriteId, dir);
            } else {
              game_.getSystem<CharacterSystem>().stopDirecting(spriteId);
            }
          }
        } else if (sprite.behaviourType == BehaviourType::Follow) {
          Sprite& target = game_.getSprite(sprite.followSpriteId);
          game_.getSystem<CharacterSystem>().moveInDirection(spriteId, directionFacingPoint(target.loc - sprite.loc));
        }
      }
    }
  }

  overcorrectionTimer_.accumulate(dt);
  while (overcorrectionTimer_.step()) {
    for (int spriteId : game_.getSprites()) {
      Sprite& sprite = game_.getSprite(spriteId);
      if (!sprite.paused &&
          sprite.behaviourType == BehaviourType::Path &&
          !sprite.path.empty() &&
          sprite.loc != sprite.path.front().endpoint) {
        if (directionFacingPoint(sprite.path.front().endpoint - sprite.loc) != sprite.path.front().dir) {
          game_.getSystem<TransformSystem>().moveSprite(spriteId, sprite.path.front().endpoint);
        }
      }
    }
  }

  for (int spriteId : game_.getSprites()) {
    Sprite& sprite = game_.getSprite(spriteId);
    if (!sprite.paused && sprite.behaviourType == BehaviourType::Path) {
      while (!sprite.path.empty() && sprite.path.front().endpoint == sprite.loc) {
        sprite.path.pop_front();
      }
      if (sprite.path.empty()) {
        game_.getSystem<CharacterSystem>().stopDirecting(spriteId);
      } else {
        if (sprite.characterState == CharacterState::Still || sprite.movementDir != sprite.path.front().dir) {
          game_.getSystem<CharacterSystem>().moveInDirection(spriteId, sprite.path.front().dir);
          if (sprite.moonwalking) {
            game_.getSystem<AnimationSystem>().setSpriteDirection(spriteId, oppositeDirection(sprite.path.front().dir));
          }
        }
      }
    }
  }
}

void BehaviourSystem::directSpriteToLocation(int spriteId, vec2i pos, PathfindingOptions options) {
  Sprite& sprite = game_.getSprite(spriteId);
  sprite.orientable = true;
  sprite.behaviourType = BehaviourType::Path;
  sprite.pathfindingDestination = pos;
  sprite.cardinalDirectionsOnly = pathfindingOptionsContains(options, PathfindingOptions::CardinalDirectionsOnly);
  sprite.moonwalking = pathfindingOptionsContains(options, PathfindingOptions::Moonwalking);

  createPath(spriteId);
}

bool BehaviourSystem::isFollowingPath(int spriteId) {
  Sprite& sprite = game_.getSprite(spriteId);
  return sprite.behaviourType == BehaviourType::Path && !sprite.path.empty();
}

struct PathNodeInfo {
  double cheapestPathCost = DBL_MAX;
  double estimatedRemainingCost = DBL_MAX;
  vec2i previousPoint;
  Direction dirFromPreviousPoint;
};

struct SearchNode {
  vec2i point;
  Direction dirFromPreviousPoint;
  double estimatedCost = DBL_MAX;
  int tiebreaker; // this actually counts downward. wow

  SearchNode(vec2i point, Direction dirFromPreviousPoint, double estimatedCost) : point(point), dirFromPreviousPoint(dirFromPreviousPoint), estimatedCost(estimatedCost) {
    static int tiebreakerCounter = 0;
    tiebreaker = tiebreakerCounter--;
  }

  bool operator>(const SearchNode& rhs) const {
    return std::tie(estimatedCost, tiebreaker) > std::tie(rhs.estimatedCost, rhs.tiebreaker);
  }
};

double estimateRemainingCost(vec2i current, vec2i dest, int movementSpeed, bool cardinalDirectionsOnly) {
  vec2i difference = dest - current;
  if (cardinalDirectionsOnly) {
    return (std::abs(difference.x()) + std::abs(difference.y())) / movementSpeed;
  } else {
    int dx = std::abs(difference.x()) / movementSpeed;
    int dy = std::abs(difference.y()) / movementSpeed;
    return (dx + dy) + (1.4 - 2) * std::min(dx, dy);
  }
}

void BehaviourSystem::createPath(int spriteId) {
  Sprite& sprite = game_.getSprite(spriteId);
  sprite.path.clear();

  const Map& map = game_.getMap();
  vec2i mapBounds = map.getMapSize() * map.getTileSize();

  // If it is not possible to reach the destination because of the parity (if
  // the movement speed is above 1), then adjust the destination.
  if (sprite.movementSpeed > 1) {
    if ((sprite.loc.x() % sprite.movementSpeed) != (sprite.pathfindingDestination.x() % sprite.movementSpeed)) {
      sprite.loc.x() = (sprite.loc.x() / sprite.movementSpeed) * sprite.movementSpeed + (sprite.pathfindingDestination.x() % sprite.movementSpeed);
    }
    if ((sprite.loc.y() % sprite.movementSpeed) != (sprite.pathfindingDestination.y() % sprite.movementSpeed)) {
      sprite.loc.y() = (sprite.loc.y() / sprite.movementSpeed) * sprite.movementSpeed + (sprite.pathfindingDestination.y() % sprite.movementSpeed);
    }
  }

  double initialCostGuess = estimateRemainingCost(sprite.loc, sprite.pathfindingDestination, sprite.movementSpeed, sprite.cardinalDirectionsOnly);

  std::map<vec2i, std::map<Direction, PathNodeInfo>> pathNodes;
  pathNodes[sprite.loc][sprite.dir] = PathNodeInfo{.cheapestPathCost = 0, .estimatedRemainingCost = initialCostGuess};

  std::priority_queue<SearchNode, std::vector<SearchNode>, std::greater<SearchNode>> openSet;
  openSet.emplace(sprite.loc, sprite.dir, initialCostGuess);

  while (!openSet.empty()) {
    SearchNode searchNode = openSet.top();
    openSet.pop();

    if (searchNode.point == sprite.pathfindingDestination) {
      // We're there!
      break;
    }

    PathNodeInfo& nodeInfo = pathNodes[searchNode.point][searchNode.dirFromPreviousPoint];

    static const std::vector<Direction> allDirections = {
      Direction::down,
      Direction::down_left,
      Direction::left,
      Direction::up_left,
      Direction::up,
      Direction::up_right,
      Direction::right,
      Direction::down_right };

    static const std::vector<Direction> cardinalDirections = {
      Direction::down,
      Direction::left,
      Direction::up,
      Direction::right };

    std::vector<Direction> directionList = sprite.cardinalDirectionsOnly ? cardinalDirections : allDirections;
    for (Direction dir : directionList) {
      double newCost = nodeInfo.cheapestPathCost;
      if (dir != searchNode.dirFromPreviousPoint) {
        newCost += 0.1;
      }
      if (isCardinalDirection(dir)) {
        newCost += 1;
      } else {
        newCost += 1.4;
      }

      vec2i newPos = searchNode.point + unitVecInDirection(dir) * sprite.movementSpeed;

      if (newPos.x() < 0 || newPos.y() < 0 || newPos.x() >= mapBounds.w() || newPos.y() >= mapBounds.h()) {
        // The path can't go outside the map.
        continue;
      }

      PathNodeInfo& neighborInfo = pathNodes[newPos][dir];

      if (neighborInfo.cheapestPathCost <= newCost) {
        // There is already a faster path through this neighbor.
        continue;
      }

      CollisionResult collision = game_.getSystem<TransformSystem>().checkCollision(spriteId, searchNode.point, newPos, dir);
      if (collision.blocked) {
        // There isn't actually an edge to this neighbor.
        continue;
      }

      neighborInfo.cheapestPathCost = newCost;
      neighborInfo.estimatedRemainingCost = estimateRemainingCost(newPos, sprite.pathfindingDestination, sprite.movementSpeed, sprite.cardinalDirectionsOnly);
      neighborInfo.previousPoint = searchNode.point;
      neighborInfo.dirFromPreviousPoint = dir;

      openSet.emplace(newPos, dir, neighborInfo.cheapestPathCost + neighborInfo.estimatedRemainingCost);
    }
  }

  if (!pathNodes.count(sprite.pathfindingDestination)) {
    // There was no path to the destination.
    return;
  }

  vec2i curPos = sprite.pathfindingDestination;
  while (curPos != sprite.loc) {
    const PathNodeInfo* nodeInfo = nullptr;
    for (const auto& pni : pathNodes[curPos]) {
      if (nodeInfo == nullptr || pni.second.cheapestPathCost < nodeInfo->cheapestPathCost) {
        nodeInfo = &pni.second;
      }
    }
    if (sprite.path.empty() || sprite.path.front().dir != nodeInfo->dirFromPreviousPoint) {
      sprite.path.push_front(PathfindingInstruction{.dir = nodeInfo->dirFromPreviousPoint, .endpoint = curPos});
    }
    curPos = nodeInfo->previousPoint;
  }
}