/*
 * GccApplication1.c
 *
 * Created: 2022-05-09 17:06:12
 * Author : ba3187ho-s
 */ 


#define F_CPU 1000000UL
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdlib.h>
#include <stdio.h>
#include "lcd.h"

#define D0 eS_PORTB0
#define D1 eS_PORTB1
#define D2 eS_PORTB2
#define D3 eS_PORTB3
#define D4 eS_PORTB4
#define D5 eS_PORTB5
#define D6 eS_PORTD5
#define D7 eS_PORTD6
#define RS eS_PORTD2
#define EN eS_PORTD4


//variabler
volatile uint8_t noDigits = 0;
volatile uint16_t ms = 0;
volatile uint8_t sec = 0;
volatile uint8_t min = 0;
volatile uint16_t msSaved = 0;
volatile uint8_t secSaved = 0;
volatile uint8_t minSaved = 0;
char buffer_ms[16];
char buffer_sec[16];
char buffer_min[16];
volatile uint8_t timeCounter = 0;


// Variabler, tilldelade med vilken bitplats de har.
uint16_t  AIN1 = 0; // PD0
uint16_t  AIN2 = 6; // PB6 - PWM
uint16_t  BIN1 = 1; // PD1
uint16_t  BIN2 = 7; // PB7 - PWM

// Variabler, tilldelade vilket värde som ska vara för PWM - (ska vara volotile?)
uint16_t pulseNBR1 = 5 ; // Värde för pulse - PWM
uint16_t pulseNBR2 = 5 ; // Värde för pulse - PWM
uint16_t periodNBR = 10; // Värde för perioden - PWM
volatile uint16_t nbr,nbr1,nbr2;

//variabler för riktningsändring
uint8_t straight =0;
uint8_t left =0;
uint8_t right =0;


// Variabler för avläsning av sensorvärde (0 / 1)
uint8_t SF1 = 0;
uint8_t SF2 = 0;
uint8_t SF3 = 0;
uint8_t SL2 = 0;
uint8_t SL1 = 0;
uint8_t SR2 = 0;
uint8_t SR1 = 0;
uint16_t avaragevalue = 0;
uint8_t counter = 0;
volatile uint16_t nbr;

// Vilken sida bilen kör av från tejpen // Höger 1 , Vänster 0
char side;
uint8_t state = 0;

// Variabler för om knappen har tryckts ner
volatile uint8_t greenButton = 0;
volatile uint8_t redButton = 0;
volatile uint8_t lengthOfButtonGREEN = 0;
volatile uint8_t lengthOfButtonRED = 0;

//Variabel för att målet är nått
uint8_t goal_is_reached = 0;

void init_stopwatch()
{
	TCCR1B |= (1<<WGM12)|(1<<CS11);   //prescaler på denna timern till 8 division factor
	TCNT1 = 0;
	OCR1A = 1250;  //betyder 10 millisekunder (1.000.000hz(cpu) / 8(prescale) = 125.000 per sekund(timerns hastighet), därefter delar på 100 (100st 10ms finns på en sekund.))
	TIMSK1 |= (1<<OCIE1A);
	sei();
}

ISR (TIMER1_COMPA_vect)
{
	ms+=10;

	if(ms>999){
		ms=0;
		sec++;
		if(sec>59){
			sec=0;
			min++;
		}
	}
}

// Placera koden för initiering här, kommer köras en gång
void setUp (){
	
	// Tilldelning av input/output av olika pins
	DDRA |= 0x00; // Sensorer 0b00000000 (alla ingångar på ADC)
	DDRB |= 0xFF; 
	DDRD |= 0b01111111; // Dual H-Bridge 0b00000011

	// Initierar PWM
	TCCR3A |= 0b10100010;
	TCCR3B |= 0b00011100;
}

// Tilldela puls-värde, nbr1 för Motor 1, nbr 2 för Motor 2
void set_pulse(int nbr1,int nbr2){
OCR3A = nbr1;
OCR3B = nbr2;
TCNT3 = 0;
}

void set_period (int nbr){
ICR3 = nbr;
}

