/*!
 *  \brief Select a node for mating in the given individual, following the constraints penalties.
 *  \param outSelectTreeIndex Tree index of the selected node.
 *  \param outSelectNodeIndex Index of the selected node.
 *  \param inSelectABranch True if node to select must be a branch, false if it must a leaf.
 *  \param inNodeReturnType Desired return type for the nodes to be selected.
 *  \param inPrimitSetIndex Primitive set index to which the tree must be associated.
 *  \param inMaxSubTreeDepth Maximum sub tree depth allowed of the node to be selected.
 *  \param inMaxSubTreeSize Maximum sub tree size allowed of the node to be selected.
 *  \param inIndividual Individual to select the node from.
 *  \param ioContext Evolutionary context.
 *  \return True if there was node to select, false if no node respected all constraints.
 */
bool STGP::CrossoverConstrainedOp::selectNodeToMateWithType(unsigned int& outSelectTreeIndex,
        unsigned int& outSelectNodeIndex,
        bool inSelectABranch,
        const std::type_info* inNodeReturnType,
        unsigned int inPrimitSetIndex,
        unsigned int inMaxSubTreeDepth,
        unsigned int inMaxSubTreeSize,
        GP::Individual& inIndividual,
        GP::Context& ioContext) const
{
	Beagle_StackTraceBeginM();
	RouletteT< std::pair<unsigned int,unsigned int> > lRoulette;
	GP::Tree::Handle lOldTreeHandle = ioContext.getGenotypeHandle();
	const unsigned int lOldTreeIndex = ioContext.getGenotypeIndex();
	ioContext.emptyCallStack();
	for(unsigned int i=0; i<inIndividual.size(); ++i) {
		if(inIndividual[i]->getPrimitiveSetIndex() != inPrimitSetIndex) continue;
		ioContext.setGenotypeHandle(inIndividual[i]);
		ioContext.setGenotypeIndex(i);
		buildRouletteWithType(lRoulette,
		                      inSelectABranch,
		                      inNodeReturnType,
		                      inMaxSubTreeDepth,
		                      inMaxSubTreeSize,
		                      0,
		                      *inIndividual[i],
		                      ioContext);
	}
	ioContext.setGenotypeIndex(lOldTreeIndex);
	ioContext.setGenotypeHandle(lOldTreeHandle);
	if(lRoulette.size() == 0) return false;
	std::pair<unsigned int,unsigned int> lSelectedNode =
	    lRoulette.select(ioContext.getSystem().getRandomizer());
	outSelectTreeIndex = lSelectedNode.first;
	outSelectNodeIndex = lSelectedNode.second;
	return true;
	Beagle_StackTraceEndM();
}
/*!
 *  \brief Standard mutate a constrained GP individual.
 *  \param ioIndividual GP individual to standard mutate.
 *  \param ioContext Context of the evolution.
 *  \return True if the individual is effectively mutated, false if not.
 */
