/* De olika biblioteken som ska inkluderas */

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

/* Definiering av globala variabler och konstanter */

#define F_CPU 16000000UL // klockhastigheten för delay.h
#define SPEED_MIN 50 // hastighetsbegränsing
#define SPEED_MAX 100 // hastighetsbegränsing
#define ACTION_TIME 2000 // tid för vanliga handlingar milisec
#define MOOD_IMPROVEMENT_TIME 600 // tiden för horror att sjunka med ett i milisec
#define CAUTION_TIME 3000 // tiden för försiktighet efter högt ljud i milisec
#define LOOK_FORWARD 150 // servo fram i mikrosekunder/10
#define LOOK_LEFT 120 // servo vänster i mikrosekunder/10 
#define LOOK_RIGHT 180 // servo höger i mikrosekunder/10 
#define RANGESENSOR 0 // ADMUX address till avståndssensorn
#define LDR_LEFT // ADMUX och extern mux addres till vänster LDR
#define LDR_LEFT_CENTER // ADMUX och extern mux addres till vänster-fram LDR
#define LDR_CENTER // ADMUX och extern mux addres till fram LDR
#define LDR_RIGHT_CENTER // ADMUX och extern mux addres till höger-fram LDR
#define LDR_RIGHT // ADMUX och extern mux addres till höger LDR
#define LDR_TOP // ADMUX och extern mux addres till rygg LDR
#define HAPPY_BEEP 1 // typ av ljud
#define SCARED_BEEP 2 // typ av lijud
#define NORMAL_BEEP 3 // typ av lijud
#define FOOD_BEEP 4 // typ av lijud
#define CLOSE_RANGE 400 // värde för range sensorn
#define SERVO_TRAVEL 700 // tid för servot att flytta på sig i milisec
#define MOTOR_STOP 0 // motorbroms
#define MOTOR_FORWARD 1 // båda motorer fram
#define MOTOR_BACKWARD 2 // båda motorer bak
#define MOTOR_ROTATE_LEFT 3 // vänster motor bak höger motor fram
#define MOTOR_ROTATE_RIGHT 4 // vänster motor fram höger motor bak
#define HORROR_LIMIT 70 // gränsvärde för flykt
#define HUNGER_LIMIT 80 // gränsvärde för matsökande
#define FULLNESS_LIMIT 20 // gränsvärde för glädjerus 
#define HAPPINESS_LIMIT 30 // gränsvärde för glädjerus
#define FOODSOURCE 900 // gränsvärde för matkälla
#define SHELTER 300 // gränsvärde för skydd
#define NORMAL 0 // sinnestillstånd
#define HAPPY 1 // sinnestillstånd
#define HUNGRY 2 // sinnestillstånd
#define SCARED 3 // sinnestillstånd
#define AVOID 4 // sinnestillstånd

volatile unsigned char motoRight, motoLeft, servoGoal, led[24], motorProportion[2], mood, beepType;
volatile unsigned int microSecX10, pwmMicroSecX10,servoInPlace, switch_ADC, hunger, horror, ldr[6], ldr_lowrange[6], range[3],
cautionTime, ledTime, moodTime, servoInPlace, b[8];
volatile long miliSec, actionTime, beepTime, cautiousTime;

void motorDir(char);
void avoid(void);

/* Initieringsfunktioner */

void io_init(){ // IO riktning

	DDRA |= (1<<PA4)|(1<<PA5)|(1<<PA6); 
	DDRB |= (1<<PB0)|(1<<PB3)|(1<<PB4)|(1<<PB5)|(1<<PB6)|(1<<PB7);
	DDRD |= (1<<PD4)|(1<<PD5)|(1<<PD7);
}

void adc_init(){ // ADC initialisering ch:0 = range, ch:1 = LDR/MUX, ch:2 = hunger/pot

    ADMUX |= (1<<REFS0); // AREF = AVcc
    ADCSRA |= (1<<ADEN)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); //ADC Enable och prescaler 128. 16000000/128 = 125000
	//int ch = 1;
	//ADMUX = (ADMUX & 0xF8)|ch;     // clears the bottom 3 bits before ORing
}

