/// @file motion_behaviors.cc

#include "behavior/motion_behaviors.hh"
#include "mind/mind.hh"
#include "eyebot_behavior_types.h"
#include "eyebot_behavior_sem.h"

namespace EyeMind {


/**
 * VWMove
 */

VWMove::VWMove() : Behavior()
{
	memset(&speed, 0, sizeof(SpeedType));
	Mind::Register(*this);
}


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


int VWMove::Execute()
{

	unsigned s=0;
	unsigned total_signal = 0;

	// reset the output speed
	memset(&speed, 0, sizeof(SpeedType));

	// Find the maximum input signal
	SpeedLink* sl = reinterpret_cast<SpeedLink*>(FindMaxNonGenericInputSignal());
	if (sl==NULL) return EXCITE_IDLE;

	// calculate weighted average of incoming signals
	SignalLink* i = inlinks.Begin();
	while (i!=NULL)
	{
		if(!i->Generic())
		{
			sl = reinterpret_cast<SpeedLink*>(i);
			s = sl->Signal();
			speed.v += (sl->Speed().v)*s;
			speed.w += (sl->Speed().w)*s;
			total_signal+=s;
		}
		i = ++inlinks;
	}

	if(total_signal)
	{
		speed.v/=total_signal;
		speed.w/=total_signal;
	}
	return EXCITE_IDLE;
};


VWMove& operator>>(SpeedLink& l, VWMove& o)
{
	SignalLinkOutputToBehavior(l,o);
	return o;
}


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


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


SpeedLink& operator<<(VWMove& n, SpeedLink& i)
{
	BehaviorInputToSignalLink(n,i);
	return i;
}


VWDrive::VWDrive() : Behavior()
{
	speed = NULL;
	Mind::Register(*this);
}


VWDrive& operator>>(SpeedLink& l, VWDrive& o)
{
	SignalLinkOutputToBehavior(l,o);
	o.speed = &l;
	return o;
}


SpeedLink& operator<<(VWDrive& n, SpeedLink& i)
{
	BehaviorInputToSignalLink(n,i);
	n.speed = &i;
	return i;
}

VWDriveTwoWheeled::VWDriveTwoWheeled() : VWDrive(),
	left_controller(MotorControlLeft::I()), right_controller(MotorControlRight::I())
{

	w_l = 0; w_r = 0;

	// Attach output FloatLinks
	BehaviorOutputToSignalLink(*this, left);
	BehaviorOutputToSignalLink(*this, right);

	// Attach controllers to FloatLinks
	left_controller << left;
	right_controller << right;

	// pre-feed the FloatLinks
	left.Float(0);
	right.Float(0);

	//get data from hdt
	twowheeldrive_bt *tw = (twowheeldrive_bt*) HDTFindEntry(B_TWOWHEELDRIVE,0);
	if(tw==NULL) OSPanic("Cannot find\n drive data\n");
	radius = tw->radius;
	width = tw->width;

	vwdrive_bt *vw = (vwdrive_bt*) HDTFindEntry(B_VWDRIVE,0);
	if(vw==NULL) OSPanic("Cannot find\n vwdrive data\n");
	left_controller.Params(vw->Kp, vw->reset_time, vw->rate_time);
	right_controller.Params(vw->Kp, vw->reset_time, vw->rate_time);
	left_controller.Limits(vw->limit_lo, vw->limit_hi);
	right_controller.Limits(vw->limit_lo, vw->limit_hi);

	// Attach the controllers to the active Id
	Id& id = *Mind::ActiveId();
	id >> left_controller;
	id >> right_controller;
}


int VWDriveTwoWheeled::Execute()
{
	SpeedType s = {0.0,0.0};

	//convert v, w speeds to rad/s for two wheels
	if(speed==NULL) return EXCITE_NONE;

	s = speed->Speed();

	float w_straight = s.v / radius;
	w_l = w_straight - s.w / radius * (width / 2.0);
	w_r = w_straight + s.w / radius * (width / 2.0);

	// pre-feed the FloatLinks
	left.Float(w_l);
	right.Float(w_r);

	return 1;
}


Wander::Wander()
{
	memset(&speed,0,sizeof(SpeedType));

	wander_bt *wr = (wander_bt*) HDTFindEntry(B_WANDER,0);
	if(wr==NULL) OSPanic("Cannot find\n wander data\n");
	v = wr->v;
	w = wr->w;
	period = (unsigned) wr->period;
	count = 0;
	sign = 1;
}


int Wander::Execute()
{
	if (++count % period == 0) {
		if(speed.v > 0)
		{
			speed.v = 0;
			speed.w = sign * w;
			sign *= -1;
		}
		else
		{
			speed.v = v;
			speed.w = 0.0;
		}
		count=0;
	}
	return EXCITE_IDLE;
}

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


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


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



VWMotion::VWMotion() : Behavior()
{
	speed.v = 0;
	speed.w = 0;
}

VWMotion::VWMotion(int device) : Behavior()
{
	motion_bt* mt = (motion_bt*)HDTFindEntry(B_MOTION, (DeviceSemantics) device);
	OSPanicIf(mt==NULL,"Cannot find motion data\n");

	speed.v = mt->v;
	speed.w = mt->w;
}

int VWMotion::Execute()
{
	return EXCITE_IDLE;
}

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


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


}; // namespace EyeMind