/*Implementation of PStep_ShortestPathAlgorithm.h

It contains the member functions of PStep_LocalSearchFixedArcs

*/

#include "PStep_ShortestPathAlgorithm.h"

//PStep_LocalSearchFixedArcs:
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

//Solve the pricing problem
//------------------------------------------------------------------------------------

//Use local search algorithm to solve the pricing problem.
//The number of arcs are fixed and elementarity is respected.
//It applies in case a p-step is created which does not start at the depot.
//All identified p-steps with nonegative reduced costs are stored in the forward list CandidatePSteps.
//NOTE: For a pre-initialized p-step with 2 arcs, we do not attempt to improve it,
//	    even though it might be possible to find an improvement. The function SwapDoublesCurrentPStep
//		is available to facilitate initializing with a cyclic p-step.
void PStep_LocalSearchFixedArcs::Run(){

	//Initialize indicator indicating whether a p-step is present after initialization
	bool PStepPresentAfterInitialization = InitialPStepProvided;
	
	//Check whether an initial p-step was provided
	if (InitialPStepProvided){
		//Initialize OutsideCustomers
		InitializeOutsideCustomers();
	}
	else{
		//Greedily generate an initial p-step
		PStepPresentAfterInitialization = GenerateGreedyPStep();
	}
	
	//Check if initialization was succesful
	if (PStepPresentAfterInitialization){
		//Initialization was succesful
		
		//Check, whether the maximum number of arcs is more than 2.
		//If it is less, greedy insert is optimal.
		//NOTE: This means that for a pre-initialized p-step with 2 arcs, we do not attempt
		//	    to improve it, even though it might be possible to find an improvement.
		if (MaxNumberOfArcs > 2){

			//Initialize local search:

			//Initialize the current best customer to be inserted by Swap
			std::vector<short int>::iterator BestSwapInsertCustomer;
			//Initialize the current best customer to be removed by Swap,
			//the customer after this iterator is removed
			std::list<short int>::iterator BestSwapRemoveCustomer;
			//Initialize increase in reduced cost of the current best Swap
			double BestSwapCostIncrease;

			//Initialize the current best customer to be inserted by SwapIntraPStep
			std::list<short int>::iterator BestSwapIntraPStepFirstCustomer;
			//Initialize the current best customer to be removed by Swap,
			//the customer after this iterator is removed
			std::list<short int>::iterator BestSwapIntraPStepSecondCustomer;
			//Initialize increase in reduced cost of the current best Swap
			double BestSwapIntraPStepCostIncrease;

			//Initialize indicator indicating whether an improvement is found
			bool ImprovementFound = true;

			//Perform local search until a termination criterion is met
			while (CurrentReducedCosts >= -Tolerance && ImprovementFound){

				//Reset ImprovementFound
				ImprovementFound = false;

				//Find best swap
				bool SwapFound = Swap(BestSwapInsertCustomer, BestSwapRemoveCustomer, BestSwapCostIncrease);

				//Find best intra swap
				bool SwapIntraPStepFound = SwapIntraPStep(BestSwapIntraPStepFirstCustomer, BestSwapIntraPStepSecondCustomer, BestSwapIntraPStepCostIncrease);

				//Process moves:
				//Check whether a swap was found and whether the corresponding cost increase is better
				//than that of the best intra p-step swap.
				//Note that an intra p-step swap can always be found.
				if (SwapFound && (!SwapIntraPStepFound || BestSwapCostIncrease < BestSwapIntraPStepCostIncrease)){
					//The best move is a swap.

					//Check whether the cost increase is negative.
					if (BestSwapCostIncrease < -Tolerance){
						//Perform swap

						//Obtain customer to be inserted
						short int InsertedCustomer = *BestSwapInsertCustomer;

						//Remove customer
						*BestSwapInsertCustomer = *BestSwapRemoveCustomer;
						//Insert customer
						*BestSwapRemoveCustomer = InsertedCustomer;

						//Update reduced costs
						CurrentReducedCosts += BestSwapCostIncrease;
						//Update demand, note that BestSwapInsertedCustomer now points to the removed customer
						CurrentDemand += DemandIncrease[InsertedCustomer - 1][*BestSwapInsertCustomer - 1];

						//Update indicator indicating an improvement is found
						ImprovementFound = true;

					}

				}
				//No swap was found or the intra p-step swap provides a better cost increase.
				//Check whether the cost increase is negative.
				else if (SwapIntraPStepFound && BestSwapIntraPStepCostIncrease<-Tolerance){
					//Perform intra p-step swap

					//Obtain first customer
					short int FirstCustomer = *BestSwapIntraPStepFirstCustomer;

					//Put second customer in the position of the first
					*BestSwapIntraPStepFirstCustomer = *BestSwapIntraPStepSecondCustomer;
					//Put first customer in the position of the second
					*BestSwapIntraPStepSecondCustomer = FirstCustomer;

					//Update reduced costs
					CurrentReducedCosts += BestSwapIntraPStepCostIncrease;

					//Update indicator indicating an improvement is found
					ImprovementFound = true;

				}

			}//end local search

		}//end check whether the maximum number of arcs is more than 2
		
		//Check whether a negative reduced cost p-step is found
		if (CurrentReducedCosts < -Tolerance){
			//A negative reduced cost p-step is found

			//Set costs, starting load and total load of the current p-step
			SetCurrentPStepCostAndLoads();

			//Check whether a candidate p-step has already been identified for the current pricing problem
			if (CandidatePSteps.begin() == CandidatePSteps.end()){
				//Add the current pricing problem to the list of solved pricing problems
				SolvedPricingProblems.push_front(&PricingProblem);
			}

			//Add p-step to list of candidates
			CandidatePSteps.push_front(CurrentPStep);

		}

	}//end check if initialization was succesful
	
}

