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

   TEST CODE: Sensor Driver
 */

#include <Mega32.h>


/*
 * Global Variable Declarations:
 */
unsigned char sensor_index;
unsigned char sensor_channel;

// TEST Variables:


/*
 * Function Declarations:
 */
void initialize();

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



interrupt [EXT_INT0] void ext_int0_isr(void) {
	sensor_channel ^= 0x01;
	if (sensor_channel == 1) {
		sensor_index--;
	}
}

interrupt [EXT_INT1] void ext_int1_isr(void) {
	sensor_channel ^= 0x02;
	if (sensor_channel == 2) {
		sensor_index++;
	}
}






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

/*
 * 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 >= 0 && sensor_index < 28) {
		// LED 7,6,5,&4 ON
		PORTB = 0b00001111;
	}
	else if (sensor_index >= 28 && sensor_index < 57) {
		// LED 6,5,&4 ON
		PORTB = 0b10001111;
	}
	else if (sensor_index >= 57 && sensor_index < 85) {
		// LED 5&4 ON
		PORTB = 0b11001111;
	}
	else if (sensor_index >= 85 && sensor_index < 114) {
		// LED 4 ON
		PORTB = 0b11101111;
	}
	else if (sensor_index >= 114 && sensor_index < 142) {
		// LED OFF
		PORTB = 0xff;
	}
	else if (sensor_index >= 142 && sensor_index < 171) {
		// LED 3 ON
		PORTB = 0b11110111;
	}
	else if (sensor_index >= 171 && sensor_index < 191) {
		// LED 3&2 ON
		PORTB = 0b11110011;
	}
	else if (sensor_index >= 191 && sensor_index < 228) {
		// LED 3,2,&1 ON
		PORTB = 0b11110001;
	}
	else if (sensor_index >= 228 && sensor_index < 256) {
		// LED 3,2,1,&0 ON
		PORTB = 0b11110000;
	}
}











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

	while(1) {
		test_direction();
	}
}

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

// Ports
	//D.2 Ch A input of Encoder --> triggers INT0
	//D.3 Ch B input of Encoder --> triggers INT1
	DDRD = 0b00000000;

// External Interrupt
	/* MCUCR - MCU Control Register:
		[3,2] Interrupt Sense Control 1 = 01 (ISR on logical change)
		[1,0] Interrupt Sense Control 0 = 01 (ISR on logical change)
	 */
	MCUCR = 0b00000101;

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

// Global Variables
	sensor_index = 128;


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