void pwm_init(){ // PWM på timer0 och timer2

	TCCR0 |= (1<<WGM00)|(1<<COM01)|(1<<WGM01)|(1<<CS02); // initiera TCCR0 med prescaler 256. Borde ge ~62,5 khz uppdatering till motorn (för högt?)
	TCCR2 |= (1<<WGM00)|(1<<COM01)|(1<<WGM01)|(1<<CS22)|(1<<CS21); // initiera TCCR2 med prescaler 256. Borde ge ~62,5 khz uppdatering till motorn (för högt?)
}

void timer1_init(){ // timer1 initiering för compare

	
    TCCR1B |= (1<<WGM12)|(1 << CS10); // prescaler 0 och CTC mode
    TCNT1 = 0; // initiera countern
    OCR1A = 160; // testvärde
    TIMSK |= (1 << OCIE1A); // aktiverar compare interrupt
}

void ext_int_init(){ // initialisering av externa interrupts

	//MCUCR |= (1<<ISC01)|(1<<ISC00);	// INT0 inställd på rising edge
	MCUCR |= (1<<ISC11)|(1<<ISC10);	// INT1 inställd på rising edge
	//GICR |= (1<<INT0); // INT0 enable
	GICR |= (1<<INT1); // INT0 enable
}

void i2c_init(){ // initialisering av i2c
	TWSR = 0x01; // prescaler 4
	TWBR = 0x08;   // 8 
}

/* avbrottsrutiner */

ISR (TIMER1_COMPA_vect){ // den här ISR körs när timer1 compare blir sann

    microSecX10++; 
	if (microSecX10 > 99){
		
		miliSec++;
		microSecX10 = 0;
		
		if (beepTime > 0)
			beepTime--;
		
		if (actionTime > 0)
			actionTime--;
	
		if (cautiousTime > 0)
			cautiousTime--;
	
		if (ledTime > 0)
			ledTime--;
		
		if (moodTime == 0)
			moodTime = MOOD_IMPROVEMENT_TIME;

		else 
			moodTime--;
		
		if (servoInPlace > 0)
			servoInPlace--;
	
	}	

	pwmMicroSecX10++; 
	if (pwmMicroSecX10 >= 1999){    //20ms upprepning
		pwmMicroSecX10 = 0;    
		PORTD |= (1<<PD5);
	}
	if(pwmMicroSecX10 >= servoGoal)  //150 = 1,5ms  
       	PORTD &= ~(1<<PD5);
}

ISR (INT0_vect){ // den här ISR körs när det kommer höga ljud

    /*if (horror > HORROR_LIMIT)
		horror = 100;
	
	else if (cautionTime == 0){
		motorDir(MOTOR_STOP);
		cautionTime = CAUTION_TIME;
	}
	
	else{
		horror = 100; 
		cautionTime = 0;
	}*/
}

ISR (INT1_vect){ // den här ISR körs när switchar aktiveras

    if (PIND & (1<<PD0)){ // bumper
		PORTB |= (1<<PB0);
		_delay_ms (1000);
		PORTB &= ~(1<<PB0);
		_delay_ms (1000);
		PORTB |= (1<<PB0);
		_delay_ms (1000);
		PORTB &= ~(1<<PB0);
	}
	
	else{ // knapp på ryggen
		PORTB |= (1<<PB0);
			_delay_ms (1500);
			PORTB &= ~(1<<PB0);
			
		
		/*motorDir(MOTOR_STOP);
		actionTime = 2000;		
		horror -= 50;
		beepType = HAPPY_BEEP;*/
	}
			
}

/* funktioner */