bool GP::MutationStandardSelectiveConstrainedOp::mutate(Beagle::Individual& ioIndividual, Beagle::Context& ioContext)
{
	Beagle_StackTraceBeginM();
	GP::Individual& lIndividual        = castObjectT<GP::Individual&>(ioIndividual);
	GP::Context& lContext              = castObjectT<GP::Context&>(ioContext);
	unsigned int lMaxTreeDepth         = mMaxTreeDepth->getWrappedValue();
	unsigned int lMaxRegenerationDepth = mMaxRegenerationDepth->getWrappedValue();
	
	//Select node to mutate
	unsigned int lChoosenTree = 0;
	unsigned int lChoosenNode = 0;
	
	bool lDoParameterSearch = (lContext.getSystem().getRandomizer().rollUniform(0.0, 1.0) <= mMutParameterPb->getWrappedValue());
	
	//Select primitive based on type
	for(unsigned int lTry = 0; lTry < 2; ++lTry) { //Do only twice
		std::vector<const std::type_info*> lDesiredTypes(1, ArgEph);
		RouletteT< std::pair<unsigned int,unsigned int> > lRoulette;
		SelectiveConstrainedSelectionOp::buildRoulette(lRoulette, lDesiredTypes, lIndividual, lContext,!lDoParameterSearch,lDoParameterSearch);
		
		if(lRoulette.size() == 0) {
			if(lDoParameterSearch) {
				Beagle_LogVerboseM(
								   ioContext.getSystem().getLogger(),
								   "mutation", "Beagle::GP::MutationStandardSelectiveConstrainedOp",
								   string("No EphemeralDouble node found.")
								   );
			}
			else {
				Beagle_LogVerboseM(
								   ioContext.getSystem().getLogger(),
								   "mutation", "Beagle::GP::MutationStandardSelectiveConstrainedOp",
								   string("No non EphemeralDouble node found.")
								   );
			}
			lDoParameterSearch = !lDoParameterSearch;
			if(lTry >= 1) {
				Beagle_LogVerboseM(
								   ioContext.getSystem().getLogger(),
								   "mutation", "Beagle::GP::MutationStandardSelectiveConstrainedOp",
								   "Unable to GP standard mutate the individual"
								   );
				return false;
			}
		}
		else { 
			std::pair<unsigned int,unsigned int> lSelectedNode = lRoulette.select(ioContext.getSystem().getRandomizer());
			lChoosenTree = lSelectedNode.first;
			lChoosenNode = lSelectedNode.second;
			break;
		}
	}
	
	if(lDoParameterSearch) {
		Beagle_LogVerboseM(
						   ioContext.getSystem().getLogger(),
						   "mutation", "Beagle::GP::MutationStandardSelectiveConstrainedOp",
						   string("Mutation applied only on EphemeralDouble node.")
						   );
	}
	else {
		Beagle_LogVerboseM(
						   ioContext.getSystem().getLogger(),
						   "mutation", "Beagle::GP::MutationStandardSelectiveConstrainedOp",
						   string("Mutation applied on node that are not EphemeralDouble.")
						   );
	}
	
	
	unsigned int lOldGenotypeIndex = lContext.getGenotypeIndex();
	GP::Tree::Handle lOldGenotypeHandle = lContext.getGenotypeHandle();
	
	Beagle_LogDebugM(
					 ioContext.getSystem().getLogger(),
					 "mutation", "Beagle::GP::MutationStandardSelectiveConstrainedOp",
					 std::string("Individual before GP standard mutation: ")+
					 ioIndividual.serialize()
					 );
	
	GP::Tree::Handle lActualTree = lIndividual[lChoosenTree];
	GP::Tree::Handle lNewTree    = castHandleT<GP::Tree>(lIndividual.getTypeAlloc()->allocate());
	lNewTree->setPrimitiveSetIndex(lActualTree->getPrimitiveSetIndex());
	lNewTree->setNumberArguments(lActualTree->getNumberArguments());
	unsigned int lChoosenNodeSubTreeSize = (*lActualTree)[lChoosenNode].mSubTreeSize;
	lNewTree->insert(lNewTree->end(), lActualTree->begin(), lActualTree->begin()+lChoosenNode);
	lContext.setGenotypeIndex(lChoosenTree);
	lContext.setGenotypeHandle(lActualTree);
	lContext.emptyCallStack();
	lActualTree->setContextToNode(lChoosenNode, lContext);
	lContext.popCallStack();
	const unsigned int lMaxSubTreeDepth =
    minOf<unsigned int>(lMaxTreeDepth - lContext.getCallStackSize(), lMaxRegenerationDepth);
	lIndividual[lChoosenTree] = lNewTree;
	lContext.setGenotypeHandle(lNewTree);
	
	unsigned int lAttempt=0;
	if(lMaxSubTreeDepth >= 1) {
		for(; lAttempt < mNumberAttempts->getWrappedValue(); lAttempt++) {
			if(mInitOp->initTree(*lNewTree, 1, lMaxSubTreeDepth, lContext) != 0) break;
		}
	} else {
		lAttempt = mNumberAttempts->getWrappedValue();
	}
	
	if(lAttempt == mNumberAttempts->getWrappedValue()) {
		lIndividual[lChoosenTree] = lActualTree;
		lContext.setGenotypeIndex(lOldGenotypeIndex);
		lContext.setGenotypeHandle(lOldGenotypeHandle);
		Beagle_LogVerboseM(
						   ioContext.getSystem().getLogger(),
						   "mutation", "Beagle::GP::MutationStandardSelectiveConstrainedOp",
						   "Unable to GP standard mutate the individual"
						   );
		return false;
	}
	
	if( !lDoParameterSearch ) {
		//Set structure id invalid
		castHandleT<TreeSTag>((lIndividual)[0])->setStructureIDInvalid();
		Beagle_LogVerboseM(
						   ioContext.getSystem().getLogger(),
						   "mutation", "Beagle::GP::MutationStandardSelectiveConstrainedOp",
						   std::string("Set structure id invalid") );
	}
	
	Beagle_LogVerboseM(
					   ioContext.getSystem().getLogger(),
					   "mutation", "Beagle::GP::MutationStandardSelectiveConstrainedOp",
					   std::string("GP standard mutate the ")+uint2ordinal(lChoosenNode+1)+
					   std::string(" node, ")+ (*lActualTree)[lChoosenNode].mPrimitive->getName()
					   +std::string(", of the ")+uint2ordinal(lChoosenTree+1)+
					   std::string(" tree with max depth ")+uint2str(lMaxSubTreeDepth)
					   );
	
	lNewTree->insert(lNewTree->end(),
					 lActualTree->begin()+lChoosenNode+lChoosenNodeSubTreeSize,
					 lActualTree->end());
	unsigned int lDiffSize =
    (*lActualTree)[lChoosenNode].mSubTreeSize - (*lNewTree)[lChoosenNode].mSubTreeSize;
	for(unsigned int l=0; l<lContext.getCallStackSize(); l++) {
		(*lNewTree)[lContext.getCallStackElement(l)].mSubTreeSize -= lDiffSize;
	}
	
	lContext.setGenotypeIndex(lOldGenotypeIndex);
	lContext.setGenotypeHandle(lOldGenotypeHandle);
	
	Beagle_LogDebugM(
					 ioContext.getSystem().getLogger(),
					 "mutation", "Beagle::GP::MutationStandardSelectiveConstrainedOp",
					 std::string("Individual after GP standard mutation: ")+
					 ioIndividual.serialize()
					 );
	
	return true;
	Beagle_StackTraceEndM("bool GP::MutationStandardSelectiveConstrainedOp::mutate(Beagle::Individual& ioIndividual, Beagle::Context& ioContext)");
}
/*!
 *  \brief Swap mutate a constrained GP individual.
 *  \param ioIndividual GP individual to swap mutate.
 *  \param ioContext Context of the evolution.
 *  \return True if the individual is effectively mutated, false if not.
 */
