// Copyright (C) 2003
// Gerhard Neumann (gerhard@igi.tu-graz.ac.at)

//                
// This file is part of RL Toolbox.
// http://www.igi.tugraz.at/ril_toolbox
//
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. The name of the author may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
// 
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
// IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
// NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "ril_debug.h"
#include "clinearfafeaturecalculator.h"
#include <assert.h>
#include <math.h>

CFeatureOperatorOr::CFeatureOperatorOr() : CFeatureCalculator()
{
	addType(FEATURESTATEDERIVATIONX);

	this->featureFactors = new std::map<CStateModifier *, rlt_real>();
}

CFeatureOperatorOr::~CFeatureOperatorOr()
{
	delete featureFactors;
}

void CFeatureOperatorOr::getModifiedState(CStateCollection *stateCol, CState *state)
{
	assert(bInit);

	std::list<CStateModifier *>::iterator it = getStateModifiers()->begin();
	std::list<CState *>::iterator stateIt = states->begin();

	CState *stateBuf;

	int i = 0;
	int numFeatures = 0;

	for (; it != getStateModifiers()->end(); it++, stateIt++)
	{
		if (stateCol->isMember(*it))
		{
			stateBuf = stateCol->getState(*it);
		}
		else
		{
			stateBuf = *stateIt;
			(*it)->getModifiedState(stateCol, stateBuf);
		}
		rlt_real featureStateFactor = (*this->featureFactors)[*it];
		if (stateBuf->getStateProperties()->isType(FEATURESTATE))
		{
			for (unsigned int j = 0; j < stateBuf->getNumDiscreteStates(); j++)
			{
				state->setDiscreteState(i, stateBuf->getDiscreteState(j) + numFeatures);
				state->setContinuousState(i, stateBuf->getContinuousState(j) * featureStateFactor);
				i ++;
			}
		}
		else
		{
			if (stateBuf->getStateProperties()->isType(DISCRETESTATE))
			{
				state->setDiscreteState(i, stateBuf->getDiscreteState(0) + numFeatures);
				state->setContinuousState(i, featureStateFactor);
				i ++;
			}
		}

		numFeatures += (*it)->getDiscreteStateSize();
	}
	normalizeFeatures(state);
}

void CFeatureOperatorOr::addStateModifier(CStateModifier *featCalc, rlt_real factor)
{
	CStateMultiModifier::addStateModifier(featCalc);
	
    if (!featCalc->isType(STATEDERIVATIONX))
	{
		type = getType() & ~ STATEDERIVATIONX;	
	}
	(*this->featureFactors)[featCalc] = factor;

}

CStateModifier *CFeatureOperatorOr::getStateModifier(int feature)
{
	std::list<CStateModifier *>::iterator it = getStateModifiers()->begin();
	int numFeatures = (*it)->getDiscreteStateSize();

	while (it != getStateModifiers()->end() && numFeatures < feature)
	{
		it ++;

		numFeatures += (*it)->getDiscreteStateSize();
	}

	if (it != getStateModifiers()->end())
	{
		return *it;
	}
	else
	{
		return NULL;
	}
}

void CFeatureOperatorOr::getFeatureDerivationX(int feature, CStateCollection *state, CMyVector *targetVector)
{
	CStateModifier *stateMod = getStateModifier(feature);

	if (stateMod->isType(FEATURESTATEDERIVATIONX))
	{
		CFeatureCalculator *featCalc = dynamic_cast<CFeatureCalculator *>(stateMod);
		featCalc->getFeatureDerivationX(feature, state, targetVector);
	}
}

void CFeatureOperatorOr::initFeatureOperator()
{
	assert(!bInit);

	std::list<CStateModifier *>::iterator it;
	int numFeatures = 0;
	int numActiveFeatures = 0;
	for (it = this->modifiers->begin(); it != modifiers->end(); it ++)
	{
		numActiveFeatures += (*it)->getNumDiscreteStates();

		numFeatures += (*it)->getDiscreteStateSize(0);
	}
	
	initFeatureCalculator(numFeatures, numActiveFeatures);
}

CFeatureOperatorAnd::CFeatureOperatorAnd() : CFeatureCalculator()
{
	addType(FEATURESTATEDERIVATIONX);
}