//Alla motorfunktioner
void driveForward (){
// Skicka värde genom AIN1, AIN2, BIN1, BIN2, till Dual H-Bridge

set_period(18);
set_pulse(8, 8);
// AIN - Motor 1
PORTD |= (1<<AIN1);
// BIN - Motor 2
PORTD |= (1<<BIN1);
}
void stopEngine(){
// Ändra värdet av pulsen till 0
set_period(65000);
set_pulse(0, 0);
// AIN - Motor 1
PORTD &= (0<<AIN1);
// BIN - Motor 2
PORTD &= (0<<BIN1);
}
void adjustToLeft (){
// Saktar in vänstra motorn
set_period(18);
set_pulse(10, 8);
// AIN - Motor 1
PORTD |= (1<<AIN1);
// BIN - Motor 2
PORTD |= (1<<BIN1);
}		
void adjustToRight (){
set_period(18);
set_pulse(8, 10);
// AIN - Motor 1
PORTD |= (1<<AIN1);
// BIN - Motor 2
PORTD |= (1<<BIN1);
}
void sharpTurnRight (){
set_period(18);
set_pulse(8, 18);

// AIN - Motor 1
PORTD |= (1<<AIN1);
// BIN - Motor 2
PORTD |= (1<<BIN1);
}
void sharpTurnLeft (){
set_period(18);
set_pulse(18, 8);
// AIN - Motor 1
PORTD |= (1<<AIN1);
// BIN - Motor 2
PORTD |= (1<<BIN1);
}
void turnAroundRight (){
	set_period(18);
	set_pulse(8, 8);

	// AIN - Motor 1
	PORTD = 0b00000001;
	// BIN - Motor 2
	//PORTD &= (0<<BIN1);
}
void turnAroundLeft (){
	set_period(18);
	set_pulse(8, 8);
	// AIN - Motor 1
	PORTD = 0b00000010;
	// BIN - Motor 2
	//PORTD |= (1<<BIN1);
}

//init_adc, initierar ADCn
void init_adc()
{
  DIDR0 = 0b11111111; //Registret inaktiverar digital avläsning på ADC0-7 (DIGITAL INPUT DISABLE). Se sidan 339 i datablad för ATMEGA1284
  ADMUX = 0b00100000; //Att sätta ADLAR-biten till 1 ger oss en vänsterjusterad 10-bitars konvertering från ADCn, då kan vi använda oss av de 8 mest signifikanta bitarna i register ADCH, det räcker för oss med 8-bitars upplösning och vår avläsningsbyte blir smidig. 
  ADCSRA = 0b10000011; //Bit 7:Aktiverar ADCn. Bit2:0 ; prescalar med division 8ggr.
}

//read_ADC, gör en analog till digital konvertering och returnerar 8-bitars värdet
unsigned char read_ADC()
{
	uint16_t adcvalue;
	ADCSRA = ADCSRA | 0b01000000;			      // påbörja en ad conversion
	while( (ADSC == 1) );							// vänta på ADIF biten att  bli en etta, då är dataregistret uppdaterat.
	adcvalue = ADCH;				              // sätter värdet adcvalue till de 8 bitarna
	return adcvalue;							 //  returnerar värdet på ADCn
}

//sensorvalue, används för att sätta gränsvärdet och avkoda för hur de 8 bitarna från adcn ska tolkas som antingen 1 eller 0.
unsigned char sensorvalue()        
{
  counter = 0;
  avaragevalue = 0;
  while (counter < 50){
	  avaragevalue += read_ADC();
	  counter++;
  }
  avaragevalue = avaragevalue/50;

  
  if ( avaragevalue < 190 ){      //här sätter vi threshold value mellan 0a och 1a på sensorn
    return 0;
  }
  else {
    return 1;
  }
} 

