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

#define output(directions,pin) (directions |= pin) // set port direction for output
#define set(port,pin) (port |= pin) // set port pin
#define clear(port,pin) (port &= (~pin)) // clear port pin
#define pin_test(pins,pin) (pins & pin) // test for port pin
#define bit_test(byte,bit) (byte & (1 << bit)) // test for bit set

#define green (1 << PD7)
#define red (1 << PD5)
#define blue (1 << PA7)
#define led1 (1 << PB3)

#define TX (1 << PD1)
#define RX (1 << PD0)

#define F_CPU 16000000	// 16 MHz oscillator.
#define BaudRate 9600
#define MYUBRR (F_CPU / 16 / BaudRate ) - 1

#define upCount 1257

volatile uint16_t greenAngle;
volatile uint16_t redAngle;
volatile uint16_t blueAngle;

uint16_t count = 0;

// Servo init
/////////////

void init_pwm() {
	TIMSK = (1<< TOIE2); //enables oveflow interrupt
    TCCR2 = (1<< CS20); //1 prescale, CTC, toggle on reach
}

void sprayG(){
	greenAngle = 103;
	return;
}

void sprayR(){
	redAngle = 102;
	return;
}
void sprayB(){
	blueAngle = 99;
	return;
}

void stopG(){
	greenAngle = 127;
	return;
}

void stopR(){
	redAngle = 125;
	return;
}
void stopB(){
	blueAngle = 105;
	return;
}

///////Serial communication init

void delayLong()
{
	unsigned int delayvar;
	delayvar = 0; 
	while (delayvar <=  65500U)		
	{ 
		asm("nop");  
		delayvar++;
	} 
}

unsigned char serialCheckRxComplete(void)
{
	return( UCSRA & _BV(RXC)) ;		// nonzero if serial data is available to read.
}

unsigned char serialCheckTxReady(void)
{
	return( UCSRA & _BV(UDRE) ) ;		// nonzero if transmit register is ready to receive new data.
}

unsigned char serialRead(void)
{
	while (serialCheckRxComplete() == 0)		// While data is NOT available to read
	{;;} 
	return UDR;
}

void serialWrite(unsigned char DataOut)
{
	while (serialCheckTxReady() == 0)		// while NOT ready to transmit 
	{;;} 
	UDR = DataOut;
}

//<<End setting up serial connection


void establishContact() {
	while (serialCheckRxComplete() == 0) { 
		serialWrite('A');
	//	serialWrite(65U);
		delayLong();
		delayLong();
		delayLong();
		delayLong();
		delayLong();
		delayLong();
		delayLong();
	}
}

void pin_setup() {	
	output(DDRD, green);
	output(DDRD, red);
	output(DDRA, blue);
	output(DDRB, led1);
	output(DDRD, TX);
	
	set(PORTB, led1);
}

ISR(TIMER2_OVF_vect) {
	if(count < upCount){
		if(count == greenAngle) clear(PORTD, green);
		else if(count == redAngle) clear(PORTD, red);
		else if(count == blueAngle) clear(PORTA, blue);
		count++;
	}
	else if(count == upCount){
		count=0;
		set(PORTD, green);
		set(PORTD, red);
		set(PORTA, blue);
	}

}

void flash(){
	clear(PORTB, led1);
	_delay_ms(100);
	set(PORTB, led1);
	_delay_ms(100);
}

int main(void) {
	count = 0;
	greenAngle=137;
	blueAngle=141;
	redAngle=135;
	
	pin_setup();	
	init_pwm();
	
	stopR();
	stopG();
	stopB();
	
	/*Set baud rate */ 
	UBRRH = (unsigned char)(MYUBRR>>8); 
	UBRRL = (unsigned char) MYUBRR;
	/* Enable receiver and transmitter   */
	UCSRB = (1<<RXEN) | (1<<TXEN); 
	/* Frame format: 8data, No parity, 1stop bit */ 
//	UCSRC = (1<<UCSZ1) | (1<<UCSZ0); 
	int inByte = 0;         // incoming serial byte
	
	sei();
	
	_delay_ms(500);
	
	clear(PORTB, led1);
	sprayR();
	sprayG();
	sprayB();
	_delay_ms(200);
	set(PORTB, led1);
	stopR();
	stopG();
	stopB();
	
	flash();
	flash();
	
	_delay_ms(1000);
	
	//establishContact();  // send a byte to establish contact until Processing responds 
	
   //
   // main loop
   //
   while (1) {
		if (serialCheckRxComplete()) {
			inByte = serialRead();
						
			if(inByte == 100){
				//spray green
				sprayG();
				_delay_ms(30);
				stopG();
			}
			else if(inByte == 101){
				//spray red
				sprayR();
				_delay_ms(30);
				stopR();
			} 
			else if(inByte == 102){
				//spray blue
				sprayB();
				_delay_ms(30);
				stopB();
			}
			else if(inByte == 103){
				//spray blue
				sprayB();
			}
			else if(inByte == 104){
				//spray blue
				stopB();
			}
			else if(inByte == 105){
				//spray blue
				sprayR();
			}
			else if(inByte == 106){
				//spray blue
				stopR();
			}
			else if(inByte == 107){
				//spray blue
				sprayG();
			}
			else if(inByte == 108){
				//spray blue
				stopG();
			}			
			
		}
	}
}