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

/* Control bits */
#define M1CTL_PORT	PORTC
#define M1CTL_DDR	DDRC
#define M2CTL_PORT	PORTC
#define M2CTL_DDR	DDRC

#define M1CTL1		PC6
#define M1CTL2		PC7

/* Remember to turn off the JTAG to use this pins */
#define M2CTL1		PC2
#define M2CTL2		PC5

#define MONE(PORT, ON, OFF)					\
	PORT = ((PORT | (1<<ON)) & ((UChar)~(1<<OFF)))

#define MTWO(PORT, ON1, ON2)			\
	PORT |= ((1<<ON1) | (1<<ON2))


namespace Motor {

	enum Select {
		M1 = 1,
		M2 = 2,
		Both = 3
	};


	static Void Dir(const enum Select Which,
			       const enum Dir::Direction Dir)
	{
		switch (Which) {
		case M1:
			switch (Dir) {
			case Dir::Left: MONE(M1CTL_PORT, M1CTL1, M1CTL2); break;
			case Dir::Right: MONE(M1CTL_PORT, M1CTL2, M1CTL1); break;
			case Dir::Stop: MTWO(M1CTL_PORT, M1CTL1, M1CTL2); break;
			}
			break;

		case M2:
			switch (Dir) {
			case Dir::Left: MONE(M2CTL_PORT, M2CTL1, M2CTL2); break;
			case Dir::Right: MONE(M2CTL_PORT, M2CTL2, M2CTL1); break;
			case Dir::Stop: MTWO(M2CTL_PORT, M2CTL1, M2CTL2); break;
			}
			break;

		case Both:
			switch (Dir) {
			case Dir::Left:
				MONE(M1CTL_PORT, M1CTL1, M1CTL2);
				MONE(M2CTL_PORT, M2CTL1, M2CTL2);
				break;
			case Dir::Right:
				MONE(M1CTL_PORT, M1CTL2, M1CTL1);
				MONE(M2CTL_PORT, M2CTL2, M2CTL1);
				break;
			case Dir::Stop:
				MTWO(M1CTL_PORT, M1CTL1, M1CTL2);
				MTWO(M2CTL_PORT, M2CTL1, M2CTL2);
				break;
			}

			break;
		}
	}

	static inline Void Speed(const enum Select Which, const UChar Speed)
	{
		switch (Which) {
		case M1:
			OCR0A = Speed;
			return;
		case M2:
			OCR2A = Speed;
			return;
		case Both:
			OCR0A = OCR2A = Speed;
			return;
		}
	}

	static inline Void Init()
	{

		/* PWM Init
		 *
		 * PWMs on PB3 and PD7 - DDRs set to 1.
		 */
		PORTB |= (1<<PB3);
		PORTD |= (1<<PD7);
		DDRB |= (1<<PB3);
		DDRD |= (1<<PD7);

		/* Control bits direction */
		M1CTL_DDR |= (1<<M1CTL1) | (1<<M1CTL2);
		M2CTL_DDR |= (1<<M2CTL1) | (1<<M2CTL2);

		Dir(Both, Dir::Stop);

		/*** Motor 1 ***/

		/* Fast PWM mode in non-inverting mode;
		 * output set to 1 at BOTTOM (0x00),
		 * set to zero on match with OC0A value.
		 * Prescaler set to '1'. Output on pin OC0A = PB3 */
		TCCR0A = (1<<COM0A1) | (1<<WGM01) | (1<<WGM00);
		TCCR0B = (1<<CS00);

		/* Power to 0 */
		OCR0A = 0;


		/*** Motor 2 ***/
		/* Same configuration of PWM */
		TCCR2A = (1<<COM2A1) | (1<<WGM21) | (1<<WGM20);
		TCCR2B = (1<<CS20);

		/* Power to 0 */
		OCR2A = 0;
	}

} /* End of namespace */