#include "Pathfind.h"

using namespace std;

// Constructor
Pathfind::Pathfind(IsoTilesMap * tm)
{
	// storing the IsoTilesMap
	_tm = tm;

	// init the membership vector => all elements == 0
	int max_nodes = _tm->GetNumRows() * _tm->GetNumCols();
	bitset<2> * flag;
	for(int i = 0; i < max_nodes; i++)
	{
		flag = new bitset<2>(0);
		_membership.push_back(flag);
	}
}

// Destroyer
Pathfind::~Pathfind()
{
	int max_nodes = _membership.size();

	for(int i = 0; i < max_nodes; i++)
		delete _membership[i];

	_membership.clear();
}

// Try to make a path from start[row,col] to end[row,col]
Path * Pathfind::MakePath(int start_row, int start_col, int end_row, int end_col, int start_dir)
{
	PathfindNode * node;
	PathfindNode * cur_node;

	_end.row = end_row;
	_end.col = end_col;

	node = new PathfindNode(start_row, start_col, GenNodeId(start_row, start_col));
	node->SetDirection(start_dir);

	// store the first node into OPEN and save the membership
	(_membership[node->GetId()])->set(OPEN);
	_open.AddNode(node);

	bool path_found = false;

	Path * path = new Path;

	// llok for the end node until there are available nodes
	while(!path_found && !_open.IsEmpty())
	{
		cur_node = _open.GetHead();
		(_membership[cur_node->GetId()])->reset();

		_closed.push_back(cur_node);
		(_membership[cur_node->GetId()])->set(CLOSED);

		if((cur_node->GetRow() == _end.row) && (cur_node->GetCol() == _end.col))
			path_found = true;
		else
			ExpandNodes(cur_node);
	}

	// no path found, return NULL
	if(!path_found)
	{
		CleanPathfindData();
		return NULL;
	}

	// create the inverted path using a stack
	stack< PathfindNode * > path_stack;
	// start from the end node
	node = cur_node;

	while(node)
	{
		path_stack.push(node);
		node = node->GetFather();
	}

	// create the path using the nodes from the stack
	while(!path_stack.empty())
	{
		node = path_stack.top();
		path_stack.pop();

		path->AddNode(node->GetRow(), node->GetCol(), node->GetCost(), node->GetDirection());
	}

	// clean data
	CleanPathfindData();

	return path;
}

// expands near nodes from one centered
// checks that nodes are inside the map, walkable and not yet into the closed list
void Pathfind::ExpandNodes(PathfindNode * center)
{
	int c_row = center->GetRow();
	int c_col = center->GetCol();

	// NORTH
	if((c_row - 1) >= 0 &&
	(_tm->IsWalkable(c_row - 1, c_col) || _tm->IsVisible(c_row - 1, c_col) != VISIBLE) &&
	!(_membership[GenNodeId(c_row - 1, c_col)])->test(CLOSED))
		GenNode(center, c_row - 1, c_col, DIR_NORTH);

	// NORTH-EAST
	if((c_row - 1) >= 0 && (c_col + 1) < _tm->GetNumCols() &&
	(_tm->IsWalkable(c_row - 1, c_col + 1) || _tm->IsVisible(c_row - 1, c_col + 1) != VISIBLE) &&
	!(_membership[GenNodeId(c_row - 1, c_col + 1)])->test(CLOSED))
		GenNode(center, c_row - 1, c_col + 1 , DIR_NORTH_EAST);

	// EAST
	if((c_col + 1) < _tm->GetNumCols() &&
	(_tm->IsWalkable(c_row, c_col + 1) || _tm->IsVisible(c_row, c_col + 1) != VISIBLE) &&
	!(_membership[GenNodeId(c_row, c_col + 1)])->test(CLOSED))
		GenNode(center, c_row, c_col + 1 , DIR_EAST);

	// SOUTH-EAST
	if((c_row + 1) < _tm->GetNumRows() && (c_col + 1) < _tm->GetNumCols() &&
	(_tm->IsWalkable(c_row + 1, c_col + 1) || _tm->IsVisible(c_row + 1, c_col + 1) != VISIBLE) &&
	!(_membership[GenNodeId(c_row + 1, c_col + 1)])->test(CLOSED))
		GenNode(center, c_row + 1, c_col + 1 , DIR_SOUTH_EAST);

	// SOUTH
	if((c_row + 1) < _tm->GetNumRows() &&
	(_tm->IsWalkable(c_row + 1, c_col) || _tm->IsVisible(c_row + 1, c_col) != VISIBLE) &&
	!(_membership[GenNodeId(c_row + 1, c_col)])->test(CLOSED))
		GenNode(center, c_row + 1, c_col , DIR_SOUTH);

	// SOUTH-WEST
	if((c_row + 1) < _tm->GetNumRows() && (c_col - 1) >= 0 &&
	(_tm->IsWalkable(c_row + 1, c_col - 1) || _tm->IsVisible(c_row + 1, c_col - 1) != VISIBLE) &&
	!(_membership[GenNodeId(c_row + 1, c_col - 1)])->test(CLOSED))
		GenNode(center, c_row + 1, c_col - 1 , DIR_SOUTH_WEST);

	// WEST
	if((c_col - 1) >= 0 &&
	(_tm->IsWalkable(c_row, c_col - 1) || _tm->IsVisible(c_row, c_col - 1) != VISIBLE) &&
	!(_membership[GenNodeId(c_row, c_col - 1)])->test(CLOSED))
		GenNode(center, c_row, c_col - 1 , DIR_WEST);

	// NORTH-WEST
	if((c_row - 1) >= 0 && (c_col - 1) >= 0 &&
	(_tm->IsWalkable(c_row - 1, c_col - 1) || _tm->IsVisible(c_row - 1, c_col - 1) != VISIBLE) &&
	!(_membership[GenNodeId(c_row - 1, c_col - 1)])->test(CLOSED))
		GenNode(center, c_row - 1, c_col - 1 , DIR_NORTH_WEST);
}

