/*!
 *  \brief Expand given module of a GP tree.
 *  \param inNodeToExpand Index of node to expand in GP tree.
 *  \param ioTree Tree from which module will be expanded.
 *  \param ioContext Evolutionary context.
 */
void GP::ModuleExpandOp::expand(unsigned int inNodeToExpand,
                                GP::Tree& ioTree,
                                GP::Context& ioContext)
{
	Beagle_StackTraceBeginM();
	// Log tree before expansion.
	Beagle_LogDebugM(
	    ioContext.getSystem().getLogger(),
	    "Tree before expansion"
	);
	Beagle_LogDebugM(
	    ioContext.getSystem().getLogger(),
	    ioTree
	);

	// Get the module index and reference.
	Module::Handle lModuleInstance = castHandleT<Module>(ioTree[inNodeToExpand].mPrimitive);
	unsigned int lModuleIndex = lModuleInstance->getIndex();
	Beagle_LogVerboseM(
	    ioContext.getSystem().getLogger(),
	    std::string("Expanding ")+uint2ordinal(lModuleIndex+1)+
	    std::string(" module (called from ")+uint2ordinal(inNodeToExpand+1)+
	    std::string(" node of the tree)")
	);
	ModuleVectorComponent::Handle lModuleVectorComponent =
	    castHandleT<ModuleVectorComponent>(ioContext.getSystem().getComponent("ModuleVector"));
	if(lModuleVectorComponent==NULL) {
		throw Beagle_RunTimeExceptionM(std::string("GP system is not configured with a module vector. ")+
		                               std::string("Consider adding a GP::ModuleVectorComponent object to the system."));
	}
	Beagle::GP::Tree::Handle lModule = (*lModuleVectorComponent)[lModuleIndex];
	Beagle_LogDebugM(
	    ioContext.getSystem().getLogger(),
	    *lModule
	);

	// Generate new tree.
	const Factory& lFactory = ioContext.getSystem().getFactory();
	GP::Tree::Alloc::Handle lTreeAlloc =
		castHandleT<GP::Tree::Alloc>(lFactory.getConceptAllocator("Genotype"));
	GP::Tree::Handle lNewTree = castHandleT<GP::Tree>(lTreeAlloc->allocate());
	std::string lArgName = lModuleInstance->getArgsName();
	ioTree.setContextToNode(inNodeToExpand, ioContext);
	for(unsigned int i=0; i<lModule->size(); ++i) {
		if((*lModule)[i].mPrimitive->getName() != lArgName) {
			lNewTree->push_back(GP::Node((*lModule)[i].mPrimitive));
		} else {
			GP::Argument::Handle lArg = castHandleT<GP::Argument>((*lModule)[i].mPrimitive);
			const unsigned int lChildIndex =
			    ioTree[inNodeToExpand].mPrimitive->getChildrenNodeIndex(lArg->getIndex(), ioContext);
			lNewTree->insert(lNewTree->end(), ioTree.begin()+lChildIndex,
			                 ioTree.begin()+lChildIndex+ioTree[lChildIndex].mSubTreeSize);
		}
	}
	ioTree.erase(ioTree.begin()+inNodeToExpand,
	             ioTree.begin()+inNodeToExpand+ioTree[inNodeToExpand].mSubTreeSize);
	ioTree.insert(ioTree.begin()+inNodeToExpand, lNewTree->begin(), lNewTree->end());
	ioTree.fixSubTreeSize();

	// Log results.
	Beagle_LogDebugM(
	    ioContext.getSystem().getLogger(),
	    "Tree after expansion"
	);
	Beagle_LogDebugM(
	    ioContext.getSystem().getLogger(),
	    ioTree
	);
	
	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 STGP::MutationStandardConstrainedOp::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();

	unsigned int lNbNodes = 0;
	for(unsigned int i=0; i<lIndividual.size(); i++) lNbNodes += lIndividual[i]->size();
	if(lNbNodes == 0) return false;
	unsigned int lChoosenNode = lContext.getSystem().getRandomizer().rollInteger(0, lNbNodes-1);
	unsigned int lChoosenTree = 0;
	for(; (lChoosenTree+1)<lIndividual.size(); lChoosenTree++) {
		if(lChoosenNode < lIndividual[lChoosenTree]->size()) break;
		else lChoosenNode -= lIndividual[lChoosenTree]->size();
	}

	const Factory& lFactory = ioContext.getSystem().getFactory();
	GP::Tree::Alloc::Handle lTreeAlloc =
		castHandleT<GP::Tree::Alloc>(lFactory.getConceptAllocator("Genotype"));

	unsigned int lOldGenotypeIndex = lContext.getGenotypeIndex();
	GP::Tree::Handle lOldGenotypeHandle = lContext.getGenotypeHandle();

	Beagle_LogDebugM(
	    ioContext.getSystem().getLogger(),
	    "Individual before GP standard mutation"
	);
	Beagle_LogDebugM(
	    ioContext.getSystem().getLogger(),
	    ioIndividual
	);

	GP::Tree::Handle lActualTree = lIndividual[lChoosenTree];
	GP::Tree::Handle lNewTree    = castHandleT<GP::Tree>(lTreeAlloc->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;
	for(; lAttempt < mNumberAttempts->getWrappedValue(); lAttempt++) {
		if(mInitOp->initTree(*lNewTree, 1, lMaxSubTreeDepth, lContext) != 0) break;
	}
	if(lAttempt == mNumberAttempts->getWrappedValue()) {
		lIndividual[lChoosenTree] = lActualTree;
		lContext.setGenotypeIndex(lOldGenotypeIndex);
		lContext.setGenotypeHandle(lOldGenotypeHandle);
		Beagle_LogVerboseM(
		    ioContext.getSystem().getLogger(),
		    "Unable to GP standard mutate the individual"
		);
		return false;
	}

	Beagle_LogVerboseM(
	    ioContext.getSystem().getLogger(),
	    std::string("GP standard mutate the ")+uint2ordinal(lChoosenNode+1)+
	    std::string(" node 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(),
	    "Individual after GP standard mutation"
	);
	Beagle_LogDebugM(
	    ioContext.getSystem().getLogger(),
	    ioIndividual
	);

	return true;
	Beagle_StackTraceEndM();
}
/*!
 *  \brief Swap subtree mutate a GP individual.
 *  \param ioIndividual GP individual to swap subtree mutate.
 *  \param ioContext Context of the evolution.
 */
bool GP::MutationSwapSubtreeOp::mutate(Beagle::Individual& ioIndividual, Beagle::Context& ioContext)
{
	Beagle_StackTraceBeginM();
	// Initial parameters checks.
	Beagle_AssertM(ioIndividual.size() > 0);
	Beagle_ValidateParameterM(mNumberAttempts->getWrappedValue()>0, "gp.try", ">0");

	// Cast method arguments.
	GP::Individual&     lGPIndiv      = castObjectT<GP::Individual&>(ioIndividual);
	GP::Context&        lContext1     = castObjectT<GP::Context&>(ioContext);
	Context::Alloc::Handle lContextAlloc =
	    castHandleT<Context::Alloc>(ioContext.getSystem().getFactory().getConceptAllocator("Context"));
	GP::Context::Handle lContextHdl2  = castHandleT<GP::Context>(lContextAlloc->clone(lContext1));

	const Factory& lFactory = ioContext.getSystem().getFactory();
	GP::Tree::Alloc::Handle lTreeAlloc =
		castHandleT<GP::Tree::Alloc>(lFactory.getConceptAllocator("Genotype"));

	// Get parameters in local values, with the total number of nodes of the mutated individual.
	bool             lMatingDone     = false;
	float            lDistrProba     = mDistributionProba->getWrappedValue();
	unsigned int     lMaxTreeDepth   = mMaxTreeDepth->getWrappedValue();
	GP::Tree::Handle lOldTreeHandle1 = lContext1.getGenotypeHandle();
	unsigned int     lOldTreeIndex1  = lContext1.getGenotypeIndex();
	unsigned int     lSizeIndiv      = 0;
	for(unsigned int i=0; i<lGPIndiv.size(); ++i) lSizeIndiv += lGPIndiv[i]->size();

	// Some outputs.
	Beagle_LogDebugM(
	    lContext1.getSystem().getLogger(),
	    "mutation",
	    "Beagle::GP::MutationSwapSubtreeOp",
	    "Individual tried for swap subtree mutation (before)"
	);
	Beagle_LogObjectDebugM(
	    lContext1.getSystem().getLogger(),
	    "mutation",
	    "Beagle::GP::MutationSwapSubtreeOp",
	    lGPIndiv
	);

	// Mutation loop. Try the given number of attempts to mutation the individual.
	for(unsigned int lAttempt=0; lAttempt < mNumberAttempts->getWrappedValue(); lAttempt++) {

		// Calculate the number of nodes in the individual
		unsigned int lNbNodes = 0;
		for(unsigned int i=0; i<lGPIndiv.size(); i++) lNbNodes += lGPIndiv[i]->size();
		if(lNbNodes == 0) return false;

		// Choose a node of the individual to mutate.
		unsigned int lNode1 = lContext1.getSystem().getRandomizer().rollInteger(0, lNbNodes-1);

		// Get the tree in which the choosen node is. Change the global node index to the tree's index.
		unsigned int lChoosenTree = 0;
		for(; lChoosenTree<lGPIndiv.size(); ++lChoosenTree) {
			if(lNode1 < lGPIndiv[lChoosenTree]->size()) break;
			Beagle_AssertM(lNode1 >= lGPIndiv[lChoosenTree]->size());
			lNode1 -= lGPIndiv[lChoosenTree]->size();
		}
		Beagle_AssertM(lChoosenTree < lGPIndiv.size());

		// Cannot do anything with an tree of size <= 1.
		if(lGPIndiv[lChoosenTree]->size() <= 1) continue;

		// Some outputs.
		Beagle_LogVerboseM(
		    lContext1.getSystem().getLogger(),
		    "mutation", "Beagle::GP::MutationSwapSubtreeOp",
		    std::string("Trying a swap subtree mutation of the ")+uint2ordinal(lChoosenTree+1)+
		    std::string(" tree")
		);

		// Make two clones of the choosen tree.
		GP::Tree::Handle lTreeClone1 = castHandleT<GP::Tree>(lTreeAlloc->clone(*lGPIndiv[lChoosenTree]));
		GP::Tree::Handle lTreeClone2 = castHandleT<GP::Tree>(lTreeAlloc->clone(*lGPIndiv[lChoosenTree]));

		// Now we decide whether the swap subtree mutation is internal or external.
		bool lMutationType = lContext1.getSystem().getRandomizer().rollUniform(0.0, 1.0) < lDistrProba;

		// Cannot do an internal mutation when there is only one branch in the tree.
		if(lTreeClone1->size() == (*lTreeClone1)[0].mPrimitive->getNumberArguments()+1)
			lMutationType = false;

		// This is special case, a linear tree. Cannot do an external mutation.
		if(lTreeClone1->size() == (*lTreeClone1)[1].mSubTreeSize+1) {
			if(lTreeClone1->size()==2) continue;   // Cannot do anything here with the tree.
			lMutationType = true;
		}

		// lMutationType is true -> internal mutation
		if(lMutationType)  {

			// If the selected node is a terminal, or a branch with a subtree made only of terminals,
			// choose another node in the same tree.
			while((*lTreeClone1)[lNode1].mSubTreeSize ==
			        ((*lTreeClone1)[lNode1].mPrimitive->getNumberArguments()+1)) {
				lNode1 = lContext1.getSystem().getRandomizer().rollInteger(0, lTreeClone1->size()-1);
			}

			// Choosing the second node, a branch in lNode1's subtree.
			unsigned int lSubTreeSizeN1 = (*lTreeClone1)[lNode1].mSubTreeSize;
			unsigned int lN2OffN1 =
			    lContext1.getSystem().getRandomizer().rollInteger(1, lSubTreeSizeN1-1);
			unsigned int lNode2 = lNode1 + lN2OffN1;
			while((*lTreeClone1)[lNode2].mPrimitive->getNumberArguments() == 0) {
				lN2OffN1 = lContext1.getSystem().getRandomizer().rollInteger(1, lSubTreeSizeN1-1);
				lNode2 = lNode1 + lN2OffN1;
			}

			// Choosing the third node, any node in lNode2's subtree.
			unsigned int lSubTreeSizeN2 = (*lTreeClone1)[lNode2].mSubTreeSize;
			unsigned int lN3OffN2 =
			    lContext1.getSystem().getRandomizer().rollInteger(1, lSubTreeSizeN2-1);

			// Ok, now we can exchange the subtrees.

			// New value of lNode1 and lNode2 for the second exchange.
			unsigned int lNode3Exch2 = lNode1 + lN3OffN2;
			unsigned int lNode1Exch2 = lNode2;

			// New value of lNode1 and lNode2 for the third exchange.
			unsigned int lNode2Exch3 = lNode1 + lN3OffN2 + lN2OffN1;
			unsigned int lNode3Exch3 = lNode2;

			// First exchange.
			lTreeClone1->setContextToNode(lNode1, lContext1);
			lTreeClone2->setContextToNode(lNode2, *lContextHdl2);
			exchangeSubTrees(*lTreeClone1, lNode1, lContext1,
			                 *lTreeClone2, lNode2, *lContextHdl2);

			// Second exchange.
			lTreeClone1->setContextToNode(lNode3Exch2, lContext1);
			lTreeClone2->setContextToNode(lNode1Exch2, *lContextHdl2);
			exchangeSubTrees(*lTreeClone1, lNode3Exch2, lContext1,
			                 *lTreeClone2, lNode1Exch2, *lContextHdl2);

			// Third exchange.
			lTreeClone1->setContextToNode(lNode2Exch3, lContext1);
			lTreeClone2->setContextToNode(lNode3Exch3, *lContextHdl2);
			exchangeSubTrees(*lTreeClone1, lNode2Exch3, lContext1,
			                 *lTreeClone2, lNode3Exch3, *lContextHdl2);

			// Checking if the tree depth is respected. If not, start again.
			if(lTreeClone1->getTreeDepth() > lMaxTreeDepth) {
				Beagle_LogVerboseM(
				    lContext1.getSystem().getLogger(),
				    "mutation", "Beagle::GP::MutationSwapSubtreeOp",
				    "Tree maximum depth exceeded. GP swap subtree mutation invalid."
				);
				continue;
			}

			lGPIndiv[lChoosenTree] = lTreeClone1;
			Beagle_LogVerboseM(
			    lContext1.getSystem().getLogger(),
			    "mutation", "Beagle::GP::MutationSwapSubtreeOp",
			    "GP swap subtree mutation valid"
			);
			lMatingDone = true;
			break;   // The swap subtree mutation is valid.
		}

		// lMutationType is false -> external mutation
		else {

			// Deterniming the minimal node index to use.
			unsigned int lMinNodeIndex = 0;
			for(; lTreeClone1->size() == ((*lTreeClone1)[lMinNodeIndex].mSubTreeSize+lMinNodeIndex);
			        ++lMinNodeIndex) {
				if(lMinNodeIndex == (lTreeClone1->size()-1)) continue; // Can't do anything with linear tree.
			}

			// Change lNode1 if less than minimum node index.
			if(lNode1 < lMinNodeIndex)
				lNode1 = lContext1.getSystem().getRandomizer().rollInteger(lMinNodeIndex,
				         lTreeClone1->size()-1);

			// Choosing second swap subtree mutation point.
			std::vector<unsigned int> lValidN2;
			for(unsigned int i=lMinNodeIndex; i<lTreeClone1->size(); ++i) {
				if((i>=lNode1) && (i<lNode1+(*lTreeClone1)[lNode1].mSubTreeSize)) continue;
				else if((lNode1>=i) && (lNode1<(i+(*lTreeClone1)[i].mSubTreeSize))) continue;
				else lValidN2.push_back(i);
			}
			unsigned int lNode2 =
			    lValidN2[lContext1.getSystem().getRandomizer().rollInteger(0, lValidN2.size()-1)];

			Beagle_LogVerboseM(
			    lContext1.getSystem().getLogger(),
			    "mutation", "Beagle::GP::MutationSwapSubtreeOp",
			    std::string("Trying an external swap subtree mutation of the ")+uint2ordinal(lNode1+1)+
			    std::string(" node with the subtree to the ")+uint2ordinal(lNode2+1)+
			    std::string(" node")
			);

			// Ok, now we can exchange the subtrees.

			// New value of lNode1 and lNode2 for the second exchange.
			unsigned int lNode1Exch2 = lNode1;
			unsigned int lNode2Exch2 = lNode2;
			if(lNode1 < lNode2) {
				lNode2Exch2 += (*lTreeClone1)[lNode2].mSubTreeSize;
				lNode2Exch2 -= (*lTreeClone1)[lNode1].mSubTreeSize;
			} else {
				lNode1Exch2 += (*lTreeClone1)[lNode1].mSubTreeSize;
				lNode1Exch2 -= (*lTreeClone1)[lNode2].mSubTreeSize;
			}

			// First exchange.
			lTreeClone1->setContextToNode(lNode1, lContext1);
			lTreeClone2->setContextToNode(lNode2, *lContextHdl2);
			exchangeSubTrees(*lTreeClone1, lNode1, lContext1,
			                 *lTreeClone2, lNode2, *lContextHdl2);

			// Second exchange.
			lTreeClone1->setContextToNode(lNode2Exch2, lContext1);
			lTreeClone2->setContextToNode(lNode1Exch2, *lContextHdl2);
			exchangeSubTrees(*lTreeClone1, lNode2Exch2, lContext1,
			                 *lTreeClone2, lNode1Exch2, *lContextHdl2);

			// Checking if the tree depth is respected. If not, start again.
			if(lTreeClone1->getTreeDepth() > lMaxTreeDepth) {
				Beagle_LogVerboseM(
				    lContext1.getSystem().getLogger(),
				    "mutation", "Beagle::GP::MutationSwapSubtreeOp",
				    "Tree maximum depth exceeded. GP swap subtree mutation invalid."
				);
				continue;
			}

			lGPIndiv[lChoosenTree] = lTreeClone1;
			Beagle_LogVerboseM(
			    lContext1.getSystem().getLogger(),
			    "mutation", "Beagle::GP::MutationSwapSubtreeOp",
			    "GP swap subtree mutation valid"
			);
			lMatingDone = true;
			break;   // The swap subtree mutation is valid.
		}
	}

	// Replace the contexts.
	lContext1.setGenotypeHandle(lOldTreeHandle1);
	lContext1.setGenotypeIndex(lOldTreeIndex1);

	if(lMatingDone) {
		Beagle_LogDebugM(
		    lContext1.getSystem().getLogger(),
		    "mutation",
		    "Beagle::GP::MutationSwapSubtreeOp",
		    "Individual after swap subtree mutation"
		);
		Beagle_LogObjectDebugM(
		    lContext1.getSystem().getLogger(),
		    "mutation",
		    "Beagle::GP::MutationSwapSubtreeOp",
		    lGPIndiv
		);
	} else {
		Beagle_LogVerboseM(
		    lContext1.getSystem().getLogger(),
		    "mutation", "Beagle::GP::MutationSwapSubtreeOp",
		    "No GP swap subtree mutation done"
		);
	}

	return lMatingDone;
	Beagle_StackTraceEndM();
}
/*!
 *  \brief Insert mutate a GP individual.
 *  \param ioIndividual GP individual to mutate.
 *  \param ioContext Context of the evolution.
 *  \return True if the individual is effectively mutated, false if not.
 */
bool GP::MutationInsertConstrainedOp::mutate(Beagle::Individual& ioIndividual, Beagle::Context& ioContext)
{
	Beagle_StackTraceBeginM();

	Beagle_LogDetailedM(
	    ioContext.getSystem().getLogger(),
	    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
	    std::string("Mutating individual with GP::MutationInsertConstrainedOp")
	);

	GP::Individual& lIndividual = castObjectT<GP::Individual&>(ioIndividual);
	GP::Context& lContext = castObjectT<GP::Context&>(ioContext);
	const unsigned int lMaxAttempts = mNumberAttempts->getWrappedValue();
	const unsigned int lMaxTreeDepth = mMaxTreeDepth->getWrappedValue();

	const Factory& lFactory = ioContext.getSystem().getFactory();
	GP::Tree::Alloc::Handle lTreeAlloc =
		castHandleT<GP::Tree::Alloc>(lFactory.getConceptAllocator("Genotype"));

	// Store original context values
	const unsigned int lOldGenotypeIndex = lContext.getGenotypeIndex();
	const GP::Tree::Handle lOldGenotypeHandle = lContext.getGenotypeHandle();

	Beagle_LogDebugM(
	    ioContext.getSystem().getLogger(),
	    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
	    "Individual before constrained GP insert mutation"
	);
	Beagle_LogObjectDebugM(
	    ioContext.getSystem().getLogger(),
	    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
	    ioIndividual
	);

	// Mutation attempts loop
	for(unsigned int i=0; i<lMaxAttempts; ++i) {

		// Choose tree and node to mutate
		const unsigned int lChosenTree = lIndividual.chooseRandomTree(lContext);
		const unsigned int lChosenNodeIndex = lIndividual.chooseRandomNode(lChosenTree, lContext);
		GP::Tree::Handle lOriginalTree = lIndividual[lChosenTree];

		// Compute depth of tree generated by mutation
		lContext.setGenotypeIndex(lChosenTree);
		lContext.setGenotypeHandle(lOriginalTree);
		lContext.emptyCallStack();
		lOriginalTree->setContextToNode(lChosenNodeIndex, lContext);
		const unsigned int lMutationDepth =
		    lContext.getCallStackSize() + lOriginalTree->getTreeDepth(lChosenNodeIndex);

		// Check that mutation will not generate a tree deeper than the maximum allowed depth
		if(lMutationDepth > lMaxTreeDepth) {
			Beagle_LogDebugM(
			    ioContext.getSystem().getLogger(),
			    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
			    std::string("Constrained insert mutation attempt failed as the generated tree will exceed ")+
			    std::string("maximum allowed tree depth")
			);
			continue;
		}

		// Create new tree
		Beagle_LogDebugM(
		    ioContext.getSystem().getLogger(),
		    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
		    std::string("Creating new tree")
		);
		GP::Tree::Handle lNewTree = castHandleT<GP::Tree>(lTreeAlloc->allocate());
		lNewTree->setPrimitiveSetIndex(lOriginalTree->getPrimitiveSetIndex());
		lNewTree->setNumberArguments(lOriginalTree->getNumberArguments());

		// Replace original tree with new tree
		lIndividual[lChosenTree] = lNewTree;
		lContext.setGenotypeHandle(lNewTree);

		// Copy unchanged part of original tree into new tree
		Beagle_LogDebugM(
		    ioContext.getSystem().getLogger(),
		    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
		    std::string("Copying unchanged part of original tree to new tree")
		);
		lNewTree->insert(lNewTree->end(),
		                 lOriginalTree->begin(),
		                 lOriginalTree->begin()+lChosenNodeIndex);
		Beagle_AssertM(lNewTree->size() == lChosenNodeIndex);

		// Generate new branch primitive to insert.
		GP::PrimitiveSet& lPrimitiveSet = lNewTree->getPrimitiveSet(lContext);
		Primitive::Handle lBranchInserted = lPrimitiveSet.select(GP::Primitive::eBranch, lContext);
		if(lBranchInserted==NULL) {
			Beagle_LogDebugM(
			    ioContext.getSystem().getLogger(),
			    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
			    std::string("Constrained insert mutation attempt failed as it seems impossible ")+
			    std::string("to select a branch primitive in the actual context")
			);
			lIndividual[lChosenTree] = lOriginalTree;
			lContext.setGenotypeHandle(lOriginalTree);
			continue;
		}
		Beagle_LogDebugM(
		    ioContext.getSystem().getLogger(),
		    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
		    std::string("Branch primitive to be inserted by mutation is primitive '")+
		    lBranchInserted->getName()+std::string("'")
		);

		// Insert new branch
		lBranchInserted = lBranchInserted->giveReference(GP::Primitive::eBranch, lContext);
		lNewTree->push_back(Node(lBranchInserted,1));
		if(lBranchInserted->validate(lContext) == false) {
			Beagle_LogDebugM(
			    ioContext.getSystem().getLogger(),
			    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
			    std::string("Constrained insert mutation attempt failed as the selected ")+
			    std::string("branch doesn't match the constraints")
			);
			lIndividual[lChosenTree] = lOriginalTree;
			lContext.setGenotypeHandle(lOriginalTree);
			continue;
		}
		const unsigned int lNbArgsInsertedBranch = lBranchInserted->getNumberArguments();
		Beagle_AssertM(lNbArgsInsertedBranch != 0);
		const unsigned int lSubtreeArgIndex =
		    lContext.getSystem().getRandomizer().rollInteger(0, lNbArgsInsertedBranch-1);
		Beagle_AssertM(lSubtreeArgIndex < lNbArgsInsertedBranch);
		const unsigned int lSubtreeSize = (*lOriginalTree)[lChosenNodeIndex].mSubTreeSize;

		// Generate inserted node subtrees
		bool lArgsGenFailed = false;
		for(unsigned int j=0; j<lNbArgsInsertedBranch; ++j) {
			if(j == lSubtreeArgIndex) {
				const unsigned int lSubtreeIndex = lNewTree->size();
				lNewTree->insert(lNewTree->end(),
				                 lOriginalTree->begin()+lChosenNodeIndex,
				                 lOriginalTree->begin()+lChosenNodeIndex+lSubtreeSize);
				lContext.pushCallStack(lSubtreeIndex);
				if(lNewTree->validateSubTree(lSubtreeIndex, lContext) == false) {
					lArgsGenFailed = true;
					break;
				}
				lContext.popCallStack();
				(*lNewTree)[lChosenNodeIndex].mSubTreeSize += lSubtreeSize;
			} else {
				Primitive::Handle lArgInserted = lPrimitiveSet.select(GP::Primitive::eTerminal, lContext);
				if(lArgInserted == NULL) {
					lArgsGenFailed = true;
					break;
				}
				lArgInserted = lArgInserted->giveReference(GP::Primitive::eTerminal, lContext);
				const unsigned int lSubtreeIndex = lNewTree->size();
				lNewTree->push_back(Node(lArgInserted,1));
				lContext.pushCallStack(lSubtreeIndex);
				if(lArgInserted->validate(lContext) == false) {
					lArgsGenFailed = true;
					break;
				}
				lContext.popCallStack();
				++(*lNewTree)[lChosenNodeIndex].mSubTreeSize;
			}
		}
		if(lArgsGenFailed) {
			Beagle_LogDebugM(
			    ioContext.getSystem().getLogger(),
			    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
			    std::string("Constrained insert mutation attempt failed as it seems impossible ")+
			    std::string("to select a terminal primitive under the inserted branch in the actual context")
			);
			lIndividual[lChosenTree] = lOriginalTree;
			lContext.setGenotypeHandle(lOriginalTree);
			continue;
		}

		// Complete new tree with rest of original tree
		Beagle_AssertM(lOriginalTree->size() >= (lChosenNodeIndex+lSubtreeSize));
		lNewTree->insert(lNewTree->end(),
		                 lOriginalTree->begin()+lChosenNodeIndex+lSubtreeSize,
		                 lOriginalTree->end());
		Beagle_AssertM(lNewTree->size() == (lOriginalTree->size()+lNbArgsInsertedBranch));

		// Correct subtree size data and terminate mutation process
		Beagle_LogDebugM(
		    ioContext.getSystem().getLogger(),
		    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
		    std::string("Correcting the 'mSubTreeSize' fields of tree")
		);
		lContext.popCallStack();
		for(unsigned int j=0; j<lContext.getCallStackSize(); ++j) {
			(*lNewTree)[lContext[j]].mSubTreeSize += lNbArgsInsertedBranch;
		}

		// Mutation successful, log messages and return
		std::ostringstream lOSS;
		lOSS << "Successfully inserted a new node at index " << lChosenNodeIndex;
		lOSS << " of the " << uint2ordinal(lChosenTree) << " tree of the actual individual";
		Beagle_LogTraceM(
		    ioContext.getSystem().getLogger(),
		    "mutation",
		    "Beagle::GP::MutationInsertConstrainedOp",
		    lOSS.str()
		);
		Beagle_LogDebugM(
		    ioContext.getSystem().getLogger(),
		    "mutation",
		    "Beagle::GP::MutationInsertConstrainedOp",
		    "Individual after constrained GP insert mutation"
		);
		Beagle_LogObjectDebugM(
		    ioContext.getSystem().getLogger(),
		    "mutation",
		    "Beagle::GP::MutationInsertConstrainedOp",
		    ioIndividual
		);
		lContext.emptyCallStack();
		lContext.setGenotypeIndex(lOldGenotypeIndex);
		lContext.setGenotypeHandle(lOldGenotypeHandle);
		return true;
	}

	// Insert mutation failed, return without mutating the individual
	lContext.emptyCallStack();
	lContext.setGenotypeIndex(lOldGenotypeIndex);
	lContext.setGenotypeHandle(lOldGenotypeHandle);
	Beagle_LogTraceM(
	    ioContext.getSystem().getLogger(),
	    "mutation", "Beagle::GP::MutationInsertConstrainedOp",
	    std::string("All constrained insert mutation attempts failed; ")+
	    std::string("resuming from mutation without modifying the individual")
	);
	return false;

	Beagle_StackTraceEndM("bool GP::MutationInsertConstrainedOp::mutate(Beagle::Individual& ioIndividual, Beagle::Context& ioContext)");
}