#include "simple_plan.hpp"

#include "reactive_planner.hpp"
#include "action_rule.hpp"
#include "plan_rule.hpp"
#include "sf_rule.hpp"

SimplePlan::SimplePlan(ptrReactivePlanner ptr, IEngineDescriptor* engine)
{
	_planner = ptr;
	_engine = engine;
	_rsrule_deactiv_sched = 0;
}

SimplePlan::~SimplePlan()
{
	for(ruleList::iterator it = _rule_list.begin(); it != _rule_list.end(); ++ it) {
		delete *it;
	}
}

//priority, weight, action, flags, sticky_timeout, fails, successes
ptrBaseRule
SimplePlan::AddActionRule(std::string & id, unsigned int pri, unsigned int wei,
	std::vector< int> & act, 
	unsigned int fla = 0, unsigned int stt = 0, 
	unsigned int fai = 1, unsigned int suc = 1)
{
	ptrBaseRule ptr = new ActionRule(id, this, pri, wei, act, fla, stt, fai, suc);
	_rule_list.push_back(ptr);

	return ptr;
}

//priority, weight, plan, flags, sticky_timeout, fails, successes
ptrBaseRule
SimplePlan::AddPlanRule(unsigned int pri, unsigned int wei,
	ptrSimplePlan pla, 
	unsigned int fla = 0, unsigned int stt = 0, 
	unsigned int fai = 1, unsigned int suc = 1)
{
	ptrBaseRule ptr = new PlanRule(this, pri, wei, pla, fla, stt, fai, suc);
	_rule_list.push_back(ptr);

	return ptr;
}

//priority, weight, action, flags, sticky_timeout, fails, successes
ptrBaseRule
SimplePlan::AddFailRule(std::string & id, unsigned int pri, unsigned int wei, 
	unsigned int fla = 0, unsigned int stt = 0, 
	unsigned int fai = 1, unsigned int suc = 1)
{
	ptrBaseRule ptr = new SFRule(SF_FAIL, id, this, pri, wei, fla, stt, fai, suc);
	return ptr;
}

//priority, weight, action, flags, sticky_timeout, fails, successes
ptrBaseRule
SimplePlan::AddSuccessRule(std::string & id, unsigned int pri, unsigned int wei, 
	unsigned int fla = 0, unsigned int stt = 0, 
	unsigned int fai = 1, unsigned int suc = 1)
{
	ptrBaseRule ptr = new SFRule(SF_SUCCESS, id, this, pri, wei, fla, stt, fai, suc);
	return ptr;
}

int
SimplePlan::GetAction()
{
	////std::cout << _rule_list.size() << st
	if(ACTIVE_DEACTIV_DEBUG == true) {
		_active_rules.Dump();
	}

	//biased random selection

	if(!_active_rules.empty()) {
		//we can select rule

		unsigned int sum = 0; //total sum of weigts
		unsigned int rand_choice; //total sum of weigts

		RuleQueueType::iterator begin, end, it;

		begin = _active_rules.top_begin();
		end = _active_rules.top_end();

		for(it = begin; it != end; ++it) {
			sum += (*it)->GetWeight();
		}

		if(sum == 0) return _engine->GetIdleAction(); //only rules with weight 0, don't execute them

		//TODO solve RAND_MAX & unsigned int range
		rand_choice = rand() % sum;
		sum = (*begin)->GetWeight();
		for(it = begin; sum < rand_choice; ) {
			sum += (*(++it))->GetWeight();
		}

		if(_planner->_active_rule == NULL || (!_planner->_active_rule->CheckFlags(BaseRule::INTERRUPT_SAFE))) {
			//no active rule or active not interupt safe

			_planner->_active_rule = *it; //override active rule

			if(_rsrule_deactiv_sched && (*it != _rsrule_deactiv_sched)) {
				//releaser safe rule is no longer executed
				_deactivate(_rsrule_deactiv_sched); //deactiv pendling rule
				_rsrule_deactiv_sched = NULL; //no pednling any more

			}

			return (*it)->GetAction(); 

		} else if (_planner->_active_rule) {
			return _planner->_active_rule->GetAction();

		} /* else no rule executed */

	}

	return _engine->GetIdleAction();

}

void
SimplePlan::_activate_rule( ptrBaseRule rule)
{
	if(rule->CheckFlags(BaseRule::RELEASER_SAFE)) {
		if(rule == _rsrule_deactiv_sched) {
			//if scheduled for deactivation and re-activation requested,
			//then cancle scheduled deactivation
			_rsrule_deactiv_sched = NULL;

		} else {
			_activate(rule);
		}
	} else {
		_activate(rule);
	}

}

void
SimplePlan::_deactivate_rule( ptrBaseRule rule)
{
	if(ACTIVE_DEACTIV_DEBUG == true) {
		//std::cout << "Deactivation scheduled: " << rule->GetAction() << std::endl;
	}

	if(rule->CheckFlags(BaseRule::RELEASER_SAFE)) {

		if(rule == _planner->GetActive()) {
			//schedule for ease
			_rsrule_deactiv_sched = rule;
		} else {
			_deactivate(rule);
		}

	} else {
		_deactivate(rule);
	}
}

void
SimplePlan::_activate( ptrBaseRule rule)
{
	rule->out();
	assert(rule != NULL);

	if(ACTIVE_DEACTIV_DEBUG == true) {
		//std::cout << "Activation scheduled: " << rule->GetAction() << std::endl;
	}

	if(ACTIVE_DEACTIV_DEBUG == true) {
		//std::cout << "Activating: " << rule->GetAction() << std::endl;
	}

	_active_rules.insert(rule);

}

void
SimplePlan::_deactivate( ptrBaseRule rule)
{
	if(rule != NULL) {

		//set rule currently inactive
		_planner->SetInactive(rule);

		if(ACTIVE_DEACTIV_DEBUG == true) {
			//std::cout << "Deactivating: " << rule->GetAction() << std::endl;
		}

		_active_rules.erase(rule);


	}
}