#include "stdafx.h"
#include "Router.h"
#include <queue>
#include <functional>

#define MAX_COST 1000000

CRouter::CRouter(CSpacialHashing *spaceHash)
{
	m_pSpaceHash = spaceHash;
}

CWire *CRouter::RouteWire(CNode *from, CNode *to, AnchorList &anchors)
{
	CWire *ret;
	AnchorList newAnchors;
	bool res = Route(from, to, newAnchors);
	if(res == true)
	{
		ret = new CWire(0, 0, newAnchors, from, 0, to, 0, CString(""));
	}else{
		ret = new CWire(0, 0, anchors, from, 0, to, 0, CString(""));
	}
	return ret;
}

bool CRouter::CompareWires(CWire *w1, CWire *w2)
{
	CPoint from1 = dynamic_cast<CNode *>(w1->m_pStartNode)->GetPoint();
	CPoint to1 = dynamic_cast<CNode *>(w1->m_pEndNode)->GetPoint();
	CPoint from2 = dynamic_cast<CNode *>(w2->m_pStartNode)->GetPoint();
	CPoint to2 = dynamic_cast<CNode *>(w2->m_pEndNode)->GetPoint();
	return EstimateCost(from1, to1) < EstimateCost(from2, to2);
}

void CRouter::RerouteWires(vector<CWire *> wires)
{
	m_pSpaceHash->GetMaxXY(&m_maxX, &m_maxY);
	pointer_to_binary_function<CWire *, CWire *, bool> f(CRouter::CompareWires);
	sort<vector<CWire *>::iterator, pointer_to_binary_function<CWire *, CWire *, bool>>(wires.begin(), wires.end(), f);

	m_costBuffer = new unsigned char[m_maxX*m_maxY];
	memset(m_costBuffer, 0, m_maxX*m_maxY*sizeof(unsigned char));

	for(vector<CWire *>::iterator i = wires.begin(); i != wires.end(); i++)
	{
		AnchorList anchors;
		m_startNode = dynamic_cast<CNode *>((*i)->m_pStartNode);
		m_destNode = dynamic_cast<CNode *>((*i)->m_pEndNode);

		m_costBuffer[m_startNode->GetPoint().x / 10 + m_startNode->GetPoint().y / 10  * m_maxX] = 0;
		m_costBuffer[m_destNode->GetPoint().x / 10 + m_destNode->GetPoint().y / 10  * m_maxX] = 0;

		bool reverse = OrderNodes(&m_startNode, &m_destNode);
		m_startNode->DisconnectWire(*i);
		m_destNode->DisconnectWire(*i);
		bool res = DoRoute(anchors, true, reverse);
		if(!res)
		{
			res = DoRoute(anchors, false, reverse);
		}
//		memset(m_costBuffer, 0, m_maxX*m_maxY*sizeof(unsigned char));
		POSITION pos = anchors.GetHeadPosition();
		while(pos)
		{
			Anchor a = anchors.GetNext(pos);
			Anchor b;
			if(pos != NULL)
				b = anchors.GetAt(pos);
			else
				break;
			int x1, x2, y1, y2;
			if(a.m_PointMain.x == b.m_PointMain.x)
			{
				int x = a.m_PointMain.x / 10;
				if(a.m_PointMain.y < b.m_PointMain.y)
				{
					y1 = a.m_PointMain.y / 10;
					y2 = b.m_PointMain.y / 10;
				}else
				{
					y2 = a.m_PointMain.y / 10;
					y1 = b.m_PointMain.y / 10;
				}
				for(int y = y1 + 1; y <= y2 - 1; y++)
				{				
					unsigned char &cost = m_costBuffer[x + y * m_maxX];
					if(cost == 1 || cost == 0)
					{
						cost = 3;
					}
				}
			}else
			{
				int y = a.m_PointMain.y / 10;
				if(a.m_PointMain.x < b.m_PointMain.x)
				{
					x1 = a.m_PointMain.x / 10;
					x2 = b.m_PointMain.x / 10;
				}else{
					x2 = a.m_PointMain.x / 10;
					x1 = b.m_PointMain.x / 10;
				}
				for(int x = x1 + 1; x <= x2 - 1; x++)
				{
					unsigned char &cost = m_costBuffer[x + y * m_maxX];
					if(cost == 1 || cost == 0)
					{
						cost = 4;
					}
				}
			}
			if(a.m_PointMain.x != m_startNode->GetPoint().x || a.m_PointMain.y != m_startNode->GetPoint().y){
				unsigned char &cost = m_costBuffer[a.m_PointMain.x / 10 + a.m_PointMain.y / 10 * m_maxX];
				if(cost == 1 || cost == 0)
				{
				//	cost = 5;
				}
			}
		}
/*		for(int i = 0; i < m_maxX * m_maxY; i++)
		{
			if(m_costBuffer[i] == 2)
			{
				m_costBuffer[i] = 0;
			}
		}*/
		(*i)->SetAnchors(anchors);
		(*i)->ConnectNode(m_startNode, true);
		(*i)->ConnectNode(m_destNode, false);
//		break;
	}

	delete [] m_costBuffer;
}

