/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
#include "stdafx.h"
#include "Pricer.h"

#include <iostream>
#include <sstream>
#include <string> 
#include <map>
#include <vector>
#include <algorithm>    // std::max
#include <memory.h>
#include "scip/cons_linear.h"
#include "scip/scip.h"
#include "objscip/objscip.h"
#include "objscip/objscipdefplugins.h"
#include "scip/scipdefplugins.h"
//#include "gurobi_c++.h"
#include "QuadRootsRevJ.h"

#include "BranchingConstraints.h"
#include "Customer.h"
#include "Label.h"
#include <queue>          // std::queue
#include <iostream>
#include <fstream>
#include <cstdint>
#include <deque>
#include <vector>

// helpers must be visible before Pricer::Pricer
static inline int bitsetWords(int n) { return (n + 63) >> 6; }

static inline void bitsetSet(std::vector<uint64_t>& bs, int idx)
{
	bs[idx >> 6] |= (1ULL << (idx & 63));
}

static inline bool yDefViolation(
	const std::vector<uint64_t>& visitedA,
	const std::vector<uint64_t>& visitedB,
	const std::vector<uint64_t>& orgRow)
{
	const size_t n = visitedA.size();
	for (size_t i = 0; i < n; ++i)
	{
		if ((visitedA[i] & orgRow[i] & ~visitedB[i]) != 0ULL)
		{
			return true;
		}
	}
	return false;
}

#include <windows.h>
#include <psapi.h>

//using namespace std;
using namespace scip;
using namespace ::std::literals;


/** Constructs the pricer object with the data needed
*
*  An alternative is to have a problem data class which allows to access the data.
*/

Pricer::Pricer(
	SCIP* scip,								/**< SCIP pointer */
	const char* _name,								/**< name of pricer */
	int									_numNodes,							/**< number of nodes */
	double								_minSpeed,							/**< minmial speed */
	double								_maxSpeed,							/**< maximal speed */
	vector<Customer>& _customerList,						/**< set of all customer*/
	double								_timeHorizon,						/**< time horizon */
	vector<vector<bool>>& _edgeSet,							/**< edges of the graph */
	vector< vector<bool>>& _minSpeedMatrix,					/**< matrix, where enty m_{i,j} is 1 if job j can suceed i at lowest speed and completing at lower tw */
	vector<SCIP_CONS*>& _customerConstraints,				/**< vector of customer constraints */
	SCIP_CONS* _robotConstraint,					/**< maximal number of machines */
	//	vector<Variable>&						_variables,							/**< added variables during solution process */
	double								_eps,								/**< computational accuracy */
	int									_addedVarsInAnIteration,			/**< maximal number of variables that are added in each iteration */
	vector<vector<ArcBranchingConstraints>>& _upperArcBranchingConstraints,	/**< includes all upper arc branching constraints of the b and b tree */
	vector<vector<ArcBranchingConstraints>>& _lowerArcBranchingConstarints,	/**< includes all lower arc branching constraints of the b and b tree */
	vector<BranchingConstraints>& _branchingConstraintsAtANode,
	int	_edgeLimit,
	double _edgesToKeep,
	int _vehicleCapacity,
	long _startTimeOptimization,
	double _timeLimit,
	vector<vector<bool>>& _orgEdgeSet,
	vector<vector<int>>& _outNeighbors
):
ObjPricer(scip, _name, "Finds tour with negative reduced cost.", 0, TRUE),
numNodes(_numNodes),
minSpeed(_minSpeed),
maxSpeed(_maxSpeed),
customerList(_customerList),
timeHorizon(_timeHorizon),
edgeSet(_edgeSet),
minSpeedMatrix(_minSpeedMatrix),
customerConstraints(_customerConstraints),
robotConstraint(_robotConstraint),
eps(_eps),
addedVarsInAnIteration(_addedVarsInAnIteration),
upperArcBranchingConstraints(_upperArcBranchingConstraints),
lowerArcBranchingConstarints(_lowerArcBranchingConstarints),
branchingConstraintsAtANode(_branchingConstraintsAtANode),
edgeLimit(_edgeLimit),
edgesToKeep(_edgesToKeep),
vehicleCapacity(_vehicleCapacity),
startTimeOptimization(_startTimeOptimization),
timeLimit(_timeLimit),
orgEdgeSet(_orgEdgeSet),
outNeighbors(_outNeighbors),
maskWords(bitsetWords(_numNodes)),
pricerCounter(0),
orgEdgeMask(_numNodes, std::vector<uint64_t>(bitsetWords(_numNodes), 0))
{
	for (int i = 0; i < _numNodes; ++i)
	{
		for (int j = 0; j < _numNodes; ++j)
		{
			if (_orgEdgeSet[i][j])
			{
				bitsetSet(orgEdgeMask[i], j);
			}
		}
	}
}

/** Destructs the pricer object. */
Pricer::~Pricer()
{
}

/** initialization method of variable pricer (called after problem was transformed)
*
*  Because SCIP transformes the original problem in preprocessing, we need to get the references to
*  the variables and constraints in the transformed problem from the references in the original
*  problem.
*/
SCIP_DECL_PRICERINIT(Pricer::scip_init)
{

	SCIP_CALL(SCIPgetTransformedCons(scip, robotConstraint, &robotConstraint));

	for (int i = 0; i < getNumNodes(); ++i)
	{
		SCIP_CALL(SCIPgetTransformedCons(scip, customerConstraints[i], &customerConstraints[i])); /*lint !e732 !e747*/
	}
	vector<SCIP_VAR*> tempVars;
	for (int i = 0; i < variables.size(); i++)
	{
		SCIP_VAR* var;
		SCIP_CALL(SCIPgetTransformedVar(scip, variables[i].getVariable(), &var));
		variables[i].setVariable(var);
	}

	return SCIP_OKAY;
}


bool sortSmallToLarge(Label& lS1, Label& lS2)
{
	return lS1.getReducedCostLabel() < lS2.getReducedCostLabel();
}

double minMy(double a, double b) {
	if (a < b) {
		return a;
	}
	else
		return b;
}

double maxMy(double a, double b) {
	if (a < b) {
		return b;
	}
	else
		return a;
}

// Calculate edges of the graph
vector<vector<bool>> edgeSetCalc(
	const vector<vector<bool>>* edgeSet,
	const vector<SCIP_Real>& red_cost,
	double dualVarR,
	int numberOfNodes,
	vector<ArcBranchingConstraints>* arcBranchCons,
	double eps)
{
	vector<vector<bool>> edges(numberOfNodes);

	for (int i = 0; i < numberOfNodes; i++)
	{
		edges[i].resize(numberOfNodes);
		for (int j = 0; j < numberOfNodes; j++)
		{
			if (red_cost[i] >= 0 || red_cost[j] >= 0)
			{
				edges[i][j] = false;
				if (i == j && dualVarR > 0)
				{
					edges[i][j] = (*edgeSet)[i][j];
				}
			}
			else
			{
				edges[i][j] = (*edgeSet)[i][j];
			}
		}
	}

	for (int i = 0; i < arcBranchCons->size(); i++)
	{
		if ((*arcBranchCons)[i].getBranchType() == true)
		{
			edges[(*arcBranchCons)[i].getIdStartCustomer()][(*arcBranchCons)[i].getIdEndCustomer()] = true;
			for (int j = 0; j < numberOfNodes; j++)
			{
				edges[j][(*arcBranchCons)[i].getIdStartCustomer()] =
					(*edgeSet)[j][(*arcBranchCons)[i].getIdStartCustomer()];
				edges[(*arcBranchCons)[i].getIdEndCustomer()][j] =
					(*edgeSet)[(*arcBranchCons)[i].getIdEndCustomer()][j];
			}
		}
	}
	for (int i = 0; i < arcBranchCons->size(); i++)
	{
		if ((*arcBranchCons)[i].getBranchType() == false)
		{
			edges[(*arcBranchCons)[i].getIdStartCustomer()][(*arcBranchCons)[i].getIdEndCustomer()] = false;
		}
		else
		{
			for (int j = 0; j < numberOfNodes; j++)
			{
				if ((*arcBranchCons)[i].getIdStartCustomer() != j && (*arcBranchCons)[i].getIdEndCustomer() != j)
				{
					edges[(*arcBranchCons)[i].getIdStartCustomer()][j] = false;
					edges[j][(*arcBranchCons)[i].getIdEndCustomer()] = false;
				}
			}
			edges[(*arcBranchCons)[i].getIdEndCustomer()][(*arcBranchCons)[i].getIdStartCustomer()] = false;
		}
	}

	for (int i = 0; i < arcBranchCons->size(); i++)
	{
		if ((*arcBranchCons)[i].getBranchType() == false)
		{
			edges[(*arcBranchCons)[i].getIdStartCustomer()][(*arcBranchCons)[i].getIdEndCustomer()] = false;
		}
	}
	return edges;
}