bool GP::MutationSwapDepthSelectiveConstrainedOp::mutate(Beagle::Individual& ioIndividual, Beagle::Context& ioContext)
{
	Beagle_StackTraceBeginM();
	GP::Individual& lIndividual  = castObjectT<GP::Individual&>(ioIndividual);
	GP::Context& lContext        = castObjectT<GP::Context&>(ioContext);
	double lDistrProba           = mDistributionProba->getWrappedValue();
	unsigned int lNumberAttempts = mNumberAttempts->getWrappedValue();
	bool lMutationDone           = false;
	
	//Select node to mutate
	unsigned int lChoosenTree = 0;
	unsigned int lChoosenNode = 0;
	
	bool lDoParameterSearch = (lContext.getSystem().getRandomizer().rollUniform(0.0, 1.0) <= mMutParameterPb->getWrappedValue());
	
	RouletteT< std::pair<unsigned int,unsigned int> > lRoulette;
	//Select primitive based on type
	for(unsigned int lTry = 0; lTry < 2; ++lTry) { //Do only twice
		std::vector<const std::type_info*> lDesiredTypes(1, ArgEph);
		DepthDependentSelectionOp::buildRoulette(lRoulette, lDesiredTypes, lIndividual, lContext,!lDoParameterSearch,lDoParameterSearch);
		
		if(lRoulette.size() == 0) {
			if(lDoParameterSearch) {
				Beagle_LogVerboseM(
								   ioContext.getSystem().getLogger(),
								   "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
								   string("No EphemeralDouble node found.")
								   );
			}
			else {
				Beagle_LogVerboseM(
								   ioContext.getSystem().getLogger(),
								   "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
								   string("No non EphemeralDouble node found.")
								   );
			}
			lDoParameterSearch = !lDoParameterSearch;
			if(lTry >= 1) {
				Beagle_LogVerboseM(
								   ioContext.getSystem().getLogger(),
								   "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
								   "Unable to GP standard mutate the individual"
								   );
				return false;
			}
		}
		else { 
			std::pair<unsigned int,unsigned int> lSelectedNode = lRoulette.select(ioContext.getSystem().getRandomizer());
			lChoosenTree = lSelectedNode.first;
			lChoosenNode = lSelectedNode.second;
			break;
		}
	}
	
	if(lDoParameterSearch) {
		Beagle_LogVerboseM(
						   ioContext.getSystem().getLogger(),
						   "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
						   string("Mutation applied only on EphemeralDouble node.")
						   );
	}
	else {
		Beagle_LogVerboseM(
						   ioContext.getSystem().getLogger(),
						   "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
						   string("Mutation applied on node that are not EphemeralDouble.")
						   );
	}
	
	GP::Tree& lTree = *lIndividual[lChoosenTree];
	if(lTree.size() == 0) return false;
	
	GP::Tree::Handle lOldTreeHandle = lContext.getGenotypeHandle();
	unsigned int lOldTreeIndex = lContext.getGenotypeIndex();
	lContext.setGenotypeHandle(lIndividual[lChoosenTree]);
	lContext.setGenotypeIndex(lChoosenTree);
	
	Beagle_LogDebugM(
					 ioContext.getSystem().getLogger(),
					 "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
					 std::string("Individual before constrained GP tree swap mutation: ")+ioIndividual.serialize()
					 );
	
	if(lTree.size() > 1) {
		bool lTypeNode = (lContext.getSystem().getRandomizer().rollUniform(0., 1.) < lDistrProba);
		//Clean the roulette to include only the choosen tree
		for(std::vector< std::pair<double, std::pair<unsigned int,unsigned int> > >::iterator lRouletteIter = lRoulette.begin(); lRouletteIter!=lRoulette.end();){
			if(lRouletteIter->second.first != lChoosenTree || (lTree[lRouletteIter->second.second].mPrimitive->getNumberArguments() != 0) != lTypeNode ) {
				lRouletteIter = lRoulette.erase(lRouletteIter++);
			} else {
				++lRouletteIter;
			}
		}
		if(lRoulette.size() > 0) 
			lChoosenNode = lRoulette.select(ioContext.getSystem().getRandomizer()).second;
	}
	
	if( !lDoParameterSearch || lRoulette.size() > 0 ) {
	
		Primitive::Handle lOriginalPrimitive = lTree[lChoosenNode].mPrimitive;
		
		Beagle_LogVerboseM(
						   ioContext.getSystem().getLogger(),
						   "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
						   std::string("Trying to constrained GP tree swap mutate the ")+uint2ordinal(lChoosenNode+1)+
						   std::string(" node (primitive: \"")+lOriginalPrimitive->getName()+
						   std::string("\" nb args: ")+uint2str(lOriginalPrimitive->getNumberArguments())+
						   std::string(") of the ")+uint2ordinal(lChoosenTree+1)+std::string(" tree")
						   );
		
		GP::PrimitiveSet& lPrimitiveSet = lTree.getPrimitiveSet(lContext);
		unsigned int lNbArgsPrimit = lTree[lChoosenNode].mPrimitive->getNumberArguments();
		lTree.setContextToNode(lChoosenNode, lContext);
		for(unsigned int lAttempt=0; lAttempt < lNumberAttempts; ++lAttempt) {
			Primitive::Handle lChoosenPrimitive = lPrimitiveSet.select(lNbArgsPrimit, lContext);
			if(lChoosenPrimitive==NULL) break;
			
			lTree[lChoosenNode].mPrimitive = lChoosenPrimitive->giveReference(lNbArgsPrimit, lContext);
			
			Beagle_LogVerboseM(
							   ioContext.getSystem().getLogger(),
							   "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
							   std::string("Trying the primitive \"")+lChoosenPrimitive->getName()+
							   std::string("\"")
							   );
			
			if(lTree.validateSubTree(lChoosenNode, lContext)) {
				lMutationDone = true;
				Beagle_LogVerboseM(
								   ioContext.getSystem().getLogger(),
								   "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
								   "Constrained GP tree swap mutation valid"
								   );
				break;
			}
			else {
				lTree[lChoosenNode].mPrimitive = lOriginalPrimitive;
				Beagle_LogVerboseM(
								   ioContext.getSystem().getLogger(),
								   "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
								   "Constrained GP tree swap mutation invalid"
								   );
			}
		}
	}
	
	lContext.setGenotypeHandle(lOldTreeHandle);
	lContext.setGenotypeIndex(lOldTreeIndex);
	
	if(lMutationDone) {
		if( !lDoParameterSearch ) {
			//Set structure id invalid
			castHandleT<TreeSTag>((lIndividual)[0])->setStructureIDInvalid();
			Beagle_LogVerboseM(
							   ioContext.getSystem().getLogger(),
							   "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
							   std::string("Set structure id invalid") );
		}
		
		Beagle_LogDebugM(
						 ioContext.getSystem().getLogger(),
						 "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
						 std::string("Individual after constrained GP swap mutation: ")+
						 ioIndividual.serialize()
						 );
	}
	else {
		Beagle_LogVerboseM(
						   ioContext.getSystem().getLogger(),
						   "mutation", "Beagle::GP::MutationSwapDepthSelectiveConstrainedOp",
						   "Unable to swap mutate the constrained individual"
						   );
	}
	
	return lMutationDone;
	Beagle_StackTraceEndM("bool GP::MutationSwapDepthSelectiveConstrainedOp::mutate(Beagle::Individual& ioIndividual, Beagle::Context& ioContext)");
}