//Local search functions
//------------------------------------------------------------------------------------

//Swap double customers from the current p-step.
//For customers that are visited multiple times, the first is maintained and
//the others are swapped.
//The customers are swapped with the best location not in the current p-step.
//It returns false if the current p-step could not be transformed into an elementary path.
//In that case, the current p-step might be altered.
bool PStep_LocalSearchFixedArcs::SwapDoublesCurrentPStep(){

	//Check whether there are customers to be swapped
	if (MaxNumberOfArcs < 3){
		return true;
	}

	//Obtain current locations
	std::list<short int>& Locations = CurrentPStep.Locations;

	//Initialize indicators indicating whether a location has been visited already
	bool* Visited = new bool[NumberOfLocations];
	//Set all indicators to false
	for (short int i = 0; i < NumberOfLocations; ++i){
		Visited[i] = false;
	}

	//Set source and sink to visited
	Visited[Source] = true;
	Visited[Sink] = true;

	//Initialize vector indicating which locations should be swapped
	std::vector<bool> Swapped;
	Swapped.push_back(false);

	//Initialize second and last location
	auto SecondLocation = Locations.begin();
	++SecondLocation;
	auto LastLocation = Locations.end();
	--LastLocation;

	//Go over all customers and set the visited list and the swapped list
	for (auto i = SecondLocation; i != LastLocation; ++i){
		//Check whether the location has already been visited
		if (Visited[*i]){
			//It needs to be swapped
			Swapped.push_back(true);
		}
		else{
			//It is visited for the first time
			Visited[*i] = true;
			Swapped.push_back(false);
		}
	}
	//Set last location not to be swapped
	Swapped.push_back(false);

	//Initialize list of customers that could be swapped into the route
	std::vector<short int> TempOutsideCustomers;
	//Go over all customers and add the customers that are not visited to TempOutsideCustomers
	for (short int i = 1; i < NumberOfLocations; ++i){
		//Check if the current customer i has not yet been visited
		if (!Visited[i]){
			TempOutsideCustomers.push_back(i);
		}
	}

	//clean
	delete[]Visited;

	//Initialize iterator pointing to swapped for the current location
	auto CurrentSwapped = Swapped.begin();
	//Go over all customers to be swapped out
	for (auto CurrentCustomer = SecondLocation; CurrentCustomer != LastLocation; ++CurrentCustomer){
		//Update CurrentSwapped
		++CurrentSwapped;
		//Check whether the current customer needs to be swapped
		if (*CurrentSwapped){
			//The current customer needs to be swapped.

			//Initialize indicator indicating whether a feasible swap is found for the current location
			bool FeasibleSwapNotYetFound = true;

			//Initialize best solution
			std::vector<short int>::iterator CurrentBestInsertCustomer;
			double CurrentBestCostIncrease;

			//Go over all outside locations
			for (auto InsertedCustomer = TempOutsideCustomers.begin(); InsertedCustomer != TempOutsideCustomers.end(); ++InsertedCustomer){

				//Check whether swapping CurrentCustomer with InsertedCustomer would not yield a vioalation
				//of the capacity constraint.
				if (CurrentDemand + DemandIncrease[*InsertedCustomer - 1][(*CurrentCustomer) - 1]<CapacityPlusTolerance){

					//Initialize previous location, removed location and insertion costs
					auto PreviousLocation = CurrentCustomer;
					--PreviousLocation;
					auto NextLocation = CurrentCustomer;
					++NextLocation;
					double SwapCost;

					//Check if the arcs required for the swap are in use
					if (ArcInUse[*PreviousLocation][*InsertedCustomer] && ArcInUse[*InsertedCustomer][*NextLocation]){

						//Compute the costs of the current swap
						SwapCost = ReducedArcCosts[*PreviousLocation][*InsertedCustomer] + ReducedArcCosts[*InsertedCustomer][*NextLocation] - ReducedArcCosts[*PreviousLocation][*CurrentCustomer] - ReducedArcCosts[*CurrentCustomer][*NextLocation];

						//Check if the insertion cost is the best found so far
						if (FeasibleSwapNotYetFound || SwapCost < CurrentBestCostIncrease){
							//The insertion cost is the best found so far, update the best
							CurrentBestInsertCustomer = InsertedCustomer;
							CurrentBestCostIncrease = SwapCost;
							FeasibleSwapNotYetFound = false;
						}

					}//end check whether the required arcs are in use

				}//end check capacity constraint

			}//end go over all outside customers

			//Check whether a feasible swap was not found
			if (FeasibleSwapNotYetFound){
				return false;
			}
			else{
				//A feasible swap is found, perform swap

				//Update reduced costs
				CurrentReducedCosts += CurrentBestCostIncrease;
				//Update demand, note that CurrentBestInsertCustomer now points to the removed customer
				CurrentDemand += DemandIncrease[*CurrentBestInsertCustomer - 1][*CurrentCustomer - 1];

				//Insert customer
				*CurrentCustomer = *CurrentBestInsertCustomer;

				//Remove inserted customer from TempOutsideCustomers
				TempOutsideCustomers.erase(CurrentBestInsertCustomer);

			}

		}
	}

	//Output
	return true;

}

