#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 ropedir DDRC
#define rope (1 << PC2)
#define led1 (1 << PB0)
#define led2 (1 << PB1)
#define led3 (1 << PB2)


#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 duty;
volatile uint16_t count = 0;
volatile uint16_t countLoop = 0;
volatile uint16_t angle = 0;

volatile uint32_t state = 0;

//char* pixels=

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

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

	/*TCCR1A = (1 << COM1A0); //toggle on Match
	TCCR1A |= (1 << COM1B0) | (1 << WGM12); //CTC
	
	TCCR1B = (1 << CS10);
	
	OCR1A = 50;
	OCR1B = 50;
	*/
}

void stop(){
	int stopDuty = 92;
	duty = stopDuty;
	/*uint8_t easing = 20;
	
	while(!(duty = stopDuty)){
		if(duty < stopDuty) duty++;
		else duty--;
		_delay_ms(easing);
	}*/
	return;
}

void up(){
	int upDuty = 97;
	uint8_t easing = 100;
	
	while(!(duty == upDuty)){
		if(duty < upDuty) duty++;
		_delay_ms(easing);
	}
	return;
}

void down(){
	duty = 90;
	return;
}

void flash(){
	set(PORTB, led2);
	_delay_ms(500);
	clear(PORTB, led2);
}

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

///////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(DDRB, led1);
	output(DDRB, led2);
	output(ropedir, rope);
	output(DDRD, TX);
}

void greenPix(){
	serialWrite(100 & 255U);
}

void redPix(){
	serialWrite(101 & 255U);
}

void bluePix(){
	serialWrite(102 & 255U);
}

void melody(){
	clear(PORTB, led1);
	serialWrite(101 & 255U);		
	_delay_ms(200);
	set(PORTB, led1);
	serialWrite(102 & 255U);
	_delay_ms(100);
	set(PORTB, led1);
	serialWrite(102 & 255U);
	_delay_ms(100);
	clear(PORTB, led1);
	serialWrite(101 & 255U);		
	_delay_ms(200);
	serialWrite(102 & 255U);
	_delay_ms(200);
	clear(PORTB, led1);
	serialWrite(101 & 255U);		
	_delay_ms(200);
	clear(PORTB, led1);
	serialWrite(101 & 255U);		
	_delay_ms(200);
	_delay_ms(1000);
}

int readAngle(){
	int _angle = 0;
	ADCSRA |= (1 << ADSC);
      //
      // wait for completion
      //
      while (ADCSRA & (1 << ADSC))
		;
	_angle = ADCL;
	_angle += (ADCH << 8);
	
	return _angle;
}

ISR(TIMER2_OVF_vect) {
	if(count < upCount){
		if(count == duty) clear(PORTC, rope);
		count++;
	}
	else if(count == upCount){
		count=0;
		set(PORTC, rope);
	}
}

void smiley(){
	switch(state){
		case 0:
			//wait for start point
			if(angle==310) state=1;
			break;
		case 1:
			if(angle==500) state=2;
			else if(angle == 300) greenPix();
			else greenPix();
			break;
		case 2:
			up();
			_delay_ms(6000);
			stop();
			state=3;
			break;
		case 3:
			if(angle > 400 && angle < 420) state=4;
			break;
		case 4:
			if(angle > 400 && angle < 420){
				redPix();
				redPix();
				_delay_ms(30);
				state=5;
			}
			break;
		case 5:	
			if(angle > 420 && angle < 440){
				redPix();
				redPix();
				_delay_ms(30);
				state=999;
			}
			break;
		case 999:
			flash();
			state=0;
		default:
			state=0;
			break;
	}
}

void sprayB(){
	serialWrite(103 & 255U);
}

void stopB(){
	serialWrite(104 & 255U);
}

void sprayR(){
	serialWrite(105 & 255U);
}

void stopR(){
	serialWrite(106 & 255U);
}

void sprayG(){
	serialWrite(107 & 255U);
}

void stopG(){
	serialWrite(108 & 255U);
}

void debug(){
	serialWrite('0' + angle / 100);
	serialWrite('0' + (angle / 10) % 10);
    serialWrite('0' + angle % 10);
	serialWrite(10);
	serialWrite(13);
}

void thecowsleg(){
	uint16_t prevAngle = 0;
	switch(state){
		case 0:
			//wait for start point
			if(angle<310){
				prevAngle = angle;
				state=1;
			}
			break;
		case 1:
			if(angle < 280) state=2;
			break;
		case 2:
			if(angle > 550 && angle < 610){
				sprayB();
				_delay_ms(70);
				state=3;
			}
			break;
		case 3:
			if(angle > 700 && angle < 770) state=4;
			stopB();
			state=999;			
			break;
		case 999:
			flash();
			up();
			_delay_ms(300);
			stop();
			state=0;
			break;
		default:
			state=0;
			break;
	}
}