void motorDir(char dir){ // motorernas riktning/stop

	switch (dir){
	
		case MOTOR_STOP: // broms
			PORTB |= (1<<PB4)|(1<<PB5)|(1<<PB6)|(1<<PB7);
			break;
		
		case MOTOR_FORWARD: // fram
			PORTB &= ~(1<<PB5)|~(1<<PB7);
			PORTB |= (1<<PB4)|(1<<PB6);
			break;
		
		case MOTOR_BACKWARD: // bak	
			PORTB &= ~(1<<PB4)|~(1<<PB6);
			PORTB |= (1<<PB5)|(1<<PB7);
			break;
			
		case MOTOR_ROTATE_LEFT: // rotation vänster
			PORTB &= ~(1<<PB4)|~(1<<PB7);
			PORTB |= (1<<PB5)|(1<<PB6);
			break;
		
		case MOTOR_ROTATE_RIGHT: // rotation höger
			PORTB &= ~(1<<PB5)|~(1<<PB6);
			PORTB |= (1<<PB4)|(1<<PB7);
			break;
	}
}	

void run_ADC(){ // starta AD-omvandling

	ADCSRA |= (1<<ADSC);
}

void beep(){ // gör ljud	
	
	static unsigned char n;
		
	if (beepTime == 0){
	
		if (n == 0 || n == 2 || n == 4 || n == 6){ // slår på buzzern
			PORTB |= (1<<PB0);	
			beepTime = b[n];
			n++;
		}
	
		else if (n == 1 || n == 3 || n == 5){ // stänger av buzzern
			PORTB &= ~(1<<PB0);
			beepTime = b[n];
			n++;
		}	

		if (n != 0 && b[n] == 0){ // stänger av buzzern
			PORTB &= ~(1<<PB0);
			b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7] = 0, 0, 0, 0, 0, 0, 0, 0;	
			beepType = 0;
		}
	    
	}

	if (b[0] == 0){
		
		switch (beepType){
		
			case HAPPY_BEEP:
				b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7] =  300, 300, 300, 300, 300, 300, 1000, 0; // tid i milisec. på, av, på, av, på, av, på, sista alltid noll
				n = 0;
				break;

			case SCARED_BEEP:
				b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7] = 500, 500, 500, 500, 500, 0, 0, 0; // tid i milisec. på, av, på, av, på, av, på, sista alltid noll
				break;

			case NORMAL_BEEP:
				b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7] = 500, 300, 500, 0, 0, 0, 0, 0; // tid i milisec. på, av, på, av, på, av, på, sista alltid noll
				break;

			case FOOD_BEEP:
				b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7] = 400, 400, 400, 1000, 400, 400, 400, 0; // tid i milisec. på, av, på, av, på, av, på, sista alltid noll
				break;
		}
	}
}

void updateLedPulse(){
	static unsigned char i;
	
	if (led[i] == 254)	// updaterar ledlistan
		i++;
	
	if (ledTime == 0 && i == 0){
    	led[i]++;
		led[23]--;
		led[22]--;
		led[1]++;
		led[2]++; 
	}

	else if (ledTime == 0 && i == 1){
    	led[i]++;
		led[0]--;
		led[23]--;
		led[2]++;
		led[3]++; 
	}
	
	else if (ledTime == 0 && i == 22){
    	led[i]++;
		led[21]--;
		led[20]--;
		led[23]++;
		led[0]++; 
	}

	else if (ledTime == 0 && i == 23){
    	led[i]++;
		led[22]--;
		led[21]--;
		led[0]++;
		led[1]++; 
	}
	
	else if (ledTime == 0 && i > 1 && i < 22){
    	led[i]++;
		led[i-1]--;
		led[i-2]--;
		led[i+1]++;
		led[i+2]++; 
	}
	
	if (ledTime == 0) // räkna ut tid till nästa byte. mellan 1 och 10 milisec
		ledTime = hunger * 9 / 1024 + 1; 

}


