/// @file mind.cc
#include "mind/mind.hh"
#include "mind/wrappers.hh"
#include <cstring>

namespace EyeMind {

// Explicit instantiations.
template Array<Updatable, INTERFACE_LIST_SIZE>;
template Array<Behavior, BEHAVIOR_LIST_SIZE>;

#ifndef REACT
#include "state/state.hh"
template Array<State, STATE_LIST_SIZE>;
template Array<State*, STATE_STACK_SIZE>;
#endif

Id* Mind::id = NULL;
Ego* Mind::ego = NULL;
SuperEgo* Mind::superego = NULL;
VWMove* Mind::vwmove = NULL;
VWDrive* Mind::vwdrive = NULL;
FeelPsds* Mind::feelpsds = NULL;
Odometry* Mind::odometry = NULL;
Dictionary* Mind::dictionary = NULL;



Mind::Mind()
{
}


void Mind::NullCheck(void* p, const char* who)
{
	if (p != NULL) {
		char out[64];
		strcpy(out, who);
		strcat(out, " already registered");
		OSPanic(out); // we don't want schizophrenia
	}
}


TimerHandle Id::timer_handles[MAX_IRQ] = {0};
unsigned Id::timer_count = 0;


Id::Id()
{
	Mind::Register(*this);
	active = false;
}


Id::~Id()
{
	for (unsigned i = 0; i < MAX_IRQ; ++i)
		OSDetachTimer(timer_handles[i]);
}



void Id::AttachToTimer(void (*function)(), float timebase)
{
	OSPanicIf(timer_count >= MAX_IRQ - 1, "Too many timer functions");

	timer_handles[timer_count] = OSAttachTimer((unsigned)(100 * timebase), function);
	++timer_count;
}


Updatable& operator<<(Id& id, Updatable& i)
{
	if (id.inputs.Find(i) >= 0)
		return i;

	id.inputs + i;

	if (i.Timebase() > 0)
		id.AttachToTimer(UpdatableFunctionWrapper::For(i), i.Timebase());
	return i;
}


Updatable& operator>>(Id& id, Updatable& o)
{
	if (id.outputs.Find(o) >= 0)
		return o;

	id.outputs + o;

	if (o.Timebase() > 0)
		id.AttachToTimer(UpdatableFunctionWrapper::For(o), o.Timebase());
	return o;
}


Behavior& operator>>(Id& id, Behavior& b)
{
	if (id.behaviors.Find(b) >= 0)
		return b;

	id.behaviors + b;
	b.root = true;

	if (b.Timebase() > 0)
		id.AttachToTimer(BehaviorFunctionWrapper::For(b), b.Timebase());
	return b;
}


void Id::ExciteBehaviors(int signal)
{
	Behavior *b;

	b = behaviors.Begin();

	while ((b = behaviors++)) 
	{
		if (b->Timebase() == 0)
			b->Excite(signal);
	}
}

#ifndef REACT

Ego::Ego()
{
	Mind::Register(*this);
	active = false;
}



void Ego::Think()
{
	if (!active)
		return;

	// Process the states which should always be true
	State* current = states.Begin();

	while(current!=NULL)
	{
		current->Evaluate();
		if(current->Value()!=TRISTATE_TRUE)
		{
			current->Resolve();
		}
		current = ++states;
	}


#if 0 // @bug not yet implemented
	/* Process the first in the list, this *should* be the most important */
	current = task.Begin();
	if(current!=NULL)
	{
		DEBUG_PRINT("Got state\n");
		if( current->Criticise() == true )
		{
			DEBUG_PRINT("Can't Satisfy");
			//current->LearnBad(superego);		// NOTE: not implemented yet
			temp = desired_states - current;	// Remove the current state from the array
			delete(temp);
		}
		else if( current->IsSatisfied() == TRUE )
		{
			DEBUG_PRINT("Satisfied\n");
			//current->LearnGood(superego);		// NOTE: not implemented yet
			temp = desired_states - current;
			delete(temp);
		}
		else
		{
			current->Satisfy();
			break;
		}
	}
	else
	{
		/* No more desired states, so create some */
		//DesiredStates(superego->CreateStrategy());
		DEBUG_PRINT("Created strategy\n");
		DEBUG_WAIT(100);
	}
#endif
}



SuperEgo::SuperEgo()
{
	Mind::Register(*this);
	active = false;
}


void SuperEgo::Plan()
{
	// INSERT INTELLIGENCE HERE
}

#endif // REACT


}; // namespace EyeMind