void CLinearMultiFeatureCalculator::getModifiedState(CStateCollection *stateCol, CState *featState)
{
	assert(equals(featState->getStateProperties()));

	memset(actualPartition, 0, sizeof(unsigned int) * numDim);
	unsigned int j = 0;
	CState *state = stateCol->getState(originalState);

/*	printf("State : ");
	for (int i = 0; i < 4; i ++)
	{
		printf("%f ", state->getNormalizedContinuousState(i));
	}
	printf("\n");*/

	getSingleActiveFeature(state, singleStateFeatures);
	unsigned int i;
	int feature = 0;

	//offset to add to the actual feature
	int featureAdd = 0;

	getFeaturePosition(getActiveFeature(state), activePosition);

	for (i = 0; i < numDim; i ++)
	{
		int singleFeatureOffset = 0;
		if (areaSize[i] % 2 == 0)
		{
			//rlt_real x1 = state->getNormalizedContinuousState(dimensions[i]);
			//rlt_real x2 = activePosition->getElement(i);
			if (state->getNormalizedContinuousState(dimensions[i]) < activePosition->getElement(i))
			{
				singleFeatureOffset ++;
			}
			singleFeatureOffset += (areaSize[i] - 1) / 2;
		}
		else
		{
			singleFeatureOffset += areaSize[i] / 2;
		}
		singleStateFeatures[i] -= singleFeatureOffset;
		activePosition->setElement(i, activePosition->getElement(i) - singleFeatureOffset * 1.0 / partitions[i] * gridScale[i]);
	}

	unsigned int featureIndex = 0;


	for (i = 0; i < areaNumPart; i++)
	{
		feature = 0;
		/*for (j = 0; j < numDim; j++)
		{
			int dist = (actualPartition[j] - areaSize[j]);
			featurePosition->setElement(j,  (activePosition->getElement(j) + 1.0 / partitions[j] * dist));

			featureAdd = (singleStateFeatures[j] + (actualPartition[j] - areaSize[j]));
			if (state->getStateProperties()->getPeriodicity(j))
			{
				featurePosition->setElement(j, featurePosition->getElement(j) - floor(featurePosition->getElement(j)));
				featureAdd = featureAdd - (int) floor((rlt_real) featureAdd / (rlt_real) partitions[j]) * partitions[j];
			}

			feature = feature + featureAdd * dimensionSize[j];
		} */
		/*for (j = 0; j < numDim; j++)
		{
			int dist = (actualPartition[j] - 1);
			featurePosition->setElement(j,  (activePosition->getElement(j) + 1.0 / partitions[j] * dist));

			featureAdd = (singleStateFeatures[j] + (actualPartition[j] - 1));
			if (state->getStateProperties()->getPeriodicity(j))
			{
				featurePosition->setElement(j, featurePosition->getElement(j) - floor(featurePosition->getElement(j)));
				featureAdd = featureAdd - (int) floor((rlt_real) featureAdd / (rlt_real) partitions[j]) * partitions[j];
			}
			
			feature = feature + featureAdd * dimensionSize[j];
		} 
		for (i = 0; i < numDim; i ++)
		{
			int singleFeatureOffset = 0;
			if (areaSize[i] % 2 == 0)
			{
				if (state->getContinuousState(dimensions[i]) < activePosition->getElement(i))
				{
					singleFeatureOffset ++;
				}
				singleFeatureOffset += (singleFeatureOffset - 1) / 2;
			}
			else
			{
				singleFeatureOffset += areaSize[i] / 2;
			}
			singleStateFeatures[i] -= singleFeatureOffset;
			activePosition->setElement(i, activePosition->getElement(i) - singleFeatureOffset * 1.0 / partitions[i]);

		}*/
		for (j = 0; j < numDim; j++)
		{
			//int dist = (actualPartition[j] - areaSize[j]);
			featurePosition->setElement(j,  activePosition->getElement(j) + (1.0 / partitions[j] * actualPartition[j])  * gridScale[j]);

			featureAdd = (singleStateFeatures[j] + actualPartition[j]);
			if (state->getStateProperties()->getPeriodicity(j) && gridScale[j] >= 1.0)
			{
				featurePosition->setElement(j, featurePosition->getElement(j) - floor(featurePosition->getElement(j)));
				featureAdd = featureAdd - (int) floor((rlt_real) featureAdd / (rlt_real) partitions[j]) * partitions[j];
			}

			feature = feature + featureAdd * dimensionSize[j];
		}
		if (feature >= 0 && (unsigned int) feature < getNumFeatures())
		{
			featState->setDiscreteState(featureIndex, feature);
			featState->setContinuousState(featureIndex, getFeatureFactor(state, featurePosition));
			featureIndex ++;
		}
		else
		{
			j = 0;
		}
				
		j = 0;
	
		actualPartition[0] ++;
		while (j < numDim && actualPartition[j] >= areaSize[j])
		{
			actualPartition[j] = 0;
			j ++;
			if (j < numDim)
			{
				actualPartition[j]++;
			}
		}
	}
	featState->setNumActiveContinuousStates(featureIndex);
	featState->setNumActiveDiscreteStates(featureIndex);

	for (; featureIndex < areaNumPart; featureIndex ++)
	{
		featState->setDiscreteState(featureIndex, 0);
		featState->setContinuousState(featureIndex, 0.0);
	}

	this->normalizeFeatures(featState);
}

