#include <stdlib.h>
#include <limits.h>

#include "heap.h"
#include "distance_map.h"
#include "dungeon.h"

typedef struct dijkstra_path {
  heap_node_t *hn;
  uint8_t pos[2];
  int32_t cost;
} dijkstra_path_t;

static int32_t dijkstra_path_cmp(const void *key, const void *with) {
  return ((dijkstra_path_t *) key)->cost - ((dijkstra_path_t *) with)->cost;
}

/**
 * Dijkstra's Algorithm - Find the shortest paths in the dungeon to the player position
 * hardness_limit - the smallest hardness value that the monsters cannot pass through
 *
 * Algorithm is modified from Dr. Sheaffer's Dijkstra code using his provided heap.
 */
void dijkstra_from_pos(dungeon_t *d, uint32_t dist[MAP_HEIGHT][MAP_WIDTH], int hardness_limit, int start_x, int start_y)
{
	// Initialize the vairables
	static dijkstra_path_t path[MAP_HEIGHT][MAP_WIDTH], *p;
	static uint32_t initialized = 0;
	heap_t h;
	uint32_t x, y;

	// Initialize the positions of each path node
	if (!initialized) {
		for (y = 0; y < MAP_HEIGHT; y++) {
			for (x = 0; x < MAP_WIDTH; x++) {
				path[y][x].pos[0] = y;
				path[y][x].pos[1] = x;
			}
		}
		initialized = 1;
	}

	// Initialize the path cost for each node
	for (y = 0; y < MAP_HEIGHT; y++) {
		for (x = 0; x < MAP_WIDTH; x++) {
			path[y][x].cost = INT_MAX;
		}
	}
	
	// The starting position gets cost 0
	path[start_y][start_x].cost = 0;

	// Initialize the heap
	heap_init(&h, dijkstra_path_cmp, NULL);

	// Initialize all of the heap pointers
	// Also initialize the distance map
	for (y = 0; y < MAP_HEIGHT; y++) {
		for (x = 0; x < MAP_WIDTH; x++) {
      dist[y][x] = INT_MAX;
			if (d->hardness[y][x] < hardness_limit) {
				path[y][x].hn = heap_insert(&h, &path[y][x]);
			} else {
				path[y][x].hn = NULL;
			}
		}
	}

	// Main Dijkstra's Algorithm implementation
	while ((p = heap_remove_min(&h))) {
		// Visit the current node, and set its distance
		p->hn = NULL;
		dist[p->pos[0]][p->pos[1]] = p->cost;
    
    // If this node is at infinity distance, don't visit its neighbors; it is disconnected
    if (p->cost == INT_MAX) {
      continue;
    }
		
		// Iterate through all neighbors
		for (y = p->pos[0] - 1; y < p->pos[0] + 2; y++) {
			for (x = p->pos[1] - 1; x < p->pos[1] + 2; x++) {
				if (y == p->pos[0] && x == p->pos[1]) continue;
				
				if ((path[y][x].hn) && (path[y][x].cost > p->cost + 1 + (d->hardness[p->pos[0]][p->pos[1]] / 85))) {
					path[y][x].cost = p->cost + 1 + (d->hardness[p->pos[0]][p->pos[1]] / 85);

					heap_decrease_key_no_replace(&h, path[y][x].hn);
				}
			}
		}
	}
}