void CFeatureOperatorAnd::getModifiedState(CStateCollection *stateCol, CState *featState)
{
	int featureOffset = 1;

	std::list<CStateModifier *>::iterator it = getStateModifiers()->begin();
	std::list<CState *>::iterator stateIt = states->begin();


	CState *stateBuf;

	for (unsigned int i = 0; i < getNumDiscreteStates();i ++)
	{
		featState->setDiscreteState(i, 0);
		featState->setContinuousState(i, 1.0);
	}

	int repetitions = getNumDiscreteStates();
	for (int j = 0; it != getStateModifiers()->end(); it ++, stateIt ++, j ++)
	{
		repetitions /= (*it)->getNumDiscreteStates();
		stateBuf = NULL;
		if (stateCol->isMember(*it))
		{
			stateBuf = stateCol->getState(*it);
		}
		else
		{
			stateBuf = *stateIt;
			(*it)->getModifiedState(stateCol, stateBuf);
		}
		
		if (stateBuf->getStateProperties()->isType(FEATURESTATE))
		{
			for (unsigned int i = 0; i < getNumDiscreteStates(); i++)
			{
				unsigned int singleStateFeatureNum = (i / repetitions) % stateBuf->getNumDiscreteStates();
				featState->setDiscreteState(i, featState->getDiscreteState(i) + featureOffset * stateBuf->getDiscreteState(singleStateFeatureNum));
				featState->setContinuousState(i, featState->getContinuousState(i) * stateBuf->getContinuousState(singleStateFeatureNum));
			}
		}
		else
		{
			for (unsigned int i = 0; i < getNumDiscreteStates(); i++)
			{
				featState->setDiscreteState(i, featState->getDiscreteState(i) + featureOffset * stateBuf->getDiscreteState(0));				
			}
		}

		featureOffset = featureOffset * (*it)->getDiscreteStateSize();
	}
	normalizeFeatures(featState);
}

void CFeatureOperatorAnd::getFeatureDerivationX(int feature, CStateCollection *stateCol, CMyVector *derivation)
{
	std::list<CStateModifier *>::iterator it = getStateModifiers()->begin();
	std::list<CState *>::iterator stateIt = states->begin();


	CState *stateBuf;

	rlt_real featureFactor = 1.0;
	CMyVector tempVector(derivation->getNumDimensions());
	
	derivation->initVector(0.0);
	int lfeature = 0;
	rlt_real lfeatureFactor = 0.0;

	for (; it != getStateModifiers()->end(); it ++, stateIt ++)
	{
		stateBuf = NULL;
		
		assert((*it)->isType(FEATURESTATEDERIVATIONX));
		CFeatureCalculator *devXFeatCalc = dynamic_cast<CFeatureCalculator *>(*it);

		tempVector.initVector(0.0);
		if (stateCol->isMember(*it))
		{
			stateBuf = stateCol->getState(*it);
		}
		else
		{
			stateBuf = *stateIt;
			(*it)->getModifiedState(stateCol, stateBuf);
		}
		unsigned int i = 0;
		lfeatureFactor = 0.0;
		lfeature = feature % (*it)->getDiscreteStateSize();
		feature = feature / (*it)->getDiscreteStateSize();

		while (i < stateBuf->getNumDiscreteStates() && stateBuf->getDiscreteState(i) != lfeature)
		{
			i++;
		}
		if (i < stateBuf->getNumDiscreteStates())
		{
			lfeatureFactor = stateBuf->getContinuousState(i);
		}

		devXFeatCalc->getFeatureDerivationX(lfeature, stateCol, &tempVector);
		
		tempVector.multScalar(featureFactor);
		derivation->multScalar(lfeatureFactor);
		
		derivation->addVector(&tempVector);

		featureFactor *= lfeatureFactor;
	}
}

void CFeatureOperatorAnd::addStateModifier(CStateModifier *featCalc)
{
	CStateMultiModifier::addStateModifier(featCalc);

	if (!featCalc->isType(STATEDERIVATIONX))
	{
		type = getType() & ~ STATEDERIVATIONX;	
	}
}

void CFeatureOperatorAnd::initFeatureOperator()
{
	assert(!bInit);
	std::list<CStateModifier *>::iterator it;
	int numFeatures = 1;
	int numActiveFeatures = 1;
	for (it = this->modifiers->begin(); it != modifiers->end(); it ++)
	{
		numActiveFeatures *= (*it)->getNumDiscreteStates();

		numFeatures *= (*it)->getDiscreteStateSize(0);
	}

	initFeatureCalculator(numFeatures, numActiveFeatures);
}

CGridFeatureCalculator::CGridFeatureCalculator(unsigned int numDim, unsigned int dimensions[], unsigned int part[], rlt_real off[], unsigned int numActiveFeatures) : CFeatureCalculator(numActiveFeatures, numActiveFeatures)
{
	this->numDim = numDim;
	this->dimensions = new unsigned int[numDim];
	this->partitions = new unsigned int[numDim];
	this->offsets = new rlt_real[numDim];

	dimensionSize = new unsigned int[numDim];

	numFeatures = 1;
	for (unsigned int i = 0; i < numDim; i ++)
	{
		this->dimensions[i] = dimensions[i];
		this->partitions[i] = part[i];
		this->offsets[i] = off[i];
        dimensionSize[i] = numFeatures;
		numFeatures *= partitions[i];
	}

	for (unsigned int i = 0; i < this->getNumDiscreteStates(); i++)
	{
		this->setDiscreteStateSize(i, numFeatures);
	}

	for (unsigned int i = 0; i < this->getNumContinuousStates(); i++)
	{
		this->setMinValue(i, 0.0);
		this->setMaxValue(i, 1.0);
	}

	gridScale = new rlt_real[numDim];

	for (int i = 0; i < numDim; i++)
	{
		gridScale[i] = 1.0;
	}

	originalState = NULL;
}

CGridFeatureCalculator::~CGridFeatureCalculator()
{
	delete offsets;
	delete partitions;
	delete dimensions;
	delete dimensionSize;

	delete gridScale;
}


