#include "constants.hpp"
#include "pathing.hpp"
#include "dbc.hpp"
#include <vector>

using std::vector;

inline void add_neighbors(PointList &neighbors, Matrix &closed, size_t y, size_t x) {
  for(matrix::box it{closed, x, y, 1}; it.next();) {
    if(closed[it.y][it.x] == 0) {
      closed[it.y][it.x] = 1;
      neighbors.emplace_back(it.x, it.y);
    }
  }
}

/*
 * Used https://github.com/HenrYxZ/dijkstra-map as a reference.
 */
void Pathing::compute_paths(Matrix &walls) {
  INVARIANT();
  dbc::check(walls[0].size() == $width,
      fmt::format("Pathing::compute_paths called with walls.width={} but paths $width={}", walls[0].size(), $width));

  dbc::check(walls.size() == $height,
      fmt::format("Pathing::compute_paths called with walls.height={} but paths $height={}", walls[0].size(), $height));

  // Initialize the new array with every pixel at limit distance
  matrix::assign($paths, WALL_PATH_LIMIT);

  Matrix closed = walls;
  PointList starting_pixels;
  PointList open_pixels;

  // First pass: Add starting pixels and put them in closed
  for(size_t counter = 0; counter < $height * $width; counter++) {
    size_t x = counter % $width;
    size_t y = counter / $width;
    if($input[y][x] == 0) {
      $paths[y][x] = 0;
      closed[y][x] = 1;
      starting_pixels.emplace_back(x,y);
    }
  }

  // Second pass: Add border to open
  for(auto sp : starting_pixels) {
    add_neighbors(open_pixels, closed, sp.y, sp.x);
  }

  // Third pass: Iterate filling in the open list
  int counter = 1; // leave this here so it's available below
  for(; counter < WALL_PATH_LIMIT && !open_pixels.empty(); ++counter) {
    PointList next_open;
    for(auto sp : open_pixels) {
      $paths[sp.y][sp.x] = counter;
      add_neighbors(next_open, closed, sp.y, sp.x);
    }
    open_pixels = next_open;
  }

  // Last pass: flood last pixels
  for(auto sp : open_pixels) {
    $paths[sp.y][sp.x] = counter;
  }
}

void Pathing::set_target(const Point &at, int value) {
  // FUTURE: I'll eventually allow setting this to negatives for priority
  $input[at.y][at.x] = value;
}

void Pathing::clear_target(const Point &at) {
  $input[at.y][at.x] = 1;
}

bool Pathing::random_walk(Point &out, bool random, int direction) {
  bool zero_found = false;

  // just make a list of the four directions
  std::array<Point, 4> dirs{{
      {out.x,out.y-1}, // north
      {out.x+1,out.y}, // east
      {out.x,out.y+1}, // south
      {out.x-1,out.y} // west
  }};

  // get the current dijkstra number
  int cur = $paths[out.y][out.x];

  // pick a random start of directions
  // BUG: is uniform inclusive of the dir.size()?
  int rand_start = Random::uniform<int>(0, dirs.size());

  // go through all possible directions
  for(size_t i = 0; i < dirs.size(); i++) {
    // but start at the random start, effectively randomizing
    // which valid direction to go
    // BUG: this might be wrong given the above ranom from 0-size
    Point dir = dirs[(i + rand_start) % dirs.size()];
    if(!shiterator::inbounds($paths, dir.x, dir.y)) continue; //skip unpathable stuff
    int weight = cur - $paths[dir.y][dir.x];

    if(weight == direction) {
      // no matter what we follow direct paths
      out = dir;
      return true;
    } else if(random && weight == 0) {
      // if random is selected and it's a 0 path take it
      out = dir;
      return true;
    } else if(weight == 0) {
      // otherwise keep the last zero path for after
      out = dir;
      zero_found = true;
    }
  }

  // if we reach this then either zero was found and
  // zero_found is set true, or it wasn't and nothing found
  return zero_found;
}

bool Pathing::INVARIANT() {
  using dbc::check;

  check($paths.size() == $height, "paths wrong height");
  check($paths[0].size() == $width, "paths wrong width");
  check($input.size() == $height, "input wrong height");
  check($input[0].size() == $width, "input wrong width");

  return true;
}