/// @file computing_behaviors.cc

#include "behavior/computing_behaviors.hh"

namespace EyeMind {

DistToSpeed::DistToSpeed() : Behavior(), distancelink(NULL)
{
	memset(&speed, 0, sizeof(SpeedType));
}


void DistToSpeed::Feed(SignalLink &l)
{
	if (!l.Generic())
		reinterpret_cast<SpeedLink&>(l).Speed(speed);

	Behavior::Feed(l);
}


DistToSpeed& operator>>(DistanceLink& l, DistToSpeed& o)
{
	SignalLinkOutputToBehavior(l,o);
//	o.distancelink = &l;
	return o;
}


DistToSpeed& operator<<(SpeedLink& l, DistToSpeed& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}


SpeedLink& operator>>(DistToSpeed& n, SpeedLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}


DistanceLink& operator<<(DistToSpeed& n, DistanceLink& i)
{
	BehaviorInputToSignalLink(n,i);
//	n.distancelink = &i;
	return i;
}

AvoidObstacles::AvoidObstacles() : DistToSpeed()
{
	t=0;
	recent = 0;

	avoid_bt *bd = (avoid_bt*) HDTFindEntry(B_AVOID,0);
	if(bd==NULL) OSPanic("Cannot find\n avoid data\n");

	d_side_avoid	= bd->d_side_avoid;
	d_front_close	= bd->d_front_close;
	d_back_close	= bd->d_back_close;
	w_avoid_high	= bd->w_avoid_high;
	w_avoid_low 	= bd->w_avoid_low;
	v_avoid_high	= bd->v_avoid_high;
	v_avoid_low 	= bd->v_avoid_low;
	grace		= bd->grace;
}


int AvoidObstacles::Execute()
{
	DistancesType temp_dist;
	DistancesType dist;

	DistanceLink* dl;

	memset(&dist,0,sizeof(DistancesType));

	// Sum all distances
	SignalLink* i = inlinks.Begin();
	while(i!=NULL)
	{
		if(!i->Generic() && i->Signal()>0)
		{
			dl = reinterpret_cast<DistanceLink*>(i);
			temp_dist = dl->Distances();
			dist.left  += temp_dist.left;
			dist.right += temp_dist.right;
			dist.front += temp_dist.front;
			dist.back  += temp_dist.back;
		}
		i=++inlinks;
	}

	// correct the signals
	dist.left  = (dist.left >0) ? dist.left :999;
	dist.right = (dist.right>0) ? dist.right:999;
	dist.front = (dist.front>0) ? dist.front:999;
	dist.back  = (dist.back >0) ? dist.back :999;


	// Make sure something is not in front of the robot
	if( (dist.front > d_front_close) && (dist.back > d_back_close) )
	{
		if(dist.left < d_side_avoid)
		{
			speed.w = -w_avoid_high;
			speed.v = v_avoid_low;
			return EXCITE_IDLE;
		}

		if(dist.right < d_side_avoid)
		{
			speed.w = w_avoid_high;
			speed.v = v_avoid_low;
			return EXCITE_IDLE;
		}

		if (t > 0)
		{
			t--;
			return EXCITE_IDLE;
		}

		// all clear, so set speed to zero
		memset(&speed, 0, sizeof(SpeedType));
		return EXCITE_NONE;
	}
	else // turn towards the direction with greatest distance
	{
		if (dist.back > dist.front)
		{
			speed.v = -v_avoid_high;
		}
		else
		{
			speed.v = v_avoid_high;
		}

		if (dist.left < dist.right)
		{
			speed.w = -w_avoid_low;
		}
		else
		{
			speed.w = w_avoid_low;
		}
		// Set a 'timer'
		t = grace;
		return EXCITE_IDLE;
	}
}

#if 0
PositionToPolar::PositionToPolar() : Behavior()
{
	memset(&coord, 0, sizeof coord);
}


void PositionToPolar::Feed(SignalLink &l)
{
	if (!l.Generic())
		reinterpret_cast<PolarLink&>(l).Polar(coord);

	Behavior::Feed(l);
}


PositionToPolar& operator>>(PositionLink& l, PositionToPolar& o)
{
	SignalLinkOutputToBehavior(l,o);
	return o;
}


PositionToPolar& operator<<(PolarLink& l, PositionToPolar& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}


PolarLink& operator>>(PositionToPolar& n, PolarLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}


PositionLink& operator<<(PositionToPolar& n, PositionLink& i)
{
	BehaviorInputToSignalLink(n,i);
	return i;
}
#endif

#if 0
AngularMomentum::AngularMomentum() : PositionToPolar()
{
	memset(&prev_coord, 0, sizeof prev_coord);
}


int AngularMomentum::Execute()
{
	PositionLink* l = reinterpret_cast<PositionLink*>
		(FindMaxNonGenericInputSignal());

	coord.phi -= l->Position().phi -  prev_coord.phi;
	prev_coord.phi = l->Position().phi;

	return l->Signal();
}
#endif

#if 0
PolarToSpeed::PolarToSpeed() : Behavior()
{
	memset(&speed, 0, sizeof speed);
}


void PolarToSpeed::Feed(SignalLink &l)
{
	if (!l.Generic()) {
		reinterpret_cast<SpeedLink&>(l).Speed(speed);
	}

	Behavior::Feed(l);
}


PolarToSpeed& operator>>(PolarLink& l, PolarToSpeed& o)
{
	SignalLinkOutputToBehavior(l,o);
	return o;
}


PolarToSpeed& operator<<(SpeedLink& l, PolarToSpeed& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}


SpeedLink& operator>>(PolarToSpeed& n, SpeedLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}


PolarLink& operator<<(PolarToSpeed& n, PolarLink& i)
{
	BehaviorInputToSignalLink(n,i);
	return i;
}



TurnCommand::TurnCommand() : PolarToSpeed(), compass(Compass::I())
{
	turncommand_bt *tc = (turncommand_bt*) HDTFindEntry(B_TURNCOMMAND,0);
	if(tc==NULL) OSPanic("Cannot find\n turncommand data\n");
	Kr	= tc->Kr;
	Kq	= tc->Kq;

	compass.Update();

	int Q = compass.Angle() + 180; // + compass_offset;
	while (Q > 180) { Q -= 360; }
	while (Q < -180) { Q += 360; }

	desired_angle = Q;
}

int TurnCommand::Execute()
{
	int max_signal = 0;
	int Qd;
	int Q;
	int e_compass;
	memset(&speed, 0, sizeof speed);

	PolarLink *pl = reinterpret_cast<PolarLink*>
		(FindMaxNonGenericInputSignal());

	if (pl==NULL) {
		DEBUG_PRINT("TC: 0\n");
		return EXCITE_NONE;
	}

	PolarType coord = pl->Polar();
	max_signal = pl->Signal();
	DEBUG_PRINTF("TC: SIG %d\n",max_signal);

//	speed.v = Kr*coord.r;
//	speed.w = Kq*coord.phi;
	//if (coord.r < 0) {
	if (max_signal == 1) {
		DEBUG_PRINT("TC: r<0\n");
		return EXCITE_NONE;
	}
	else if (max_signal == 2) {
		DEBUG_PRINT("TC: 2\n");
		// Ball far...
		speed.v = Kr*coord.r;
		speed.w = Kq*coord.phi;
	}
	else if (max_signal > 3) {
		DEBUG_PRINT("TC: 3\n");
		// Ball close...
		Qd = desired_angle;
		compass.Update();
		Q = compass.Angle();
		e_compass = Qd - Q;
		while (e_compass > 180) { e_compass -= 360; }
		while (e_compass < -180) { e_compass += 360; }
		speed.v = 0.1;
		if (e_compass < 10) {
			speed.w = -0.2;
		}
		else if (e_compass > 10) {
			speed.w = 0.2;
		}
		else {
			speed.w = 0.0;
			speed.v = 0.2;
		}
	}
	//DEBUG_PRINT("TC: 1\n");
	return EXCITE_IDLE;
	//return pl->Signal();
}
#endif

}; // namespace EyeMind