bool sortEdges(vector<double> u, vector<double> v)
{
	return u[1] < v[1];
}

bool sortEdgesIndex(vector<double> u, vector<double> v)
{
	return u[0] < v[0];
}

vector<vector<bool>> edgeSetCalcLimitedEdges(
	const vector<vector<bool>>* edgeSet,
	const vector<SCIP_Real>& red_cost,
	double dualVarR,
	int numberOfNodes,
	vector<ArcBranchingConstraints>* arcBranchCons,
	double eps,
	int edgeLimit)
{
	vector<vector<bool>> edges(numberOfNodes);

	for (int i = 0; i < numberOfNodes; i++)
	{
		edges[i].resize(numberOfNodes);
		for (int j = 0; j < numberOfNodes; j++)
		{
			if (red_cost[i] >= 0 || red_cost[j] >= 0)
			{
				edges[i][j] = false;
				if (i == j && dualVarR > 0)
				{
					edges[i][j] = (*edgeSet)[i][j];
				}
			}
			else
			{
				edges[i][j] = (*edgeSet)[i][j];
			}
		}
	}

	for (int i = 0; i < arcBranchCons->size(); i++)
	{
		if ((*arcBranchCons)[i].getBranchType() == true)
		{
			edges[(*arcBranchCons)[i].getIdStartCustomer()][(*arcBranchCons)[i].getIdEndCustomer()] = true;
			for (int j = 0; j < numberOfNodes; j++)
			{
				edges[j][(*arcBranchCons)[i].getIdStartCustomer()] =
					(*edgeSet)[j][(*arcBranchCons)[i].getIdStartCustomer()];
				edges[(*arcBranchCons)[i].getIdEndCustomer()][j] =
					(*edgeSet)[(*arcBranchCons)[i].getIdEndCustomer()][j];
			}
		}
	}
	for (int i = 0; i < arcBranchCons->size(); i++)
	{
		if ((*arcBranchCons)[i].getBranchType() == false)
		{
			edges[(*arcBranchCons)[i].getIdStartCustomer()][(*arcBranchCons)[i].getIdEndCustomer()] = false;
		}
		else
		{
			for (int j = 0; j < numberOfNodes; j++)
			{
				if ((*arcBranchCons)[i].getIdStartCustomer() != j && (*arcBranchCons)[i].getIdEndCustomer() != j)
				{
					edges[(*arcBranchCons)[i].getIdStartCustomer()][j] = false;
					edges[j][(*arcBranchCons)[i].getIdEndCustomer()] = false;
				}
			}
			edges[(*arcBranchCons)[i].getIdEndCustomer()][(*arcBranchCons)[i].getIdStartCustomer()] = false;
		}
	}

	for (int i = 0; i < arcBranchCons->size(); i++)
	{
		if ((*arcBranchCons)[i].getBranchType() == false)
		{
			edges[(*arcBranchCons)[i].getIdStartCustomer()][(*arcBranchCons)[i].getIdEndCustomer()] = false;
		}
	}

	for (int i = 0; i < numberOfNodes; i++)
	{
		vector<vector<double>> values(0);
		for (int j = 0; j < numberOfNodes; j++)
		{
			if (i != j)
			{
				if (edges[i][j])
				{
					vector<double> edge = { (double)j, red_cost[j] };
					if (values.size() < edgeLimit)
					{
						values.push_back(edge);
					}
					else
					{
						values.push_back(edge);
						sort(values.begin(), values.end(), sortEdges);
						values.pop_back();
					}
				}
			}
		}
		int counter = 0;
		if (values.size() == edgeLimit)
		{
			sort(values.begin(), values.end(), sortEdgesIndex);
			for (int j = 0; j < numberOfNodes; j++)
			{
				if (counter != edgeLimit && values[counter][0] == j)
				{
					counter++;
				}
				else
				{
					edges[i][j] = false;
				}
			}
		}
	}

	return edges;
}


vector<vector<bool>> edgeSetCalcLimitedEdgesRating(
	const vector<vector<bool>>* edgeSet,
	const vector<SCIP_Real>& red_cost,
	double dualVarR,
	int numberOfNodes,
	vector<ArcBranchingConstraints>* arcBranchCons,
	double eps,
	double edgesToKeep,
	vector<Customer>& customers,
	double minSpeed)
{
	vector<vector<bool>> edges(numberOfNodes);

	for (int i = 0; i < numberOfNodes; i++)
	{
		edges[i].resize(numberOfNodes);
		for (int j = 0; j < numberOfNodes; j++)
		{
			if (red_cost[i] >= -eps || red_cost[j] >= -eps)
			{
				edges[i][j] = false;
				if (i == j && dualVarR > 0)
				{
					edges[i][j] = (*edgeSet)[i][j];
				}
			}
			else
			{
				edges[i][j] = (*edgeSet)[i][j];
			}
		}
	}

	for (int i = 0; i < arcBranchCons->size(); i++)
	{
		if ((*arcBranchCons)[i].getBranchType() == true)
		{
			edges[(*arcBranchCons)[i].getIdStartCustomer()][(*arcBranchCons)[i].getIdEndCustomer()] = true;
			for (int j = 0; j < numberOfNodes; j++)
			{
				edges[j][(*arcBranchCons)[i].getIdStartCustomer()] =
					(*edgeSet)[j][(*arcBranchCons)[i].getIdStartCustomer()];
				edges[(*arcBranchCons)[i].getIdEndCustomer()][j] =
					(*edgeSet)[(*arcBranchCons)[i].getIdEndCustomer()][j];
			}
		}
	}
	for (int i = 0; i < arcBranchCons->size(); i++)
	{
		if ((*arcBranchCons)[i].getBranchType() == false)
		{
			edges[(*arcBranchCons)[i].getIdStartCustomer()][(*arcBranchCons)[i].getIdEndCustomer()] = false;
		}
		else
		{
			for (int j = 0; j < numberOfNodes; j++)
			{
				if ((*arcBranchCons)[i].getIdStartCustomer() != j && (*arcBranchCons)[i].getIdEndCustomer() != j)
				{
					edges[(*arcBranchCons)[i].getIdStartCustomer()][j] = false;
					edges[j][(*arcBranchCons)[i].getIdEndCustomer()] = false;
				}
			}
			edges[(*arcBranchCons)[i].getIdEndCustomer()][(*arcBranchCons)[i].getIdStartCustomer()] = false;
		}
	}

	for (int i = 0; i < arcBranchCons->size(); i++)
	{
		if ((*arcBranchCons)[i].getBranchType() == false)
		{
			edges[(*arcBranchCons)[i].getIdStartCustomer()][(*arcBranchCons)[i].getIdEndCustomer()] = false;
		}
	}

	for (int i = 0; i < numberOfNodes; i++)
	{
		double lowerTW = customers[i].getLowerTW();
		double smallestAvSpeed = INT16_MAX;
		vector<vector<double>> values(0);

		for (int j = 0; j < numberOfNodes; j++)
		{
			if (i != j)
			{
				if (edges[i][j])
				{
					vector<double> edge = { (double)j, (double)customers[i].getCDist(j) / (customers[j].getUpperTW() - lowerTW) };
					if (edge[1] < smallestAvSpeed)
					{
						smallestAvSpeed = edge[1];
					}
					values.push_back(edge);
				}
			}
		}

		int counter = 0;
		for (int j = 0; j < numberOfNodes; j++)
		{
			if (i != j)
			{
				if (edges[i][j])
				{
					values[counter][1] = (smallestAvSpeed / values[counter][1]) * red_cost[values[counter][0]];
					counter++;
				}
			}
		}
		sort(values.begin(), values.end(), sortEdges);

		int numberOfEdgesToRemove = (1 - edgesToKeep) * counter;
		for (int removedEdges = 0; removedEdges < numberOfEdgesToRemove; removedEdges++)
		{
			edges[i][values.back()[0]] = false;
			values.pop_back();
		}
	}

	return edges;
}


double costFct(double workLoad, double speed, bool isfarkas)
{
	if (!isfarkas)
	{
		return workLoad * (0.0036 * speed * speed - 0.1015 * speed + 0.8848);
	}
	else
	{
		return 0.0;
	}
	return 0.0;
}

