#include "map.hpp"
#include <vector>
#include <fmt/core.h>

using std::vector, std::pair;
using namespace fmt;

void dump_map(const std::string &msg, Matrix &map) {
  println("----------------- {}", msg);
  for(auto row : map) {
    for(auto col : row) {
      print("{} ", col);
    }
    print("\n");
  }
}


void add_neighbors(PairList &neighbors, Matrix &closed, size_t j, size_t i) {
  size_t h = closed.size();
  size_t w = closed[0].size();
  vector<size_t> rows{j - 1, j, j + 1};
  vector<size_t> cols{i - 1, i, i + 1};

  for(auto row : rows) {
    for(auto col : cols) {
      if((0 <= row && row < h) &&
          (0 <= col && col < w) &&
          closed[row][col] == 0)
      {
        closed[row][col] = 1;
        neighbors.push_back({.j=row, .i=col});
      }
    }
  }
}

Matrix dijkstra_map(Matrix &input_map, Matrix &walls_map, int limit) {
  size_t h = input_map.size();
  size_t w = input_map[0].size();

  // Initialize the new array with every pixel at limit distance
  // NOTE: this is normally ones() * limit
  limit = limit == 0 ? h * w : limit;
  Matrix new_arr = Matrix(h, MatrixRow(w, limit));
  Matrix closed = walls_map;
  PairList starting_pixels;
  PairList open_pixels;

  // First pass: Add starting pixels and put them in closed
 for(size_t counter = 0; counter < h * w; counter++) {
    size_t i = counter % w;
    size_t j = counter / w;
    if(input_map[j][i] == 0) {
      new_arr[j][i] = 0;
      closed[j][i] = 1;
      starting_pixels.push_back({.j=j,.i=i});
    }
  }

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

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

  // Last pass: flood last pixels
  for(auto sp : open_pixels) {
    new_arr[sp.j][sp.i] = counter;
  }

  return new_arr;
}