// Example of world building, display, and successor computation for the artificial 
// intelligence path-finding lab
//
// Author: Didier LIME
// Adapted by : Jovian HERSEMEULE
// Date: 2018-10-03

#include <iostream>
#include <unistd.h>

using namespace std;

// Default parameters
#define DEFAULT_LENGTH 6
#define DEFAULT_HEIGHT 5
#define DEFAULT_ANIMATION true
#define DEFAULT_ANIMATION_DELAY 50000 // us
#define DEFAULT_COLOR_ENABLE true

// Tile codes
#define FLOOR_TILE 0
#define WALL_TILE 1
#define TARGET_TILE 2
#define HOLE_TILE 3

// Typical cost
#define FLOOR_COST 0.04f
#define HOLE_COST -1.1f
#define TARGET_COST 1.01f

unsigned int identifyTile(unsigned int y, unsigned int x, unsigned int l) {
	return y * l + x;
}

void moveCursorUp(const unsigned int& h) {
	cout << "\033[" << h << 'A';
}

class World
{
	private:
		// Number of columns
		unsigned int l;

		// Number of lines
		unsigned int h;

		// Size of the array
		const unsigned int size;

		// Unidimensional array for tiles
		int* board;

		// Unidimensional array of costs
		float* cost;

		// Number of empty tiles
		unsigned int tileQuantity;

	public:
		// Constructor
		World(unsigned int l_, unsigned int h_)
			:l(l_), h(h_), size(l_ * h_), tileQuantity(l_ * h_)
		{
			board = new int[size]();
			cost = new float[size]();

			// Clean
			for (unsigned int k(0); k < size; k++) {
				board[k] = FLOOR_TILE;
				cost[k] = FLOOR_COST;
			}

			// Add walls to the first and last columns
			for (unsigned int i = 0; i < h; i++)
			{
				board[i * l] = WALL_TILE;
				board[i * l + l - 1] = WALL_TILE;
				tileQuantity -= 2;
			}

			// Add walls to the first and last lines
			for (unsigned int j = 0; j < l; j++)
			{
				board[j] = WALL_TILE;
				board[(h - 1) * l + j] = WALL_TILE;
				tileQuantity -= 2;
			}

			// Add hardcoded elements
			board[identifyTile(2, 2, l)] = WALL_TILE;

			board[identifyTile(1, 4, l)] = TARGET_TILE;
			cost[identifyTile(1, 4, l)] = TARGET_COST;

			board[identifyTile(2, 4, l)] = HOLE_TILE;
			cost[identifyTile(2, 4, l)] = HOLE_COST;
		}

		// Copy constructor
		World(const World& other)
			:l(other.l), h(other.h), size(other.size), tileQuantity(other.tileQuantity)
		{
			board = new int[size]();
			cost = new float[size]();

			for (int k(0); k < size; k++) {
				board[k] = other.board[k];
				cost[k] = other.cost[k];
			}
		}

		// Display the costs
		void displayCosts() {
			int current;
			for (unsigned int i = 0; i < h; i++) {
				for (unsigned int j = 0; j < l; j++) {
					current = identifyTile(i, j, l);
					cout << '|';
					if (board[current] == WALL_TILE)
						cout << "-  -";
					else
						cout << cost[current];
				} 
				cout << '|' << endl;
			}
		}

		// Display the world
		void displayBoard()
		{
			for (unsigned int i = 0; i < h; i++)
			{
				for (unsigned int j = 0; j < l; j++)
				{
					switch (board[identifyTile(i, j, l)])
					{
						case FLOOR_TILE:
							cout << " ";
							break;
						case WALL_TILE:
							if (DEFAULT_COLOR_ENABLE) cout << "\033[0;43m";
							cout << "#";
							break;
						case HOLE_TILE:
							if (DEFAULT_COLOR_ENABLE) cout << "\033[1;35m";
							cout << "X";
							break;
						case TARGET_TILE:
							if (DEFAULT_COLOR_ENABLE) cout << "\033[1;36m";
							cout << "O";
							break;
					}

					if (DEFAULT_COLOR_ENABLE) cout << "\033[0;30m";
				}

				cout << endl;
			}
		}

		// compute the successors of tile number i in world w
		// we return the number n of valid successors
		// the actual list is in array r where only the first n
		// elements are significant
		unsigned int successors(unsigned int i, unsigned int r[4])
		{
			unsigned int n = 0;

			if (i >= 0 && i < size && board[i] != WALL_TILE)
			{
				// if i is a correct tile number (inside the array and not on a wall)
				// look in the four adjacent tiles and keep only those with no wall
				const unsigned int moves[] = { i - 1, i + 1, i - l, i + l};

				for (unsigned int k = 0; k < 4; k++)
				{
					if (board[moves[k]] != WALL_TILE)
					{
						r[n] = moves[k];
						n++;
					}
				}
			}

			return n;
		}

		// Mark a point in the world
		void markOne(unsigned int tile, int value = HOLE_TILE) {
			board[tile] = value;
		}

		void showProperties() {
			cout << "Length : " << l << endl;
			cout << "Height : " << h << endl;
			cout << "Number of floor tiles : " << tileQuantity << endl;
		}

		// Value iteration
		void valueIteration(float gamma, float epsilon) {
			unsigned int counter(0);
			unsigned int buffer[size];
			unsigned int succs[4];
			unsigned int succsCount;

			while (!isConverged(gamma, epsilon)) {
				for (unsigned int k(0); k < size; k ++) {
					succsCount = successors(k, succs);

					// todo
				}
			}
		}
};

int main()
{
	// Create a world
	const unsigned int l(DEFAULT_LENGTH), h(DEFAULT_HEIGHT);

	World w(l, h);

	// Display it
	cout << endl << "Default world" << endl;
	w.showProperties();
	w.displayBoard();
	w.displayCosts();

	const bool animation(DEFAULT_ANIMATION);

	// 1
	cout << endl << "Value iteration" << endl;

	World w1(w);
	w1.displayBoard();
	w1.displayCosts();

	// 2
	cout << endl << "Policy iteration" << endl;

	World w2(w);
	w2.displayBoard();
	w2.displayCosts();

	// End
	return 0;
}