void calcCost(Label* label, vector<SCIP_Real>* redCost, double minSpeed, double maxSpeed, double dualVarR, double eps, bool isfarkas, bool calculateOptCost, bool calculateMinSpeedCost)
{
	double fuelCost = 0;
	double wageCost = 0;
	double dualVariables = 0;
	int weight = 0;
	for (int i = label->getSequence()->size() - 1; i >= 0; i--)
	{
		if (i == 0)
		{
			double tmpCost = costFct((*label->getSequence())[i].getDistFromDepot(),
				maxMy((*label->getSequence())[i].getProcessingSpeed(), minSpeed), false);
			fuelCost += tmpCost;

		}
		else
		{
			double tmpCost = costFct((*label->getSequence())[i - 1].getCDist((*label->getSequence())[i].getId()), maxMy((*label->getSequence())[i].getProcessingSpeed(), minSpeed), false);
			fuelCost += tmpCost;
		}
		double tmpCost = (*redCost)[(*label->getSequence())[i].getId()];
		dualVariables += tmpCost;
	}
	fuelCost += costFct(label->getSequence()->back().getDistToDepot(), minSpeed, false);
	dualVariables -= dualVarR;

	if (calculateMinSpeedCost)
	{
		label->setPriceMinSpeed(fuelCost + dualVariables);
	}
	
	label->setDualVariableCost(dualVariables);

	if (calculateOptCost)
	{
		label->setOptCost(fuelCost);
		if (!isfarkas)
		{
			label->setReducedCostLabel(label->getDualVariableCost() + label->getOptCost());
		}
		else
		{
			label->setReducedCostLabel(label->getDualVariableCost());
		}
	}
}

void ceilCalc(double* value, double ceilBound, double eps)
{
	if (*value - ceilBound < eps && *value - ceilBound > -eps)
	{
		*value = ceilBound;
	}
}

void thresholdCalc(double* value, double floorBound, double eps)
{
	if (*value - floorBound < eps && *value - floorBound > -eps)
	{
		*value = floorBound;
	}
}

void ceilAndFloorCalc(double* value, double floorBound, double ceilBound, double eps)
{
	if (*value - ceilBound > -eps && *value - ceilBound < eps)
	{
		*value = ceilBound;
	}
	else if (*value - floorBound < eps && *value - floorBound > -eps)
	{
		*value = floorBound;
	}
}

double max3(double a, double b, double c) {
	if (a < b) {
		if (b < c)
		{
			return c;
		}
		else
		{
			return b;
		}
	}
	else
	{
		if (a < c)
		{
			return c;
		}
		else
		{
			return a;
		}
	}
}



/* This method checks if all the completion times of all jobs are within their time windows
 and sets the speed vector startIndex = position of job completing at lower tw*/
bool timeASequenceWithoutWage(double startTime, double endTime,
	int startIndex, int endIndex, vector<Customer>* sequence,
	double minSpeed, double maxSpeed, Label* label, bool isForward, double eps)
{
	double workLoad = 0;


	//Calculate average speed
	double sumServiceTimes = 0;

	// Calculate work load of the subsequence
	for (int i = startIndex + 1; i <= endIndex; i++)
	{
		if (i != 0)
		{
			workLoad += (double long)(*sequence)[i - 1].getCDist((*sequence)[i].getId());
			sumServiceTimes += (*sequence)[i - 1].getServiceTime();
		}
		else
		{
			workLoad += (*sequence)[i].getDistFromDepot();
		}
	}

	double speed = workLoad / (endTime - startTime - sumServiceTimes);
	if (speed < 0)
	{
		return false;
	}
	//	ceilAndFloorCalc(&speed, minSpeed, maxSpeed, eps);
		// Check if speed is greater than max speed
	if (speed > maxSpeed)
	{
		return false;
	}
	// Calculate maximal deviation of a job when processed at average speed
	double maxDeviation = 0;
	int positionOfJob = 0;
	double time = startTime;

	// Set times for the given sequence
	for (int i = startIndex + 1; i <= endIndex; i++)
	{
		(*sequence)[i].setProcessingSpeed(speed);
		if (i == 0)
		{
			time += 1.0 * (*sequence)[i].getDistFromDepot() / (speed);
		}
		else
		{

			time += 1.0 * ((*sequence)[i - 1].getCDist((*sequence)[i].getId())) / (speed);
			time += (*sequence)[i - 1].getServiceTime();
		}

		ceilAndFloorCalc(&time, (*sequence)[i].getLowerTW(), (*sequence)[i].getUpperTW(), eps);
		(*sequence)[i].setCompletionTime(time);

		// Check if the job completes outside its time window
		double deviation = max3(0.0, time - (*sequence)[i].getUpperTW(),
			(*sequence)[i].getLowerTW() - time);
		if (deviation > maxDeviation)
		{
			maxDeviation = deviation;
			positionOfJob = i;
		}
	}

	bool extensionPossible1 = false;
	bool extensionPossible2 = false;

	//	cout << "max " << maxDeviation << endl;
		// Check if a deviation exists
	if (maxDeviation - eps > 0)
	{
		// Check if the job with the largest deviation completes after its time window
		if ((*sequence)[positionOfJob].getCompletionTime() > (*sequence)[positionOfJob].getUpperTW())
		{
			(*sequence)[positionOfJob].setCompletionTime((*sequence)[positionOfJob].getUpperTW());
			extensionPossible1 = timeASequenceWithoutWage(startTime, (*sequence)[positionOfJob].getUpperTW(), startIndex,
				positionOfJob, sequence, minSpeed, maxSpeed, label, isForward, eps);
			extensionPossible2 = timeASequenceWithoutWage((*sequence)[positionOfJob].getUpperTW(), endTime, positionOfJob, endIndex,
				sequence, minSpeed, maxSpeed, label, isForward, eps);

			if (positionOfJob < label->getIndexInSequenceOnBound() && !isForward)
			{
				label->setIndexInSequenceOnBound(positionOfJob);
			}
		}
		// Check if the job with the largest deviation completes earlier than its time window
		else if ((*sequence)[positionOfJob].getCompletionTime() < (*sequence)[positionOfJob].getLowerTW())
		{
			(*sequence)[positionOfJob].setCompletionTime((*sequence)[positionOfJob].getLowerTW());
			if (positionOfJob > label->getIndexInSequenceOnBound() && isForward)
			{
				label->setIndexInSequenceOnBound(positionOfJob);
			}
			extensionPossible1 = timeASequenceWithoutWage(startTime, (*sequence)[positionOfJob].getLowerTW(), startIndex,
				positionOfJob, sequence, minSpeed, maxSpeed, label, isForward, eps);
			extensionPossible2 = timeASequenceWithoutWage((*sequence)[positionOfJob].getLowerTW(), endTime, positionOfJob, endIndex,
				sequence, minSpeed, maxSpeed, label, isForward, eps);
		}
	}
	// If deviation exists and extension is not possible, return false
	if ((!extensionPossible1 || !extensionPossible2) && maxDeviation - eps > 0)
	{
		return false;
	}
	else
	{
		return true;
	}
}

void speedEnlargementWage(Label& label, double* v, double vNew, int startIndex,
	int* indexOnBound, vector<double>& speeds, double completionTime, double eps)
{
	double serviceTimeSum = 0;
	double workLoadSum = 0;
	double startOfServiceFirstJob = completionTime;
	bool startIndexWasZero = false;
	if (startIndex == 0)
	{
		startIndexWasZero = true;
		workLoadSum = (*label.getSequence())[0].getDistFromDepot();
		if (startOfServiceFirstJob + 1.0 * ((*label.getSequence())[0].getDistFromDepot()) / vNew <
			(*label.getSequence())[0].getLowerTW())
		{
			vNew = 1.0 * ((*label.getSequence())[0].getDistFromDepot()) /
				((*label.getSequence())[0].getLowerTW() - completionTime);
			(*indexOnBound) = 0;
			label.setIndexInSequenceOnBound(0);
			if (label.getSequence()->size() == 1)
			{
				label.getSequence()->front().setCompletionTime(label.getSequence()->back().getLowerTW());
				label.getSequence()->front().setProcessingSpeed(vNew);
			}
		}
		else
		{
			label.getSequence()->front().setCompletionTime(startOfServiceFirstJob + 1.0 * ((*label.getSequence())[0].getDistFromDepot()) / vNew);
			label.getSequence()->front().setProcessingSpeed(vNew);
		}
		speeds[0] = vNew;
		completionTime += 1.0 * ((*label.getSequence())[0].getDistFromDepot()) / vNew;
		startIndex++;
		serviceTimeSum += (*label.getSequence())[0].getServiceTime();
	}
	else
	{
		startOfServiceFirstJob += (*label.getSequence())[startIndex - 1].getServiceTime();
	}

	for (int i = startIndex; i < label.getSequence()->size(); i++)
	{
		workLoadSum += (*label.getSequence())[i - 1].getCDist((*label.getSequence())[i].getId());
		if (completionTime + (*label.getSequence())[i - 1].getServiceTime() + 1.0 * ((*label.getSequence())[i - 1].getCDist((*label.getSequence())[i].getId())) / vNew <
			(*label.getSequence())[i].getLowerTW())
		{

			vNew = 1.0 * workLoadSum / ((*label.getSequence())[i].getLowerTW() - startOfServiceFirstJob - serviceTimeSum);
			(*indexOnBound) = i;
			label.setIndexInSequenceOnBound(i);
			completionTime = (*label.getSequence())[i].getLowerTW();
		}

		speeds[i] = vNew;
		completionTime = startOfServiceFirstJob + serviceTimeSum + 1.0 * workLoadSum / vNew;
		thresholdCalc(&completionTime, (*label.getSequence())[i].getLowerTW(), eps);
		serviceTimeSum += (*label.getSequence())[i].getServiceTime();
	}
	workLoadSum = 0;
	serviceTimeSum = 0;
	if (startIndexWasZero)
	{
		workLoadSum += (*label.getSequence())[0].getDistFromDepot();
		serviceTimeSum += (*label.getSequence())[0].getServiceTime();
	}
	for (int i = startIndex; i < label.getSequence()->size(); i++)
	{
		workLoadSum += (*label.getSequence())[i - 1].getCDist((*label.getSequence())[i].getId());
		speeds[i] = vNew;
		completionTime = startOfServiceFirstJob + serviceTimeSum + 1.0 * workLoadSum / vNew;
		thresholdCalc(&completionTime, label.getSequence()->back().getLowerTW(), eps);
		(*label.getSequence())[i].setProcessingSpeed(vNew);
		(*label.getSequence())[i].setCompletionTime(completionTime);
		serviceTimeSum += (*label.getSequence())[i].getServiceTime();
	}
	if (startIndexWasZero)
	{
		completionTime = 1.0 * ((*label.getSequence())[0].getDistFromDepot()) / vNew;
		thresholdCalc(&completionTime, label.getSequence()->back().getLowerTW(), eps);
		label.getSequence()->front().setCompletionTime(completionTime);
		label.getSequence()->front().setProcessingSpeed(vNew);
	}
	(*v) = vNew;
}

