int moveCostG(int x, int y, int dx, int dy) {
	int g = (abs(dx - x) + abs(dy - y));
	return g;
}

int moveCostH(int dx, int dy, int nx, int ny){
	int h = (abs(nx - dx) + abs(ny - dy));
	return h;
}

int moveCostF(int x, int y, int dx, int dy, int nx, int ny) {
	return (moveCostG(x, y, dx, dy) + moveCostH(dx, dy, nx, ny));
}

bool searchList(std::vector<coordList> n_list, int search_term_x, int search_term_y) {
	int size = sizeof(n_list)/sizeof(coordList);
	for (i = 0; i < size; i++) {
		if ((n_list[i].x == search_term_x) && (n_list[i].y == search_term_y)){
			return true;
		}
	}
	return false;
}

void pathfinding(int x, int y, int nx, int ny) {
	int size;
	int dx = x;
	int dy = y;
	bool found = false;
  int boolMap[MAX_X][MAX_Y];
  for (i = 0; i < MAX_X; i++) {
    for (j = 0; j < MAX_Y; j++) {
      if (!map[i][j].layer1.passable || !map[i][j].layer2.passable) {
        boolMap[i][j] = 1;
      }
      else {
        boolMap[i][j] = 0;
      }
    }
  }
	/*
  for (i = 0; i < MAX_X; i++) {
    for (j = 0; j < MAX_Y; j++) {
      printf("%d", boolMap[i][j]);
    }
    printf("\n");
  }
	*/




	//Creating lists
	std::vector<coordList> openList;
	std::vector<coordList> closedList;

	//Adding starting location to closed list
	coordList current;
	current.x = x;
	current.y = y;
	current.f = moveCostF(x, y, dx, dy, nx, ny);
	openList.push_back(current);
	closedList.push_back(current);
	map[current.x][current.y].layer1.colour = TCODColor::purple;

	//Adding next doors
	current.x = dx + 1;
	current.y = dy;
	current.f = moveCostF(x, y, dx + 1, dy, nx, ny);
	if (boolMap[current.x][current.y] == 0){
		if (searchList(closedList, current.x, current.y) == true) {
			//found in closed -- ignore it
		}
		else if (searchList(openList, current.x, current.y) == false){
			openList.push_back(current);
			map[current.x][current.y].layer1.colour = TCODColor::red;
		}
		else {
			//If T is already in the open list: Check if the F score
			//is lower when we use the current generated path to get there.
			//If it is, update its score and update its parent as well.
			//????
			//openList.push_back(current);
		}
	}

	current.x = dx - 1;
	current.y = dy;
	current.f = moveCostF(x, y, dx - 1, dy, nx, ny);

	if (boolMap[current.x][current.y] == 0){
		if (searchList(closedList, current.x, current.y) == true) {
			//found in closed -- ignore it
		}
		else if (searchList(openList, current.x, current.y) == false){
			openList.push_back(current);
			map[current.x][current.y].layer1.colour = TCODColor::red;
		}
		else {
			//If T is already in the open list: Check if the F score
			//is lower when we use the current generated path to get there.
			//If it is, update its score and update its parent as well.
			//????
			//openList.push_back(current);
		}
	}

	current.x = dx;
	current.y = dy + 1;
	current.f = moveCostF(x, y, dx, dy + 1, nx, ny);

	if (boolMap[current.x][current.y] == 0){
		if (searchList(closedList, current.x, current.y) == true) {
			//found in closed -- ignore it
		}
		else if (searchList(openList, current.x, current.y) == false){
			openList.push_back(current);
			map[current.x][current.y].layer1.colour = TCODColor::red;
		}
		else {
			//If T is already in the open list: Check if the F score
			//is lower when we use the current generated path to get there.
			//If it is, update its score and update its parent as well.
			//????
			//openList.push_back(current);
		}
	}

	current.x = dx;
	current.y = dy - 1;
	current.f = moveCostF(x, y, dx, dy - 1, nx, ny);

	if (boolMap[current.x][current.y] == 0){
		if (searchList(closedList, current.x, current.y) == true) {
			//found in closed -- ignore it
		}
		else if (searchList(openList, current.x, current.y) == false){
			openList.push_back(current);
			map[current.x][current.y].layer1.colour = TCODColor::red;
		}
		else {
			//If T is already in the open list: Check if the F score
			//is lower when we use the current generated path to get there.
			//If it is, update its score and update its parent as well.
			//????
			//openList.push_back(current);
		}
	}
	printf("Assigning initial variables");
	getchar();
	DisplayMapTest();
	openList.erase(openList.begin());

	//--- NOW ALL INITIAL SQUARES HAVE BEEN CALCULATED
	int counter = 0;
	bool first_time = true;
	//While?
	while (found == false) {
		if (first_time == false) {

			//Add new walkable tiles
			current.x = dx + 1;
			current.y = dy;
			current.f = moveCostF(x, y, dx + 1, dy, nx, ny);
			if (boolMap[current.x][current.y] == 0){
				if (searchList(closedList, current.x, current.y) == true) {
					//found in closed -- ignore it
				}
				else if (searchList(openList, current.x, current.y) == false){
					openList.push_back(current);
					map[current.x][current.y].layer1.colour = TCODColor::red;
				}
				else {
					//If T is already in the open list: Check if the F score
					//is lower when we use the current generated path to get there.
					//If it is, update its score and update its parent as well.
					//????
					//openList.push_back(current);
				}
			}

			current.x = dx - 1;
			current.y = dy;
			current.f = moveCostF(x, y, dx - 1, dy, nx, ny);

			if (boolMap[current.x][current.y] == 0){
				if (searchList(closedList, current.x, current.y) == true) {
					//found in closed -- ignore it
				}
				else if (searchList(openList, current.x, current.y) == false){
					openList.push_back(current);
					map[current.x][current.y].layer1.colour = TCODColor::red;
				}
				else {
					//If T is already in the open list: Check if the F score
					//is lower when we use the current generated path to get there.
					//If it is, update its score and update its parent as well.
					//????
					//openList.push_back(current);
				}
			}

			current.x = dx;
			current.y = dy + 1;
			current.f = moveCostF(x, y, dx, dy + 1, nx, ny);

			if (boolMap[current.x][current.y] == 0){
				if (searchList(closedList, current.x, current.y) == true) {
					//found in closed -- ignore it
				}
				else if (searchList(openList, current.x, current.y) == false){
					openList.push_back(current);
					map[current.x][current.y].layer1.colour = TCODColor::red;
				}
				else {
					//If T is already in the open list: Check if the F score
					//is lower when we use the current generated path to get there.
					//If it is, update its score and update its parent as well.
					//????
					//openList.push_back(current);
				}
			}

			current.x = dx;
			current.y = dy - 1;
			current.f = moveCostF(x, y, dx, dy - 1, nx, ny);

			if (boolMap[current.x][current.y] == 0){
				if (searchList(closedList, current.x, current.y) == true) {
					//found in closed -- ignore it
				}
				else if (searchList(openList, current.x, current.y) == false){
					openList.push_back(current);
					map[current.x][current.y].layer1.colour = TCODColor::red;
				}
				else {
					//If T is already in the open list: Check if the F score
					//is lower when we use the current generated path to get there.
					//If it is, update its score and update its parent as well.
					//????
					//openList.push_back(current);
				}
			}

		}
		printf("Adding new to open list");
		getchar();
		DisplayMapTest();
		first_time = false;
		size = sizeof(openList)/sizeof(coordList);
		printf("Current X, D, N\n");
		printf("X: %d, Y: %d\n", x, y);
		printf("DX: %d, DY: %d\n", dx, dy);
		printf("NX: %d, NY: %d\n\n", nx, ny);
		int lowest = 999999999;
		int lowest_place;
		printf("Open list tiles and scores (size: %d)\n", size);
		for (i = 0; i < size + 1; i++) {
			if ((openList[i].f < lowest) && ((abs(openList[i].x - dx) == 1) || (abs(openList[i].y - dy) == 1))){
				lowest = openList[i].f;
				lowest_place = i;
			}
			printf("X:%d Y:%d: F:%d\n", openList[i].x, openList[i].y, openList[i].f);
		}
		printf("\nLowest score\n");
		printf("X:%d Y:%d: F:%d\n", openList[lowest_place].x, openList[lowest_place].y, openList[lowest_place].f);
		closedList.push_back(openList[lowest_place]);
		map[openList[lowest_place].x][openList[lowest_place].y].layer1.colour = TCODColor::purple;
		dx = openList[lowest_place].x;
		dy = openList[lowest_place].y;
		if ((dx == nx) && (dy == ny)) {
			printf("FOUND IT!");
			found = true;
			break;
		}
		openList.erase(openList.begin() + lowest_place);
		printf("Removed lowest one and added to closed list ");
		getchar();
		DisplayMapTest();

		printf("END");
		getchar();
		DisplayMapTest();

		counter++;
		if (counter == 10) {
			break;
		}
	}

}