/*
	HPA v0.1
	Purpose: A test implementation of path-finding algorithms. So far, basic A* and annotated A* (AA*) have
	been implemented. This allows us to path find, as well as path find paths for units of differing size. The
	demo has a unit of size 1 and a larget unit of size 2. The next algorithms to be implemented are Hierarchical
	A*, which pre-processes a number of paths between larger super-nodes, and does an A* on this smaller set of nodes.
	Author:	Ian Holowka
	Notes:	Parts of the code may need optimization (especially how the data is stored in the A*). These parts have
	been marked. There may also be some code which could be made neater, as this program was designed to focus on
	getting an A* test running.
*/

#include "Enums.h"
#include "Game.h"
#include <stdlib.h>
#include <vector>
#include <queue>
#include <sstream>
using namespace std;

// Singleton pattern
Finder* Finder::instance = 0;

Finder* Finder::Instance() {
	if(instance == 0) {
		instance = new Finder();
	}
	return instance;
}

// Constructor
Finder::Finder() {
	currentPath = 0;
	straight_dist = 1;
	diagonal_dist = (float) sqrt(2.0);
	//goal = Vec2(-1, -1);
	movesPerUpdate = 1000;
	//GenerateEntrances?
	showClearanceMap = false;
	ResetFinder();
}

// A path's render call, show the nodes in the path
void Path::Render() {
	for(Vec2s::iterator viter = path.begin(); viter != path.end(); viter++) {
		Vec2 curr = (*viter);
		Vec2 pos = Game::Instance()->GetCenterOfCell(curr);
		GFX::Instance()->GFXline(pos.x - CELL_SIZE / 2, pos.y - CELL_SIZE / 2, pos.x + CELL_SIZE / 2, pos.y + CELL_SIZE / 2, Color(255, 0, 0));
		GFX::Instance()->GFXline(pos.x - CELL_SIZE / 2, pos.y + CELL_SIZE / 2, pos.x + CELL_SIZE / 2, pos.y - CELL_SIZE / 2, Color(255, 0, 0));
	}
}

// Mover is the class for our agents who follow paths
Mover::Mover() {
	showPath = true;
	state = MSTATE_IDLE;
	path = 0;
	col = Color(128,0,0);
}

Mover::Mover(int x, int y, int size){
	showPath = true;
	state = MSTATE_IDLE;
	timeToMove = 5;
	path = 0;
	col = Color(128,0,0);
	pos = Vec2(x, y);
	this->size = size;
}

// State machine for mover movement
void Mover::Update() {
	timeToMove--;
	if(timeToMove <= 0) {
		timeToMove = 5;
		if(state == MSTATE_FOLLOWINGPATH) {
			if(!path || path->path.empty()) {
				state = MSTATE_IDLE;
			}
			else {
				Vec2 pos = path->path.back();
				path->path.pop_back();
				this->pos = pos;
			}
		}
		else if(state == MSTATE_WAITINGFORPATH) {

		}
		else {

		}
	}
}

// Render calls for our movers
void Mover::Render() {
	if(path && showPath) {
		//GFX::Instance()->GFXtext_center("RENDERING PATH!", 50, 50, Color(255,255,255), Color(0,0,0));
		path->Render();
	}

	Game *g = Game::Instance();

	Vec2 pos = g->GetCenterOfCell(this->pos);
	Vec2 sa = Vec2((size - 1) * CELL_SIZE/2, (size - 1) * CELL_SIZE/2);
	pos = pos + sa;
	gfx->GFXtriangle(pos.x - CELL_SIZE/2 - sa.x, pos.y + CELL_SIZE/2 + sa.y, pos.x, pos.y - CELL_SIZE/2 - sa.y, pos.x + CELL_SIZE/2 + sa.x, pos.y + CELL_SIZE/2 + sa.y, col);
	if(state == MSTATE_IDLE) {
		gfx->GFXtext_center("IDLE", pos.x, pos.y - 20, Color(255,255,255), Color(0,0,0));
	}
	else if(state == MSTATE_FOLLOWINGPATH) {
		gfx->GFXtext_center("PATHING", pos.x, pos.y - 20, Color(255,255,255), Color(0,0,0));
	}
	else if(state == MSTATE_WAITINGFORPATH) {
		gfx->GFXtext_center("WAITING", pos.x, pos.y - 20, Color(255,255,255), Color(0,0,0));
	}
}