void timeASequenceCheckLowerSpeedFW(vector<Customer>* sequence, double minSpeed, double eps, Label* label)
{
	for (int i = 0; i < sequence->size(); i++)
	{
		if ((*sequence)[i].getProcessingSpeed() < minSpeed)
		{
			(*sequence)[i].setProcessingSpeed(minSpeed);
			if (i == 0)
			{
				double completionTime = maxMy((*sequence)[i].getLowerTW(), (*sequence)[i].getDistFromDepot() / minSpeed);
				ceilAndFloorCalc(&completionTime, (*sequence)[i].getLowerTW(), (*sequence)[i].getUpperTW(), eps);
				(*sequence)[i].setCompletionTime(completionTime);
				(*sequence)[i].setProcessingSpeed(minSpeed);
			}
			else
			{
				double completionTime = maxMy((*sequence)[i].getLowerTW(),
					(*sequence)[i - 1].getCompletionTime() + (*sequence)[i - 1].getServiceTime() + ((*sequence)[i - 1].getCDist((*sequence)[i].getId()) / minSpeed));
				ceilAndFloorCalc(&completionTime, (*sequence)[i].getLowerTW(), (*sequence)[i].getUpperTW(), eps);
				(*sequence)[i].setCompletionTime(completionTime);
			}
		}
	}
	label->setIndexInSequenceOnBound(-1);
	for (int i = sequence->size() - 1; i >= 0; i--)
	{

		if ((*sequence)[i].getCompletionTime() < (*sequence)[i].getLowerTW() + eps && (*sequence)[i].getCompletionTime() > (*sequence)[i].getLowerTW() - eps)
		{
			label->setIndexInSequenceOnBound(i);
			break;
		}
	}
}

// Post optimization of time sequence due to driver wage
bool timeASequence(double startTime, double endTime,
	int startIndex, int endIndex, vector<Customer>* sequence,
	double minSpeed, double minSpeedWage, double maxSpeed, Label* label, bool isForward, double eps)
{

	if (!timeASequenceWithoutWage(0, endTime, -1, endIndex, sequence, minSpeed, maxSpeed, label, isForward, eps))
	{
		return false;
	}
	else
	{
		timeASequenceCheckLowerSpeedFW(label->getSequence(), minSpeed, eps, label);
		// Calculate index on bound
		int indexOnLowerBound = -1;
		for (int i = (int)(label->getSequence()->size() - 1); i >= 0; i--)
		{
			if ((*label->getSequence())[i].getCompletionTime() == (*label->getSequence())[i].getLowerTW())
			{
				indexOnLowerBound = i;
			}
		}
		label->setIndexInSequenceOnBound(indexOnLowerBound);
		vector<double> speeds(label->getSequence()->size());
		for (int i = 0; i < label->getSequence()->size(); i++)
		{
			speeds[i] = (*label->getSequence())[i].getProcessingSpeed();
		}
		//	int indexOnLowerBound = label->getIndexInSequenceOnBound();
		double v = label->getSequence()->back().getProcessingSpeed();

		while (label->getSequence()->back().getCompletionTime() - eps > label->getSequence()->back().getLowerTW() && label->getSequence()->back().getProcessingSpeed() + eps < minSpeedWage)
		{
			int workLoads = 0;
			for (int i = (int)(label->getSequence()->size() - 1); i > indexOnLowerBound; i--)
			{
				if ((speeds[i] - eps > v || speeds[i] + eps < v) && v < minSpeedWage)
				{
					speedEnlargementWage(*label, &v, minMy(minSpeedWage, speeds[i]), i + 1, &indexOnLowerBound, speeds, (*label->getSequence())[i].getUpperTW(), eps);

					i = (int)(label->getSequence()->size());
					workLoads = 0;
					continue;
				}
				//		if (i != label.getSequence()->size()) {
				i > 0 ? workLoads += (*label->getSequence())[i - 1].getCDist((*label->getSequence())[i].getId()) :
					workLoads += (*label->getSequence())[i].getDistFromDepot();
				//	}
			}
			if (v + eps < minSpeedWage && label->getSequence()->back().getCompletionTime() > label->getSequence()->back().getLowerTW())
			{
				if (workLoads == 0 && label->getSequence()->size() > 1)
				{
					workLoads += (*label->getSequence())[label->getSequence()->size() - 2].getCDist((*label->getSequence()).back().getId());
				}
				if (indexOnLowerBound != -1)
				{
					speedEnlargementWage(*label, &v, minSpeedWage, indexOnLowerBound + 1, &indexOnLowerBound,
						speeds, (*label->getSequence())[indexOnLowerBound].getLowerTW(), eps);
				}
				else
				{
					speedEnlargementWage(*label, &v, minSpeedWage, indexOnLowerBound + 1, &indexOnLowerBound, speeds, 0, eps);
				}
			}
		}
	}
	timeASequenceCheckLowerSpeedFW(label->getSequence(), minSpeed, eps, label);
}

/** perform pricing
*
*  @todo compute shortest length restricted tour w.r.t. duals
*/
SCIP_RETCODE Pricer::pricing(
	SCIP* scip,               /**< SCIP data structure */
	bool                  isfarkas            /**< whether we perform Farkas pricing */
)
{
	SCIP_Real dualVarR = 0;
	// Calculate reduced cost for forward label
	if (isfarkas)
	{
		dualVarR = (SCIPgetDualfarkasLinear(scip, getRobotCon()));
	}
	else {
		dualVarR = SCIPgetDualsolLinear(scip, getRobotCon());
	}

	int currentNode = (int)SCIPnodeGetNumber(SCIPgetCurrentNode(scip));

	// Check if first node has been added already
	if (currentNode == 1 && getBranchingConstraintsAtANode()->size() == 0)
	{
		getBranchingConstraintsAtANode()->push_back(BranchingConstraints(currentNode));
	}

	// Search for branching constraints at a node in b and b tree
	BranchingConstraints* branchingConstraintsAtANode(0);
	for (int i = 0; i < getBranchingConstraintsAtANode()->size(); i++)
	{
		if (currentNode == (*getBranchingConstraintsAtANode())[i].getNodeNumber())
		{
			branchingConstraintsAtANode = &(*getBranchingConstraintsAtANode())[i];
			break;
		}
	}
	/* allocate array for reduced costs */
	vector< SCIP_Real > red_cost(getNumNodes());


	// Arc branching
	for (vector<ArcBranchingConstraints>::iterator it = branchingConstraintsAtANode->getArcBranchingConstraints()->begin();
		it != branchingConstraintsAtANode->getArcBranchingConstraints()->end(); it++)
	{
		if (it->getBranchType() == true) {
			if (isfarkas)
			{
				red_cost[it->getIdStartCustomer()] -= SCIPgetDualfarkasSetppc(scip, it->getCon());
			}
			else {
				red_cost[it->getIdStartCustomer()] -= SCIPgetDualsolSetppc(scip, it->getCon());
			}
		}
	}

	// Calculate reduced cost
	for (int i = 0; i < getNumNodes(); i++)
	{
		if (isfarkas)
		{
			red_cost[i] -= SCIPgetDualfarkasSetppc(scip, getCustomerCon(i));
		}
		else {
			red_cost[i] -= SCIPgetDualsolSetppc(scip, getCustomerCon(i));
		}
	}

	vector<vector<bool>> edges = (*getEdgeSet());
	vector<Label> kVariables;
	// Calculate graph
	edges = edgeSetCalcLimitedEdgesRating(
		getEdgeSet(),
		red_cost,
		dualVarR,
		getNumNodes(),
		branchingConstraintsAtANode->getArcBranchingConstraints(),
		getEps(),
		getEdgesToKeep(),
		(*getCustomerList()),
		getMinSpeed()
	);

	vector<vector<Label>> forwardTable(getNumNodes());

	
	bool rootNode = false;
	if (currentNode == 1)
	{
		rootNode = true;
	}
	// Perform forward labeling
	forwardLabeling(red_cost, dualVarR, &forwardTable, edges, branchingConstraintsAtANode, isfarkas, kVariables, rootNode, true);

	if ((kVariables.size() == 0 || kVariables.front().getReducedCostLabel() >= -getEps()))
	{
		vector<vector<Label>> forwardTable(getNumNodes());

		edges = edgeSetCalc(getEdgeSet(), red_cost, dualVarR, getNumNodes(), branchingConstraintsAtANode->getArcBranchingConstraints(), getEps());


		forwardLabeling(red_cost, dualVarR, &forwardTable, edges, branchingConstraintsAtANode, isfarkas, kVariables, rootNode, false);
	}
	if (kVariables.size() != 0 && kVariables.front().getReducedCostLabel() <= -getEps())
	{
		for (vector<Label>::iterator it = kVariables.begin(); it != kVariables.end(); it++)
		{
			if (it->getReducedCostLabel() <= -getEps())
			{
				addVariable(scip, &(*it), branchingConstraintsAtANode);
			}
		}
	}
	else
	{
		SCIPisNegative(scip, 1);
	}
	return SCIP_OKAY;
}