//Initialize OutsideLocations
//It puts all customers that are not in CurrentPStep in OutsideLocations.
void PStep_LocalSearchFixedArcs::InitializeOutsideCustomers(){

	//Empty OutsideLocations.
	OutsideCustomers.clear();

	//Initialize indicator indicating whether a customer is possibly outside
	bool CurrentLocationPossiblyOutside;

	//Put all customers that are not in the current p-step in OutsideCustomers
	for (short int i = 1; i < NumberOfLocations; ++i){
		//Reset indicator
		//If it is not reachable at all, set it to false immediately
		CurrentLocationPossiblyOutside = Reachable[i - 1];
		//Go over all customers in CurrentPStep and see if i is there
		for (auto j = CurrentPStep.Locations.begin(); j != CurrentPStep.Locations.end() && CurrentLocationPossiblyOutside; ++j){
			if (i == (*j)){
				CurrentLocationPossiblyOutside = false;
			}
		}
		//Check whether the current customer is outside the current p-step
		if (CurrentLocationPossiblyOutside){
			OutsideCustomers.push_back(i);
		}
	}

}

//Generate an initial p-step from source to sink, inserting customers greedily until
//MaxNumberOfArcs arcs are included.
//It is assumed that CurrentPStep is empty.
//The PStep is stored in CurrentPStep.
//If no feasible PStep could be made, output false.
bool PStep_LocalSearchFixedArcs::GenerateGreedyPStep(){

	//Obtain Locations
	std::list<short int>& Locations = CurrentPStep.Locations;

	//Initialize Locations

	//Add source and sink:
	Locations.push_back(Source);
	Locations.push_back(Sink);
	//Initialize current demand
	CurrentDemand = 0;
	if (Source > 0){
		CurrentDemand += Demand[Source - 1];
	}
	if (Sink > 0){
		CurrentDemand += Demand[Sink - 1];
	}
	//Update the reduced cost constant by adding the reduced arc cost from source to sink
	CurrentReducedCosts = ReducedCostConstant + ReducedArcCosts[Source][Sink];

	//Check whether the maximum number of arcs is 1, in that case terminate.
	if (MaxNumberOfArcs == 1){
		return ArcInUse[Source][Sink];
	}

	//Initialize OutsideLocations
	InitializeOutsideCustomers();

	//Insert customers until a total of MaxNumberOfArcs arcs are included:

	//Initialize the current best customer to be inserted
	std::vector<short int>::iterator BestInsertCustomer;
	//Initialize best insert position, the current best customer is inserted before this position
	std::list<short int>::iterator BestInsertPosition;
	//Initialize increase in reduced cost of the current best insert
	double BestInsertCostIncrease;

	//Initialize indicator indicating whether the insert failed
	//Go over all to be inserted arcs
	for (short int ArcCounter = 1; ArcCounter < MaxNumberOfArcs; ++ArcCounter){

		//Find best insert
		if (Insert(BestInsertCustomer, BestInsertPosition, BestInsertCostIncrease)){

			//The insert was succesful, add the best customer at the insert position
			Locations.insert(BestInsertPosition, *BestInsertCustomer);
			//Update the corresponding load
			CurrentDemand += Demand[(*BestInsertCustomer) - 1];
			//Update the remaining customers
			OutsideCustomers.erase(BestInsertCustomer);
			//Update reduced costs
			CurrentReducedCosts += BestInsertCostIncrease;

		}
		else{
			//The insert failed, hence so did the greedy insertion
			return false;
		}

	}

	return true;

}