void updateLed(){ // uppdaterar lysdioderna över i2c	
	
	char n;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);	// start
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 8)
			break;
	}	

	TWDR = 0b11000010|TW_WRITE; // chipp 1

	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 18)
			break;
	}
	
	if ((TWSR & 0xF8) != 18)
		return;

	TWDR = 0b10000001;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;

	TWDR = led[0];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[1];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[2];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[3];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[4];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[5];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[6];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[7];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[8];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[9];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[10];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[11];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b11111111;

	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;

	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b11111100;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b11111100;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b11111100;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b11111100;

	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO); // stop

	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);	// start
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 8)
			break;
	}	

	TWDR = 0b11000100|TW_WRITE; // chipp 2 

	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 18)
			break;
	}
	
	if ((TWSR & 0xF8) != 18)
		return;

	TWDR = 0b10000001;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;

	TWDR = led[0];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[1];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[2];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[3];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[4];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[5];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[6];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[7];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[8];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[9];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[10];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = led[11];
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b11111111;

	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b00000000;

	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b11111100;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b11111100;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b11111100;
	
	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWDR = 0b11111100;

	for (n = 0; n <= 20; n++){
		TWCR = (1<<TWINT)|(1<<TWEN);	// send
		while (!(TWCR & (1 << TWINT))); // Wait for TWINT flag set in TWCR Register
		if ((TWSR & 0xF8) == 28)
			break;
	}
	
	if ((TWSR & 0xF8) != 28)
		return;
	
	TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO); // stop

}				
	
void input(){ 
		
	static char adcAdress = 0;
	static char	servoPos = 0; 
	
	
	if (!(ADCSRA & (0<<ADSC) && servoInPlace == 0)){ // om servo är på plats och ADC är redo switcha muxen till denna, kör ADC:n och sätt adcAdress = 7
	
		ADMUX = (ADMUX & 0xF8)|RANGESENSOR;
		run_ADC();  
		adcAdress = 7;
	}
	
	if (!(ADCSRA & (1<<ADSC))){ // om ADC:n är färdig spara värdet på rätt plats
		
		switch (adcAdress){
			
			case 0:
				ldr[0] = ADC;
				break;
								
			case 1:
				ldr[1] = ADC;
				break;

			case 2:
				ldr[2] = ADC;
				break;

			case 3:
				ldr[3] = ADC;
				break;

			case 4:
				ldr[4] = ADC;
				break;

			case 5:
				ldr[5] = ADC;
				break;

			case 6:
				hunger = ADC; // pot
				break;

			case 7: // spara värdet från range sensorn efter ADC:n är färdig, sätt ny servo destination och sätt tid att vänta för servots rörelse
				
				if (servoPos == 0){ // fram
					
					range[1] = ADC;
					servoGoal = LOOK_LEFT;
					servoInPlace = SERVO_TRAVEL;
					servoPos = 1;
				}
				
				else if (servoPos == 1){ // vänster
					
					range[0] = ADC;
					servoGoal = LOOK_FORWARD;
					servoInPlace = SERVO_TRAVEL;
					servoPos = 2;					
				}
				
				else if (servoPos == 2){ // fram
					
					range[1] = ADC;
					servoGoal = LOOK_RIGHT;
					servoInPlace = SERVO_TRAVEL;
					servoPos = 3;
				}
				
				else{ // höger
				
					range[2] = ADC;
					servoGoal = LOOK_FORWARD;
					servoInPlace = SERVO_TRAVEL;
					servoPos = 0;
				}
				break;	
		}
		
		if (adcAdress < 7) // 6 x LDR + potentiometer rotation
			adcAdress++;
		
		else
			adcAdress = 0;
	   
		switch (adcAdress){

			case 0:
				ADMUX = (ADMUX & 0b11111000)|(1<<PA1);
				PORTA = (PORTA & 0b11000111)|(1<<PA3);
				break;
									
			case 1:
				ADMUX = (ADMUX & 0b11111000)|(1<<PA1);
				PORTA = (PORTA & 0b11000111)|(1<<PA5)|(1<<PA6);
				break;

			case 2:
				ADMUX = (ADMUX & 0b11111000)|(1<<PA1);
				PORTA = (PORTA & 0b11000111)|(1<<PA4);
				break;

			case 3:
				ADMUX = (ADMUX & 0b11111000)|(1<<PA1);
				PORTA = (PORTA & 0b11000111)|(1<<PA5);
				break;

			case 4:
				ADMUX = (ADMUX & 0b11111000)|(1<<PA1);
				PORTA = (PORTA & 0b11000111);
				break;

			case 5:
				ADMUX = (ADMUX & 0b11111000)|(1<<PA1);
				PORTA = (PORTA & 0b11000111)|(1<<PA3)|(1<<PA5);
				break;

			case 6:
				ADMUX = (ADMUX & 0xF8)|(1<<PA2);
				break;

		}
		run_ADC();
	}
}