SCIP_DECL_PRICERREDCOST(Pricer::scip_redcost)
{
	SCIPdebugMessage("call scip_redcost ...\n");
	/* set result pointer, see above */
	*result = SCIP_SUCCESS;

	/* call pricing routine */
	SCIP_CALL(pricing(scip, false));

	

	return SCIP_OKAY;
} /*lint !e715*/

SCIP_DECL_PRICERFARKAS(Pricer::scip_farkas)
{
	SCIPdebugMessage("call scip_farkas ...\n");

	/* call pricing routine */
	SCIP_CALL(pricing(scip, true));

	

	return SCIP_OKAY;
} /*lint !e715*/


   /** add tour variable to problem */
SCIP_RETCODE Pricer::addVariable(
	SCIP* scip,				/**< SCIP data structure	*/
	Label* label,				/**< Label of with lowest cost */
	BranchingConstraints* branchingConsAtNode
)
{
	double cost = label->getOptCost();

	/* create meaningful variable name */
	char tmp_name[255];
	char var_name[255];
	(void)SCIPsnprintf(var_name, 255, "x");
	vector<int> sequenceOfIds(0);
	for (vector<Customer>::iterator it = label->getSequence()->begin(); it != label->getSequence()->end(); ++it)
	{

		// strncpy(tmp_name, var_name, 255);
		strncpy_s(tmp_name, sizeof(tmp_name), var_name, 255);
		(void)SCIPsnprintf(var_name, 255, "%s_%d", tmp_name, it->getId());
		sequenceOfIds.push_back(it->getId());
	}
	SCIPdebugMessage("new variable <%s>\n", var_name);

	/* create the new variable: Use upper bound of infinity such that we do not have to care about
	* the reduced costs of the variable in the pricing. The upper bound of 1 is implicitly satisfied
	* due to the set partitioning constraints.
	*/

	SCIP_VAR* var;
	SCIP_CALL(SCIPcreateVar(scip, &var, var_name,
		0.0,                    // lower bound
		1.0,					// upper bound
		cost,					// objective
		SCIP_VARTYPE_BINARY,	// variable type
		false, false, NULL, NULL, NULL, NULL, NULL));

	/* change the upper bound of the binary variable to lazy since the upper bound is already enforced
	  * due to the objective function the set covering constraint;
	  * The reason for doing is that, is to avoid the bound of x <= 1 in the LP relaxation since this bound
	  * constraint would produce a dual variable which might have a positive reduced cost
	  */
	SCIP_CALL(SCIPchgVarUbLazy(scip, var, 1.0));
	Variable variable(var, sequenceOfIds);

	/* add new variable to the list of variables to price into LP (score: leave 1 here) */
	SCIP_CALL(SCIPaddPricedVar(scip, var, 1.0));

	/* add variable to Customer constraints */
	for (int i = 0; i < sequenceOfIds.size(); i++)
	{
		SCIP_CALL(SCIPaddCoefSetppc(scip, getCustomerCon(sequenceOfIds[i]), var));

		// Add variable to arc branching constraints 
		if (i < label->getSequence()->size() - 1)
		{
			if ((*getUpperArcBranchingCons())[sequenceOfIds[i]][sequenceOfIds[i + 1]].getIdStartCustomer() != -1)
			{
				SCIP_CALL(SCIPaddCoefSetppc(scip, (*getUpperArcBranchingCons())[sequenceOfIds[i]][sequenceOfIds[i + 1]].getCon(), var));
				SCIP_CALL(SCIPaddCoefLinear(scip, (*getLowerArcBranchingCons())[sequenceOfIds[i]][sequenceOfIds[i + 1]].getCon(), var, -1.0));
			}
		}
	}

	for (int i = 0; i < getVariables()->size(); i++)
	{
		if ((*getVariables())[i].getOrder().size() == variable.getOrder().size())
		{
			bool identical = true;
			for (int j = 0; j < variable.getOrder().size(); j++)
			{
				if ((*getVariables())[i].getOrder()[j] != variable.getOrder()[j])
				{
					identical = false;
					break;
				}
			}
			if (identical)
			{
				cout << "identical vars contained" << endl;
				char fileName[255];
				(void)SCIPsnprintf(fileName, 255, "test.lp");
				SCIP_CALL(SCIPwriteTransProblem(scip, fileName, "lp", FALSE));
			}
		}
	}


	addVariable(variable);
	/* add coefficient into robot constraint */
	SCIP_CALL(SCIPaddCoefLinear(scip, getRobotCon(), var, 1));

	/* cleanup */
	SCIP_CALL(SCIPreleaseVar(scip, &var));

	return SCIP_OKAY;
}