//Find the best insert in CurrentPStep
//The best location to insert from OutsideLocations will be stored in CurrentBestCustomer,
//Location 0 is the depot and is never considered for insertion.
//The best insert position is stored as the reference CurrentBestPosition to the next location in
//CurrentPStep.
//The best cost increase is stored in CurrentBestCostIncrease.
//We aim for the lowest cost increase, ideally negative, i.e., a decrease.
//If no feasible insert is found the function outputs false.
bool PStep_LocalSearchFixedArcs::Insert(std::vector<short int>::iterator& CurrentBestCustomer, std::list<short int>::iterator& CurrentBestPosition, double& CurrentBestCostIncrease){

	//Initialize indicator indicating whether a feasible insert is not yet found
	bool FeasibleInsertNotYetFound = true;

	//Obtain locations
	std::list<short int>& Locations = CurrentPStep.Locations;
	//Obtain iterator of the second location
	std::list<short int>::iterator SecondLocation = Locations.begin();
	++SecondLocation;

	//Go over all outside locations
	for (auto CurrentCustomer = OutsideCustomers.begin(); CurrentCustomer != OutsideCustomers.end(); ++CurrentCustomer){

		//Obtain current demand
		double CurrentCustomerDemand = Demand[(*CurrentCustomer) - 1];
		//Check whether the current customer can be added to the route
		if (CurrentDemand + CurrentCustomerDemand <= CapacityPlusTolerance){
			//Consider every position on the route for insertion:

			//Initialize previous location and insertion costs
			short int PreviousLocation = Source;
			double InsertionCost;

			//Go over all customers on the route
			for (auto NextCustomer = SecondLocation; NextCustomer != Locations.end(); ++NextCustomer){

				//Check if the arcs required for insertion are in use
				if (ArcInUse[PreviousLocation][*CurrentCustomer] && ArcInUse[*CurrentCustomer][*NextCustomer]){

					//Compute costs of insertion
					InsertionCost = ReducedArcCosts[PreviousLocation][*CurrentCustomer] + ReducedArcCosts[*CurrentCustomer][*NextCustomer] - ReducedArcCosts[PreviousLocation][*NextCustomer];
					//Check if the insertion cost is the best found so far
					if (FeasibleInsertNotYetFound || InsertionCost < CurrentBestCostIncrease){
						//The insertion cost is the best found so far, update the best
						CurrentBestCustomer = CurrentCustomer;
						CurrentBestPosition = NextCustomer;
						CurrentBestCostIncrease = InsertionCost;
						FeasibleInsertNotYetFound = false;
					}

				} //end check whether the required arcs for insertion are in use

				//Update the previous location
				PreviousLocation = *NextCustomer;

			}//end go over all customers on the route

		}//end check whether the demand of CurrentCustomer fits in the current route

	}//end go over all outside customers

	//Output
	return !FeasibleInsertNotYetFound;

}

