#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) {
  size_t h = closed.size();
  size_t w = closed[0].size();
  vector<size_t> rows{y - 1, y, y + 1};
  vector<size_t> cols{x - 1, x, x + 1};

  for(size_t row : rows) {
    for(size_t col : cols) {
      if((0 <= row && row < h) &&
          (0 <= col && col < w) &&
          closed[row][col] == 0)
      {
        // BUG: maybe value here?
        closed[row][col] = 1;
        neighbors.push_back({.x=col, .y=row});
      }
    }
  }
}

/*
 * Used https://github.com/HenrYxZ/dijkstra-map as a reference.
 */
void Pathing::compute_paths(Matrix &walls) {
  INVARIANT();
  // 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; // BUG: is this right?
    size_t y = counter / $width;
    if($input[y][x] == 0) {
      $paths[y][x] = 0;
      closed[y][x] = 1;  // BUG: value here?
      starting_pixels.push_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) {
  // BUG: not using value here but it can be < 0 for deeper slopes
  $input[at.y][at.x] = 0;
}

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

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;
}