// generate a node from the passed one, in according with the passed direction
void Pathfind::GenNode(PathfindNode * from,int new_row, int new_col, int direction)
{
	int g;
	int id = GenNodeId(new_row, new_col);
	int rot_cost;

	PathfindNode * node;

	// compute cost of the node
	if((direction % 2) == 0)
		g = ORTHO_COST;
	else
		g = DIAGONAL_COST;

	// rotation needed
	if(direction != from->GetDirection())
	{
		// compute rotation cost
		if(abs(direction - from->GetDirection()) <= (NUM_VIEWS / 2))
			rot_cost = ROTATION_COST * abs(direction - from->GetDirection());
		else
			rot_cost = ROTATION_COST * (NUM_VIEWS -
				abs(direction - from->GetDirection()));

		g += rot_cost;
	}

	// node not present in any list
	if(!(_membership[id])->test(OPEN))
	{
		node = new PathfindNode(new_row, new_col, GenNodeId(new_row, new_col));

		node->SetDirection(direction);
		node->SetCost(g);
		// set F = G + H
		node->SetScores(g + from->GetG(), ComputeHeuristics(node));
		// set the father of the node
		node->SetFather(from);

		(_membership[id])->set(OPEN);
		_open.AddNode(node);
	}
	// node is inside the open
	else
	{
		node = _open.SearchNode(id);

		if(node->GetG() > (g + from->GetG()))
		{
			node->SetRow(new_row);
			node->SetCol(new_col);
			node->SetDirection(direction);
			node->SetCost(g);
			node->SetScores(g + from->GetG(), ComputeHeuristics(node));
			node->SetFather(from);

			// restore the heap properties
			_open.RestoreHeap();
		}
	}
}

// compute the heuristics value of the remaining path from a passed node
// in this case DIAGONAL DISTANCE
int Pathfind::ComputeHeuristics(PathfindNode * node)
{
	int h1_diag = abs(node->GetRow() - _end.row);
	int h2_diag = abs(node->GetCol() - _end.col);

	int h_diag = ((h1_diag < h2_diag) ? h1_diag : h2_diag);

	int h_straight = (abs(node->GetRow() - _end.row) + abs(node->GetCol() - _end.col));

	return ((DIAGONAL_COST * h_diag) + (ORTHO_COST * (h_straight - (2 * h_diag))));
}

// erase unused resource
void Pathfind::CleanPathfindData()
{
	// reset membership elements
	int num_elem = _membership.size();

	for(int i = 0; i < num_elem; i++)
		_membership[i]->reset();

	// clear OPEN
	_open.Clear();

	// clear CLOSED
	num_elem = _closed.size();

	for(int i = 0; i < num_elem; i++)
		delete _closed[i];

	_closed.clear();
}