//Find the best swap between customer in the current p-step and customers from OutsideLocations.
//The start and end locations of the current p-step are never swapped.
//The best customer to be removed is stored as an iterator, CurrentBestRemoveCustomer, pointing
//to the customer in the list before the customer to be removed.
//The best location to insert from OutsideLocations will be stored in CurrentBestInsertCustomer.
//The best cost increase is stored in CurrentBestCostIncrease.
//We aim for the lowest cost increase, ideally negative, i.e., a decrease.
//If no feasible insert is found the function outputs false
bool PStep_LocalSearchFixedArcs::Swap(std::vector<short int>::iterator& CurrentBestInsertCustomer, std::list<short int>::iterator& CurrentBestRemoveCustomer, double& CurrentBestCostIncrease){

	//Initialize indicator indicating whether a feasible insert is not yet found
	bool FeasibleSwapNotYetFound = true;

	//Obtain locations
	std::list<short int>& Locations = CurrentPStep.Locations;
	//Obtain iterator of the second location
	std::list<short int>::iterator SecondLocation = Locations.begin();
	++SecondLocation;
	//Obtain iterator to a potential third location
	std::list<short int>::iterator ThirdLocation = SecondLocation;
	++ThirdLocation;

	//Go over all outside locations
	for (auto InsertedCustomer = OutsideCustomers.begin(); InsertedCustomer != OutsideCustomers.end(); ++InsertedCustomer){

		//Obtain current demand increase
		double* CurrentDemandIncrease = DemandIncrease[(*InsertedCustomer) - 1];

		//Consider every position on the p-step for insertion:

		//Initialize previous location, removed location and insertion costs
		short int PreviousLocation = Source;
		auto RemovedCustomer = SecondLocation;
		double SwapCost;

		//Go over all customers on the route
		for (auto NextLocation = ThirdLocation; NextLocation != Locations.end(); ++NextLocation){

			//Check if the arcs required for the swap are in use
			if (ArcInUse[PreviousLocation][*InsertedCustomer] && ArcInUse[*InsertedCustomer][*NextLocation]){

				//Check whether swapping CurrentCustomer with RemovedCustomer would not yield a vioalation
				//of the capacity constraint
				if (CurrentDemand + CurrentDemandIncrease[*RemovedCustomer - 1] < CapacityPlusTolerance){

					//Compute the costs of the current swap
					SwapCost = ReducedArcCosts[PreviousLocation][*InsertedCustomer] + ReducedArcCosts[*InsertedCustomer][*NextLocation] - ReducedArcCosts[PreviousLocation][*RemovedCustomer] - ReducedArcCosts[*RemovedCustomer][*NextLocation];

					//Check if the insertion cost is the best found so far
					if (FeasibleSwapNotYetFound || SwapCost < CurrentBestCostIncrease){
						//The insertion cost is the best found so far, update the best
						CurrentBestInsertCustomer = InsertedCustomer;
						CurrentBestRemoveCustomer = RemovedCustomer;
						CurrentBestCostIncrease = SwapCost;
						FeasibleSwapNotYetFound = false;
					}

				}//end go over all customers on the route

			}//end check whether the required arcs are in use

			//Update the previous location and swapped customer
			PreviousLocation = *RemovedCustomer;
			RemovedCustomer = NextLocation;

		}//end check capacity constraint

	}//end go over all outside customers

	//Output
	return !FeasibleSwapNotYetFound;

}