bool CRouter::OrderNodes(CNode **start, CNode **end)
{
	int startFreeDirs = 0;
	int endFreeDirs = 0;
	for(int i = 0; i < 4; i++)
	{
		if((*start)->IsDirectionFree(i))
		{
			startFreeDirs++;
		}
	}
	for(int i = 0; i < 4; i++)
	{
		if((*end)->IsDirectionFree(i))
		{
			endFreeDirs++;
		}
	}
	if(startFreeDirs != endFreeDirs)
	{
		if(startFreeDirs > endFreeDirs)
		{
			CNode *t = (*start);
			*start = *end;
			*end = t;
			return true;
		}
	}else
	{
		if(EstimateCost((*start)->GetPoint(), CPoint(m_maxX / 2, m_maxY / 2)) > EstimateCost((*end)->GetPoint(), CPoint(m_maxX / 2, m_maxY / 2)))
		{
			CNode *t = (*start);
			*start = *end;
			*end = t;
			return true;
		}
	}
	return false;
}

bool CRouter::Route(CNode *fromNode, CNode *toNode, AnchorList &anchors)
{
	m_startNode = fromNode;
	m_destNode = toNode;
	m_pSpaceHash->GetMaxXY(&m_maxX, &m_maxY);
	bool reverse = OrderNodes(&m_startNode, &m_destNode);

	m_costBuffer = new unsigned char[m_maxX*m_maxY];
	memset(m_costBuffer, 0, m_maxX*m_maxY*sizeof(unsigned char));

	bool res = DoRoute(anchors, true, reverse);
	if(!res)
	{
		res = DoRoute(anchors, false, reverse);
	}

	delete [] m_costBuffer;
	return res;
}