// Not currently used.
void Mover::DoneWithPath() {
	path->moversCurrentlyUsing--;
	if(path) delete(path);
	path = 0;
}

// Generates a path and follows it
void Mover::MoveTo(Vec2 target) {
	NewPath(target);
	//if(path) followingPath = true;
}

// Requests and waits for a path from Finder
void Mover::NewPath(Vec2 target) {
	state = MSTATE_WAITINGFORPATH;
	Finder::Instance()->FindPath(this, pos, target, PATH_STANDARD, 0, CELLS_X - 1, 0, CELLS_Y - 1);
}

void Mover::ReceivePath(Path *thePath) {
	if(thePath) {
		path = new Path(*thePath);
		state = MSTATE_FOLLOWINGPATH;
	}
	else {
		path = 0;
		state = MSTATE_IDLE;
	}

}

void Finder::FindPath(Mover *mover, Vec2 from, Vec2 to, PathType pathType, int x1, int x2, int y1, int y2) {

	// Once everything's implemented, do some checking to see if the path is cached here

	if(from.Valid() && to.Valid()) {
		paths.push_back(PathDefinition(mover, from, to, pathType, x1, x2, y1, y2));
	}
}

// Clears our pathfinding variables when finding a new path
void Finder::ResetFinder() {
	Game *g = Game::Instance();

	lastPathMoves = 0;
	currentPath = 0;

	open.clear();
	consideredNodes.clear();

	// Lots of resetting could get slow
	for(int i = 0; i < CELLS_X; i++) {
		for(int j = 0; j < CELLS_Y; j++) {
			int grid = g->GetGridAt(Vec2(i, j));
			nodes[i][j].blocked = (grid == 0) ? false : true;
			nodes[i][j].state = NS_UNCHECKED;
			nodes[i][j].fScore = 99999;
			nodes[i][j].gScore = 99999;
			nodes[i][j].x = i;
			nodes[i][j].y = j;
			cameFrom[i][j] = Vec2(-1, -1);
		}
	}
	GenerateClearanceMap();
}

// Generates a map based on what nodes can fit which movers, based on size.
void Finder::GenerateClearanceMap() {
	int i, j, k, m;

	for(i = 0; i < CELLS_X; i++) {
		for(j = 0; j < CELLS_Y; j++) {
			if(nodes[i][j].blocked) {
				clearanceMap[i][j] = 0;
				continue;
			}
			k = 1;
			bool blocked = false;
			while(i + k < CELLS_X && j + k < CELLS_Y && !blocked) {
				
				for(m = 0; m <= k && !blocked; m++) {
					if(	nodes[i + k][j + m].blocked ||
						nodes[i + m][j + k].blocked) {
						blocked = true;
					}
				}

				if(!blocked)
					k++;
			}
			
			clearanceMap[i][j] = k;
		}
	}
}

// Shows the clearance map, and/or current path being generated
void Finder::Render() {

	if(showClearanceMap) {
		for(int i = 0; i < CELLS_X; i++) {
			for(int j = 0; j < CELLS_Y; j++) {
				Vec2 pos = Game::Instance()->GetCenterOfCell(Vec2(i, j));
				std::stringstream clearance;
				clearance << clearanceMap[i][j];
				std::string str = clearance.str();
				char *stringer = (char *)str.c_str();
				if(nodes[i][j].blocked)
					GFX::Instance()->GFXtext_center(stringer, pos.x, pos.y, Color(255,255,255), Color(0,0,0));
				else
					GFX::Instance()->GFXtext_center(stringer, pos.x, pos.y, Color(0,0,0), Color(200,200,200));	
			}
		}
	}

	for(Vec2s::iterator viter = consideredNodes.begin(); viter != consideredNodes.end(); viter++) {
		Vec2 curr = (*viter);
		Vec2 pos = Game::Instance()->GetCenterOfCell(curr);
		GFX::Instance()->GFXline(pos.x - CELL_SIZE / 2, pos.y, pos.x + CELL_SIZE / 2, pos.y, Color(0, 0, 255));
		GFX::Instance()->GFXline(pos.x, pos.y - CELL_SIZE / 2, pos.x, pos.y + CELL_SIZE / 2, Color(0, 0, 255));
	}

	if(currentPath)
		Vec2 pos = Game::Instance()->GetCenterOfCell(currentPath->to);
	GFX::Instance()->GFXrectfill(pos.x - CELL_SIZE / 2, pos.y - CELL_SIZE / 2, pos.x + CELL_SIZE / 2, pos.y + CELL_SIZE / 2, Color(0,230,0));
	
	/*std::stringstream pathmoves;
	pathmoves << lastPathMoves;
	std::string str = "Path Length: " + pathmoves.str();
	char *stringer = (char *)str.c_str();
	gfx->GFXtext(stringer, 20, 600, Color(255, 255, 255), Color(0,0,0));*/
}

