#include <pic.h>
#include <stdio.h>

__CONFIG(0x3F72);

#define timer_preset_value 1	// 1 ms, max 52
#define timer1_prescaler 0b11	//2^3=8
#define set_timer1L TMR1L = 65536L - (2500L * timer_preset_value) / (1 << timer1_prescaler);
#define set_timer1H TMR1H = (65536L - (2500L * timer_preset_value) / (1 << timer1_prescaler))>>8;
#define gecikme_ayarla(x) bekleme_suresi = x / timer_preset_value;

#define DUR 0
#define ILERI 1
#define GERI 2
#define SOLA_DON 3
#define SAGA_DON 4



#define filter_window_width 10

unsigned int bekleme_suresi = 0;

extern void init_comms(void);
extern void putch(unsigned char byte);
extern unsigned char getch(void);
extern unsigned char getche(void);

void main(void)
{	
	float sharp_on_mesafe,sharp_sag_mesafe,sharp_sol_mesafe;
	char yeni_karar,eski_karar = DUR;
	char sensor_durumu = 0;
		
	init_comms();
		
	// Kesme konfigürasyonları
	TMR1IF = 0;
	TMR1IE = 1;
	PEIE = 1;
	GIE = 1;
	//
	
	//TMR1 ayarları
	set_timer1L;
	set_timer1H;
	T1CON = (timer1_prescaler << 4) | 0x01;
	//
	
	//
	ADCON0 = 0b10000001;
	ADCON1 = 0b01000000;
	
	TRISA = 0b11111111;
	
	gecikme_ayarla(1000);
	while(bekleme_suresi);

	//Robotun on kodlari
	printf("LH1000 LM1400 LL1800 RH2000 RM1600 RL1200 VS3000 ");
	gecikme_ayarla(10);
	while(bekleme_suresi);
	
	printf("LF1700 LR1300 RF1300 RR1700 HT1500 ");
	gecikme_ayarla(10);
	while(bekleme_suresi);
	
	gecikme_ayarla(1000);
	while(bekleme_suresi);

	
	while(1)
	{
	
		//AN0 secildi, On Sharp	
		CHS1 = 0;
		CHS0 = 0;
					
		gecikme_ayarla(10);
		while(bekleme_suresi);
		
		GODONE = 1;
		while(GODONE);
		
		sharp_on_mesafe = ADRESH;

		
		//AN1 secildi, Sag Sharp	
		CHS1 = 0;
		CHS0 = 1;
					
		gecikme_ayarla(10);
		while(bekleme_suresi);
		
		GODONE = 1;
		while(GODONE);
		
		sharp_sag_mesafe = ADRESH;
		
		//AN2 secildi, Sol Sharp	
		CHS1 = 1;
		CHS0 = 0;
					
		gecikme_ayarla(10);
		while(bekleme_suresi);
		
		GODONE = 1;
		while(GODONE);
		
		sharp_sol_mesafe = ADRESH;
		
		if(sharp_on_mesafe > 13)
		{
			sensor_durumu = sensor_durumu | 0b010;
		}
		else
		{
			sensor_durumu = sensor_durumu & 0b101;
		}	
		
		if(sharp_sag_mesafe > 15)
		{
			sensor_durumu = sensor_durumu | 0b001;
		}
		else
		{
			sensor_durumu = sensor_durumu & 0b110;
		}
		
		if(sharp_sol_mesafe > 15)
		{
			sensor_durumu = sensor_durumu | 0b100;
		}
		else
		{
			sensor_durumu = sensor_durumu & 0b011;
		}
		
		switch(sensor_durumu)
		{
			case 0b000:
				yeni_karar=ILERI;
				break;
			case 0b010:
				yeni_karar = SAGA_DON;
				break;
			case 0b011:
				yeni_karar = SOLA_DON;
				break;
			case 0b111:
				yeni_karar = SAGA_DON;
				break;
			case 0b110:
				yeni_karar = SAGA_DON;
				break;
			case 0b101:
				yeni_karar=ILERI;
				break;
			case 0b001:
				yeni_karar=SOLA_DON;
				break;
			case 0b100:
				yeni_karar= SAGA_DON;
				break;
			default:
				yeni_karar=DUR;
				break;
		}		
		
		if(yeni_karar != eski_karar)
		{
			switch(yeni_karar)
			{
				case DUR:
					printf("XSTOP \r");
					gecikme_ayarla(10);
					while(bekleme_suresi);
					break;
				case ILERI:
					printf("XL100 XR100 XS100 ");
					gecikme_ayarla(500);
					while(bekleme_suresi);
					break;
				case GERI:
					printf("XL-100 XR-100 XS100 \r");
					gecikme_ayarla(500);
					while(bekleme_suresi);
					break;
				case SAGA_DON:
					printf("XL-100 XR100 XS100 \r");
					gecikme_ayarla(500);
					while(bekleme_suresi);
					break;
				case SOLA_DON:
					printf("XL100 XR-100 XS100 \r");
					gecikme_ayarla(500);
					while(bekleme_suresi);
					break;					
			}				
			
			eski_karar = yeni_karar;
		}
	}		
}

void interrupt master_kesme()
{
	if(TMR1IE)
	{
		if(TMR1IF)
		{			
			set_timer1L;
			set_timer1H;
			TMR1IF = 0;

			if(bekleme_suresi)
				bekleme_suresi--;
		}
	}
}