/// @file control_behaviors.cc

#include "behavior/control_behaviors.hh"
#include "mind/mind.hh"

#include <cmath>

namespace EyeMind {

MotorControl::MotorControl(DCMotor& m) : Behavior(), motor(m)
{
	Threshold(-1);	// set threshold to -1 so Id can excite

	w_min = 0;
	w_max = 0;

	y_min = motor.Min();
	y_max = motor.Max();

	K_P = 0.0;
	T_N = 0.0;
	T_V = 0.0;

	y[0] = 0;
	y[1] = 0;

	x_d[0] = 0;
	x_d[1] = 0;
	x_d[2] = 0;

	velocity = NULL;

	motorcontrol_bt *mc = (motorcontrol_bt*) HDTFindEntry(B_MOTORCONTROL,0);
	if(mc==NULL) OSPanic("Cannot find\n motorcontrol data\n");
	time_limit				= mc->time_limit;
	dband					= mc->dband;

	// run at half the speed of the motor updates
	this->Timebase(2 * motor.Timebase());

	Id& id = *Mind::ActiveId();
	id >> motor; // Attach motor to id updatables
}


int MotorControl::Execute()
{
	float w; // desired value
	float T = Timebase();
	float v = motor.Velocity();

	// clip desired value
	if (velocity != NULL)
		w = clip(velocity->Float(), w_min, w_max);
	else
		w = 0;

	// Check for a stall.
	if (w!=0.0 && (v < dband) && (v > -dband))
	{
		time_stalled++;
		// send a maximum voltage to the motors
		if(time_stalled<time_limit)
		{
			if(w<0)
			{
				motor.Signal((int)y_min);
			}
			else
			{
				motor.Signal((int)y_max);
			}
		}
	}
	else
	{
		time_stalled=0;
	}

	// shift previous values and calculate new controller input
	x_d[2] = x_d[1];
	x_d[1] = x_d[0];
	x_d[0] = w - v;
	y[1] = y[0];

	// the control algorithm:
	// PID with first-order approximation
	y[0] = K_P * (  x_d[0] * (1 + T/T_N + T_V/T) +
			x_d[1] * (-1 - 2*T_V/T) +
			x_d[2] * (T_V/T) ) +
		y[1];
	// clip controller output (very important)
	y[0] = clip(y[0], y_min, y_max);
	motor.Signal((int)y[0]);

	return EXCITE_IDLE;
}


void MotorControl::Feed(SignalLink& l)
{
	SignalLink* i = outlinks.Begin();
	while (i != NULL) {
		if (!i->Generic())
			reinterpret_cast<FloatLink&>(*i).Float(y[0]);
		i = ++inlinks;
	}
}

void MotorControl::Params(float p, float re, float ra)
{
	K_P = p;

	if (re == 0.0)
		// obviously one wants to turn off the I component, but does
		// not have a clue, so:
		T_N = MAXFLOAT;
	else
		T_N = re;

	T_V = ra;
}


void MotorControl::Limits(float lower, float upper)
{
	w_min = lower;
	w_max = upper;
}


MotorControl& operator>>(FloatLink& l, MotorControl& o)
{
	SignalLinkOutputToBehavior(l,o);
	o.velocity = &l;
	return o;
}


FloatLink& operator<<(MotorControl& n, FloatLink& i)
{
	BehaviorInputToSignalLink(n,i);
	n.velocity = &i;
	return i;
}


MotorControl& operator<<(FloatLink& l, MotorControl& i)
{
	SignalLinkInputToBehavior(l,i);
	return i;
}


FloatLink& operator>>(MotorControl& n, FloatLink& o)
{
	BehaviorOutputToSignalLink(n,o);
	return o;
}


}; // namespace EyeMind