bool CRouter::DoRoute(AnchorList &anchors, bool limitFrame, bool reverse)
{
	int minX, minY, maxX, maxY;

#ifdef DEBUG_ROUTE	
	ofstream of("C:\\log.txt");
	int nodeCounter = 0;
#endif
	CPoint from = m_startNode->GetPoint();
	CPoint to = m_destNode->GetPoint();
	if(limitFrame)
	{
		minX = min(from.x, to.x) - abs(to.x - from.x ) / 5;
		maxX = max(from.x, to.x) + abs(to.x - from.x ) / 5;
		minY = min(from.y, to.y) - abs(to.y - from.y ) / 5;
		maxY = max(from.y, to.y) + abs(to.y - from.y ) / 5;
	}
	m_pSpaceHash->GetMaxXY(&m_maxX, &m_maxY);

	unsigned char *closed = new unsigned char[m_maxX*m_maxY];
	memset(closed, 0, m_maxX*m_maxY*sizeof(bool));

	pointer_to_binary_function<CRoutingNode *, CRoutingNode *, bool> f(CRoutingNode::compare);
	priority_queue<CRoutingNode *, vector<CRoutingNode *>, pointer_to_binary_function<CRoutingNode *, CRoutingNode *, bool>> q(f);
	vector<CRoutingNode *> toDelete;
	CRoutingNode start(from, NULL, 0, 0);
#ifdef DEBUG_ROUTE	
	start.id = nodeCounter++;
#endif
	q.push(&start);
	CRoutingNode *solution = NULL;
	while(!q.empty())
	{
		CRoutingNode *node = q.top();
		q.pop();

#ifdef DEBUG_ROUTE	
		of << "Popped node: X=" << node->GetPoint().x << "; Y=" << node->GetPoint().y <<endl;
#endif

		int direction = CWire::GetDirection(node->GetParentNode() != NULL ? 
			node->GetParentNode()->GetPoint() : node->GetPoint(), node->GetPoint());
		if(direction == -1)
		{
			closed[node->GetPoint().x / 10 + node->GetPoint().y / 10 * m_maxX] = 0xff;
		}else{
			closed[node->GetPoint().x / 10 + node->GetPoint().y / 10 * m_maxX] |= 1 << (direction&1);
		}
		if(node->GetPoint() == to){
			solution = node;
			break;
		}
		vector<CPoint> points;
		vector<int> costs;
		int count = GetAdjacent(node->GetPoint(), node->GetParentNode() != NULL ? 
			node->GetParentNode()->GetPoint() : node->GetPoint(), points, costs);
		bool nocheap = true;
		bool some = false;
		for(int i = 0; i < count; i++){
			if(limitFrame && (points[i].x < minX || points[i].x > maxX || points[i].y < minY || points[i].y > maxY)){
				continue;
			}
			if(closed[points[i].x / 10 + points[i].y / 10 * m_maxX] & (1 << (CWire::GetDirection(node->GetPoint(), points[i])&1))){
				continue;
			}
			some = true;
			int cost;
			/*if(points[i].x == to.x && points[i].y == to.y){

				cost = node->GetCost() + 10;
			}else{
				cost = node->GetCost() + costs[i];
			}*/
			cost = node->GetCost() + costs[i];
			if(cost < MAX_COST)
			{
				CRoutingNode *childNode = new CRoutingNode(points[i], node, cost, EstimateCost(points[i], to));
#ifdef DEBUG_ROUTE	
				childNode->id = nodeCounter++;
#endif
				toDelete.push_back(childNode);
				q.push(childNode);

#ifdef DEBUG_ROUTE	
				of << "Pushed node: X=" << childNode->GetPoint().x << "; Y=" << childNode->GetPoint().y << "; COST=" << cost <<endl;
#endif
			}
		}
#ifdef DEBUG_ROUTE	
		of.flush();
#endif
	}

	vector<CRoutingNode *> path;
	if(solution != NULL){
		CRoutingNode *node = solution;
		while(node != NULL){
			path.push_back(node);
			node = node->GetParentNode();
		}
		int lastDir = CWire::GetDirection(path[path.size()-1]->GetPoint(), path[path.size()-2]->GetPoint());
		for(int i = path.size() - 1; i > 0; i--){
			int dir = CWire::GetDirection(path[i]->GetPoint(), path[i - 1]->GetPoint());
			if(dir != lastDir){
				if(!reverse)
				{
					anchors.AddTail(Anchor(path[i]->GetPoint()));
				}else{
					anchors.AddHead(Anchor(path[i]->GetPoint()));
				}
			}
			lastDir = dir;
		}		
		if(!reverse)
		{
			anchors.AddTail(path[0]->GetPoint());
			anchors.AddHead(from);
		}else
		{
			anchors.AddHead(path[0]->GetPoint());
			anchors.AddTail(from);
		}
	}
	for(vector<CRoutingNode *>::iterator i = toDelete.begin(); i != toDelete.end(); i++)
	{
		delete (*i);
	}
	delete [] closed;
	if(solution != NULL){
		return true;
	}
	return false;

}