unsigned int CGridFeatureCalculator::getNumDimensions()
{
	return numDim;
}

void CGridFeatureCalculator::setGridScale(int dimension, rlt_real scale)
{
	gridScale[dimension] = scale;
}

void CGridFeatureCalculator::getFeaturePosition(unsigned int feature, CMyVector *position)
{
	int partition = 0;
	unsigned int temp = feature;
	
	for (unsigned int i = 0; i < numDim; i++)
	{
		partition = temp % partitions[i];
		position->setElement(i, offsets[i] + 1.0 / partitions[i] * (0.5 + partition) * gridScale[i]);

		temp = temp / partitions[i];
	}
}

unsigned int CGridFeatureCalculator::getActiveFeature(CState *state)
{
	rlt_real part = 0;
	int singleStateFeature = 0;
	unsigned int feature = 0;

	for (unsigned int i = 0; i < numDim; i++)
	{	
		assert(dimensions[i] < state->getNumContinuousStates());

		part = (state->getNormalizedContinuousState(dimensions[i]) - offsets[i]);

		if (state->getStateProperties()->getPeriodicity(i) && gridScale[i] >= 1.0)
		{
			part = part - floor(part);
		}

		singleStateFeature = (int) floor(part * partitions[i] / gridScale[i]);

		if (singleStateFeature < 0)
			singleStateFeature = 0;
		if (singleStateFeature >= partitions[i])
			singleStateFeature = partitions[i] - 1;

		feature += singleStateFeature * dimensionSize[i];
	}
	return feature;
}

void CGridFeatureCalculator::getSingleActiveFeature(CState *state, unsigned int *activeFeature)
{
	rlt_real part = 0;
	int tempSingleStateFeature = 0;

	for (unsigned int i = 0; i < numDim; i++)
	{	
		assert(dimensions[i] < state->getNumContinuousStates());

		part = (state->getNormalizedContinuousState(dimensions[i]) - offsets[i]);

		if (state->getStateProperties()->getPeriodicity(i) && gridScale[i] >= 1.0)
		{
			part = part - floor(part);
		}

		tempSingleStateFeature = (unsigned int) floor(part * partitions[i] / gridScale[i]);

		if (tempSingleStateFeature < 0)
			tempSingleStateFeature = 0;
		if (tempSingleStateFeature >= partitions[i])
			tempSingleStateFeature = partitions[i] - 1;

		activeFeature[i] = tempSingleStateFeature;
	}
}

unsigned int CGridFeatureCalculator::getFeatureIndex(int position[])
{
	unsigned int feature = 0;

	for (unsigned int i = 0; i < numDim; i++)
	{	
		feature +=  position[i] * dimensionSize[i];
	}
	return feature;
}

CTilingFeatureCalculator::CTilingFeatureCalculator(unsigned int numDim, unsigned int dimensions[], unsigned int partitions[], rlt_real offsets[]) : CGridFeatureCalculator(numDim, dimensions, partitions, offsets, 1)
{
}

CTilingFeatureCalculator::~CTilingFeatureCalculator()
{
}
	
void CTilingFeatureCalculator::getModifiedState(CStateCollection *state, CState *featState)
{
	featState->setDiscreteState(0, getActiveFeature(state->getState(originalState)));
	featState->setContinuousState(0, 1.0);
}


CLinearMultiFeatureCalculator::CLinearMultiFeatureCalculator(unsigned int numDim, unsigned int dimensions[], unsigned int partitions[], rlt_real offsets[], unsigned int numActiveFeatures) : CGridFeatureCalculator(numDim, dimensions, partitions, offsets, numActiveFeatures)
{
	areaSize = new unsigned int[numDim];	

	activePosition = new CMyVector(numDim);
	featurePosition = new CMyVector(numDim);
	actualPartition = new unsigned int[numDim];
	singleStateFeatures = new unsigned int[numDim];
}

CLinearMultiFeatureCalculator::~CLinearMultiFeatureCalculator()
{
	delete areaSize;

	delete activePosition;
	delete featurePosition;
	delete [] actualPartition;
	delete [] singleStateFeatures;

}

void CLinearMultiFeatureCalculator::initAreaSize()
{
	memset(areaSize, 0, sizeof(unsigned int) * numDim);
}
	
void CLinearMultiFeatureCalculator::calcNumActiveFeatures()
{
	areaNumPart = 1;
	for (unsigned int i = 0; i < numDim; i++)
	{
		areaNumPart *= areaSize[i];
	}
	this->continuousStates = areaNumPart;
	this->discreteStates = areaNumPart;

	numActiveFeatures = areaNumPart;

	this->discreteStateSize = new unsigned int[discreteStates];
	this->minValues = new rlt_real[continuousStates];
	this->maxValues = new rlt_real[continuousStates];

	for (unsigned int i = 0; i < this->getNumDiscreteStates(); i++)
	{
		this->setDiscreteStateSize(i, numFeatures);
	}

	for (unsigned int i = 0; i < this->getNumContinuousStates(); i++)
	{
		this->setMinValue(i, 0.0);
		this->setMaxValue(i, 1.0);
	}
}