void behemot(){
	switch(state){
		case 0:
			//wait for start point
			if(angle<310){
				state=1;
			}
		break;
		case 1:
			if(angle < 200) state=2;
		break;
		case 2:
			if(angle > 300 && angle < 320){
				sprayR();
				state=3;
			}
		break;
		case 3:
			if(angle > 400 && angle < 420){
				stopR();
				if(countLoop==6){
					state=4;
					countLoop=0;
				}
				else{
					countLoop=countLoop+1;
					state=999;
				}
				
			}
		break;
		
		//arm and head
		case 4:
			//wait for start point
			if(angle<310){
				state=5;
			}
		break;
		case 5:
			if(angle < 200) state=6;
		break;
		case 6: 
			if(angle > 280 && angle < 320){
				sprayR();
				state=7;
			}
		break;
		case 7:
			if(angle > 340 && angle < 360){
				sprayG();
				state=8;
			}
		break;			
		case 8:
			if(angle > 380 && angle < 400){
				stopR();
				state=9;
			}
		break;
		case 9:
			if(angle > 420 && angle < 440){
				stopG();
				if(countLoop==3){
					state=10;
					countLoop=0;
				}
				else{
					state=998;
					countLoop++;
				}
			}		
		break;
		
		//head
		case 10:
			//wait for start point
			if(angle<310){
				state=11;
			}
		break;
		case 11:
			if(angle < 200) state=12;
		break;
		case 12:
			if(angle > 250 && angle < 320){
				sprayR();
				state=13;
			}
		break;
		case 13:
			if(angle > 340 && angle < 350){
				sprayG();
				state=14;
			}
		break;
		case 14:
			if(angle > 400 && angle < 460){
				stopR();
				state=15;				
			}
		break;
		case 15:
			if(angle > 420 && angle < 440){
				stopG();
				if(countLoop==4){
					state=16;
					countLoop=0;
				}
				else{
					countLoop=countLoop+1;
					state=997;
				}
			}		
		break;
		
		//ears
		case 16:
			//wait for start point
			if(angle<310){
				state=17;
			}
		break;
		case 17:
			if(angle < 200) state=18;
		break;
		case 18:
			if(angle > 220 && angle < 240){
				sprayR();
				state=19;
			}
		break;
		case 19:
			if(angle > 280 && angle < 340){
				stopR();
				state = 20;
			}
		break;
		case 20:
			if(angle > 390 && angle < 410){
				sprayR();
				state =21;
			}
		break;			
		case 21:
			if(angle > 440 && angle < 490){
				stopR();
				if(countLoop==2){
					state=1000; //fin
					countLoop=0;
				}
				else{
					countLoop=countLoop+1;
					state=996;
				}
			}
		break;					
		case 999:
			flash();
			up();
			_delay_ms(200);
			stop();
			state=0;
		break;
		case 998:
			flash();
			up();
			_delay_ms(200);
			stop();
			state=4;
		break;
		case 997:
			flash();
			up();
			_delay_ms(200);
			stop();
			state=10;
		break;
		case 996:
			flash();
			up();
			_delay_ms(200);
			stop();
			state=16;
		break;
		case 1000:
			_delay_ms(100);
			flash2();
		break;
		default:
			state=0;
		break;
	}
}

void abstract(){
	switch(state){
		case 0:
			//wait for start point
			if(angle<310){
				state=1;
			}
		break;
		case 1:
			if(angle < 200) state=2;
		break;
		case 2:
			if(angle > 260 && angle < 280){
				sprayG();
				state=3;
			}
		break;
		case 3:
			if(angle > 300){
				stopG();
				state=4;
			}
		break;
		case 4:
		flash();
		up();
		_delay_ms(400);
		stop();
		state=0;		
		break;
	}
}

int main(void) {
	duty=93;
	count = 0;
	countLoop =0;
	
	pin_setup();
	init_pwm();
	
	//Set Serial connection up!
	/*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);
	
	//Set ADC up!
	ADCSRA = (1 << ADEN) | //enable ADC
		 (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // ADC prescaler @ /128

	//int inByte = 0;         // incoming serial byte
	angle=0;
	
	//initialize statemachine
	state=0;	
	
	//enable interrupts!
	sei();	
	
	/****************
		 SETUP
	****************/	
	flash();
	stop();

	/****************
	   MAIN LOOP
	****************/
   while (1) {		
	angle = readAngle();	
	abstract();
	}
			
}