// If we're looking for a path, the finder will continue its search here,
// considering movesPerUpdate nodes per update.
void Finder::Update() {
	
	// Let's do one path at a time. We could interleave, but doing so could
	// potentially take a lot of memory!
	if(!paths.empty()) {
		currentPath = &paths.front();
		int retval = this->FindPathASTAR(movesPerUpdate);	// Arbitrary amount, lets us see the path be generated if we want
		if(retval < 0) { 
			//Game::Instance()->test3 = true;
			currentPath->mover->ReceivePath(0);
			paths.pop_front();
			this->ResetFinder();
		}
		else if(retval == 1) {
			currentPath->mover->ReceivePath(currentPath->thePath);
			paths.pop_front();
			this->ResetFinder();
		}
	}
}



// Do a standard a* pathfind.
int Finder::FindPathASTAR(int time) {

	// Let's store the nodes in a big static array, but... have a priority queue with pointers to the open set.
	// Bah. Let's do this later. 
	//std::priority_queue<PathNode*, vector<PathNode*>,less<PathNode*>> open;

	if(!currentPath) return -2;

	int x1 = currentPath->x1;
	int x2 = currentPath->x2;
	int y1 = currentPath->y1;
	int y2 = currentPath->y2;
	Vec2 from = currentPath->from;
	Vec2 to = currentPath->to;
	
	Game *g = Game::Instance();

	// Make sure this is a valid request
	if(x2 <= x1 || y2 <= y1)	return -2;
	if(!g->InGrid(Vec2(x1, y1)) || !g->InGrid(Vec2(x2, y2)) || !g->InGrid(from) || !g->InGrid(to)) return -2;
	if(nodes[(int)to.x][(int)to.y].blocked == true || clearanceMap[(int)to.x][(int)to.y] < currentPath->mover->size) return -2;

	// Testing as x1,y1 0 and x2,y2 max

	// if this is the first iteration of the path (need a better indicator, it is possible (not probable though)
	// that we can hit time = 0 and open.empty at the same time. 
	if(open.empty()) {
		open.push_back(&nodes[(int)from.x][(int)from.y]);
		nodes[(int)from.x][(int)from.y].gScore = 0;
		nodes[(int)from.x][(int)from.y].fScore = from.dist(to);
	}

	// Search until we have run out of moves this update or we run out of nodes to consider.
	while(!open.empty() && time > 0) {
		float bestfScore = 99999;
		PathNode *best = 0;
		std::vector<PathNode*>::iterator piter;
		std::vector<PathNode*>::iterator deleteMe;

		// Chunky, O(n). Can do better.
		// Get best candidate node off the open set.
		for(piter = open.begin(); piter != open.end(); piter++) {
			PathNode *curr = (*piter);
			if(curr->fScore < bestfScore) {
				best = curr;
				bestfScore = curr->fScore;
				deleteMe = piter;
			}
		}
		if(best->x == (int)to.x && best->y == (int)to.y) {
			//Path *npath = new Path(GetPath(to));
			currentPath->thePath = new Path(GetPath(to));
			if(currentPath->thePath) lastPathMoves = (int)currentPath->thePath->path.size();
			return 1;
		}

		best->state = NS_CLOSED;
		
		open.erase(deleteMe);

		ProcessAdjacentNodes(best);
		time--;
	}
	
	if(time == 0) return 0;

	return -1;
}

// Can we move to this node? Can only be used if we're pathfinding (requires currentPath)
bool Finder::OpenNode(Vec2 target) {
	return Game::Instance()->InGrid(target) && !nodes[(int)target.x][(int)target.y].blocked
			&& nodes[(int)target.x][(int)target.y].state != NS_CLOSED
			&& clearanceMap[(int)target.x][(int)target.y] >= currentPath->mover->size;
}