CRBFFeatureCalculator::CRBFFeatureCalculator(unsigned int numDim, unsigned int dimensions[], unsigned int partitions[], rlt_real offsets[], rlt_real sigma[]) : CLinearMultiFeatureCalculator(numDim, dimensions, partitions, offsets, 0)
{
	this->sigma = new rlt_real[numDim];
	memcpy(this->sigma, sigma, sizeof(rlt_real) * numDim);
	sigmaMaxSize = 2.0;
	initAreaSize();
	addType(FEATURESTATEDERIVATIONX);
}

CRBFFeatureCalculator::CRBFFeatureCalculator(unsigned int numDim, unsigned int dimensions[], unsigned int partitions[], rlt_real offsets[], rlt_real sigma[], unsigned int areaSize[]) : CLinearMultiFeatureCalculator(numDim, dimensions, partitions, offsets, 0)
{
	this->sigma = new rlt_real[numDim];
	memcpy(this->sigma, sigma, sizeof(rlt_real) * numDim);
	sigmaMaxSize = 2.0;
	memcpy(this->areaSize, areaSize, sizeof(unsigned int) * numDim);
	calcNumActiveFeatures();

	addType(FEATURESTATEDERIVATIONX);
}


CRBFFeatureCalculator::~CRBFFeatureCalculator()
{
	delete sigma;
}

void CRBFFeatureCalculator::initAreaSize()
{
	for (unsigned int i = 0; i < numDim; i++)
	{
		areaSize[i] = (2 * (unsigned int)floor(sigmaMaxSize * sigma[i] * partitions[i]) + 1);
		if(areaSize[i] <= 1)
		{
			areaSize[i] += 1;
		}
	}
	calcNumActiveFeatures();
}

rlt_real CRBFFeatureCalculator::getFeatureFactor(CState *state, CMyVector *position)
{
	rlt_real exponent = 0.0;
	
//	printf("position : ");
//	position->saveASCII(stdout);

	for (unsigned int i = 0; i < numDim; i++)
	{
		rlt_real difference = fabs(state->getNormalizedContinuousState(i) - position->getElement(i));

		if (state->getStateProperties()->getPeriodicity(i) && difference > 0.5)
		{
			difference = 1 - difference;
		}

		exponent += pow(difference / (sigma[i] * gridScale[i]), 2) / 2;
	}
//	printf(" Factor %f \n", my_exp(- exponent));

	return my_exp(- exponent);
}

void CRBFFeatureCalculator::getFeatureDerivationX(int feature, CStateCollection *stateCol, CMyVector *targetVector)
{
	getFeaturePosition(feature, activePosition);
	CState *state = stateCol->getState();
	rlt_real factor = getFeatureFactor(state, activePosition);
	
	for (unsigned int i = 0; i < this->numDim; i ++)
	{
		targetVector->setElement(dimensions[i],  factor * (- state->getSingleStateDifference(dimensions[i], activePosition->getElement(i))) / pow(sigma[i], 2));
	}
}


CLinearInterpolationFeatureCalculator::CLinearInterpolationFeatureCalculator(unsigned int numDim, unsigned int dimensions[], unsigned int partitions[], rlt_real offsets[]) : CLinearMultiFeatureCalculator(numDim, dimensions, partitions, offsets, 0)
{
	initAreaSize();
}

CLinearInterpolationFeatureCalculator::~CLinearInterpolationFeatureCalculator()
{
}

rlt_real CLinearInterpolationFeatureCalculator::getFeatureFactor(CState *state, CMyVector *featPos)
{
	rlt_real factor = 1.0;
	
	for (unsigned int i = 0; i < numDim; i++)
	{

		rlt_real difference = fabs(state->getNormalizedContinuousState(i) - featPos->getElement(i));

		if (state->getStateProperties()->getPeriodicity(i) && difference > 0.5)
		{
			difference = 1 - difference;
		}
		factor *= 1 - difference * partitions[i];
	}
	return factor;
}