// This method tries to add a job to a given sequence and
// calculates the new speed vector
bool extensionProcedureForward(Label* label, Customer customer, vector<double>* redCost, SCIP_Real dualValR,
	double minSpeed, double maxSpeed, vector<vector<bool>>* minSpeedOrder, bool isfarkas,
	vector<vector<bool>>* edges, double eps, double vehCapacity)
{
	// This value corresponds to the position of the job in the sequence,
	// which completes at its lower tw
	int startIndex = label->getIndexInSequenceOnBound();
	double completionTime = 0;

	if (label->getCargoWeight() + customer.getDemand() - eps > vehCapacity)
	{
		return false;
	}
	label->addCargoWeight(customer.getDemand());

	if (label->getSequence()->size() == 0)
	{
		//Check if job can be processed at minimal speed, completing at lower tw bound
		if ((*minSpeedOrder)[customer.getId()][customer.getId()])
		{
			customer.setCompletionTime(customer.getLowerTW());
			customer.setProcessingSpeed(minSpeed);
			label->setIndexInSequenceOnBound(0);
		}
		else
		{
			double completionTime = maxMy(customer.getLowerTW(), minMy(customer.getDistFromDepot() / minSpeed, customer.getUpperTW()));
			ceilAndFloorCalc(&completionTime, customer.getLowerTW(), customer.getUpperTW(), eps);
			customer.setCompletionTime(completionTime);
			double speed = (maxMy(minSpeed, customer.getDistFromDepot() / (customer.getCompletionTime())));
			ceilAndFloorCalc(&speed, minSpeed, maxSpeed, eps);
			customer.setProcessingSpeed(speed);
			if (customer.getCompletionTime() == customer.getLowerTW())
			{
				label->setIndexInSequenceOnBound(0);
			}
		}

		// Add job to the sequence
		label->addACustomer(customer);
		// Calculate reduced cost of the added job
		calcCost(label, redCost, minSpeed, maxSpeed, dualValR, eps, isfarkas, true, true);

		return true;
	}
	// Sequence contains jobs
	else if ((*edges)[label->getSequence()->back().getId()][customer.getId()])
	{
		// Check if job can be added at lowest processing speed and complets at the lower time window
		// MinSpeedOrder is preprocessed, whereas entry i,j is true 
		// if job j can be prcoessed at min speed and starts at upper tw of i and completes at lower tw of j
		if ((*minSpeedOrder)[label->getSequence()->back().getId()][customer.getId()])
		{
			customer.setCompletionTime(customer.getLowerTW());
			customer.setProcessingSpeed(minSpeed);

			// Add job to the sequence
			label->addACustomer(customer);
			// Set position of job that completes at lower tw equal to the added job
			label->setIndexInSequenceOnBound((int)(label->getSequence()->size() - 1));
			// Update reduced cost of the extended label
			calcCost(label, redCost, minSpeed, maxSpeed, dualValR, eps, isfarkas, true, true);

			return true;
		}
		else
		{
			// Earliest startIndex time
			double lower = (label->getSequence()->back().getCompletionTime() + label->getSequence()->back().getServiceTime());
			// Latest completion time
			double upper = customer.getUpperTW();

			// Check if the job could be added and processed at lower speed, compared to the last job of the sequence
			if ((upper > lower) &&
				label->getSequence()->back().getProcessingSpeed() >=
				(label->getSequence()->back().getCDist(customer.getId())) / (upper - lower) + eps)
			{
				//Insert the job
				//Calculate processing speed of the job
				double processingSpeed = maxMy(minSpeed, (label->getSequence()->back().getCDist(customer.getId())) / (upper - lower));
				double completionTime = maxMy(customer.getLowerTW(),
					lower + (label->getSequence()->back().getCDist(customer.getId())) / processingSpeed);
				ceilAndFloorCalc(&completionTime, customer.getLowerTW(), customer.getUpperTW(), eps);
				customer.setCompletionTime(completionTime);
				ceilAndFloorCalc(&processingSpeed, minSpeed, maxSpeed, eps);
				customer.setProcessingSpeed(processingSpeed);

				// Add the job to the sequence
				label->addACustomer(customer);
				// Check if the job completes at the lower tw bound
				if (customer.getCompletionTime() == customer.getLowerTW())
				{
					label->setIndexInSequenceOnBound((int)(label->getSequence()->size() - 1));
				}
				// Update reduced cost of the label
				calcCost(label, redCost, minSpeed, maxSpeed, dualValR, eps, isfarkas, true, true);
				return true;
			}
			// Job is processed at higher speed compared to the last job of the sequence
			// Insert the job and equalize speeds
			else
			{
				// Check if non of the jobs in the sequence complete at the lower tw bound
				double startTime = 0.0;
				if (label->getIndexInSequenceOnBound() != -1)
				{
					startTime = (*label->getSequence())[label->getIndexInSequenceOnBound()].getCompletionTime();
				}
				// Add job to the sequence
				label->addACustomer(customer);
				int tempIndexAtLowerBound = label->getIndexInSequenceOnBound();
				// Check if exentsion is possible, otherwise remove the job!
				if (!timeASequenceWithoutWage(startTime, upper, label->getIndexInSequenceOnBound(),
					(int)(label->getSequence()->size() - 1), label->getSequence(), minSpeed, maxSpeed, label, true, eps)) {
					return false;
				}
				else {
					timeASequenceCheckLowerSpeedFW(label->getSequence(), minSpeed, eps, label);
					//Calculate cost of the new sequence
					calcCost(label, redCost, minSpeed, maxSpeed, dualValR, eps, isfarkas, true, true);
					return true;
				}
			}
		}
	}
	else
	{
		return false;
	}
	//	}
	return false;
}

void calcRootsCostFunctionFW(Label& labelA, int indexA, Label& labelB, int indexB, long double* reals, long double* imag)
{

	double alpha = 0.0036;
	double beta = -0.1015;
	double gamma = 0.8848;

	double wa = (*labelA.getWorkLoads())[indexA];
	double wb = (*labelB.getWorkLoads())[indexB];

	double pi = (*labelA.getPrices())[indexA - 1] - (*labelB.getPrices())[indexB - 1] -
		costFct((*labelA.getWorkLoads())[indexA], (*labelA.getSpeeds())[indexA - 1], false) +
		costFct((*labelB.getWorkLoads())[indexB], (*labelB.getSpeeds())[indexB - 1], false) +
		wa * gamma - wb * gamma;



	double wta = -(*labelA.getCompletionTimes())[(indexA - 1)] + (*labelA.getWorkLoads())[indexA] / (*labelA.getSpeeds())[indexA - 1];
	double wtb = -(*labelB.getCompletionTimes())[(indexB - 1)] + (*labelB.getWorkLoads())[indexB] / (*labelB.getSpeeds())[(indexB - 1)];

	long double coeff[5];

	coeff[0] = 1.0;
	coeff[1] = 2 * wtb + 2 * wta + beta * wa * wa / pi - beta * wb * wb / pi;
	coeff[2] = wtb * wtb + 4 * wta * wtb + wta * wta + wta * wta + alpha * wa * wa * wa / pi - alpha * wb * wb * wb / pi + 2 * beta * wa * wa * wtb / pi + beta * wa * wa * wta / pi - 2 * beta * wb * wb * wta / pi - beta * wb * wb * wtb / pi;
	coeff[3] = 2 * wta * wtb * wtb + 2 * wta * wta * wtb + 2 * alpha * wa * wa * wa * wtb / pi - 2 * alpha * wb * wb * wb * wta / pi + beta * wa * wa * wtb * wtb / pi + 2 * beta * wta * wtb * wa * wa / pi - beta * wb * wb * wta * wta / pi - 2 * beta * wta * wtb * wb * wb / pi;
	coeff[4] = wta * wta * wtb * wtb + alpha * wa * wa * wa * wtb * wtb / pi - alpha * wb * wb * wb * wtb * wtb / pi + beta * wa * wa * wta * wtb * wtb / pi - beta * wb * wb * wta * wta * wtb / pi;
	QuadCubicRoots(coeff, 4, reals, imag);
}

bool isSecondFunctionLowerFW(Label& labelA, int indexA, Label& labelB, int indexB, double lower, double upper)
{
	long double vNewALower = (long double)((*labelA.getWorkLoads())[indexA]) / (lower - (*labelA.getCompletionTimes())[indexA - 1] + (*labelA.getWorkLoads())[indexA] / (*labelA.getSpeeds())[indexA - 1]);
	long double pAOld = (*labelA.getPrices())[(indexA - 1)];
	long double costNewA = costFct((long double)(*labelA.getWorkLoads())[indexA], vNewALower, false);
	long double costOldA = costFct((long double)(*labelA.getWorkLoads())[indexA], (*labelA.getSpeeds())[indexA - 1], false);
	long double pANew = pAOld + costNewA - costOldA;

	long double vNewBLower = (long double)(*labelB.getWorkLoads())[indexB] / (lower - (*labelB.getCompletionTimes())[indexB - 1] + (*labelB.getWorkLoads())[indexB] / (*labelB.getSpeeds())[indexB - 1]);
	long double pBOld = (*labelB.getPrices())[(indexB - 1)];
	long double costNewB = costFct((long double)(*labelB.getWorkLoads())[indexB], vNewBLower, false);
	long double costOldB = costFct((long double)(*labelB.getWorkLoads())[indexB], (*labelB.getSpeeds())[indexB - 1], false);
	long double pBNew = pBOld + costNewB - costOldB;

	long double diff2 = pANew - pBNew;

	if (diff2 >= 0)
	{
		return true;
	}
	vNewALower = ((long double)(*labelA.getWorkLoads())[indexA] / (upper - (*labelA.getCompletionTimes())[indexA - 1] + (*labelA.getWorkLoads())[indexA] / (*labelA.getSpeeds())[indexA - 1]));
	pAOld = (*labelA.getPrices())[(indexA - 1)];
	costNewA = costFct((long double)(*labelA.getWorkLoads())[indexA], vNewALower, false);
	costOldA = costFct((long double)(*labelA.getWorkLoads())[indexA], (*labelA.getSpeeds())[indexA - 1], false);
	pANew = pAOld + costNewA - costOldA;

	vNewBLower = ((long double)(*labelB.getWorkLoads())[indexB] / (upper - (*labelB.getCompletionTimes())[indexB - 1] + (*labelB.getWorkLoads())[indexB] / (*labelB.getSpeeds())[indexB - 1]));
	pBOld = (long double)(*labelB.getPrices())[(indexB - 1)];
	costNewB = costFct((long double)(*labelB.getWorkLoads())[indexB], vNewBLower, false);
	costOldB = costFct((long double)(*labelB.getWorkLoads())[indexB], (*labelB.getSpeeds())[indexB - 1], false);
	pBNew = pBOld + costNewB - costOldB;

	diff2 = pANew - pBNew;

	
	if (diff2 >= 0)
	{
		return true;
	}
	return false;
}