void normal(){ // kör runt normalt
	
	static unsigned char look;
	
	motorDir(MOTOR_FORWARD);
	
	if (actionTime == 0); // tid har paserat
		
		if (look != 0);
			
			if (look == 1 || look == 4){
				motorDir(MOTOR_ROTATE_LEFT);
				look++;
				actionTime = ACTION_TIME / 4;
			}			

			else if (look == 2 || look == 3){
				motorDir(MOTOR_ROTATE_RIGHT);
				look++;
				actionTime = ACTION_TIME / 4;
			}
			else
				look = 0;		

		
		if (miliSec%2 == 0){ // kör fram
			motorProportion[0], motorProportion[1] = 3, 3; // motor proprtioner fram
			actionTime = ACTION_TIME;
		}

		else if (miliSec%3 == 0){ // stanna och titta runt
			motorDir(MOTOR_STOP);
			look++;
			actionTime = ACTION_TIME / 4;
			beepType = NORMAL_BEEP;
		}
		
		else if (miliSec%5 == 0){ // sväng till höger
			motorProportion[0], motorProportion[1] = 3, 2;	// motor proportioner höger					
			actionTime = ACTION_TIME;
		}

		else{ // sväng till vänster
			motorProportion[0], motorProportion[1] = 2, 3; // motor proportioner vänster		
			actionTime = ACTION_TIME;
		}
}

void happy(){ // kör runt glatt
	
	static char look;
	
	motorDir(MOTOR_FORWARD);
	
	if (actionTime == 0); // tid har paserat
		
		if (look != 0);
			
			if (look == 1 || look == 4){
				motorDir(MOTOR_ROTATE_LEFT);
				look++;
				actionTime = ACTION_TIME / 4;
			}			

			else if (look == 2 || look == 3){
				motorDir(MOTOR_ROTATE_RIGHT);
				look++;
				actionTime = ACTION_TIME / 4;
			}
			
			else
				look = 0;		

		
		if (miliSec%2 == 0){ // kör fram
			motorProportion[0], motorProportion[1] = 3, 3; // motor proportioner fram
			actionTime = ACTION_TIME;
		}

		else if (miliSec%3 == 0){ // stanna och titta runt
			motorDir(MOTOR_STOP);
			look++;
			actionTime = ACTION_TIME / 4;
			beepType = HAPPY_BEEP;
		}
		
		else if (miliSec%5 == 0){ // sväng till höger
			motorProportion[0], motorProportion[1] = 3, 2; // motor proportioner höger	
			actionTime = ACTION_TIME;
		}

		else if (miliSec%7 == 0){ // snurra vänster
			motorDir(MOTOR_ROTATE_LEFT);
			beep(HAPPY_BEEP);			
			actionTime = ACTION_TIME;
			beepType = HAPPY_BEEP;
		}
		
		else if (miliSec%9 == 0){ // snurra höger
			motorDir(MOTOR_ROTATE_LEFT);
			beep(HAPPY_BEEP);
			actionTime = ACTION_TIME;
			beepType = HAPPY_BEEP;
		}
				
		else{ // sväng till vänster
			motorProportion[0], motorProportion[1] = 2, 3; // motor proportioner vänster	
			actionTime = ACTION_TIME;
		}

}