void CLinearInterpolationFeatureCalculator::initAreaSize()
{
	for (unsigned int i = 0; i < numDim; i ++)
	{
		areaSize[i] = 2;
	}
	calcNumActiveFeatures();
}






CSingleStateFeatureCalculator::CSingleStateFeatureCalculator(int dimension, int numPartitions, rlt_real *partitions, int numActiveFeatures) : CFeatureCalculator(numPartitions, numActiveFeatures)
{
	this->dimension = dimension;
	this->partitions = new rlt_real[numPartitions];
	memcpy(this->partitions, partitions, sizeof(rlt_real) * numPartitions);

	originalState = NULL;
}

CSingleStateFeatureCalculator::~CSingleStateFeatureCalculator()
{
	delete partitions;
}

void CSingleStateFeatureCalculator::getModifiedState(CStateCollection *stateCol, CState *featState)
{
	CState *state = stateCol->getState(originalState);
	CStateProperties *properties = state->getStateProperties();
	rlt_real contState = state->getContinuousState(dimension);
	rlt_real width = properties->getMaxValue(dimension) - properties->getMinValue(dimension);


	if (contState < partitions[0] && properties->getPeriodicity(dimension))
	{
		contState += width;
	}

	unsigned int activeFeature = 0, featureIndex = 0;
	unsigned int featureNum = 0, realfeatureNum = 0;

	rlt_real part = partitions[activeFeature];

	while (activeFeature < numFeatures && part < contState)
	{
		activeFeature++;

		part = partitions[activeFeature];

		if (part < partitions[0])
		{
			assert(properties->getPeriodicity(dimension));
			part += width;
		}
	}

	
	if (activeFeature == numFeatures && !properties->getPeriodicity(dimension))
	{
		featureNum ++;
	}
	
	for (; realfeatureNum < this->numActiveFeatures; realfeatureNum++, featureNum ++)
	{
		if (featureNum % 2 == 0)
		{
			featureIndex = activeFeature + featureNum / 2;
		}
		else
		{
			featureIndex = activeFeature - (featureNum / 2 + 1);
		}

		if (state->getStateProperties()->getPeriodicity(dimension))
		{
			featureIndex = featureIndex % numFeatures;
		}
		
		if (featureIndex >= 0 && featureIndex < numFeatures)
		{
			featState->setDiscreteState(realfeatureNum, featureIndex);
			featState->setContinuousState(realfeatureNum, getFeatureFactor(featureIndex, contState, state->getSingleStateDifference(dimension, partitions[featureIndex])));
		}
		else
		{
			featState->setContinuousState(realfeatureNum, 0.0);
			featState->setDiscreteState(realfeatureNum, 0);
		}
	}
	this->normalizeFeatures(featState);
}


CSingleStateRBFFeatureCalculator::CSingleStateRBFFeatureCalculator(int dimension, int numPartitions, rlt_real *partitions, int numActiveFeatures, rlt_real *l_sigma) : CSingleStateFeatureCalculator(dimension, numPartitions, partitions, numActiveFeatures)
{
	this->sigma = new rlt_real[numPartitions];
	memcpy(this->sigma, l_sigma, sizeof(rlt_real) * numPartitions);


	addType(FEATURESTATEDERIVATIONX);
}


CSingleStateRBFFeatureCalculator::CSingleStateRBFFeatureCalculator(int dimension, int numPartitions, rlt_real *partitions, int numActiveFeatures) : CSingleStateFeatureCalculator(dimension, numPartitions, partitions, numActiveFeatures)
{
	this->sigma = new rlt_real[numPartitions];

	if (numPartitions > 1)
	{
		sigma[0] = (partitions[1] - partitions[0]) / 2;
		sigma[numPartitions - 1] = (partitions[numPartitions - 1] - partitions[numPartitions - 2]) / 2;

		for (int i = 1; i < numPartitions - 1; i ++)
		{
			sigma[i] = (partitions[i + 1] - partitions[i - 1]) / 4;
		}
	}

	addType(FEATURESTATEDERIVATIONX);
}

void CSingleStateRBFFeatureCalculator::setSigma(int i, rlt_real l_sigma)
{
	this->sigma[i] = l_sigma;
}

rlt_real CSingleStateRBFFeatureCalculator::getSigma(int i)
{
	return sigma[i];
}

