/***********************************
 * (C) 2008 by Tomasz bla Fortuna <bla@thera.be>.
 * License: GPL3+ (See Docs/LICENSE)
 *
 * Desc: PIDs controlling robot balance and steering
 *
 * TODO: Move public vars to functions and make them const
 ********************/


namespace Control {
	/* Type used by balancing PID */
	typedef Long BType;

	/*** PID parameters
	 * A - Angle
	 * M - Motor Speed
	 */

	/*** The angle we will try to keep ***/
	static BType DestAngle;

	/*** Resulting motor power ***/
	static BType Answer;

	static BType Power_Display;
	static UChar Power;

	/* Answer divider */
	static const BType KADiv = 1000;

	/*** Integral part ***/
	static volatile BType I_A = 0;

	/*** Temporary values - public for debuging ***/
	static volatile BType A_P_tmp;
	static volatile BType A_I_tmp;
	static volatile BType A_D_tmp;

#define _limit(x, lim) do { if (x>lim) x = lim; else if (x<(-lim)) x = -lim; } while(0)

	static inline void Balance(void)
	{
		/* Private variables */
		const BType Angle = Angle::Angle - DestAngle;

		static BType Prev_Angle;


		/* TODO: Read here all data changed by external interrupts 
		   (Including ADC and encoder) and then enable interrupts */

//		sei(); /* Check for race conditions with ADC interrupt */

		if (Angle > 5000L || Angle < -5000L) {
			Motor::Dir(Motor::Both, Dir::Stop);			
			Util::Signal::Set(Util::Signal::RED);
			return;
		} else {
			Util::Signal::Set(Util::Signal::GREEN);
		}

		/*** Angle Error; Calculate it's PI values ***/
		A_P_tmp = Cfg::KA_P * Angle;
		A_D_tmp = Cfg::KA_D * (Angle - Prev_Angle);
		Prev_Angle = Angle;

		/* Integral */
		I_A += Angle;
		_limit(I_A, Cfg::KA_ISize);
		A_I_tmp = Cfg::KA_I * I_A;

		/*** Low-pass filter the answer to limit 'quirks' ***/
		Answer = A_P_tmp + A_I_tmp + A_D_tmp;

		/*** Calculate final answer ***/
		Answer /= KADiv;

		/* Cut off impossible answers, direct motors, calc abc */
		const BType LimAnswer = 255;
		if (Answer < 0) {
			Motor::Dir(Motor::Both, Dir::Right);
			if (Answer <= -LimAnswer)
				Power = (UChar)LimAnswer;
			else
				Power = -Answer;
		} else {
			Motor::Dir(Motor::Both, Dir::Left);
			if (Answer >= LimAnswer)
				Power = (UChar)LimAnswer;
			else
				Power = Answer;
		}

		Motor::Speed(Motor::Both, Power);

		/* Only for display */
		if (Answer < 0)
			Power_Display = -Power;
		else
			Power_Display = Power;

	}

	/****************************************
	 * Steering PID controller
	 ****************************************/

	static volatile BType M_PP_tmp; /* Position */
	static volatile BType M_P_tmp;
	static volatile BType M_I_tmp;
	static volatile BType M_D_tmp;

	static volatile BType I_M = 0, I_Md = 0;
	static const BType KMDiv = 1000;
	
	/* Wheel destination position */
	static BType Destination;

	static inline void Steer(void)
	{
		BType Distance = Encoder::Position - Destination;
		_limit(Distance, 50);

		/*** Get wheel velocity ***/
		Encoder::dPosition = Encoder::Position - Encoder::PrevPosition;

		Encoder::PrevPosition = Encoder::Position;

		/* Calculate it's PI values */
		M_P_tmp = Cfg::KM_P * Distance;
		M_D_tmp = Cfg::KM_D * Encoder::dPosition;

		/* Position Integral */
		I_M += Encoder::Position;
		_limit(I_M, Cfg::KM_ISize);

		M_I_tmp = Cfg::KM_I * I_M;

		/* dPosition Integral */
		I_Md += Encoder::dPosition;
		_limit(I_Md, Cfg::KM_ISize);

		M_I_tmp += Cfg::KM_Id * I_M;

		BType Answer = - (M_P_tmp + M_I_tmp + M_D_tmp + M_PP_tmp) / KMDiv;
		_limit(Answer, Cfg::TravelAngle);

		DestAngle = Filter::DestAngle::Push(Answer);		
	}

}