///=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
/// MIT License
///
/// Copyright (c) 2020 Mikko Romppainen
/// 
/// Permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to deal
/// in the Software without restriction, including without limitation the rights
/// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
/// copies of the Software, and to permit persons to whom the Software is
/// furnished to do so, subject to the following conditions:
/// 
/// The above copyright notice and this permission notice shall be included in all
/// copies or substantial portions of the Software.
/// 
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
/// SOFTWARE.
///=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=

#include <algorithm>
#include <map>
#include <vector>

int main() {
	// Typedef for position
	typedef std::pair<int, int> Position;

	// Game level
	static const int LEVEL_WIDTH = 13;
	static const int LEVEL_HEIGHT = 9;
	int level[LEVEL_HEIGHT][LEVEL_WIDTH] = {
		{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
		{1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1},
		{1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 1},
		{1, 0, 1, 1, 0, 1, 1, 0, 0, 0, 1, 0, 1},
		{1, 0, 0, 1, 0, 0, 1, 1, 1, 1, 1, 0, 1},
		{1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1},
		{1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 1},
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1},
		{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}
	};/*
	int level[LEVEL_HEIGHT][LEVEL_WIDTH] = {
		{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
		{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
		{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}
	};*/

	// Start and end positions
	Position start(1, 1);
	Position end(LEVEL_WIDTH - 2, LEVEL_HEIGHT - 2);

	// Lambda for printing level
	auto printLevel = [&level]() {
		for (size_t y = 0; y < LEVEL_HEIGHT; ++y) {
			for (size_t x = 0; x < LEVEL_WIDTH; ++x) {
				printf("%d", level[y][x]);
			}
			printf("\n");
		}
	};

	// Lambda for checking is node walkable
	auto isWalkable = [&level](const Position& pos) -> bool {
		if (pos.first < 0 || pos.second < 0 || pos.first >= LEVEL_WIDTH || pos.second >= LEVEL_HEIGHT) return false;
		return !level[pos.second][pos.first];
	};

	// Lambda for getting adjacent nodes
	auto getAdjacentPositions = [&level, isWalkable](const Position& pos) -> std::vector<Position> {
		std::vector<Position> canditates;
		canditates.push_back(Position(pos.first - 1, pos.second));
		canditates.push_back(Position(pos.first + 1, pos.second));
		canditates.push_back(Position(pos.first, pos.second - 1));
		canditates.push_back(Position(pos.first, pos.second + 1));
		std::vector<Position> res;
		for (Position p : canditates) {
			if (isWalkable(p)) res.push_back(p);
		}
		return res;
	};

	printf("Search level:\n");
	printLevel();

	// Struct for search node
	struct Node {
		Node(Position pos, Node* prev) : position(pos), prevNode(prev) {
		}

		Position position;
		Node* prevNode;

		float getG() const {
			// Just compute previous nodes and return cost to travel from start to this node
			float res = 0.0f;
			const Node* scan = this;
			while (scan->prevNode) {
				res += 1.0f;
				scan = scan->prevNode;
			}
			return res;
		}

		float getH(const Position& end) const {
			// For heurestic, use euclidean length of the vector from this node to end position.
			float dx = float(end.first - position.first);
			float dy = float(end.second - position.second);
			return sqrtf(dx*dx + dy * dy);
		}

		float getF(const Position& end) const {
			// F = G + H
			return getG() + getH(end);
		}
	};

	// Open list
	std::vector<Node*> openList;
	std::map<Position, Node*> closedList;

	// A*
	openList.push_back(new Node(start, 0)); // Add start node to open list
	Node* result = 0; // Save possible A* result here
	while (openList.size() > 0) {
		Node* N = openList[0]; // Pop lowest f-cost from open list
		openList.erase(openList.begin(), openList.begin() + 1); // Remove it from open list
		closedList[N->position] = N; // Add it to the closed list

		// Has reached destination?
		if (N->position.first == end.first && N->position.second == end.second) {
			result = N;
			break; // while(openList.size() > 0)
		}

		std::vector<Position> adjacentPositions = getAdjacentPositions(N->position); // Get neighbour nodes, where we can walk

		for (Position pos : adjacentPositions) {
			// If already at the closed list, ignore it
			if (closedList.find(pos) != closedList.end()) {
				continue;
			}

			// If already at open list?
			auto it = std::find_if(openList.begin(), openList.end(), [pos](const Node* n) -> bool {
				return n->position == pos;
			});
			
			Node* N_ = new Node(pos, N); // We came to N_-node from N-node.

			if (it != openList.end()) {
			    // Found from open list, if new F is smaller than prev route to this position, reset prev node from this route
				if ((*it)->getG() > N_->getG()) {
					(*it)->prevNode = N;
				}
				// Delete N_, because we do not need it anymore
				delete N_;
			} else {
				openList.push_back(N_); // Add new node to the open list
			}
		}
		// Sort open list by ascending order by F-cost.
		std::sort(openList.begin(), openList.end(), [&end](const Node* lhs, const Node* rhs) -> bool {
			return lhs->getF(end) < rhs->getF(end);
		});
	}

	if (result != 0) {
		printf("\nPath found:\n");
		while (result != 0) {
			level[result->position.second][result->position.first] = 2; // Mark path to the level
			result = result->prevNode;
		}
		printLevel();
	}
	else {
		printf("\nPath not found!\n");
	}

    // Remember to cleanup. i.e. call delete to each node in open and closed lists.
	for (auto o : openList) {
		delete o;
	}

	for (auto o : closedList) {
		delete o.second;
	}

	printf("\nPress ENTER to continue...\n");
	getchar();
	return 0;
}