/***********************************
 * (C) 2007-2008 by Tomasz bla Fortuna <bla@thera.be>.
 * License: GPL3+ (See Docs/LICENSE)
 *
 *   Motor Encoder
 ********************/

namespace Encoder {

#define ENCODER_DDR	DDRB
#define ENCODER_PIN	PINB
#define ENCODER_PORT	PORTB
#define ENCODER_L	(1<<PB0)
#define ENCODER_P	(1<<PB1)
#define ENCODER_LP	(ENCODER_L | ENCODER_P)


	Short OldState = (ENCODER_L | ENCODER_P);
	Short State = 0;

	Int Position = 0;
	Int PrevPosition = 0;
	Int dPosition = 0;

	enum Dir::Direction Dir = Dir::Left;
	volatile UChar Update;

	/* Determine direction and speed */
	ISR(PCINT1_vect)
	{
		static UChar tmp;
		/* Latch port fast */

		State = ENCODER_PIN & ENCODER_LP;	/* Previous state is in old state now */

		tmp = State ^ OldState;
		if (tmp == 0) return;
		OldState = State;
		if (tmp == ENCODER_LP) {
			/* Both changed - update position but not direction */
			Position += Dir;
			return;
		}

		/* tmp now has information what changed:
		 * 0 - nothing
		 * ENCODER_P  - P
		 * ENCODER_L  - L
		 */

		if (State & tmp)	/* Determine direction using bit which changed */
			Dir = Dir::Right;
		else	Dir = Dir::Left;
	
		switch (tmp)
		{
		case ENCODER_L:
			if (!(State & ENCODER_P))
				Dir = Dir::Direction(- Dir);
			break;

		case ENCODER_P:
			if (State & ENCODER_L)
				Dir = Dir::Direction(- Dir);
		}

		Position += Dir;

		/* Calculate speed */
/*		Cnt++;
		if (Cnt % 5 == 0) {
			Speed = 10 * (PrevPosition - Position) / TCNT1;
			PrevPosition = Position;
			TCNT1 = 0;
		} */
	}


	/*
	 * Built-in PID for controlling motor speed
	 */
#if 0
	namespace PID {
		typedef Int Type;
		Int DestSpeed;
		static const Type P = 100;
		static const Type I = 70;
		static const Type D = 30;

		static Type DPrev;
		static Type PTmp, ITmp, DTmp;
		static Type Answer;
		static UChar Power;
		static Int Power_Display;

		static inline Void Set(Int NewSpeed)
		{
			DestSpeed = NewSpeed;
		}


		static inline Void Keep(void)
		{
			static Int Error;
			static Type Tmp;

			Error = DestSpeed - Speed;
			PTmp = P * Error;

			DTmp = D * (Error - DPrev);
			DPrev = Error;

			/* Do this only if we won't make it too big */
			ITmp += I * Error;
			static const Type ILim = 1200;
			if (ITmp < -ILim)
				ITmp = -ILim;
			else
				if (ITmp > ILim)
					ITmp = ILim; 
			
			Answer = 2 * (PTmp + ITmp + DTmp);
			Answer /= 10;
			
			// Direct motors
			if (Answer < 0)
				Motor::Dir(Motor::Both, Dir::Left);
			else
				Motor::Dir(Motor::Both, Dir::Right);
			
			/*  ABS */
			if (Answer < 0)
				Tmp = - Answer;
			else
				Tmp = Answer;

			const UChar Base = 0;
			if (Tmp > 255 - Base)
				Power = 255;
			else	Power = Base + Tmp;
			Motor::Speed(Motor::Both, Power);

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

	}
#endif

	static inline Void Init(void)
	{
		/* Connect encoder to pins PB0, PB1;
		 * Pin Change Interrupt 1 - PCI1, PCMSK1 covers this. */

		/* Enable inputs with pull-ups */
		ENCODER_PORT |= ENCODER_LP;
		ENCODER_DDR &= (UChar)(~ENCODER_LP);

		EICRA = (1<<ISC10);			/* Any edge raises interrupt */
		PCMSK1 = (1<<PCINT8) | (1<<PCINT9);	/* Interrupt on PB0 and PB1 */
		PCICR = (1<<PCIE1);
 	}

} /* End of namespace */