//read_sensors, cykel för att köra runt på de olika sensorerna igenom ADC och lagra deras värden. Används i mainprogram i någon form av loop.
void read_sensors ()        //
{
  ADMUX = 0b00100000;
 _delay_us(1);                  //ser till att channel ADC0 är först ut i cykeln
  SL1 = sensorvalue();				 //sätter variabeln till sensorvärdet.         
  ADMUX = 0b00100001;
 _delay_us(1); 							//ADC2
  SL2 = sensorvalue();          
  ADMUX = 0b00100011;
 _delay_us(1);          
  SF1 = sensorvalue();
  ADMUX = 0b00100100;
 _delay_us(1);           
  SF2 = sensorvalue();         
  ADMUX = 0b00100101;         
 _delay_us(1); 
  SF3 = sensorvalue();       
  ADMUX = 0b00100111;
 _delay_us(1); 
  SR1 = sensorvalue();				//ADC7      
  ADMUX = 0b00100110;
  _delay_us(1);
  SR2 = sensorvalue();
    
    // Om alla sensorer visar 1, då har vi nått målet, stoppa motorerna
    if (SL1==1 && SL2==1 && SR1==1 && SR2==1 && SF1==1 && SF2==1 && SF3==1)
    {
	    stopEngine();
	    redButton = 1;
		greenButton= 0;
		goal_is_reached = 1;

    }
    
  }

void turningLeft(){
  // funktion gör att bilen kan rotera vänster, 
  
	  if( (SF1==1) || (SF2==1) || (SF3==1)){
		  while((SF1==1) || (SF2==1) || (SF3==1)){
			  read_sensors();
			  turnAroundLeft();
		  }
		  while(SF2 == 0){
			  read_sensors();
			  turnAroundLeft();
		  }
	  }
	  else if((SF1==0) && (SF2==0) && (SF3==0)){
		  while(SF2 ==0 ){
			  read_sensors();
			  turnAroundLeft();
		  }
	  }
  }

void turningRight(){
  // funktionnen gör att bilen kan rotera till höger 
	  if((SF1==1) || (SF2==1) || (SF3==1)){
		  while((SF1==1) || (SF2==1) || (SF3==1)){
			  read_sensors();
			  turnAroundRight();
		  }
		  while(SF2 == 0){
			  read_sensors();
			  turnAroundRight();
		  }
	  }
	  else if((SF1==0) && (SF2==0) && (SF3==0)){
		  while(SF2 == 0 ){
			  read_sensors();
			  turnAroundRight();
		  }
	  }
  }
// turningLeft och turningRigh använder SF2 sensor som en referenspunkt för hur lång bilen ska rotera  
void current_state()
{
	if (state == 0){
		if ( (SF3==1) && (SF2==1) && (SF1==1) ){
			state = 0;
		}
		else {
			state = 1;
		}
	}
	
	if (state == 1){
		if ( ((SF3==1) && (SF1==1)) || ((SF3==0) && (SF1==0))	){
				state = 1;
			}
			else if ((SF3==1) && (SF2==0) && (SF1==0))	{
				state = 2;
			}
			else if ((SF3==0) && (SF2==0) && (SF1==1))	{
				state = 3;
		}
	}
		
	if (state==2){
			if((SF3==1) && (SF1==0)){
				state=2;
			}
			else if ((SF2==1) && (SF1==1)){
				state=1;
			}
			else if ((SF3==0) && (SF2==0) && (SF1==0)){
				state=4;
			}	
		}

		if (state==3){
			if((SF3==0) && (SF1==1)){
				state=3;
			}
			else if ((SF3==1) && (SF2==1)){
				state=1;
			}
			else if ((SF3==0) && (SF2==0) && (SF1==0)){
				state=5;
			}
		}
		
		if (state==4){
			if ((SF3==0) && (SF2==0) && (SF1==0)){
						state=4;
					}
					else if(SF3==1){
						state=2;
					} 
					
		}
		
		if (state==5){
					if ((SF3==0) && (SF2==0) && (SF1==0)){
						state=5;
					}
					else if(SF1==1){
						state=3;
					}
				}
	}


//declaration
uint8_t straight1 = 0;
uint8_t right1 = 0;
uint8_t left1 = 0;
uint8_t sum = 0;
// sum, left1, right1, straight1 används för att skilja av två alternativ ( bara 1 riktning att ta  
// eller det finns fler än 1 riktning att ta)