//Find the best swap between two customers in the current p-step.
//The start and end locations of the current p-step are never swapped.
//The customers to be swapped are stored as iterators, CurrentBestFirstCustomer and
//CurrentBestSecondCustomer, pointing to the customers in the list to be swapped.
//The best cost increase is stored in CurrentBestCostIncrease.
//We aim for the lowest cost increase, ideally negative, i.e., a decrease.
//If no feasible insert is found the function outputs false
//NOTE: It is assumed that the current p-step contains at least four locations
bool PStep_LocalSearchFixedArcs::SwapIntraPStep(std::list<short int>::iterator& CurrentBestFirstCustomer, std::list<short int>::iterator& CurrentBestSecondCustomer, double& CurrentBestCostIncrease){

	//Initialize indicator indicating whether a feasible insert is not yet found
	bool FeasibleSwapNotYetFound = true;

	//Obtain locations
	std::list<short int>& Locations = CurrentPStep.Locations;
	//Obtain iterator of the second location
	std::list<short int>::iterator SecondLocation = Locations.begin();
	++SecondLocation;
	//Obtain iterator to the last location
	std::list<short int>::iterator LastLocation = Locations.end();
	--LastLocation;
	//Obtain iterator to the semi last location
	std::list<short int>::iterator SemiLastLocation = LastLocation;
	--SemiLastLocation;
	//Initialize Swap cost
	double SwapCost;

	//Initialize customer before and after the first customer
	short int BeforeFirstCustomer = Source;
	std::list<short int>::iterator AfterFirstCustomer = SecondLocation;
	//Initialize customer before and after the second customer
	short int BeforeSecondCustomer;
	std::list<short int>::iterator AfterSecondCustomer;

	//Go over all first customers to swap
	for (auto FirstCustomer = SecondLocation; FirstCustomer != SemiLastLocation; ++FirstCustomer){

		//Update after next customer
		++AfterFirstCustomer;

		//Initialize after second location
		AfterSecondCustomer = AfterFirstCustomer;
		++AfterSecondCustomer;

		//Initialize before second location, it will actually currently be SecondCustomer
		BeforeSecondCustomer = *AfterFirstCustomer;

		//Check if the arcs required for the swap are in use
		if (ArcInUse[BeforeFirstCustomer][BeforeSecondCustomer] && ArcInUse[BeforeSecondCustomer][*FirstCustomer] && ArcInUse[*FirstCustomer][*AfterSecondCustomer]){

			//Process the succesor of FirstCustomer as Second Customer
			//NOTE: BeforeSecondCustomer is currently actually SecondCustomer
			SwapCost = ReducedArcCosts[BeforeFirstCustomer][BeforeSecondCustomer] + ReducedArcCosts[BeforeSecondCustomer][*FirstCustomer] + ReducedArcCosts[*FirstCustomer][*AfterSecondCustomer] - ReducedArcCosts[BeforeFirstCustomer][*FirstCustomer] - ReducedArcCosts[*FirstCustomer][BeforeSecondCustomer] - ReducedArcCosts[BeforeSecondCustomer][*AfterSecondCustomer];

			//Check if the insertion cost is the best found so far
			if (FeasibleSwapNotYetFound || SwapCost < CurrentBestCostIncrease){
				//The insertion cost is the best found so far, update the best
				CurrentBestFirstCustomer = FirstCustomer;
				CurrentBestSecondCustomer = AfterFirstCustomer;
				CurrentBestCostIncrease = SwapCost;
				FeasibleSwapNotYetFound = false;
			}

		}//end check whether the required arcs are in use

		//Go over all second customers to swap
		for (auto SecondCustomer = AfterSecondCustomer; SecondCustomer != LastLocation; ++SecondCustomer){

			//Update after second customer
			++AfterSecondCustomer;

			//Check if the arcs required for the swap are in use
			if (ArcInUse[BeforeFirstCustomer][*SecondCustomer] && ArcInUse[*SecondCustomer][*AfterFirstCustomer] && ArcInUse[BeforeSecondCustomer][*FirstCustomer] && ArcInUse[*FirstCustomer][*AfterSecondCustomer]){

				//Compute the costs of the current swap
				SwapCost = ReducedArcCosts[BeforeFirstCustomer][*SecondCustomer] + ReducedArcCosts[*SecondCustomer][*AfterFirstCustomer] - ReducedArcCosts[BeforeFirstCustomer][*FirstCustomer] - ReducedArcCosts[*FirstCustomer][*AfterFirstCustomer] + ReducedArcCosts[BeforeSecondCustomer][*FirstCustomer] + ReducedArcCosts[*FirstCustomer][*AfterSecondCustomer] - ReducedArcCosts[BeforeSecondCustomer][*SecondCustomer] - ReducedArcCosts[*SecondCustomer][*AfterSecondCustomer];

				//Check if the insertion cost is the best found so far
				if (FeasibleSwapNotYetFound || SwapCost < CurrentBestCostIncrease){
					//The insertion cost is the best found so far, update the best
					CurrentBestFirstCustomer = FirstCustomer;
					CurrentBestSecondCustomer = SecondCustomer;
					CurrentBestCostIncrease = SwapCost;
					FeasibleSwapNotYetFound = false;
				}

			}//end check whether the required arcs are in use

			//Update before the second customer
			BeforeSecondCustomer = *SecondCustomer;

		}//end go over all second customers

		//Update before first customer
		BeforeFirstCustomer = *FirstCustomer;

	}//end go over all first customers

	return !FeasibleSwapNotYetFound;
}