// Checks each adjacent node and adds them to the open list, and updates node scores
void Finder::ProcessAdjacentNodes(PathNode *target) {
	int dir;
	Game *g = Game::Instance();
	for(dir = DIR_UPLEFT; dir < DIR_NONE; dir++) {
		Vec2 offset = g->DirToVec2((Direction)dir);
		Vec2 testPos = Vec2(target->x, target->y) + offset;
		PathNode *testNode = &nodes[(int)testPos.x][(int)testPos.y];
		if(dir == DIR_UPLEFT) {
			if(		!OpenNode(Vec2(target->x, target->y) + g->DirToVec2(DIR_UP))
				&&	!OpenNode(Vec2(target->x, target->y) + g->DirToVec2(DIR_LEFT)))
				continue;
		}
		else if(dir == DIR_UPRIGHT) {
			if(		!OpenNode(Vec2(target->x, target->y) + g->DirToVec2(DIR_UP))
				&&	!OpenNode(Vec2(target->x, target->y) + g->DirToVec2(DIR_RIGHT)))
				continue;
		}
		else if(dir == DIR_DOWNRIGHT) {
			if(		!OpenNode(Vec2(target->x, target->y) + g->DirToVec2(DIR_DOWN))
				&&	!OpenNode(Vec2(target->x, target->y) + g->DirToVec2(DIR_RIGHT)))
				continue;
		}
		else if(dir == DIR_DOWNLEFT) {
			if(		!OpenNode(Vec2(target->x, target->y) + g->DirToVec2(DIR_DOWN))
				&&	!OpenNode(Vec2(target->x, target->y) + g->DirToVec2(DIR_LEFT)))
				continue;
		}

		if(OpenNode(testPos)) {
			float tempgScore = target->gScore + Vec2(target->x, target->y).dist(testPos);
			float dirPenalty = 0;
			bool tempIsBetter = false;
	
			// Penalize direction changes
			//if(Game::Vec2ToDir(Vec2(target->x, target->y) - cameFrom[target->x][target->y]) != dir) {
			//	dirPenalty = 2;
			//}

			if(!InOpenSet(testNode)) {
				open.push_back(testNode);
				consideredNodes.push_back(Vec2(testNode->x, testNode->y));
				tempIsBetter = true;
			}
			else if(tempgScore < target->gScore) {
				tempIsBetter = true;
			}

			if(tempIsBetter) {
				float h_diag = min(abs(testNode->x-currentPath->to.x), abs(testNode->y-currentPath->to.y));
				float h_straight = abs(testNode->x-currentPath->to.x) + abs(testNode->y-currentPath->to.y);
				float hScore = diagonal_dist * h_diag + straight_dist * (h_straight - 2*h_diag);
				
				cameFrom[testNode->x][testNode->y] = Vec2(target->x, target->y);
				testNode->gScore = tempgScore;
			
				testNode->fScore = testNode->gScore + 4*hScore;
				//testNode->fScore = hScore;
				//testNode->fScore = Vec2(testNode->x, testNode->y).dist(currentPath->to);
			}
			
		}
			
	}

}

// Is the given node in the open set?
bool Finder::InOpenSet(PathNode *target) {
	// Optimize this! (Binary Heap)
	for(std::vector<PathNode *>::iterator piter = open.begin(); piter != open.end(); piter++) {
		PathNode *curr = (*piter);
		if(curr == target) return true;
	}
	return false;
}

// Due to using a queue-like data structure, the path is created in reverse, this is a
// little hack to fix it. 
// Not used anymore.
Vec2s Finder::FixPath(Vec2s backwardsPath) {
	Vec2s newPath;
	
	while(!backwardsPath.empty()) {
		newPath.push_back(backwardsPath.back());
		backwardsPath.pop_back();
	}
	return newPath;
}

// Recursively generates the path
Vec2s Finder::GetPath(Vec2 to) {
	Vec2s emptyPath;

	if(cameFrom[(int)to.x][(int)to.y] != Vec2(-1, -1)) {
		Vec2s thePath = GetPath(cameFrom[(int)to.x][(int)to.y]);
		// Optimize this
		thePath.insert(thePath.begin(), to);
		return thePath;
	}
	else
		return emptyPath;
}