/**
 * State-full hierarchical planning action selection mechanism
 * for virtual agents
 *
 * Frantisek Farka
 * 2011
 *
 */

#include <cassert>

#include "sf_hrp_asm.hpp"

CSFHRPASM::CSFHRPASM(IEngineDescriptor* engine) {

	m_activePlan = null;
	m_engine = engine;

}

CSFHRPASM::~CSFHRPASM() {

	planVector::iterator pit;
	for(pit = m_plans.begin(); pit != m_plans.end(); ++pit) {
		delete (*pit);
	}

	conditionVector::iterator cit;
	for(cit = m_conditions.begin(); cit != m_conditions.end(); ++cit) {
		delete (*cit);
	}
}

void
CSFHRPASM::SetCondition(int cond, bool val) {

	elementMap::iterator mit = m_conditionElements.find( cond);
	elementMap::iterator last = m_conditionElements.upper_bound( cond);

	assert(mit != m_conditionElements.end()); //setting unregistered condition

	for( ; mit != last; ++mit) {
		(mit->second)->SetValue(val);
	}

	return;
}


int
CSFHRPASM::GetAction() {

	CSFHRPPlan* highest;
	//select plan with highest priority - candidate for execution
 	if(!m_holding.empty()) { //i. e. some holding plan exists
		highest = (*m_holding.rbegin());
	} else {
		//we have no candidate for execution, just check wheter last
		//has finished switch out
		if(m_activePlan && !m_activePlan->IsSwitched()) {
			//there is active plan, which wasn't switched out yet
			//we don't have to check wheter is executing, because holding is empty
			return m_activePlan->GetAction();
		}

		//there is obviously no holding or switching out plan
		return m_engine->GetIdleAction();
	}

	//now we have some highest plan
	if(m_activePlan == null) {
		m_activePlan = highest;
	} else if(highest != m_activePlan) {

		//TODO state machine !!!!!
		if(!m_activePlan->IsSwitched() /*&& m_activePlan->IsActive()*/) {
			//current plan is not highest holding
			// but is still holding
			//we initiate switch-out of old one
			m_activePlan->SwitchOut();
		} /*else {
			//switched or inactive
			m_activePlan = null;
		}*/

		//if there was no switch out
		if(m_activePlan->IsSwitched()) {
			
			//new candidate is resuming from switched state
			if(highest->IsSwitched()) {
				highest->SwitchIn();
			}

			//set new candidet as active
			m_activePlan = highest;
		} 
	}

	return m_activePlan->GetAction();


	//we suspend current and 
	//TOP LEVEL (plan) selection algorithm
	//some plan is still switching out 
	if(m_activePlan) {
		if(m_activePlan->IsSwitched()) {
			m_activePlan = null;
		} else {
			return m_activePlan->GetAction();
		}
	}


	

}

void
CSFHRPASM::Activate(CSFHRPPlan* ptrPlan) {

	m_holding.insert( ptrPlan); //silently ignores duplicate occurance

}

void
CSFHRPASM::Deactivate(CSFHRPPlan* ptrPlan) {

	m_holding.erase( ptrPlan); //silently ignores nonexistent element

}

void
CSFHRPASM::AddPlan(CSFHRPPlan* ptrPlan) {

	m_plans.push_back( ptrPlan); //silently ignores nonexistent element

}

void
CSFHRPASM::AddCondition(CCondition* ptrCondition) {

	m_conditions.push_back( ptrCondition); //silently ignores nonexistent element

}

void
CSFHRPASM::RegisterElement(int code, CElement* ptrElement) {

	
	//insert new condition element
	m_conditionElements.insert( std::pair< int, CElement* >(code, ptrElement) );

}