// vid fallet där det finns fler än 1 riktning att ta, används variabler straight, left, right 
// för att bilen kan välja en tillgänglig riktning. 
int PossibilityForDirectionChange() {
	// returnera 0 om det finns ingen möjlighet till riktningsändring, bilen
	// behåller sin riktning. returnera 1 om det finns möjlighet 
	// till riktningsändring
	if ((SL1 == 1) || (SR1 == 1)) {
		return 1;
	}
	return 0;
}	

void LookingforDirection() {
	if (SL1 == 1) {
		// sensor SL1 returnerar 1, möjlighet till vänstersväng
		left = 1;
    left1 =1;

		while
		(((SL1 == 1) || (SL2 == 1)) && goal_is_reached==0) {
			read_sensors();
			driveForward();
			if (SR1 == 1) {
				// sensor SR3 returnerar 1 --> möjlighet till högersväng
				right = 3;
        right1 =1;
			}
		}
		// om sensor SF2 returnerar 1 --> möjlighet till att köra rakt fram
		
		if ((SF2 == 1)||(SF1 == 1)||(SF3 == 1)) {
			straight = 2;
      straight1 =1;
		}
	}

	else if (SR1 == 1) {
		right = 3;
    right1 =1;
		// sensor SR3 returnerar 1, möjlighet till högersväng
		while
		(((SR2 == 1) || (SR1 == 1)) && goal_is_reached==0) {
			read_sensors();
			driveForward();
			if (SL1 == 1) {
				left = 1;
        left1 =1;
				// om sensor SL1 returnerar 1 --> möjlighet till vänstersväng
			}
		}
		if ((SF2 == 1)||(SF1 == 1)||(SF3 == 1)) {
			// om sensor SF2 returnerar 1 --> möjlighet till att köra rakt fram
			straight = 2;
      straight1 =1;
		}
	}
	sum = straight1+left1+right1;
  // sum = 1 om det finns bara en riktning att ta 
  // sum > 1 om det finns fler än en riktning att ta 
}

void ChangingDirection() {
	// In denna funktion, ska man checka om sum = 1 eller sum >1
	// sum = 1, bara en riktning att ta --> bilen ska ta den riktning som är tillgänglig 
	// sum > 1, en random funktion drar igång, genererar ett tal från 1 till 3.
	stopEngine();

  if ((sum == 1) && (goal_is_reached==0)){
    if(left == 1){
      turningLeft();
    }
    else if (right == 3){
      turningRight();
    }
    else {
      driveForward();
    }
  }
  else if((sum > 1) && (goal_is_reached==0)){
    // rand funktion:
    uint8_t number = (ms % 3)+1;
 
  while((number != left)&&(number != right )&&(number != straight)){
    number = (number % 3) +1;
    }
    // funktion som genererar ett tal från 1 till 3
    // tanken här är att randfunktion genererar talet från 1 till 3, vilket i sin tur ska matcha 
    //(lika med)EN AV VARIABLER (left, right, straight), 
    //om det matchar -->bilen svänga till den riktning
    //om det matchar inte -->genererar ny tal igen.
    
  if(left == number && goal_is_reached==0){
    turningLeft();
    }
  else if (right == number && goal_is_reached==0){
    turningRight();
    }
  else if (straight == number && goal_is_reached==0){
    driveForward();
    }
  }

if (goal_is_reached==0)
{
	driveForward();
}



// variabler för riktningar nollställas för de kommande riktningsändringar
straight = 0;
left = 0;
right = 0;
sum = 0;
left1 =0;
right1= 0;
straight1 =0;  
}

//Läser av GRÖN knapp
void readButtonGREEN (){

	// Kollar om knappen har blivit nedryckt
	if ((PIND & 0b10000000) == 0b10000000){
		//För varje gång som den identifierar knappen nedtryckt¨, öka värdet på lengthOfButton
		lengthOfButtonGREEN++;
	}
	
	// Om knappen varit nedtryckt länge än en viss tid, identifiera detta som att knappen definitivt har blivit tryckt
	if(lengthOfButtonGREEN>3){
		lengthOfButtonRED = 0;
		lengthOfButtonGREEN = 0;
		greenButton = 1;
		redButton = 0;
		goal_is_reached = 0;
		timeCounter = 0;
		ms = 0;
		sec = 0;
		min = 0;
		Lcd8_Clear();
	}
	
}