int CRouter::GetCost(CPoint point, int direction, int prevDirection)
{
	if(prevDirection == -1)
	{
		if(m_startNode->IsDirectionFree((direction + 2) % 4))
		{
			return m_CostFree;
		}else
		{
			return m_CostSolid;
		}
	}
	int costInfo = m_costBuffer[point.x / 10 + point.y / 10 * m_maxX];
	if(costInfo == 0){
		CCommonObject *obj = m_pSpaceHash->HashTest(point);
		CCommonObject *nodeObj = NULL;
		if(obj != NULL && obj != m_destNode)
		{
			nodeObj = m_pSpaceHash->HashTest(point, 7);
		}
		if(obj == NULL){
			costInfo = 1;
		}
		else if(obj == m_destNode || nodeObj == m_destNode)
		{
			costInfo = 7;
		}
		else
		{
			if(obj->m_nObjType != 4){
				costInfo = 2;
			}else{
				//int wDir = ((CWire *)obj)->GetDirAtPoint(point);
				int wDir = m_pSpaceHash->GetWireDirAtPoint(obj, point);
				if(wDir == -1){
					costInfo = 1;
				}else{
					if(wDir == 0){
						costInfo = 3;
					}else if(wDir == 1){
						costInfo = 4;
					}else{
						costInfo = 5;
					}
				}
			}
		}
		if(costInfo != 7)
		{
			m_costBuffer[point.x / 10 + point.y / 10 * m_maxX] = costInfo;
		}
	}
	if(costInfo == 1)
	{
		if(direction == prevDirection || prevDirection == -1)
		{
			return m_CostFree;
		}else{
			return m_CostCorner;
		}
	};
	if(costInfo == 3 || costInfo == 4){
		if(direction == prevDirection){
			if(direction == 1 || direction == 3){
				if(costInfo == 3){
					return m_CostParallel;
				}else{
					return m_CostCross;
				}
			}else{
				if(costInfo == 4){
					return m_CostParallel;
				}else{
					return m_CostCross;
				}
			}
		}else{
			return m_CostParallel;
		}
	};
	if(costInfo == 5){
		return m_CostCrossCross;
	}
	/*if(costInfo == 6)
	{
		if(m_startNode->IsDirectionFree((direction + 2) % 4))
		{
			return m_CostFree;
		}else
		{
			return m_CostSolid;
		}
	}*/
	if(costInfo == 7)
	{
		if(m_destNode->IsDirectionFree(direction))
		{
			return m_CostFree;
		}else
		{
			return m_CostSolid;
		}
	}
	return m_CostSolid;
}

int CRouter::GetAdjacent(CPoint point, CPoint prevPoint, vector<CPoint> &points, vector<int> &costs)
{
	int maxX, maxY;
	m_pSpaceHash->GetMaxXY(&maxX, &maxY);
	int prevDirection = CWire::GetDirection(prevPoint, point);
	points.clear();
	if(point.x != 0 && prevDirection != 0){
		points.push_back(CPoint(point.x - 10, point.y));
	}
	if(point.y != 0 && prevDirection != 1){
		points.push_back(CPoint(point.x, point.y - 10));
	}
	if(point.x < maxX * 10 - 10 && prevDirection != 2){
		points.push_back(CPoint(point.x + 10, point.y));
	}
	if(point.y < maxY * 10 - 10 && prevDirection != 3){
		points.push_back(CPoint(point.x, point.y + 10));
	}

	for(unsigned int i = 0; i < points.size(); i++){
		int direction = CWire::GetDirection(point, points[i]);		
		int cost = 10 + GetCost(points[i], direction, prevDirection);
		costs.push_back(cost);
	}
	return (int)points.size();
}

int CRouter::EstimateCost(CPoint from, CPoint to)
{
	return abs(from.x - to.x) + abs(from.y - to.y);
}

CRoutingNode::CRoutingNode(CPoint point, CRoutingNode *parent, int cost, int heuristic)
: m_Point(point), m_pParentNode(parent), m_Cost(cost), m_Heuristic(heuristic)
{
	m_OrderValue = m_Cost + m_Heuristic;
}

CRoutingNode *CRoutingNode::GetParentNode()
{
	return m_pParentNode;
}


CPoint CRoutingNode::GetPoint(){
	return m_Point;
}

int CRoutingNode::GetCost(){
	return m_Cost;
}
