#include "sf_hrp_plan.hpp"
#include "sf_hrp_asm.hpp"
#include "condition.hpp"

//TODO remove
#include <iostream>
#include <cassert>

#include "base_rule.hpp"

CSFHRPPlan::CSFHRPPlan(CSFHRPASM* agent, IEngineDescriptor* engd, int prio, CCondition* cond, 
	InitPhase* init, ReactivePlanner* rplanner, TermPhase* term) 
	: m_agent(agent), m_prio(prio), m_engineDescriptor(engd), m_condition(cond), 
	m_init(init), m_rplanner(rplanner), m_term(term)
{
	assert(rplanner != 0);
	m_state = INITIALIZATION;
}

CSFHRPPlan::~CSFHRPPlan()
{
	//delete m_engineDescriptor;

	delete m_rplanner;
	delete m_init;
	delete m_term;
}

int
CSFHRPPlan::GetAction()
{
	int action;

	switch(m_state) {
		case INITIALIZATION:

			if(m_init->Ended()) { //switch to exec phase
				m_state = EXECUTION;
				return GetAction();
			}

			return m_init->GetAction();
			
			break;
		case EXECUTION:
			
			action = m_rplanner->GetAction();

			if(action == SF_FAIL || action == SF_SUCCESS) {
				if(action == SF_FAIL) {
					m_term->SetCase( false);
				} else {
					m_term->SetCase( true);
				}
				m_state = TERMINATION;
				return GetAction();
			}

			return action;
			break;
		case TERMINATION:
			
			if(m_term->Ended()) {
				m_state = FINISHED;
				return GetAction();
			}

			return m_term->GetAction();
			break;
		case FINISHED:
			return m_engineDescriptor->GetIdleAction();
			break;

		case SWITCHING_IN:
			return m_engineDescriptor->GetIdleAction();
			m_state = EXECUTION;
			break;
		case SWITCHING_OUT:
			return m_engineDescriptor->GetIdleAction();
			m_state = SWITCHED;
			break;
		case SWITCHED:
			return m_engineDescriptor->GetIdleAction();
			break;
		default:
			assert(false);
			break;
	}

	assert( false);
	return -2;

}

int 
CSFHRPPlan::GetPriority() {
	return m_prio;
}

void
CSFHRPPlan::Activate() {
	//TODO
	//DEBUG
	//m_state = SWITCHING_IN;
	//std::cout << "Activating plan ..." << std::endl;
	m_agent->Activate(this);
}

void
CSFHRPPlan::Deactivate() {
	
	//TODO implement call to switch out object and change state
	//DEBUG, one tick switchout
	//m_state = SWITCHING_OUT;
	//std::cout << "Deactivating plan ..." << std::endl;
	m_agent->Deactivate(this);

}

bool
CSFHRPPlan::IsSwitched() {
	return m_state == SWITCHED;
}

void
CSFHRPPlan::SwitchOut() {

	m_state = SWITCHING_OUT;
}

void
CSFHRPPlan::SwitchIn() {

	m_state = SWITCHING_IN;
}

bool
CSFHRPPlan::IsActive() {

	return m_condition->GetValue();
}