//Set the cost, starting load and total load of a p-step for which the locations are already set.
void PStep_LocalSearchFixedArcs::SetCurrentPStepCostAndLoads(){

	//Obtain Locations, cost and loads
	std::list<short int>& Locations = CurrentPStep.Locations;
	double& Cost = CurrentPStep.Cost;
	double& StartingLoad = CurrentPStep.StartingLoad;
	double& TotalLoad = CurrentPStep.TotalLoad;

	//Initialize costs and loads
	Cost = 0;
	StartingLoad = 0;
	TotalLoad = 0;

	//Initialize the PreviousLocation as the first location
	auto PreviousLocation = Locations.begin();
	//Initialize NextLocation
	auto NextLocation = PreviousLocation;
	++NextLocation;
	//Go over all following locations
	for (; NextLocation != Locations.end(); ++NextLocation){
		//Update the cost
		Cost += TravelCosts[*PreviousLocation][*NextLocation];
		//Update PreviousLocation
		PreviousLocation = NextLocation;
	}

	//Check whether minimal or maximal starting load is required
	if (MinimalStartingLoad){
		//Minimal starting load is used:
		//Check whether the first location is not the depot
		if (Locations.front() > 0){
			//Update starting load
			StartingLoad = Demand[Locations.front() - 1];
		}
		//Set total load
		TotalLoad = CurrentDemand;
	}
	else{
		//Maximal starting load is used:
		//Initialize starting load
		StartingLoad = Capacity - CurrentDemand;
		//Check whether the first location is not the depot
		if (Locations.front() > 0){
			//Update starting load
			StartingLoad += Demand[Locations.front() - 1];
		}
		//Set total load
		TotalLoad = Capacity;
	}

}