void hungry(){ // leta ljus					
	
	if (ldr[5] >= FOODSOURCE){
		motorDir(MOTOR_STOP);
		beepType = FOOD_BEEP;
		return;
	} 
	
	motorDir(MOTOR_FORWARD);
		
	if (ldr_lowrange[0] > ldr_lowrange[2] || ldr_lowrange[0] > ldr_lowrange[2])	
		motorProportion[0], motorProportion[1] = 1, 3;//motor prop vänster					
	
	else if (ldr_lowrange[3] > ldr_lowrange[2] || ldr_lowrange[4] > ldr_lowrange[2])
		motorProportion[0], motorProportion[1] = 3, 1;//motor prop höger						
		
	else
		motorProportion[0], motorProportion[1] = 3, 3;//motor prop fram						

}

void scared(){ // leta mörker		
	
	if (ldr[5] <= SHELTER){
		motorDir(MOTOR_STOP);
		beepType = SCARED_BEEP;
		return;
	} 
	
	motorDir(MOTOR_FORWARD);
		
	if (ldr_lowrange[0] < ldr_lowrange[2] || ldr_lowrange[0] < ldr_lowrange[2])	
		motorProportion[0], motorProportion[1] = 1, 3; //motor proportioner vänster					
	
	else if (ldr_lowrange[3] < ldr_lowrange[2] || ldr_lowrange[4] < ldr_lowrange[2])
		motorProportion[0], motorProportion[1] = 3, 1; //motor proportioner höger					
		
	else
		motorProportion[0], motorProportion[1] = 3, 3; //motor proportioner fram			

}

void avoid(){ // backa och kör åt annat håll
	
	static char tic;
	
	if (actionTime == 0 && tic == 0){
		motorDir(MOTOR_BACKWARD);
		actionTime = ACTION_TIME / 2; 
		tic++;
	}
	
	if (actionTime != 0)
		return;

	if (range[0] >= CLOSE_RANGE){ 
		
		if (range[1] >= CLOSE_RANGE){ 	
				
			if (range[2] >= CLOSE_RANGE){ // om alla tre rangevärden är >= CLOSE_RANGE
				motorDir(MOTOR_BACKWARD);
				motorProportion[0], motorProportion[1] = 3, 3; // motor proportioner fram
				actionTime = ACTION_TIME / 2;
			}
			
			else{ // om vänster och fram rangevärde är >= CLOSE_RANGE
			motorDir(MOTOR_ROTATE_RIGHT);
			actionTime = ACTION_TIME / 2;
			}
			
		}

		else if (range[2] >= CLOSE_RANGE){ // om vänster och höger rangevärde är >= CLOSE_RANGE
			motorDir(MOTOR_BACKWARD);
			motorProportion[0], motorProportion[1] = 3, 3; // motor proportioner fram
			actionTime = ACTION_TIME / 2;
		}

		else{ // om vänster rangevärde är >= CLOSE_RANGE
			motorDir(MOTOR_FORWARD);
			actionTime = ACTION_TIME / 2;
			motorProportion[0], motorProportion[1] = 3, 1; //motor proportioner höger					
		}
	}

	else if (range[2] >= CLOSE_RANGE){ 
		
		if (range[1] >= CLOSE_RANGE){ // om höger och fram rangevärde är >= CLOSE_RANGE	
		motorDir(MOTOR_ROTATE_LEFT);
		actionTime = ACTION_TIME / 2;
		}
			
		else{ // om höger rangevärde är >= CLOSE_RANGE
		motorDir(MOTOR_FORWARD);
		actionTime = ACTION_TIME / 2;
		motorProportion[0], motorProportion[1] = 1, 3;//motor proportioner vänster					
		}

	}

	else if (range[1] >= CLOSE_RANGE){ // om fram rangevärde är >= CLOSE_RANGE	
	motorDir(MOTOR_BACKWARD);
	motorProportion[0], motorProportion[1] = 3, 3; // motor proportioner fram
	actionTime = ACTION_TIME / 2;
	}	
	
	else{
		tic = 0;
		mood = NORMAL;
	}
}

