/* (C) Eitan Sherer & Kenji Hashimoto
   ECE476 Final Project: Inverted Pendulum Balancer

   TEST CODE: Sensor Driver
 */

#include <Mega32.h>

#define encoder_CPR 1024

/*
 * Global Variable Declarations:
 */
unsigned int sensor_index;


// TEST Variables:
char prev;


/*
 * Function Declarations:
 */
void initialize();
char sensor_checkChAB();
char sensor_checkChA();
char sensor_checkChB();
void sensor_updateIndex(char);


// TEST FUNCTIONS:
void test_initialize();
void test_resolution();
void test_direction();



interrupt [EXT_INT0] void ext_int0_isr(void) {
	char channel;
	char test;

	// B leads A in clockwise rotation
	// A leads B in counterclockwise rotation

	channel = sensor_checkChAB();

	if (channel == 3) {
		channel = prev;
	}
	else if (channel == 0) {
		if (prev == 2) channel = 1;
		else channel = 2;
	}

	if (channel == 1) { // CCW rotation
		//PORTB = 0x0f;
		sensor_updateIndex(1);
		//sensor_index--;
	}
	else if (channel == 2) { // CW rotation
		//PORTB = 0xf0;
		sensor_updateIndex(0);
		//sensor_index++;
	}

	prev = channel;

	//test_resolution();

}






/*
 * TEST FUNCTION: test_intialize()
 */
void test_initialize() {
	DDRB = 0xff;
	PORTB = 0xff;

	DDRA = 0xff;
	PORTA = 0x00;
}

/*
 * TEST FUNCTION: test_resolution()
 */
void test_resolution() {
	if (sensor_index >= 1000) PORTB=0x00;
	if (sensor_index > 1100) PORTB=0xff;
}

/*
 * TEST FUNCTION: test_direction()
 */
void test_direction() {
	sensor_index &= 0x03ff;

	if (sensor_index > 5 && sensor_index <= 128) {
		// LED 3 ON
		PORTB = 0b11110111;
	}
	else if (sensor_index > 128 && sensor_index <= 256) {
		// LED 3&2 ON
		PORTB = 0b11110011;
	}
	else if (sensor_index > 256 && sensor_index <= 384) {
		// LED 3,2,&1 ON
		PORTB = 0b11110001;
	}
	else if (sensor_index > 384 && sensor_index <= 512) {
		// LED 3,2,1,&0 ON
		PORTB = 0b11110000;
	}
	else if (sensor_index > 512 && sensor_index <= 640) {
		// LED 7,6,5,&4 ON
		PORTB = 0b00001111;
	}
	else if (sensor_index > 640 && sensor_index <= 768) {
		// LED 6,5,&4 ON
		PORTB = 0b10001111;
	}
	else if (sensor_index > 768 && sensor_index <= 896) {
		// LED 5&4 ON
		PORTB = 0b11001111;
	}
	else if (sensor_index > 896 && sensor_index < 1019) {
		// LED 4 ON
		PORTB = 0b11101111;
	}
	else {
		PORTB = 0xff;
	}
}









/*
 * FUNCTION: main()
 */
void main(void) {
	test_initialize();
	initialize();

	while(1) {
		test_direction();
	}
}

/*
 * SENSOR FUNCTION: sensor_updateIndex(char dir)
 *		dir		0: Clockwise rotation
 *				1: Counter-clockwise rotation
 */
void sensor_updateIndex(char dir) {

	if (dir == 0) {
		sensor_index++;

		if (sensor_index == encoder_CPR) sensor_index = 0;
	}
	else {
		if (sensor_index == 0) sensor_index = encoder_CPR;

		sensor_index--;

	}
}

/*
 * SENSOR FUNCTION: sensor_checkChAB()
 *		Returns - 	1 - CCW (A leads B)
 *					2 - CW (B leads A)
 */
char sensor_checkChAB() {
	return ~PIND & 0x03;
}

/*
 * SENSOR FUNCTION: sensor_checkChA()
 *		Returns - 	0 if channel A LOW
 *					1 if channel A HIGH
 */
char sensor_checkChA() {
	if (~PIND & 0x01) return 1;
	else return 0;
}

/*
 * SENSOR FUNCTION: sensor_checkChB()
 *		Returns - 	0 if channel B LOW
 *					1 if channel B HIGH
 */
char sensor_checkChB() {
	if (~PIND & 0x02) return 1;
	else return 0;
}

/*
 * FUNCTION: initialize()
 */
void initialize() {

// Ports
	//D.0 Ch A input of Encoder
	//D.1 Ch B input of Encoder
	//D.2 INT0 input for External Interrupt 0
	DDRD = 0b00000000;

// External Interrupt
	/* MCUCR - MCU Control Register:
		[3,2] Interrupt Sense Control 1 = **
		[1,0] Interrupt Sense Control 0 = 10 (ISR on falling edge)
	 */
	MCUCR = 0b00000010;

	/* GICR - General Interrupt Control Register:
		[7] External Interrupt Request 1 Enable = 0
		[6] External Interrupt Request 0 Enable = 1
		[5] External Interrupt Request 2 Enable = 0
	 */
	GICR = 0b01000000;

// Global Variables
	sensor_index = 0;


// Enable Interrupts
	#asm ("sei");
}