#define F_CPU 1000000UL			//internal 1MHz osc

#include <avr/io.h>
#include <util/delay.h>



//PB0	D3R		common anode for Display Module 3 right digit
//PB2	D3L		common anode for Display Module 3 left digit
//PB6	T0		push button 0 (unused)
//PB7	T1		push button 1 (unused)

//PC0	D0L		common anode for Display Module 0 left digit
//PC1	D0R		common anode for Display Module 0 right digit
//PC2	D1L		common anode for Display Module 1 left digit
//PC3	D1R		common anode for Display Module 1 right digit
//PC4	D2L		common anode for Display Module 2 left digit
//PC5	D2R		common anode for Display Module 2 right digit
//PC6	Reset	Reset pin

//PD0	A		Segment A for all display modules
//PD1	B		Segment B for all display modules
//PD2	C		...
//PD3	D
//PD4	E
//PD5	F
//PD6	G
//PD7




uint8_t RightDisplayDigit1 = 0;
uint8_t RightDisplayDigit2 = 0;
uint8_t RightDisplayDigit3 = 0;
uint8_t RightDisplayDigit4 = 0;

uint8_t LeftDisplayDigit1 = 0;
uint8_t LeftDisplayDigit2 = 0;
uint8_t LeftDisplayDigit3 = 0;
uint8_t LeftDisplayDigit4 = 0;

#define DisplayOnTime 1000
#define DisplayOffTime 10


uint16_t TestCounter = 0;


void SetSegments(uint8_t States)
	{
		//set the A, B, C, D, E, F, G lines to specified values
		PORTD = PORTD & 0b10000000;
		PORTD = PORTD | (0b01111111 & (~States));
		
	}

void SetDisplay0R(bool StateToSet)
	{
		if(StateToSet == true)
			{
				//PC1 high
				PORTC |= 0b00000010;
			}
		if(StateToSet == false)
			{
				//PC1 low
				PORTC &= 0b11111101;
			}
	}

void SetDisplay0L(bool StateToSet)
{
	if(StateToSet == true)
	{
		//PC0 high
		PORTC |= 0b00000001;
	}
	if(StateToSet == false)
	{
		//PC0 low
		PORTC &= 0b11111110;
	}
}

void SetDisplay1R(bool StateToSet)
{
	if(StateToSet == true)
	{
		//PC3 high
		PORTC |= 0b00001000;
	}
	if(StateToSet == false)
	{
		//PC3 low
		PORTC &= 0b11110111;
	}
}

void SetDisplay1L(bool StateToSet)
{
	if(StateToSet == true)
	{
		//PC2 high
		PORTC |= 0b00000100;
	}
	if(StateToSet == false)
	{
		//PC2 low
		PORTC &= 0b11111011;
	}
}

void SetDisplay2R(bool StateToSet)
{
	if(StateToSet == true)
	{
		//PC5 high
		PORTC |= 0b00100000;
	}
	if(StateToSet == false)
	{
		//PC5 low
		PORTC &= 0b11011111;
	}
}

void SetDisplay2L(bool StateToSet)
{
	if(StateToSet == true)
	{
		//PC4 high
		PORTC |= 0b00010000;
	}
	if(StateToSet == false)
	{
		//PC4 low
		PORTC &= 0b11101111;
	}
}

void SetDisplay3R(bool StateToSet)
{
	if(StateToSet == true)
	{
		//PB0 high
		PORTB |= 0b00000001;
	}
	if(StateToSet == false)
	{
		//PB0 low
		PORTB &= 0b111111110;
	}
}

void SetDisplay3L(bool StateToSet)
{
	if(StateToSet == true)
	{
		//PB2 high
		PORTB |= 0b000000100;
	}
	if(StateToSet == false)
	{
		//PB2 low
		PORTB &= 0b111111011;
	}
}

