/*Implementation of PStep_ShortestPathAlgorithm.h

It contains the member functions of PStep_BiDirectionalLayeredLabelingAlgorithm

*/

#include "PStep_ShortestPathAlgorithm.h"

//PStep_BiDirectionalLayeredLabelingAlgorithm:
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//Solve the shortest path problem
//------------------------------------------------------------------------------------

//Solve the shortest path problem
void PStep_BiDirectionalLayeredLabelingAlgorithm::Run(){

	//Initialization:
	//*************************

	//Obtain Tolerance
	double Tolerance = Dominator.Tolerance;

	//Obtain pricing problem info for easy reference
	short int NumberOfCustomers = PricingProblem.NumberOfCustomers;
	short int Source = PricingProblem.Source;
	short int Sink = PricingProblem.Sink;
	short int MaxNumberOfForwardArcs = PricingProblem.MaxNumberOfForwardArcs;
	short int MaxNumberOfBackwardArcs = PricingProblem.MaxNumberOfBackwardArcs;
	bool AllowedToReachSinkEarly = PricingProblem.AllowedToReachSinkEarly;
	double** (&ReducedArcCosts) = PricingProblem.ReducedArcCosts;

	//Obtain arcs:
	//Initialize support
	std::list<std::list<PStep_Arc>>* TempOutArcs;
	std::list<std::list<PStep_Arc>>* TempInArcs;
	//Check whether a limited set of arcs should be used
	if (UseLimitedArcs){
		//Limited arcs should be used
		TempOutArcs = &PricingProblem.CurrentTreeNodeLimitedOutArcs;
		TempInArcs = &PricingProblem.CurrentTreeNodeLimitedInArcs;
	}
	else{
		//All arcs should be used
		TempOutArcs = &PricingProblem.CurrentTreeNodeOutArcs;
		TempInArcs = &PricingProblem.CurrentTreeNodeInArcs;
	}
	//Set arcs
	std::list<std::list<PStep_Arc>> &OutArcs = *TempOutArcs;
	std::list<std::list<PStep_Arc>> &InArcs = *TempInArcs;

	//Reset Output/Labels
	PricingProblem.ResetBiDirectionalLayeredLabelingAlgorithmMembers();

	//Obtain Labels
	std::list<ForwardLabel>** ForwardLabels = PricingProblem.LayeredForwardLabels;
	std::list<BackwardLabel>** BackwardLabels = PricingProblem.LayeredBackwardLabels;
	std::forward_list<PStep_MergeLabel> &CandidateMergeLabels = PricingProblem.CandidateMergeLabels;
	
	//Obtain and reset counter of CandidateMergeLabels
	int &NumberOfMergeLabels = Merger.NumberOfMergeLabels;
	NumberOfMergeLabels = 0;

	//Create initial forward and backward label:
	//Initialize labels
	ForwardLabels[0][0].emplace_front();
	BackwardLabels[0][0].emplace_front();
	//Obtain label
	ForwardLabel& InitialForwardLabel = ForwardLabels[0][0].front();
	BackwardLabel& InitialBackwardLabel = BackwardLabels[0][0].front();
	//Set values of label
	InitialForwardLabel.NumberOfCustomers = NumberOfCustomers;
	InitialBackwardLabel.NumberOfCustomers = NumberOfCustomers;
	InitialForwardLabel.LastLocation = Source;
	InitialBackwardLabel.FirstLocation = Sink;
	InitialForwardLabel.Cost = PricingProblem.ReducedCostConstant;
	InitialBackwardLabel.Cost = 0;
	for (short int i = 0; i < NumberOfCustomers; ++i){
		InitialForwardLabel.Reachable[i] = PricingProblem.Reachable[i];
		InitialBackwardLabel.Reachable[i] = PricingProblem.Reachable[i];
	}
	if (Source == 0){
		InitialForwardLabel.Load = 0;
	}
	else{
		InitialForwardLabel.Load = PricingProblem.Demand[Source - 1];
	}
	if (Sink == 0){
		InitialBackwardLabel.Load = 0;
	}
	else{
		InitialBackwardLabel.Load = PricingProblem.Demand[Sink - 1];
		InitialBackwardLabel.Reachable[Sink - 1] = false;
	}

	//Initialize out arcs of the source
	std::list<std::list<PStep_Arc>>::iterator InitialOutArcs = OutArcs.begin();
	//Initalize support
	short int Counter = 0;
	//Obtain the out arcs of the initial location
	while (Counter < Source){
		++Counter;
		++InitialOutArcs;
	}

	//Initialize the in arcs of the sink
	std::list<std::list<PStep_Arc>>::iterator InitialInArcs = InArcs.begin();
	//Reinitalize support
	Counter = 0;
	//Obtain the out arcs of the initial location
	while (Counter < Sink){
		++Counter;
		++InitialInArcs;
	}

	//Initizalize indicator indicating whether the source and sink are connected
	bool SourceAndSinkAreConnected = PricingProblem.CurrentTreeNodeArcIsRelevant[Source][Sink];
	
	//Forward Labeling:
	//*************************************************************************************

	//Check whether the maxnumber of forward arcs is greater than 0
	if (MaxNumberOfForwardArcs > 0){
		
		//Extend the initial Label:
		//*************************

		//Go over all arcs out of the initial customer
		for (auto CurrentArc = InitialOutArcs->begin(); CurrentArc != InitialOutArcs->end(); ++CurrentArc){
			//Obtain next node
			short int NextNode = CurrentArc->EndNode;
			//Check whether the next node is not the sink nor the depot and is reachable
			if (NextNode != Sink && NextNode>0 && InitialForwardLabel.Reachable[NextNode - 1]){
				//Extend
				Extender.Extend(InitialForwardLabel, *CurrentArc, ForwardLabels[1][NextNode - 1]);
			}

		}

		//Extend the labels in the layers after the initial layer:
		//********************************************************

		//Go over all layers after the initial layer and prior to the last layer
		for (short int Layer = 1; Layer < MaxNumberOfForwardArcs; ++Layer){
			//Initialize current customer
			short int CurrentNode = 0;
			//Go over all customers
			for (auto CurrentOutArcs = ++OutArcs.begin(); CurrentOutArcs != OutArcs.end(); ++CurrentOutArcs){
				//Update current customer
				++CurrentNode;
				//Go over all arcs
				for (auto CurrentArc = CurrentOutArcs->begin(); CurrentArc != CurrentOutArcs->end(); ++CurrentArc){
					//Obtain next node
					short int NextNode = CurrentArc->EndNode;
					//Check whether the next node is not the sink, nor the depot
					if (NextNode != Sink && NextNode > 0){

						//Obtain current list of Labels
						std::list<ForwardLabel>& CurrentListOfForwardLabels = ForwardLabels[Layer][CurrentNode - 1];
						//Obtain the target list of labels
						std::list<ForwardLabel>& TargetListOfForwardLabels = ForwardLabels[Layer + 1][NextNode - 1];
						//Extend all labels to the the next node
						for (auto CurrentForwardLabel = CurrentListOfForwardLabels.begin(); CurrentForwardLabel != CurrentListOfForwardLabels.end(); ++CurrentForwardLabel){
							//Check whether the next node is reachable
							if (CurrentForwardLabel->Reachable[NextNode - 1]){

								//Initialize new label
								ForwardLabel NewForwardLabel;

								//Extend
								Extender.Extend(*CurrentForwardLabel, *CurrentArc, NewForwardLabel);

								//Perform dominance:

								//Initialize indicator indicating that the new label itself is not yet dominated
								bool NewLabelIsNotDominated = true;
								//Initialize iterators
								std::list<ForwardLabel>::iterator NextLabel = TargetListOfForwardLabels.begin();
								//Go over all labels with lower reduced costs and check whether the new label is dominated
								while (NewLabelIsNotDominated && NextLabel != TargetListOfForwardLabels.end() && NextLabel->Cost < NewForwardLabel.Cost - Tolerance){
									//Check if the next label dominates the new label
									if (Dominator.Dominate(*NextLabel, NewForwardLabel)){
										//Update indicators
										NewLabelIsNotDominated = false;
									}
									//Otherwise, no dominance is observed
									else{
										//Update indicator
										++NextLabel;
									}
								}//end go over all labels to check dominance over new label

								//Go over all labels with "the same" reduced costs and check whether the new label dominates
								while (NewLabelIsNotDominated && NextLabel != TargetListOfForwardLabels.end() && NextLabel->Cost <= NewForwardLabel.Cost + Tolerance){
									//Check whether the new label dominates the next label
									if (Dominator.Dominate(NewForwardLabel, *NextLabel)){
										//Remove the next label
										NextLabel = TargetListOfForwardLabels.erase(NextLabel);
									}
									//The new label does not dominate, check whether the new label is dominated by the next
									else if (Dominator.Dominate(*NextLabel, NewForwardLabel)){
										//Update indicator
										NewLabelIsNotDominated = false;
									}
									else{
										//Neither labels dominate, update NextLabel
										++NextLabel;
									}
								}//end go over all labels to check dominance of new label

								//Check whether the new label is not dominated
								if (NewLabelIsNotDominated){

									//Add the NewLabel at the current position in the list
									NextLabel = TargetListOfForwardLabels.emplace(NextLabel, NewForwardLabel);
									//Update NextLabel
									++NextLabel;

									//Go over all labels with higher reduced costs and check whether the new label dominates
									while (NextLabel != TargetListOfForwardLabels.end()){
										//Check whether the new label dominates the next label
										if (Dominator.Dominate(NewForwardLabel, *NextLabel)){
											//Remove the next label
											NextLabel = TargetListOfForwardLabels.erase(NextLabel);
										}
										//Otherwise, no dominance is observed
										else{
											++NextLabel;
										}
									}//end go over all labels to check dominance of new label

								}//end check whether the new label was dominated

							}//end check whether the next node is reachable
						}//end go over all labels

					}//end check whether the next node is not the depot

				}//end go over all out arcs
			}//end go over all customers
		}//end go over all layers after the initial layer and prior to the last layer

	}//end check whether the MaxNumberOfForward arcs is greater than 0

	//Backward Labeling:
	//*************************************************************************************

	//Check whether the maxnumber of backward arcs is greater than 0
	if (MaxNumberOfBackwardArcs > 0){

		//Extend the initial Label:
		//*************************

		//Go over all arcs into the sink
		for (auto CurrentArc = InitialInArcs->begin(); CurrentArc != InitialInArcs->end(); ++CurrentArc){
			//Obtain next node
			short int NextNode = CurrentArc->StartNode;
			//Check whether the next node is not the sink, not the depot and MaxNumberOfBackwardArcs is not 0
			if (NextNode != Source && NextNode>0 && InitialBackwardLabel.Reachable[NextNode - 1] && MaxNumberOfBackwardArcs > 0){
				//Extend
				Extender.Extend(InitialBackwardLabel, *CurrentArc, BackwardLabels[1][NextNode - 1]);
			}

		}

		//Extend the labels in the layers after the initial layer:
		//********************************************************

		//Go over all layers after the initial layer and prior to the last layer
		for (short int Layer = 1; Layer < MaxNumberOfBackwardArcs; ++Layer){
			//Initialize current customer
			short int CurrentNode = 0;
			//Go over all customers
			for (auto CurrentInArcs = ++InArcs.begin(); CurrentInArcs != InArcs.end(); ++CurrentInArcs){
				//Update current customer
				++CurrentNode;
				//Go over all arcs
				for (auto CurrentArc = CurrentInArcs->begin(); CurrentArc != CurrentInArcs->end(); ++CurrentArc){
					//Obtain next node
					short int NextNode = CurrentArc->StartNode;
					//Check whether the nest node is not the source, nor the depot
					if (NextNode != Source && NextNode > 0){

						//Obtain current list of Labels
						std::list<BackwardLabel>& CurrentListOfBackwardLabels = BackwardLabels[Layer][CurrentNode - 1];
						//Obtain the target list of labels
						std::list<BackwardLabel>& TargetListOfBackwardLabels = BackwardLabels[Layer + 1][NextNode - 1];
						//Extend all labels to the the next node
						for (auto CurrentBackwardLabel = CurrentListOfBackwardLabels.begin(); CurrentBackwardLabel != CurrentListOfBackwardLabels.end(); ++CurrentBackwardLabel){
							//Check whether the next node is reachable
							if (CurrentBackwardLabel->Reachable[NextNode - 1]){

								//Initialize new label
								BackwardLabel NewBackwardLabel;

								//Extend
								Extender.Extend(*CurrentBackwardLabel, *CurrentArc, NewBackwardLabel);

								//Perform dominance:

								//Initialize indicator indicating that the new label itself is not yet dominated
								bool NewLabelIsNotDominated = true;
								//Initialize iterators
								std::list<BackwardLabel>::iterator NextLabel = TargetListOfBackwardLabels.begin();
								//Go over all labels with lower reduced costs and check whether the new label is dominated
								while (NewLabelIsNotDominated && NextLabel != TargetListOfBackwardLabels.end() && NextLabel->Cost < NewBackwardLabel.Cost - Tolerance){
									//Check if the next label dominates the new label
									if (Dominator.Dominate(*NextLabel, NewBackwardLabel)){
										//Update indicators
										NewLabelIsNotDominated = false;
									}
									//Otherwise, no dominance is observed
									else{
										//Update indicator
										++NextLabel;
									}
								}//end go over all labels to check dominance over new label

								//Go over all labels with "the same" reduced costs and check whether the new label dominates
								while (NewLabelIsNotDominated && NextLabel != TargetListOfBackwardLabels.end() && NextLabel->Cost <= NewBackwardLabel.Cost + Tolerance){
									//Check whether the new label dominates the next label
									if (Dominator.Dominate(NewBackwardLabel, *NextLabel)){
										//Remove the next label
										NextLabel = TargetListOfBackwardLabels.erase(NextLabel);
									}
									//The new label does not dominate, check whether the new label is dominated by the next
									else if (Dominator.Dominate(*NextLabel, NewBackwardLabel)){
										//Update indicator
										NewLabelIsNotDominated = false;
									}
									else{
										//Neither labels dominate, update NextLabel
										++NextLabel;
									}
								}//end go over all labels to check dominance of new label

								//Check whether the new label is not dominated
								if (NewLabelIsNotDominated){

									//Add the NewLabel at the current position in the list
									NextLabel = TargetListOfBackwardLabels.emplace(NextLabel, NewBackwardLabel);
									//Update NextLabel
									++NextLabel;

									//Go over all labels with higher reduced costs and check whether the new label dominates
									while (NextLabel != TargetListOfBackwardLabels.end()){
										//Check whether the new label dominates the next label
										if (Dominator.Dominate(NewBackwardLabel, *NextLabel)){
											//Remove the next label
											NextLabel = TargetListOfBackwardLabels.erase(NextLabel);
										}
										//Otherwise, no dominance is observed
										else{
											++NextLabel;
										}
									}//end go over all labels to check dominance of new label

								}//end check whether the new label was dominated

							}//end check whether the next node is reachable
						}//end go over all labels

					}//end check whether the next node is not the depot

				}//end go over all in arcs
			}//end go over all customers
		}//end go over all layers after the initial layer and prior to the last layer

	}//end check whether MaxNumberOfBackward arcs is greater than 0

	//Merge forward and backward labels:
	//*************************************************************************************

	//Initialize support variable indicating from which forward layer onward we merge
	//forward labels with backward labels in the last layer of backward labels.
	short int ForwardLayer = MaxNumberOfForwardArcs;

	//Check whether p-steps are allowed that use less than MaxNumberOfArcs
	if (AllowedToReachSinkEarly || MaxNumberOfForwardArcs == 0){

		//Update ForwardLayer
		ForwardLayer = 1;

		//Initialize backwardlayer
		short int BackwardLayer = 1;
		if (MaxNumberOfForwardArcs == 0 && !AllowedToReachSinkEarly){
			BackwardLayer = MaxNumberOfBackwardArcs;
		}

		//Merge labels with the InitialForwardLabel:
		//******************************************

		//Merge the initial forward label with the initial backward label
		//Check whether the resulting reduced costs would be negative
		if (SourceAndSinkAreConnected && (AllowedToReachSinkEarly || MaxNumberOfBackwardArcs == 0) && InitialForwardLabel.Cost + ReducedArcCosts[Source][Sink] + InitialBackwardLabel.Cost < -Tolerance){
			//Merge
			Merger.Merge(InitialForwardLabel, InitialBackwardLabel);
		}

		//Go over all remaining layers of backward labels
		for (; BackwardLayer <= MaxNumberOfBackwardArcs; ++BackwardLayer){
			//Go over all arcs out of the source
			for (auto CurrentArc = InitialOutArcs->begin(); CurrentArc != InitialOutArcs->end() && NumberOfMergeLabels < MaxMergeLabels; ++CurrentArc){
				//Obtain next node
				short int NextNode = CurrentArc->EndNode;
				//Check whether the nextNode is not the sink nor the depot
				if (NextNode != Sink && NextNode != 0){
					//Obtain the labels in the current layer for the location pointed to by the out arc
					std::list<BackwardLabel> &CurrentBackwardLabels = BackwardLabels[BackwardLayer][NextNode-1];
					//Initialize iterator
					std::list<BackwardLabel>::iterator CurrentBackwardLabel = CurrentBackwardLabels.begin();
					//Go over all current backward labels until all have been processed or the reduced costs are negative
					//NOTE: we assume CurrentBackwardLabels is sorted to increasing reduced costs.
					while (CurrentBackwardLabel != CurrentBackwardLabels.end() && NumberOfMergeLabels < MaxMergeLabels && InitialForwardLabel.Cost + ReducedArcCosts[Source][NextNode] + CurrentBackwardLabel->Cost < -Tolerance){
						//Merge
						Merger.Merge(InitialForwardLabel, *CurrentBackwardLabel);
						//Update current backward label
						++CurrentBackwardLabel;
					}//end go over all current backward labels to merge
				}//end chech whether the nextNode is not the sink nor the depot
			}//end go over all arcs out of the source
		}//end go over all remaining backward layers

	}//end check whether p-steps are allowed that use less than MaxNumberOfArcs

	//Merge labels with the last layer of BackwardLabels:
	//***************************************************
	//Check whether the last layer of backward labels is also the first,
	//i.e., the only label in this layer would correspond to the InitialBackwardLabel from the sink.
	if (MaxNumberOfBackwardArcs == 0){
		//The last layer of backward labels only contains the InitialBackwardLabel.

		//Go over all remaining layers of forward labels
		for (; ForwardLayer <= MaxNumberOfForwardArcs && NumberOfMergeLabels < MaxMergeLabels; ++ForwardLayer){
			//Go over all arcs into the sink
			for (auto CurrentArc = InitialInArcs->begin(); CurrentArc != InitialInArcs->end() && NumberOfMergeLabels < MaxMergeLabels; ++CurrentArc){
				//Obtain next node
				short int NextNode = CurrentArc->StartNode;
				//Check whether the nextNode is not the source nor the depot
				if (NextNode != Source && NextNode != 0){
					//Obtain the labels in the current layer for the location pointed to by the in arc
					std::list<ForwardLabel> &CurrentForwardLabels = ForwardLabels[ForwardLayer][NextNode-1];
					//Initialize iterator
					std::list<ForwardLabel>::iterator CurrentForwardLabel = CurrentForwardLabels.begin();
					//Go over all current forward labels until all have been processed or the reduced costs are negative
					//NOTE: we assume CurrentForwardLabels are sorted to increasing reduced costs.
					while (CurrentForwardLabel != CurrentForwardLabels.end() && NumberOfMergeLabels < MaxMergeLabels && CurrentForwardLabel->Cost + ReducedArcCosts[NextNode][Sink] + InitialBackwardLabel.Cost < -Tolerance){
						//Merge
						Merger.Merge(*CurrentForwardLabel, InitialBackwardLabel);
						//Update current forward label
						++CurrentForwardLabel;
					}//end go over all current forward labels to merge
				}//end chech whether the nextNode is not the source nor the depot
			}//end go over all arcs into the sink
		}//end go over all remaining layers of forward labels

	}
	else{
		//The last layer of backward labels should not contain the sink.

		//Obtain the last layer of backward labels
		std::list<BackwardLabel>* &LastLayerOfBackwardLabels = BackwardLabels[MaxNumberOfBackwardArcs];
		//Go over all layers of forward labels to merge
		for (; ForwardLayer <= MaxNumberOfForwardArcs && NumberOfMergeLabels < MaxMergeLabels; ++ForwardLayer){
			//Initialize current customer
			short int CurrentNode = 0;
			//Go over all customers
			for (auto CurrentOutArcs = ++OutArcs.begin(); CurrentOutArcs != OutArcs.end() && NumberOfMergeLabels < MaxMergeLabels; ++CurrentOutArcs){
				//Update current customer
				++CurrentNode;
				//Go over all arcs
				for (auto CurrentArc = CurrentOutArcs->begin(); CurrentArc != CurrentOutArcs->end() && NumberOfMergeLabels < MaxMergeLabels; ++CurrentArc){
					//Obtain next node
					short int NextNode = CurrentArc->EndNode;
					//Check whether the next node is not the sink, nor the depot
					if (NextNode != Sink && NextNode > 0){

						//Obtain current list of Labels
						std::list<ForwardLabel>& CurrentListOfForwardLabels = ForwardLabels[ForwardLayer][CurrentNode - 1];
						//Obtain the target list of labels
						std::list<BackwardLabel>& CurrentListOfBackwardLabels = LastLayerOfBackwardLabels[NextNode - 1];
						
						//Check whethher the lists of forward and backward labels are not empty
						if (!CurrentListOfForwardLabels.empty() && !CurrentListOfBackwardLabels.empty()){
							//Obtain the reduced cost of the current arc
							double CurrentReducedArcCost = ReducedArcCosts[CurrentNode][NextNode];

							//Initialize indicator indicating that the reduced costs resulting from a merge might still be negative
							bool ReducedCostsMightBeNegative = true;

							//Initialize iterator of the forward labels
							std::list<ForwardLabel>::iterator CurrentForwardLabel = CurrentListOfForwardLabels.begin();
							//Go over all forward labels until all have been processed or the reduced costs will not be positive anymore.
							while (CurrentForwardLabel != CurrentListOfForwardLabels.end() && NumberOfMergeLabels < MaxMergeLabels  && ReducedCostsMightBeNegative){

								//Initialize iterator of the backward labels
								std::list<BackwardLabel>::iterator CurrentBackwardLabel = CurrentListOfBackwardLabels.begin();

								//Check the first backward label
								if (CurrentForwardLabel->Cost + CurrentReducedArcCost + CurrentBackwardLabel->Cost < -Tolerance){
									//Merge
									Merger.Merge(*CurrentForwardLabel, *CurrentBackwardLabel);

									//Update the current backward label
									++CurrentBackwardLabel;

									//Go over all current backward labels until all have been processed or the reduced costs are negative
									//NOTE: we assume CurrentBackwardLabels is sorted to increasing reduced costs.
									while (CurrentBackwardLabel != CurrentListOfBackwardLabels.end() && NumberOfMergeLabels < MaxMergeLabels && CurrentForwardLabel->Cost + CurrentReducedArcCost + CurrentBackwardLabel->Cost < -Tolerance){
										//Merge
										Merger.Merge(*CurrentForwardLabel, *CurrentBackwardLabel);
										//Update current backward label
										++CurrentBackwardLabel;
									}//end go over all current backward labels to merge

								}
								else{
									//No negative reduced cost merges can occur anymore for the current list of forward and backward labels.
									//This is under the assumption that both are sorted in increasing order of reduced costs.
									ReducedCostsMightBeNegative = false;
								}

								//Update forward label
								++CurrentForwardLabel;
							}//end go over forward labels

						}//end check whether the lists of forward and backward labels are not empty
					}//end check whether the next node is not the sink or depot
				}//end go over all arcs out of the current customer
			}//end go over all customers
		}//Go over all layers of forward labels to merge

	}//end check whether the the last layer of backward labels is also the first

	//Add the current pricing problem to the list of solved pricing problems
	SolvedPricingProblems.push_front(&PricingProblem);

}