//Läser av RÖD knapp
void readButtonRED (){

	if ((PINC & 0b00000001) == 0b00000001){
		//För varje gång som den identifierar knappen nedtryckt¨, öka värdet på lengthOfButton
		lengthOfButtonRED++;
	}
	
	if(lengthOfButtonRED>3){
		lengthOfButtonRED = 0;
		lengthOfButtonGREEN = 0;
		greenButton = 0;
		redButton = 1;
		stopEngine();
		goal_is_reached = 1;
	}

}

void countDigit(int n){
	if ((n/10) == 0){
		noDigits = 1;
	}
	else if (n/10 >= 1){
		noDigits = 2;
	}
}

void writeTime(){
	itoa(ms, buffer_ms, 10);
	itoa(min, buffer_min, 10);
	itoa(sec, buffer_sec, 10);
	Lcd8_Set_Cursor(1,1);
	Lcd8_Write_String("Time: ");
	Lcd8_Set_Cursor(1,8);
	Lcd8_Write_String(buffer_min);
	
	countDigit(sec);
	
	if(noDigits == 1){
		Lcd8_Set_Cursor(1,9);
		Lcd8_Write_String(":0");
		Lcd8_Set_Cursor(1,11);

		Lcd8_Write_String(buffer_sec);
	}
	else if(noDigits > 1)	{
		Lcd8_Set_Cursor(1,9);
		Lcd8_Write_String(":");
		Lcd8_Set_Cursor(1,10);
		Lcd8_Write_String(buffer_sec);
	}
	Lcd8_Set_Cursor(1, 12);
	Lcd8_Write_String(":");
	Lcd8_Set_Cursor(1, 13);
	Lcd8_Write_String(buffer_ms);
	}

void writeSavedTime(){
		
			msSaved = ms;
			secSaved = sec;
			minSaved = min;
		
		if (timeCounter==0)
		{
			
		
		itoa(msSaved, buffer_ms, 10);
		itoa(minSaved, buffer_min, 10);
		itoa(secSaved, buffer_sec, 10);
		Lcd8_Set_Cursor(2,1);
		Lcd8_Write_String("Time: ");
		Lcd8_Set_Cursor(2,8);
		Lcd8_Write_String(buffer_min);
		
		countDigit(sec);
		
		if(noDigits == 1){
			Lcd8_Set_Cursor(2,9);
			Lcd8_Write_String(":0");
			Lcd8_Set_Cursor(2,11);

			Lcd8_Write_String(buffer_sec);
		}
		else if(noDigits > 1)	{
			Lcd8_Set_Cursor(2,9);
			Lcd8_Write_String(":");
			Lcd8_Set_Cursor(2,10);
			Lcd8_Write_String(buffer_sec);
		}
		Lcd8_Set_Cursor(2, 12);
		Lcd8_Write_String(":");
		Lcd8_Set_Cursor(2, 13);
		Lcd8_Write_String(buffer_ms);
		
		timeCounter++;
		}
	}

int main(void)
{
	
// Initierar, körs en gång.
setUp();
stopEngine();
init_adc();
init_stopwatch();
Lcd8_Init();
Lcd8_Set_Cursor(1,1);

while (1) {
	// Läs av om någon knapp trycks
	readButtonGREEN();
	readButtonRED();

	
	//Läs av sensor
	read_sensors();
	current_state();
	if (redButton==1){
		writeSavedTime();
	}

	
	if((greenButton==1) && (redButton==0)){
		if(!PossibilityForDirectionChange()){
		   switch (state) {
				case 0:
				stopEngine();
				break;

				case 1:
				driveForward();
				break;

				case 2:
				adjustToLeft();
				break;

				case 3:
				adjustToRight();
				break;

				case 4:
				turnAroundLeft();
				break;

				case 5:
				turnAroundRight();
				break;
				
				default:
				turnAroundLeft();
			}
		}
		if(PossibilityForDirectionChange()){
			LookingforDirection();
			ChangingDirection();
			
			}		
		
		}
	} 

}