void SetDisplayDigit(uint8_t DigitNumber)
	{
		if(DigitNumber == 0x00)
			{
				SetDisplay0R(true);
				SetDisplay0L(false);
				
				SetDisplay1R(false);
				SetDisplay1L(false);
				
				SetDisplay2R(false);
				SetDisplay2L(false);
				
				SetDisplay3R(false);
				SetDisplay3L(false);
			}
		if(DigitNumber == 0x01)
		{
			SetDisplay0R(false);
			SetDisplay0L(true);
			
			SetDisplay1R(false);
			SetDisplay1L(false);
			
			SetDisplay2R(false);
			SetDisplay2L(false);
			
			SetDisplay3R(false);
			SetDisplay3L(false);
		}
		if(DigitNumber == 0x02)
		{
			SetDisplay0R(false);
			SetDisplay0L(false);
			
			SetDisplay1R(true);
			SetDisplay1L(false);
			
			SetDisplay2R(false);
			SetDisplay2L(false);
			
			SetDisplay3R(false);
			SetDisplay3L(false);
		}
		if(DigitNumber == 0x03)
		{
			SetDisplay0R(false);
			SetDisplay0L(false);
			
			SetDisplay1R(false);
			SetDisplay1L(true);
			
			SetDisplay2R(false);
			SetDisplay2L(false);
			
			SetDisplay3R(false);
			SetDisplay3L(false);
		}
		if(DigitNumber == 0x04)
		{
			SetDisplay0R(false);
			SetDisplay0L(false);
			
			SetDisplay1R(false);
			SetDisplay1L(false);
			
			SetDisplay2R(true);
			SetDisplay2L(false);
			
			SetDisplay3R(false);
			SetDisplay3L(false);
		}
		if(DigitNumber == 0x05)
		{
			SetDisplay0R(false);
			SetDisplay0L(false);
			
			SetDisplay1R(false);
			SetDisplay1L(false);
			
			SetDisplay2R(false);
			SetDisplay2L(true);
			
			SetDisplay3R(false);
			SetDisplay3L(false);
		}
		if(DigitNumber == 0x06)
		{
			SetDisplay0R(false);
			SetDisplay0L(false);
			
			SetDisplay1R(false);
			SetDisplay1L(false);
			
			SetDisplay2R(false);
			SetDisplay2L(false);
			
			SetDisplay3R(true);
			SetDisplay3L(false);
		}
		if(DigitNumber == 0x07)
		{
			SetDisplay0R(false);
			SetDisplay0L(false);
			
			SetDisplay1R(false);
			SetDisplay1L(false);
			
			SetDisplay2R(false);
			SetDisplay2L(false);
			
			SetDisplay3R(false);
			SetDisplay3L(true);
		}
		if(DigitNumber == 0x08)
		{
			SetDisplay0R(false);
			SetDisplay0L(false);
			
			SetDisplay1R(false);
			SetDisplay1L(false);
			
			SetDisplay2R(false);
			SetDisplay2L(false);
			
			SetDisplay3R(false);
			SetDisplay3L(false);
		}
		if(DigitNumber == 0x09)
		{
			SetDisplay0R(true);
			SetDisplay0L(true);
			
			SetDisplay1R(true);
			SetDisplay1L(true);
			
			SetDisplay2R(true);
			SetDisplay2L(true);
			
			SetDisplay3R(true);
			SetDisplay3L(true);
		}
	}

//General functions

void Init (void)
	{
			DDRB = 0b00000101;
			PORTB = 0b00000101;
			
			DDRC = 0b00111111;
			PORTC = PORTC | 0b00111111;
			
			DDRD = 0b11111111;
			PORTD = 0b0000000;
	}

uint8_t GetNumberOfDigitsFromUint16(uint16_t Input)
	{
		//returns how many digits there are
		if(Input == 0)
			{
				return 0;
			}
		if(Input <= 9)
			{
				return 1;
			}
		if(Input <= 99)
			{
				return 2;
			}
		if(Input <= 999)
			{
				return 3;
			}
		if(Input <= 9999)
			{
				return 4;
			}
		else
			return 5;
	}

uint8_t DigitToSevenSegment (uint8_t InputDigit)
	{
		//Converts a number (0 to 9) to a seven segment representation of that number
		//The segment values are represented like this:
		//Bit0 -> A
		//Bit1 -> B
		//and so on, the unused bits are 0
		
		//If the input value is out of range, an E for ERROR will be returned
		
		if(InputDigit >= 10)
			{
				//out of range, return "E" for ERROR
				return 0b01111001;
			}
		switch (InputDigit)
			{
				case 0:
				return 0b00111111;
				break;
				
				case 1:
				return 0b00000110;
				break;
				
				case 2:
				return 0b01011011;
				break;
				
				case 3:
				return 0b01001111;
				break;
				
				case 4:
				return 0b01100110;
				break;
				
				case 5:
				return 0b01101101;
				break;
				
				case 6:
				return 0b01111101;
				break;
				
				case 7:
				return 0b00000111;
				break;
				
				case 8:
				return 0b01111111;
				break;
				
				case 9:
				return 0b01100111;
				break;
			}
	}

