#include <iostream>
#include <cmath>
#include "nlsr_lsdb.hpp"
#include "nlsr_rtc.hpp"
#include "nlsr_map.hpp"
#include "nlsr_lsa.hpp"
#include "nlsr_nexthop.hpp"
#include "nlsr.hpp"

namespace nlsr {

using namespace std;

void 
RoutingTableCalculator::allocateAdjMatrix()
{
	adjMatrix = new double*[numOfRouter];
	for(int i = 0; i < numOfRouter; ++i) 
	{
    adjMatrix[i] = new double[numOfRouter];
	}
}

void 
RoutingTableCalculator::initMatrix()
{
	for(int i=0;i<numOfRouter;i++)
	{
		for(int j=0;j<numOfRouter;j++)
			adjMatrix[i][j]=0;
	}
}

void
RoutingTableCalculator::makeAdjMatrix(Nlsr& pnlsr, Map pMap)
{
	std::list<AdjLsa> adjLsdb=pnlsr.getLsdb().getAdjLsdb();
	for( std::list<AdjLsa>::iterator it=adjLsdb.begin(); 
	                                                 it!= adjLsdb.end() ; it++)
	{
		string linkStartRouter=(*it).getOrigRouter();
		int row=pMap.getMappingNoByRouterName(linkStartRouter);
		std::list<Adjacent> adl=(*it).getAdl().getAdjList();
		for( std::list<Adjacent>::iterator itAdl=adl.begin(); 
																								itAdl!= adl.end() ; itAdl++)
		{
			string linkEndRouter=(*itAdl).getAdjacentName();
			int col=pMap.getMappingNoByRouterName(linkEndRouter);
			double cost=(*itAdl).getLinkCost();
			if ( (row >= 0 && row<numOfRouter) && (col >= 0 && col<numOfRouter) )
			{
				adjMatrix[row][col]=cost;
			}
		}
		
	}
}

void
RoutingTableCalculator::printAdjMatrix()
{
	for(int i=0;i<numOfRouter;i++)
	{
		for(int j=0;j<numOfRouter;j++)
			printf("%f ",adjMatrix[i][j]);
		printf("\n");
	}
}

void 
RoutingTableCalculator::adjustAdMatrix(int source, int link, double linkCost)
{
	for ( int i = 0; i < numOfRouter; i++ ){
		if ( i == link ){
			adjMatrix[source][i]=linkCost;
		}
		else{
			adjMatrix[source][i]=0;
		}
	}
}

int 
RoutingTableCalculator::getNumOfLinkfromAdjMatrix(int sRouter)
{
	int noLink=0;
	for(int i=0;i<numOfRouter;i++)
	{	
		if ( adjMatrix[sRouter][i] > 0 )
		{
			noLink++;
		}
	}
	return noLink;
}

void 
RoutingTableCalculator::getLinksFromAdjMatrix(int *links, 
                                                 double *linkCosts, int source)
{
	int j=0;
	for (int i=0; i <numOfRouter; i++)
	{
		if ( adjMatrix[source][i] > 0 )
		{
			links[j]=i;
			linkCosts[j]=adjMatrix[source][i];
			j++;
		}
	}
}

void 
RoutingTableCalculator::freeAdjMatrix()
{
	for(int i = 0; i < numOfRouter; ++i) 
	{
    	delete [] adjMatrix[i];
	}
	delete [] adjMatrix;
}


void 
RoutingTableCalculator::allocateLinks()
{
	links=new int[vNoLink];
}

void RoutingTableCalculator::allocateLinkCosts()
{
	linkCosts=new double[vNoLink];
}


void 
RoutingTableCalculator::freeLinks()
{
	delete [] links;
}
void 
RoutingTableCalculator::freeLinksCosts()
{
	delete [] linkCosts;
}

void
LinkStateRoutingTableCalculator::calculatePath(Map& pMap, 
                                                  RoutingTable& rt, Nlsr& pnlsr)
{
	cout<<"LinkStateRoutingTableCalculator::calculatePath Called"<<endl;
	allocateAdjMatrix();
	initMatrix();
	makeAdjMatrix(pnlsr,pMap);
	cout<<pMap;
	printAdjMatrix();
	string routerName=pnlsr.getConfParameter().getRouterPrefix();
	int sourceRouter=pMap.getMappingNoByRouterName(routerName);
	int noLink=getNumOfLinkfromAdjMatrix(sourceRouter);
	allocateParent();
	allocateDistance();

	if ( pnlsr.getConfParameter().getMaxFacesPerPrefix() == 1 )
	{
		// Single Path
		doDijkstraPathCalculation(sourceRouter);
		// print all ls path -- debugging purpose
		printAllLsPath(sourceRouter);
		// update routing table
		addAllLsNextHopsToRoutingTable(pnlsr, rt, pMap, sourceRouter);
	}
	else
	{
		// Multi Path
		setNoLink(getNumOfLinkfromAdjMatrix(sourceRouter));
		allocateLinks();
		allocateLinkCosts();

		getLinksFromAdjMatrix(links, linkCosts, sourceRouter);
		for (int i=0 ; i < vNoLink; i++)
		{
			
			adjustAdMatrix(sourceRouter,links[i], linkCosts[i]);
			printAdjMatrix();
			doDijkstraPathCalculation(sourceRouter);
			// print all ls path -- debugging purpose
			printAllLsPath(sourceRouter);
			//update routing table
			addAllLsNextHopsToRoutingTable(pnlsr, rt, pMap, sourceRouter);
		}
		
		freeLinks();
	  freeLinksCosts();
	}

	freeParent();
	freeDistance();
	freeAdjMatrix();
}

void 
LinkStateRoutingTableCalculator::doDijkstraPathCalculation(int sourceRouter)
{
	int i;
	int v,u;
	int *Q=new int[numOfRouter];
	int head=0;
	/* Initiate the Parent */
	for (i = 0 ; i < numOfRouter; i++){
		parent[i]=EMPTY_PARENT;
		distance[i]=INF_DISTANCE;
		Q[i]=i;
	} 

	if ( sourceRouter != NO_MAPPING_NUM ){
		distance[sourceRouter]=0;
		sortQueueByDistance(Q,distance,head,numOfRouter);

		while (head < numOfRouter )
		{
			u=Q[head];
			if(distance[u] == INF_DISTANCE)
			{
				break;
			}

			for(v=0 ; v <numOfRouter; v++)
			{
				if( adjMatrix[u][v] > 0 )
				{
					if ( isNotExplored(Q,v,head+1,numOfRouter) )
					{
						if( distance[u] + adjMatrix[u][v] <  distance[v])
						{
							distance[v]=distance[u] + adjMatrix[u][v] ;
							parent[v]=u;
						}	

					}

				}

			}

			head++;
			sortQueueByDistance(Q,distance,head,numOfRouter);
		}
	}
	delete [] Q;
}

void 
LinkStateRoutingTableCalculator::addAllLsNextHopsToRoutingTable(Nlsr& pnlsr,
																 RoutingTable& rt, Map& pMap, int sourceRouter)
{
	cout<<"LinkStateRoutingTableCalculator::addAllNextHopsToRoutingTable Called";
	cout<<endl;
	for(int i=0; i < numOfRouter ; i++)
	{
		if ( i!= sourceRouter )
		{
			int nextHopRouter=getLsNextHop(i,sourceRouter);
			double routeCost=distance[i];
			string nextHopRouterName=pMap.getRouterNameByMappingNo(nextHopRouter);
			int nxtHopFace=
              pnlsr.getAdl().getAdjacent(nextHopRouterName).getConnectingFace();
			cout<<"Dest Router: "<<pMap.getRouterNameByMappingNo(i)<<endl;
			cout<<"Next hop Router: "<<nextHopRouterName<<endl;
			cout<<"Next hop Face: "<<nxtHopFace<<endl;
			cout<<"Route Cost: "<<routeCost<<endl;
			cout<<endl;
			// Add next hop to routing table
			NextHop nh(nxtHopFace,routeCost);
			rt.addNextHop(pMap.getRouterNameByMappingNo(i),nh);
			
		}
	}
}

int 
LinkStateRoutingTableCalculator::getLsNextHop(int dest, int source)
{
	int nextHop;
	while ( parent[dest] != EMPTY_PARENT )
	{
		nextHop=dest;
		dest=parent[dest];

	}

	if ( dest != source )
	{
		nextHop=NO_NEXT_HOP;	
	}

	return nextHop;
}

void
LinkStateRoutingTableCalculator::printAllLsPath(int sourceRouter)
{
	cout<<"LinkStateRoutingTableCalculator::printAllLsPath Called"<<endl;
	cout<<"Source Router: "<<sourceRouter<<endl;
	for(int i=0; i < numOfRouter ; i++)
	{
		if ( i!= sourceRouter )
		{
			printLsPath(i);
			cout<<endl;
		}
	}
}

void
LinkStateRoutingTableCalculator::printLsPath(int destRouter)
{
	if (parent[destRouter] != EMPTY_PARENT )
	{
		printLsPath(parent[destRouter]);
	}

	cout<<" "<<destRouter;
}

void 
LinkStateRoutingTableCalculator::sortQueueByDistance(int *Q,
                                             double *dist,int start,int element)
{
	for ( int i=start ; i < element ; i ++) 
	{
		for( int j=i+1; j<element; j ++)
		{
			if (dist[Q[j]] < dist[Q[i]])
			{
				int tempU=Q[j];
				Q[j]=Q[i];
				Q[i]=tempU;
			}
		}
	}

}

int 
LinkStateRoutingTableCalculator::isNotExplored(int *Q, 
                                                   int u,int start, int element)
{
	int ret=0;
	for(int i=start; i< element; i++)
	{
		if ( Q[i] == u )
		{
			ret=1;
			break;
		}
	}
	return ret;
}

void 
LinkStateRoutingTableCalculator::allocateParent()
{
	parent=new int[numOfRouter];
}

void 
LinkStateRoutingTableCalculator::allocateDistance()
{
	distance= new double[numOfRouter];
}

void 
LinkStateRoutingTableCalculator::freeParent()
{
	delete [] parent;
}

void LinkStateRoutingTableCalculator::freeDistance()
{
	delete [] distance;
}


void
HypRoutingTableCalculator::calculatePath(Map& pMap, 
                                                  RoutingTable& rt, Nlsr& pnlsr)
{
	makeAdjMatrix(pnlsr,pMap);
	string routerName=pnlsr.getConfParameter().getRouterPrefix();
	int sourceRouter=pMap.getMappingNoByRouterName(routerName);
	int noLink=getNumOfLinkfromAdjMatrix(sourceRouter);
	setNoLink(noLink);
	allocateLinks();
	allocateLinkCosts();

	getLinksFromAdjMatrix(links, linkCosts, sourceRouter);

	for(int i=0 ; i < numOfRouter ; ++i)
	{
			int k=0;
			if ( i != sourceRouter)
			{
				allocateLinkFaces();
				allocateDistanceToNeighbor();
				allocateDistFromNbrToDest();

				for(int j=0; j<vNoLink; j++)
				{
					string nextHopRouterName=pMap.getRouterNameByMappingNo(links[j]);
					int nextHopFace=
              pnlsr.getAdl().getAdjacent(nextHopRouterName).getConnectingFace();
          double distToNbr=getHyperbolicDistance(pnlsr,pMap,
                                                         sourceRouter,links[j]);
					double distToDestFromNbr=getHyperbolicDistance(pnlsr, 
					                                                    pMap,links[j],i);
					if ( distToDestFromNbr >= 0 )
					{
							linkFaces[k] = nextHopFace;
							distanceToNeighbor[k] = distToNbr;
							distFromNbrToDest[k] = distToDestFromNbr;
							k++;
					}
				}

				addHypNextHopsToRoutingTable(pnlsr,pMap,rt,k,i);

				freeLinkFaces();
				freeDistanceToNeighbor();
				freeDistFromNbrToDest();	
			}
	}

	freeLinks();
	freeLinksCosts();
	freeAdjMatrix();
}

void
HypRoutingTableCalculator::addHypNextHopsToRoutingTable(Nlsr& pnlsr,Map& pMap,
                                        RoutingTable& rt, int noFaces, int dest)
{
	for(int i=0 ; i < noFaces ; ++i)
	{
		string destRouter=pMap.getRouterNameByMappingNo(dest);
		NextHop nh(linkFaces[i],distFromNbrToDest[i]);
		rt.addNextHop(destRouter,nh);
		if( isDryRun == 1 )
		{
			rt.addNextHopToDryTable(destRouter,nh);
		}
	}
	
} 

double 
HypRoutingTableCalculator::getHyperbolicDistance(Nlsr& pnlsr,
                                                  Map& pMap, int src, int dest)
{
	double distance=0.0;

	string srcRouterKey=pMap.getRouterNameByMappingNo(src)+"/3";
	string destRouterKey=pMap.getRouterNameByMappingNo(dest)+"/3";

	double srcRadius=(pnlsr.getLsdb().getCorLsa(srcRouterKey).first).getCorRadius();
	double srcTheta=(pnlsr.getLsdb().getCorLsa(srcRouterKey).first).getCorTheta();

	double destRadius=(pnlsr.getLsdb().getCorLsa(destRouterKey).first).getCorRadius();
	double destTheta=(pnlsr.getLsdb().getCorLsa(destRouterKey).first).getCorTheta();


	double diffTheta = fabs (srcTheta - destTheta);

	if (diffTheta > MATH_PI)
	{
		diffTheta = 2 * MATH_PI - diffTheta; 	
	}

	if ( srcRadius != -1 && destRadius != -1 )
	{
		if (diffTheta == 0)
			distance = fabs (srcRadius - destRadius); 
		else
			distance = acosh((cosh(srcRadius)*cosh(destRadius))-
					(sinh(srcRadius)*sinh(destRadius)*cos(diffTheta)));
	}else
	{
		distance = -1;
	}

	return distance;
}

void 
HypRoutingTableCalculator::allocateLinkFaces()
{
	linkFaces=new int[vNoLink];
}

void 
HypRoutingTableCalculator::allocateDistanceToNeighbor()
{
	distanceToNeighbor=new double[vNoLink];
}

void 
HypRoutingTableCalculator::allocateDistFromNbrToDest()
{
	distFromNbrToDest=new double[vNoLink];
}

void 
HypRoutingTableCalculator::freeLinkFaces()
{
	delete [] linkFaces;
}

void 
HypRoutingTableCalculator::freeDistanceToNeighbor()
{
	delete [] distanceToNeighbor;
}

void 
HypRoutingTableCalculator::freeDistFromNbrToDest()
{
	delete [] distFromNbrToDest;
}

}//namespace nlsr