void CSingleStateRBFFeatureCalculator::getFeatureDerivationX(int feature, CStateCollection *stateCol, CMyVector *targetVector)
{
	rlt_real distance = stateCol->getState(originalState)->getSingleStateDifference(dimension, partitions[feature]);

	//rlt_real dev = my_exp(- pow(distance / sigma, 2) / 2) * distance * (- 1 / pow(sigma,2));
	//targetVector->setElement(dimension, targetVector->getElement(dimension) + dev);
}

rlt_real CSingleStateRBFFeatureCalculator::getFeatureFactor(int partition, rlt_real contState, rlt_real difference)
{
	rlt_real distance = fabs(difference);

	return my_exp(- pow(distance / sigma[partition], 2) / 2) ;
}


CSingleStateLinearInterpolationFeatureCalculator::CSingleStateLinearInterpolationFeatureCalculator(int dimension, int numPartitions, rlt_real *partitions) : CFeatureCalculator(numPartitions, 2)
{
	this->dimension = dimension;
	this->partitions = new rlt_real[numPartitions];
	memcpy(this->partitions, partitions, sizeof(rlt_real) * numPartitions);

	originalState = NULL;

	addType(FEATURESTATEDERIVATIONX);
}

CSingleStateLinearInterpolationFeatureCalculator::~CSingleStateLinearInterpolationFeatureCalculator()
{
	delete partitions;
}

void CSingleStateLinearInterpolationFeatureCalculator::getModifiedState(CStateCollection *stateCol, CState *featState)
{
	CState *state = stateCol->getState(originalState);
	rlt_real contState = state->getContinuousState(dimension);
	unsigned int activeFeature = 0;
	rlt_real k = 0.0;
	CStateProperties *stateProp = state->getStateProperties();

	while (activeFeature < numFeatures && partitions[activeFeature] < contState)
	{
		activeFeature++;
	}

	if (!stateProp->getPeriodicity(dimension) && (activeFeature == numFeatures || activeFeature == 0))
	{
		if (activeFeature == numFeatures)
		{
			activeFeature = numFeatures - 1;
		}
		featState->setDiscreteState(0, activeFeature);
		featState->setContinuousState(0, 1.0);
		featState->setDiscreteState(1, 0);
		featState->setContinuousState(1, 0.0);
	}
	else
	{
		if (stateProp->getPeriodicity(dimension) && (activeFeature == numFeatures || activeFeature == 0))
		{
			activeFeature = activeFeature % numFeatures;
			k = state->getSingleStateDifference(dimension, partitions[activeFeature]) / stateProp->getMirroredStateValue(dimension, partitions[activeFeature] - partitions[(activeFeature - 1) % numFeatures]);
			
		}
		else
		{
			k = (partitions[activeFeature] - contState) / (partitions[activeFeature] - partitions[activeFeature - 1]);
		}
		featState->setDiscreteState(0, activeFeature);
		featState->setContinuousState(0, 1.0 - k);
		featState->setDiscreteState(1, (activeFeature - 1) % numFeatures);
		featState->setContinuousState(1, k);
	}
}

void CSingleStateLinearInterpolationFeatureCalculator::getFeatureDerivationX(int feature, CStateCollection *stateCol, CMyVector *targetVector)
{
	rlt_real distance = fabs(stateCol->getState(originalState)->getSingleStateDifference(dimension, partitions[feature]));

	rlt_real dev = 0;
	
	if (distance > 0)
	{
		dev = - 1.0 /  getMirroredStateValue(dimension, partitions[feature] - partitions[(feature + 1) % numFeatures]);
	}
	else
	{
		dev = 1.0 / getMirroredStateValue(dimension, partitions[feature] - partitions[(feature + 1) % numFeatures]);
	}

	targetVector->setElement(dimension, dev);
}

CFeatureStateNNInput::CFeatureStateNNInput(CFeatureCalculator *l_featureStateCalc) : CStateModifier(l_featureStateCalc->getNumFeatures(), 0)
{
	this->featureStateCalc = l_featureStateCalc;
	this->featureState = new CState(l_featureStateCalc);	
}

CFeatureStateNNInput::~CFeatureStateNNInput()
{
	delete featureState;
}


void CFeatureStateNNInput::getModifiedState(CStateCollection *stateCol, CState *state)
{
	CState *featureStateBuff;
	state->initVector(0.0);

	if (stateCol->isMember(featureStateCalc))
	{
		featureStateBuff = stateCol->getState(featureStateCalc);
	}
	else
	{
		featureStateCalc->getModifiedState(stateCol, featureState);
		featureStateBuff = featureState;
	}

	for (int i = 0;i < featureStateCalc->getNumActiveFeatures(); i++)
	{
		state->setContinuousState(featureStateBuff->getDiscreteState(i), featureStateBuff->getContinuousState(i));
	}
}