uint8_t ReturnDigitFromNumber(uint8_t DigitSelect, uint16_t Number)
	{
		uint8_t NumberOfDigits = GetNumberOfDigitsFromUint16(Number);
		if(DigitSelect > 5)
			{
				//Maximum number that can be stored in 16 bits is 65535 which has 5 digits
			}
			else
				{
					if(DigitSelect < NumberOfDigits)
						{
							//Trying to access a digit that is 0
							return 0x00;
						}
					
					uint8_t Digit1 = (Number % 10);			//DOES NOT WORK
					uint8_t Digit2 = 0;
					uint8_t Digit3 = 0;
					uint8_t Digit4 = 0;
					uint8_t Digit5 = 0;
					
					uint8_t ReturnValue = 0x00;
					
					switch (DigitSelect)
						{
							case 0:
							ReturnValue = 0x00;
							break;
							
							case 1:
							ReturnValue = Digit1;
							break;
							
							case 2:
							ReturnValue = Digit2;
							break;
							
							case 3:
							ReturnValue = Digit3;
							break;
							
							case 4:
							ReturnValue = Digit4;
							break;
							
							case 5:
							ReturnValue = Digit5;
							break;
						}
					if(ReturnValue >= 10)
						{
							//error, return 0x10 so the 7 segemnt display will show "E" for error
							return 0x10;
						}
						else
							{
								return ReturnValue;
							}
				}
	}

void DisplayValues(void)
	{
		RightDisplayDigit1 = ReturnDigitFromNumber(1,TestCounter);
		RightDisplayDigit2 = ReturnDigitFromNumber(2,TestCounter);
		RightDisplayDigit3 = ReturnDigitFromNumber(3,TestCounter);
		RightDisplayDigit4 = ReturnDigitFromNumber(4,TestCounter);
		
		
		
		//Display the values
		SetDisplayDigit(0);											//Enable display 0
		SetSegments(DigitToSevenSegment(RightDisplayDigit1));		//display the value of "RightDisplayDigit1" on display 0
		_delay_us(DisplayOnTime);
		SetDisplayDigit(8);											//All displays off
		_delay_us(DisplayOffTime);
		SetDisplayDigit(1);											//Enable display 1
		SetSegments(DigitToSevenSegment(RightDisplayDigit2));		//display the value of "RightDisplayDigit2" on display 1
		_delay_us(DisplayOnTime);
		SetDisplayDigit(8);											//All displays off
		_delay_us(DisplayOffTime);
		SetDisplayDigit(2);											//Enable display 2
		SetSegments(DigitToSevenSegment(RightDisplayDigit3));		//display the value of "RightDisplayDigit3" on display 2
		_delay_us(DisplayOnTime);
		SetDisplayDigit(8);											//All displays off
		_delay_us(DisplayOffTime);
		SetDisplayDigit(3);											//Enable display 3
		SetSegments(DigitToSevenSegment(RightDisplayDigit4));		//display the value of "RightDisplayDigit4" on display 3
		_delay_us(DisplayOnTime);
		SetDisplayDigit(8);											//All displays off
		_delay_us(DisplayOffTime);		
		SetDisplayDigit(4);											//Enable display 4
		SetSegments(DigitToSevenSegment(LeftDisplayDigit1));		//display the value of "LeftDisplayDigit1" on display 4
		_delay_us(DisplayOnTime);
		SetDisplayDigit(8);											//All displays off
		_delay_us(DisplayOffTime);
		SetDisplayDigit(5);											//Enable display 5
		SetSegments(DigitToSevenSegment(LeftDisplayDigit2));		//display the value of "LeftDisplayDigit2" on display 5
		_delay_us(DisplayOnTime);
		SetDisplayDigit(8);											//All displays off
		_delay_us(DisplayOffTime);
		SetDisplayDigit(6);											//Enable display 6
		SetSegments(DigitToSevenSegment(LeftDisplayDigit3));		//display the value of "LeftDisplayDigit3" on display 6
		_delay_us(DisplayOnTime);
		SetDisplayDigit(8);											//All displays off
		_delay_us(DisplayOffTime);
		SetDisplayDigit(7);											//Enable display 7
		SetSegments(DigitToSevenSegment(LeftDisplayDigit4));		//display the value of "LeftDisplayDigit5" on display 7
		_delay_us(DisplayOnTime);
		SetDisplayDigit(8);											//All displays off
		_delay_us(DisplayOffTime);
		
	}





int main(void)
	{
		Init();
		_delay_ms(1000);
		
		uint16_t LoopCounter = 0;
		
		while(1)
			{
				//The main loop	
				if(LoopCounter >= 100)
					{
						LoopCounter = 0;
						TestCounter++;
					}
				
				//Display the values
				DisplayValues();
				LoopCounter++;
				
			}
	}