bool isSecondLabelDominated_Extension(
	Label& a,
	Label& b,
	const vector<vector<uint64_t>>& orgEdgeMask,
	const vector<vector<uint64_t>>& currentEdgeMask,
	bool isFarkas,
	double eps,
	bool rootNode,
	bool heuristicPricing
)
{
	// Capacity check first (O(1), uses cached cargoWeight)
	if (b.getCargoWeight() < a.getCargoWeight() - eps)
		return false;

	// Y/heuristic customer condition using bitsets (fast)
	const int lastId = a.getSequence()->back().getId();
	if (rootNode || heuristicPricing)
	{
		if (yDefViolation(a.getVisitedMask(), b.getVisitedMask(), currentEdgeMask[lastId]))
			return false;
	}
	else if(yDefViolation(a.getVisitedMask(), b.getVisitedMask(), orgEdgeMask[lastId]))
	{
		return false;
	}
	// Cache vectors to avoid repeated indirections
	const auto& aTimes = *a.getCompletionTimes();
	const auto& bTimes = *b.getCompletionTimes();
	const auto& aPrices = *a.getPrices();
	const auto& bPrices = *b.getPrices();

	if (isFarkas)
	{
		if (a.getDualVariableCost() > b.getDualVariableCost()) return false;
		if (aTimes.back() > bTimes.back()) return false;
		return true;
	}

	const int aSize = (int)aTimes.size();
	const int bSize = (int)bTimes.size();

	if (bSize == 1)
	{
		const double bTime = bTimes.back();
		const double bPrice = bPrices.back();
		for (int i = 0; i < aSize; ++i)
		{
			if (aTimes[i] <= bTime)
			{
				if (aPrices[i] <= bPrice)
					return true;
				return false;
			}
		}
		return false;
	}

	if (aSize == 1)
	{
		const double aTime = aTimes.back();
		const double bFront = bTimes.front();
		if (aTime > bFront ||
			a.getPriceMinSpeed() > b.getPriceMinSpeed())
		{
			return false;
		}
		return true;
	}

	if (bTimes.back() < aTimes.back() || b.getPriceMinSpeed() < a.getPriceMinSpeed())
		return false;

	int indexA = 1;
	int indexB = 1;

	while (true)
	{
		const double lower = maxMy(aTimes[indexA], bTimes[indexB]);
		const double upper = minMy(aTimes[indexA - 1], bTimes[indexB - 1]);

		if (isSecondFunctionLowerFW(a, indexA, b, indexB, lower, upper))
			return false;

		long double reals[4];
		long double imag[4];
		calcRootsCostFunctionFW(a, indexA, b, indexB, reals, imag);
		for (int i = 0; i < 4; i++)
		{
			if (imag[i] == 0 && lower - eps <= reals[i] && reals[i] <= upper + eps)
				return false;
		}

		if (aTimes[indexA] == bTimes[indexB])
		{
			if ((indexA + 1) == aSize && (indexB + 1) < bSize)
				return false;
			if ((indexB + 1) == bSize)
				return true;
			indexA++;
			indexB++;
		}
		else if (aTimes[indexA] < bTimes[indexB])
		{
			if (indexB + 1 == bSize)
				return true;
			indexB++;
		}
		else
		{
			if (indexA + 1 == aSize)
				return false;
			indexA++;
		}
	}
}


void speedEnlargementForwardLabeling(Label& label, double* v, double vNew, int startIndex,
	int* indexOnBound, vector<double>& speeds, double completionTime, int workload, bool isFarkas, double eps)
{
	double serviceTimeSum = 0;
	double workLoadSum = 0;
	double startOfServiceFirstJob = completionTime;


	if (startIndex == 0)
	{
		workLoadSum = (*label.getSequence())[0].getDistFromDepot();
		if (startOfServiceFirstJob + 1.0 * ((*label.getSequence())[0].getDistFromDepot()) / vNew <
			(*label.getSequence())[0].getLowerTW())
		{
			vNew = 1.0 * ((*label.getSequence())[0].getDistFromDepot()) /
				((*label.getSequence())[0].getLowerTW() - completionTime);
			(*indexOnBound) = 0;
		}
		speeds[0] = vNew;
		completionTime += 1.0 * ((*label.getSequence())[0].getDistFromDepot()) / vNew;
		startIndex++;
		serviceTimeSum += (*label.getSequence())[0].getServiceTime();

	}
	else
	{
		startOfServiceFirstJob += (*label.getSequence())[startIndex - 1].getServiceTime();
	}

	for (int i = startIndex; i < label.getSequence()->size(); i++)
	{
		workLoadSum += (*label.getSequence())[i - 1].getCDist((*label.getSequence())[i].getId());
		if (completionTime + (*label.getSequence())[i - 1].getServiceTime() + 1.0 * ((*label.getSequence())[i - 1].getCDist((*label.getSequence())[i].getId())) / vNew <
			(*label.getSequence())[i].getLowerTW())
		{

			vNew = 1.0 * workLoadSum / ((*label.getSequence())[i].getLowerTW() - startOfServiceFirstJob - serviceTimeSum);
			(*indexOnBound) = i;
			completionTime = (*label.getSequence())[i].getLowerTW();
		}

		speeds[i] = vNew;
		completionTime = startOfServiceFirstJob + serviceTimeSum + 1.0 * workLoadSum / vNew;
		thresholdCalc(&completionTime, label.getSequence()->back().getLowerTW(), eps);
		serviceTimeSum += (*label.getSequence())[i].getServiceTime();
	}
	label.getSpeeds()->push_back(vNew);
	label.getCompletionTimes()->push_back(completionTime);

	double newCost = label.getPrices()->back() + costFct(workload, vNew, false) - costFct(workload, *v, false);
	//	double wageReduction = wage * (label.getCompletionTimes()->back() - (*label.getCompletionTimes())[label.getCompletionTimes()->size() - 2]);
	label.getPrices()->push_back(newCost); //+ wageReduction);



	(*v) = vNew;
}

void costFunctionForwardLabeling(Label& label, double maxSpeed, double eps, bool isFarkas)
{
	
	vector<double> speeds(label.getSequence()->size());
	for (int i = 0; i < label.getSequence()->size(); i++)
	{
		speeds[i] = (*label.getSequence())[i].getProcessingSpeed();
	}
	int indexOnLowerBound = label.getIndexInSequenceOnBound();
	double v = label.getSequence()->back().getProcessingSpeed();
	label.getSpeeds()->push_back(v);
	label.getCompletionTimes()->push_back(label.getSequence()->back().getCompletionTime());
	label.getWorkLoads()->push_back(0);
	label.getPrices()->push_back(label.getPriceMinSpeed());

	while (label.getCompletionTimes()->back() - eps > label.getSequence()->back().getLowerTW() && label.getSpeeds()->back() < maxSpeed)
	{
		int workLoads = 0;
		double loadCost = 0;
		int load = 0;
		for (int i = (int)(label.getSequence()->size() - 1); i > indexOnLowerBound; i--)
		{
			if (speeds[i] - eps > v || speeds[i] + eps < v)
			{
				speedEnlargementForwardLabeling(label, &v, speeds[i], i + 1, &indexOnLowerBound, speeds, (*label.getSequence())[i].getUpperTW(), workLoads, isFarkas, eps);
				label.getWorkLoads()->push_back(workLoads);
				i = (int)(label.getSequence()->size());
				workLoads = 0;
				continue;
			}
			//		if (i != label.getSequence()->size()) {
			load += (*label.getSequence())[i].getDemand();
			if (i > 0)
			{
				workLoads += (*label.getSequence())[i - 1].getCDist((*label.getSequence())[i].getId());
			}
			else {
				workLoads += (*label.getSequence())[i].getDistFromDepot();
			}
		}
		if (v < maxSpeed && label.getCompletionTimes()->back() > label.getSequence()->back().getLowerTW())
		{
			if (workLoads == 0)
			{
				workLoads += (*label.getSequence())[label.getSequence()->size() - 2].getCDist((*label.getSequence()).back().getId());
			//	cout << "Speed enlargement case workload zero is used" << endl;
			}
			if (indexOnLowerBound != -1)
			{
				speedEnlargementForwardLabeling(label, &v, maxSpeed, indexOnLowerBound + 1, &indexOnLowerBound,
					speeds, (*label.getSequence())[indexOnLowerBound].getLowerTW(), workLoads, isFarkas, eps);
			}
			else
			{
				speedEnlargementForwardLabeling(label, &v, maxSpeed, indexOnLowerBound + 1, &indexOnLowerBound, speeds, 0, workLoads, isFarkas, eps);
			}

			label.getWorkLoads()->push_back(workLoads);
		}
	}

	if (isFarkas)
	{
		double compTime = label.getCompletionTimes()->back();
		double speed = label.getSpeeds()->back();

		label.getCompletionTimes()->clear();
		label.getSpeeds()->clear();
		label.getWorkLoads()->clear();
		label.getPrices()->clear();
		label.getWorkLoads()->clear();

		label.getCompletionTimes()->push_back(compTime);
		label.getSpeeds()->push_back(speed);
		label.getPrices()->push_back(label.getDualVariableCost());
		label.getWorkLoads()->push_back(0);
	}
	// Verification
	for (int i = 0; i < label.getCompletionTimes()->size() - 1; i++)
	{
		if ((*label.getCompletionTimes())[i] + eps < (*label.getCompletionTimes())[i + 1] || (*label.getSpeeds())[i] - eps >(*label.getSpeeds())[i + 1])
		{
			cout << "error in cost function";
		}
	}
}