void decision(){

	static int i;	

	if (actionTime != 0 && (PIND & (1<<PD2))) 
		actionTime = CAUTION_TIME;
	
	if (mood == AVOID)
		return;
	
	if (horror > HORROR_LIMIT){
		mood = SCARED;
	 	for (i = 0; i < 6; i++)
			ldr_lowrange[i] = ldr[i] / 8;
	}	

	else if (hunger > HUNGER_LIMIT){
		mood = HUNGRY;
		for (i = 0; i < 6; i++)
			ldr_lowrange[i] = ldr[i] / 8;
	}	

	else if (hunger < FULLNESS_LIMIT && horror < HAPPINESS_LIMIT)
		mood = HAPPY;

	else 
		mood = NORMAL;
}

void action(){	

	//beep();
	//updateLedPulse();
	//updateLed();		
	
	if (cautionTime != 0) // väntar på mer oljud
		return;
	
	switch (mood){ // kör rätt beteende									
	
		case NORMAL:
			normal();
			break;

		case HAPPY:
			happy();
			break;
			
		case HUNGRY:
			hungry();
			break;

		case SCARED:
			scared();
			break;

		case AVOID:
			avoid();
			break;
	}
	
	
	motoRight = (hunger * (SPEED_MAX - SPEED_MIN) / 1023 + SPEED_MIN) * motorProportion[1] / 3;
	motoLeft = (hunger * (SPEED_MAX - SPEED_MIN) / 1023 + SPEED_MIN) * motorProportion[0] / 3;
	OCR0 = motoRight; // uppdaterar PWM duty
	OCR2 = motoLeft; // uppdaterar PWM duty
	
}

int main(){

	servoGoal = LOOK_FORWARD;
	
	io_init(); // io-riktning
	ext_int_init(); // externa interrupts
	timer1_init(); // startar timer1
	pwm_init(); // startar PWM 
	adc_init();
	//i2c_init();
	sei();	// aktiverar globala interrupts
	//mood = NORMAL;
	//beepType = NORMAL_BEEP;
	//led[0], led[1], led[2], led[3], led[4], led[5] = 254, 127, 0, 0, 0, 0; 
	//led[6], led[7], led[8], led[9], led[10], led[11] = 0, 0, 0, 0, 0, 0; 
	//led[12], led[13], led[14], led[15], led[16], led[17] = 0, 0, 0, 0, 0, 0; 
	//led[18], led[19], led[20], led[21], led[22], led[23] = 0, 0, 0, 0, 0, 127;
	//motorProportion[0], motorProportion[1] = 3, 3;		
	volatile unsigned char n = 0;
	while(1){
		
		ADMUX = (ADMUX & 0xF8)|RANGESENSOR;     // clears the bottom 3 bits before ORing
		run_ADC();
		_delay_ms (300);
		range[1] = ADC;
		
		if (range[1] < CLOSE_RANGE){
			PORTB |= (1<<PB0);
			_delay_ms (500);
			PORTB &= ~(1<<PB0);
			range[1] = 0;
		}

		//ADMUX = (ADMUX & 0xF8)|ch;     // clears the bottom 3 bits before ORing
		
		//ADMUX = (ADMUX & 0xF8)|ch;     // clears the bottom 3 bits before ORing		 
		//input();
		//decision();
		//action();
		if (n < 3){
		
			PORTB &= ~(1<<PB4)|~(1<<PB7);
			PORTB |= (1<<PB5)|(1<<PB6);
			OCR0 = 250; // uppdaterar PWM duty
			OCR2 = 250; // uppdaterar PWM duty
			_delay_ms (5000); 	
			PORTB &= ~(1<<PB5)|~(1<<PB7);
			PORTB |= (1<<PB4)|(1<<PB6);
			OCR0 = 250; // uppdaterar PWM duty
			OCR2 = 250; // uppdaterar PWM duty
			_delay_ms (5000);
			PORTB &= ~(1<<PB4)|~(1<<PB6);
			PORTB |= (1<<PB5)|(1<<PB7);
			OCR0 = 250; // uppdaterar PWM duty
			OCR2 = 250; // uppdaterar PWM duty
			_delay_ms (5000);
			n++;
		}
	}
}