/// @file sensor_behaviors.cc

#include "behavior/sensor_behaviors.hh"
#include "mind/mind.hh"
#include <cmath>
#include <cstring>
//#include "../../fmath.h"
#include "eyebot_behavior_types.h"
#include "eyebot_behavior_sem.h"


namespace EyeMind {

#if 0
/**
 * FeelPsds
 */

FeelPsds::FeelPsds() : Behavior(), psdl(PsdLeft::I()), psdf(PsdFront::I()),
	psdr(PsdRight::I()), psdb(PsdBack::I())
{
	memset(&dist, 0, sizeof dist);

	Mind::Register(*this);
}


void FeelPsds::Feed(SignalLink &l)
{
	if (!l.Generic())
		reinterpret_cast<DistanceLink&>(l).Distances(dist);

	Behavior::Feed(l);
}


int FeelPsds::Execute()
{
	psdl.Update();
	dist.left = psdl.X();

	psdf.Update();
	dist.front = psdf.X();

	psdr.Update();
	dist.right = psdr.X();

	psdb.Update();
	dist.back = psdb.X();

	//DEBUG_PRINTF("%d ",dist.left);
	//DEBUG_PRINTF("%d ",dist.front);
	//DEBUG_PRINTF("%d\n",dist.right);

	return EXCITE_IDLE;	// return lowest possible signal
}


FeelPsds& operator<<(DistanceLink& l, FeelPsds& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}


DistanceLink& operator>>(FeelPsds& n, DistanceLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}

#endif

/**
 * FeelFrontBackPsds
 */

FeelFrontBackPsds::FeelFrontBackPsds() : Behavior(), psdf(PsdFront::I()), psdb(PsdBack::I())
{
	memset(&dist, 0, sizeof dist);
}

void FeelFrontBackPsds::Feed(SignalLink &l)
{
	if (!l.Generic())
		reinterpret_cast<DistanceLink&>(l).Distances(dist);

	Behavior::Feed(l);
}


int FeelFrontBackPsds::Execute()
{
	psdf.Update();
	dist.front = psdf.X();

	psdb.Update();
	dist.back = psdb.X();

	return EXCITE_IDLE;
}


FeelFrontBackPsds& operator<<(DistanceLink& l, FeelFrontBackPsds& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}


DistanceLink& operator>>(FeelFrontBackPsds& n, DistanceLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}



/**
 * FeelLeftRightPsds
 */

FeelLeftRightPsds::FeelLeftRightPsds() : Behavior(), psdl(PsdLeft::I()), psdr(PsdRight::I())
{
	memset(&dist, 0, sizeof dist);
}


void FeelLeftRightPsds::Feed(SignalLink &l)
{
	if (!l.Generic())
		reinterpret_cast<DistanceLink&>(l).Distances(dist);

	Behavior::Feed(l);
}


int FeelLeftRightPsds::Execute()
{
	psdl.Update();
	dist.left = psdl.X();

	psdr.Update();
	dist.right = psdr.X();

	return EXCITE_IDLE;	// return lowest possible signal
}


FeelLeftRightPsds& operator<<(DistanceLink& l, FeelLeftRightPsds& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}


DistanceLink& operator>>(FeelLeftRightPsds& n, DistanceLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}




#if 0
Odometry::Odometry() : Behavior ()
{
	memset(&pos, 0, sizeof(PositionType));
	Reset();
	Mind::Register(*this);
}


void Odometry::Position(float x, float y, float q)
{
	pos.x = x;
	pos.y = y;
	pos.phi = q;
}


void Odometry::Reset()
{ 
	Position(0, 0, 0);
}


void Odometry::Angle(float q)
{
	pos.phi = q;

	// Make sure angle stays within +/- pi
	while(pos.phi > M_PI)
		pos.phi -= 2.0 * M_PI;

	while(pos.phi < -M_PI)
		pos.phi += 2.0 * M_PI;
}


Odometry& operator<<(PositionLink& l, Odometry& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}


PositionLink& operator>>(Odometry& n, PositionLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}



OdometryTwoWheeled::OdometryTwoWheeled() : Odometry(),
	motor_left(DCMotorLeft::I()), motor_right(DCMotorRight::I())
{
	///@bug Should get these from HDT
	//vw_type vwt = (vw_type*) HDTFindEntry(VW, DIFFERENTIAL_DRIVE);
	//if(vwt==NULL) OSPanic("Cannot find VW data\nexiting...\n");

	///@bug should attach, but id does not find motor[1] in output list, so adds it again
	//Id& id = *(Mind::ActiveId());
	//id >> *motor[0]; // Attach motor to id updatables
	//id >> *motor[1]; // Attach motor to id updatables

	Q_left = 0;
	prev_Q_left = motor_left.Position();

	Q_right = 0;
	prev_Q_right = motor_right.Position();

	//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;
	
	Timebase(10 * motor_left.Timebase());

	Reset();
}


int OdometryTwoWheeled::Execute()
{
	Q_left = motor_left.Position();
	Q_right = motor_right.Position();

	float dist_left = (Q_left - prev_Q_left) * radius;
	float dist_right = (Q_right - prev_Q_right) * radius;

	float dist_ave = (dist_right + dist_left) / 2.0;
	float dist_dif = (dist_right- dist_left) / 2.0;

	prev_Q_left = Q_left;
	prev_Q_right = Q_right;

	pos.phi += dist_dif / width; // add half the updated angle
	pos.x += dist_ave * fcos(pos.phi); // update position
	pos.y += dist_ave * fsin(pos.phi);
	pos.phi += dist_dif / width; // add final half to updated angle


	// Make sure angle stays within +/- pi
	if(pos.phi > M_PI) {
		pos.phi -= 2.0*M_PI;
	}

	if(pos.phi < -M_PI) {
		pos.phi += 2.0*M_PI;
	}

	//DEBUG_PRINTF("%3.2f ",pos.x);
	//DEBUG_PRINTF("%3.2f ",pos.y);
	//DEBUG_PRINTF("%3.2f\n",pos.phi);

	return EXCITE_LOW_MIN;
}


void OdometryTwoWheeled::Feed(SignalLink& l)
{
	if ( !l.Generic() )
		reinterpret_cast<PositionLink&>(l).Position(pos);

	Behavior::Feed(l);
}
#endif


DetectStall::DetectStall() : Behavior(),
		left_controller(MotorControlLeft::I()), right_controller(MotorControlRight::I())
{
	memset(&original_speed, 0, sizeof(SpeedType));
	memset(&new_speed, 0, sizeof(SpeedType));

	stall_bt* st = (stall_bt*)HDTFindEntry(B_STALL, 0);
	OSPanicIf(st==NULL,"Cannot find\n stall data\n");

	time_limit = st->delay;
	K = st->gain;
}

int DetectStall::Execute()
{
	SpeedLink* l = reinterpret_cast<SpeedLink*>
		(FindMaxNonGenericInputSignal());

	if(l==NULL) return EXCITE_NONE;

	if ( ( left_controller.Stalled() ) || ( right_controller.Stalled() ) )
	{
		if (t == 0) // new stall
		{
			DEBUG_BEEP;
			original_speed = l->Speed();
		}

		t++;
		new_speed.w -= K*original_speed.w;
		new_speed.v -= K*original_speed.v;
		return EXCITE_IDLE;
	}

	if (t > 0) // recovering from a stall (but our wheels are moving)
	{
		t++;
		if (t > time_limit)
		{
			t = 0;
		}
		return EXCITE_IDLE;
	}

	memset(&new_speed, 0, sizeof(new_speed)); // reset the output speed
	return EXCITE_NONE; // no stall so stop signal flow
}

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

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


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


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


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

/**
 * ListenRadio
 */

int ListenRadio::Execute()
{
	radio.Update();

	if(radio.NewMessage())
	{
		DEBUG_PRINTF("%s\n",radio.Message());
//		DEBUG_BEEP;
		return 1;
	}

	return 0;
}


ListenRadio& operator<<(CharArrayLink& l, ListenRadio& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}


CharArrayLink& operator>>(ListenRadio& n, CharArrayLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	o.CharArray(n.radio.Message());
	return o;
}


/**
 * ListenIRtv
 */

int ListenIRtv::Execute()
{
	if(ir.Update())
		return 1;
	else
		return 0;
}


void ListenIRtv::Feed(SignalLink& l)
{
	if ( !l.Generic() )
		reinterpret_cast<IntLink&>(l).Int(ir.Key());

	Behavior::Feed(l);
}


ListenIRtv& operator<<(IntLink& l, ListenIRtv& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}


IntLink& operator>>(ListenIRtv& n, IntLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}


}; // namespace EyeMind