void Pricer::forwardLabeling(
	vector<SCIP_Real>& redCost,
	SCIP_Real dualValR,
	vector<vector<Label>>* table,
	vector<vector<bool>>& edges,
	BranchingConstraints* branchingCons,
	bool isfarkas,
	vector<Label>& kVariables, 
	bool rootNode,
	bool heuristicPricing)
{

	std::vector<std::vector<uint64_t>> currentEdgeMask(customerList.size(), std::vector<uint64_t>(bitsetWords(customerList.size()), 0));
	
	for (int i = 0; i < customerList.size(); ++i)
	{
		for (int j = 0; j < customerList.size(); ++j)
		{
			if (edges[i][j])
			{
				bitsetSet(currentEdgeMask[i], j);
			}
		}
	}
	

	SCIPdebugMessage("Enter RSP ");

	long labelCounter = 1;          // used as unique label id
	std::deque<Label> listQueue;

	// Alive map for lazy deletion (index = label id)
	std::vector<uint8_t> labelAlive(1, 0);
	auto markAlive = [&](long id)
		{
			if (id >= (long)labelAlive.size()) labelAlive.resize(id + 1, 0);
			labelAlive[id] = 1;
		};
	auto markDead = [&](long id)
		{
			if (id >= 0 && id < (long)labelAlive.size()) labelAlive[id] = 0;
		};
	auto isAlive = [&](long id) -> bool
		{
			return (id >= 0 && id < (long)labelAlive.size() && labelAlive[id] != 0);
		};

	bool branchingConTrueExists = false;
	vector<bool> startNodeOfTrueBranchingCon(numNodes, false);
	for (vector<ArcBranchingConstraints>::iterator it = branchingCons->getArcBranchingConstraints()->begin();
		it != branchingCons->getArcBranchingConstraints()->end(); it++)
	{
		if (it->getBranchType() == true)
		{
			edges[it->getIdEndCustomer()][it->getIdEndCustomer()] = false;
			branchingConTrueExists = true;
			startNodeOfTrueBranchingCon[it->getIdStartCustomer()] = true;
		}
	}

	// Build adjacency list from edges
	vector<vector<int>> outNeighbors(numNodes);
	for (int i = 0; i < numNodes; ++i)
	{
		for (int j = 0; j < numNodes; ++j)
		{
			if (edges[i][j])
			{
				outNeighbors[i].push_back(j);
			}
		}
	}

	// Fast lookup by id
	vector<Customer*> customerById(numNodes, nullptr);
	for (vector<Customer>::iterator it = getCustomerList()->begin(); it != getCustomerList()->end(); ++it)
	{
		int id = it->getId();
		if (id >= 0 && id < numNodes)
		{
			customerById[id] = &(*it);
		}
	}

	// Initial labels
	for (vector<Customer>::iterator it = getCustomerList()->begin(); it != getCustomerList()->end(); ++it)
	{
		int id = it->getId();
		if (id < 0 || id >= numNodes) continue;

		if (edges[id][id])
		{
			vector<bool> visitedNodes(getNumNodes());
			visitedNodes[id] = true;

			Label label(labelCounter, id, visitedNodes);
			labelCounter++;

			std::vector<uint64_t> mask(maskWords, 0);
			bitsetSet(mask, id);
			label.setVisitedMask(mask);

			extensionProcedureForward(&label,
				(*it), &redCost, dualValR, getMinSpeed(),
				getMaxSpeed(),
				getMinSpeedMatrix(),
				isfarkas, &edges, eps, getVehicleCapacity());

			if (label.getSequence()->size() != 0)
			{
				costFunctionForwardLabeling(label, getMaxSpeed(),getEps(), isfarkas);

				listQueue.push_back(label);
				markAlive(label.getId());

				if (!branchingConTrueExists ||
					!startNodeOfTrueBranchingCon[label.getSequence()->back().getId()])
				{
					(*table)[id].push_back(label);

					if (kVariables.size() < getVarsToBeAddedInAnIteration() && label.getReducedCostLabel() < -eps)
					{
						kVariables.push_back(label);
						sort(kVariables.begin(), kVariables.end(), sortSmallToLarge);
					}
					else if (label.getReducedCostLabel() < -eps && label.getReducedCostLabel() < kVariables.back().getReducedCostLabel())
					{
						kVariables.push_back(label);
						sort(kVariables.begin(), kVariables.end(), sortSmallToLarge);
						kVariables.pop_back();
					}
				}
			}
		}
	}
	while (!listQueue.empty())
	{
		Label returnLabel = listQueue.front();
		listQueue.pop_front();


		if (!isAlive(returnLabel.getId()))
		{
			continue;
		}
		int curr_node = returnLabel.getCurrentNodeId();
		vector<bool>* visitedNodes = returnLabel.getVisitedCustomer();

		for (int nextId : outNeighbors[curr_node])
		{
			if (nextId == curr_node) continue;
			if ((*visitedNodes)[nextId] || !edges[curr_node][nextId]) continue;

			Customer* nextCust = customerById[nextId];
			if (nextCust == nullptr) continue;

			vector<bool> nextVisitedNodes = *visitedNodes;
			nextVisitedNodes[nextId] = true;

			Label label(labelCounter, nextId, nextVisitedNodes);
			labelCounter++;

			//label.setPrice(returnLabel.getPrice());
			label.setIndexInSequenceOnBound(returnLabel.getIndexInSequenceOnBound());
			label.setCargoWeight(returnLabel.getCargoWeight());

			// IMPORTANT: copy parent mask, then set new bit
			std::vector<uint64_t> mask = returnLabel.getVisitedMask();
			bitsetSet(mask, nextId);
			label.setVisitedMask(mask);

			for (int i = 0; i < returnLabel.getSequence()->size(); i++)
			{
				label.getSequence()->push_back((*returnLabel.getSequence())[i]);
			}
			if (extensionProcedureForward(&label,
				(*nextCust), &redCost,
				dualValR,
				getMinSpeed(),
				getMaxSpeed(),
				getMinSpeedMatrix(),
				isfarkas, &edges, eps,
				getVehicleCapacity()))
			{
				costFunctionForwardLabeling(label, getMaxSpeed(), getEps(), isfarkas);

				bool dominated = false;

			for (int k = 0; k < (*table)[nextId].size(); k++)
				{
					if (!branchingConTrueExists || !startNodeOfTrueBranchingCon[(*table)[nextId][k].getSequence()->back().getId()])
					{
						if (isSecondLabelDominated_Extension((*table)[nextId][k], label, orgEdgeMask, currentEdgeMask, isfarkas, eps, rootNode, heuristicPricing))
						{
							dominated = true;
							break;
						}
					}
					bool dominates = false;
					if(isSecondLabelDominated_Extension(label, (*table)[nextId][k], orgEdgeMask, currentEdgeMask, isfarkas, eps, rootNode, heuristicPricing)
						&& (!branchingConTrueExists || !startNodeOfTrueBranchingCon[label.getSequence()->back().getId()]))
					{
						dominates = true;
						if (dominates)
						{
							// no queue erase; just mark dead
							markDead((*table)[nextId][k].getId());

							for (vector<Label>::iterator lQIt = kVariables.begin(); lQIt != kVariables.end(); lQIt++)
							{
								if (lQIt->getId() == (*table)[nextId][k].getId())
								{
									kVariables.erase(lQIt);
									break;
								}
							}

							(*table)[nextId].erase((*table)[nextId].begin() + k);
							k--;
						}
					}
				}
				
				if (!dominated)
				{

					listQueue.push_back(label);
					markAlive(label.getId());

					if (!branchingConTrueExists ||
						!startNodeOfTrueBranchingCon[label.getSequence()->back().getId()])
					{
						(*table)[nextId].push_back(label);
						if (kVariables.size() < getVarsToBeAddedInAnIteration() && label.getReducedCostLabel() < -eps)
						{
							kVariables.push_back(label);
							sort(kVariables.begin(), kVariables.end(), sortSmallToLarge);
						}
						else if (label.getReducedCostLabel() < -eps && label.getReducedCostLabel() < kVariables.back().getReducedCostLabel())
						{
							kVariables.push_back(label);
							sort(kVariables.begin(), kVariables.end(), sortSmallToLarge);
							kVariables.pop_back();
						}
					}
				}
			}
		}
		if (labelCounter % 100000 == 0 && timeLimitReached())
		{
			break;
		}
	}
}
