#include <avr/io.h>
#include <avr/interrupt.h>
#include <string.h>



//***************DELAY CODE**************
//delay exactly 1 clock cycle
void delay_cycles(unsigned int timed) {
	while(timed>0)
		timed--;
	return;
}
//****************************************

//*************************BEEPER CODE**********************
//beep for cycle time at certain frequency
void beep (unsigned int cycles, unsigned int freq) {
	DDRB |= 1<<PB0;					//make PB0 an output
	long i;
	while (cycles > 0) {
		PORTB |= 1<<PB0;			// buzzer pin high
		for(i=0;i<freq;i++);
		PORTB &= !(1<<PB0);		// buzzer pin low
		for(i=0;i<freq;i++);
		cycles--;
	}
	return;
}
//**********************************************************


//*****required for ADC conversion (screws up everything!)
/*

//this came from the main:
//usart_putchar(ADCL);
//******end of ADC code
*/


//main function, begins with initialization
//NOTE servo actuation code does not work outside of main for some odd reason
int main(void) {

	#include <servoPositions.h>//this include ONLY works in main cause the compiler is gay like that
	unsigned int step=0;
	unsigned int i=0;
	
	//DDRn Data Direction Registers (0 for input vs 1 for output)
	//DDRC = 0xFF; //configure all ports for output
	//DDRC = 0x00; //configure all ports for input
	//pull up resistors generally good for inputs
	//PORTC = 0xFF; //turns pullup resistors on for all C ports
	//PORTC = 0x00; //turns pullup resistors off for all C ports
	//DDRD |= 1<<PD1;					// OR set PD1 to output
	//DDRC |= 1<<PC3;					// OR set PC3 to output
	
	//DDRC |= 0<<PC6;//set ADC6 to input
	//PORTC |= 1<<PC6;//turn on ADC6 pullup resistor
	
	
	
	/*
	//ADC init
	ADMUX = 0; // channel 0 //ADC init
    ADCSRA |= _BV(ADEN) | _BV(ADPS1) | _BV(ADPS2);//ADC init
	
	ADCSRA |= _BV(ADSC);//ADC Start Conversion
	
	//!(ADCSRA & _BV(ADIF));//ADC wait for conversion
	
	//ADCSRA |= _BV(ADIF);//ADC clear ADIF
	
	//if(ADCL>200)
		//beep(1500,28);
	*/
	
	
	
	//beep(25, 1000);
	//beep(250, 180);
	beep(1500, 25);
	//beep(4500, 12);
	//delay_cycles(35000);
	
	DDRC |= 0x7C;	//setup portC for output 0b1111100 (PC0-PC5)
	//DDRC |= 0x3	//setup portC for input 0b0000011 (PC6-PC7)
	//motor1table[4]=100;
		
	while(1) {
		i=0;
		
		//delay_cycles(60000);
		while(i<7) {
		//***************SERVO ACTUATION CODE***************
		//accepts a time in clock cycles to pulse each servo
		PORTC |= 1<<PC3; //servo 1
			delay_cycles(servo1table[step]);
		PORTC &= !(1<<PC3);
		
		PORTC |= 1<<PC2; //servo 2
			delay_cycles(servo2table[step]);
		PORTC &= !(1<<PC2);
			
		PORTC |= 1<<PC1; //servo 3
			delay_cycles(servo3table[step]);
		PORTC &= !(1<<PC1);
			
		PORTC |= 1<<PC0; //servo 4
			delay_cycles(servo4table[step]);
		PORTC &= !(1<<PC0);
			
		PORTC |= 1<<PC4; //servo 5
			delay_cycles(servo5table[step]);
		PORTC &= !(1<<PC4);
			
		PORTC |= 1<<PC5; //motor 1
			delay_cycles(motor1table[step]);
		PORTC &= !(1<<PC5);
		//**************************************************
		i++;
		}
		
		//in future, have it ++ only if the encoder detects a change in position
		//		-also have it reset to 0 if it detects restart mark
		step++;//step=0;
		
		if (step>=8) {
			//beep(1500, 25);//step=0;
			step=0;
			}
			
		/*
		PORTD |= 1<<PD1;		// LED on
		for(i=0;i<10000;i++);
		PORTD &= !(1<<PD1);		// LED off
		for(i=0;i<10000;i++);*/
	}
	return 0;
}