unsigned rx1;
unsigned i,j,right,left,k;
unsigned long counter1,counter2,counter3;
char txt[6] = "mikro";
unsigned adcRes;
const unsigned int right_forward=0b00000001;
const unsigned int right_reverse=0b00000000;
const unsigned int left_forward=0b00000011;
const unsigned int left_reverse=0b00000010;
const unsigned int motor_delay=50;
const unsigned int IR_threshold=14;
const unsigned int IR_wall_threshold=10;
signed int right_motor,left_motor;
unsigned int behavior,last_behavior;

void motor_init()
{
     /*Uart1_Write_Char(0x80);
     Uart1_Wait_Tx();
     Uart1_Write_Char(0x02);
     Uart1_Wait_Tx();
     Uart1_Write_Char(0);
     Uart1_Wait_Tx();*/
     Delay_100ms();
     right_motor=0;
     left_motor=0;
     PORTCbits.RC14=0;
     Delay_100ms();
     Delay_ms(10);
     LATCbits.LATC14=1;
     PORTCbits.RC14=1;
}

void increase_right()
{
     if(right_motor<-7)
     {
         right_motor+=7;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_reverse);
         Uart1_Wait_Tx();
         Uart1_Write_Char(abs(right_motor));
         Uart1_Wait_Tx();
     }
     else if(right_motor<0)
     {
         right_motor=0;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_reverse);
         Uart1_Wait_Tx();
         Uart1_Write_Char(abs(right_motor));
         Uart1_Wait_Tx();
     }
     else if(right_motor<14)
     {
         right_motor+=7;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_motor);
         Uart1_Wait_Tx();
     }
     else if(right_motor<21)
     {
         right_motor=21;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_motor);
         Uart1_Wait_Tx();
     }
     else
     {
         left_motor=0;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_motor);
         Uart1_Wait_Tx();
     }
}
void decrease_right()
{
     if(right_motor>7)
     {
         right_motor-=7;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_motor);
         Uart1_Wait_Tx();
     }
     else if(right_motor>0)
     {
         right_motor=0;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_motor);
         Uart1_Wait_Tx();
     }
     else if(right_motor>-14)
     {
         right_motor-=7;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_reverse);
         Uart1_Wait_Tx();
         Uart1_Write_Char(abs(right_motor));
         Uart1_Wait_Tx();
     }
     else if(right_motor>-21)
     {
         right_motor=-21;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(right_reverse);
         Uart1_Wait_Tx();
         Uart1_Write_Char(abs(right_motor));
         Uart1_Wait_Tx();
     }
     else
     {
         left_motor=0;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_motor);
         Uart1_Wait_Tx();
     }
}

void increase_left()
{
     if(left_motor<-7)
     {
         left_motor+=7;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_reverse);
         Uart1_Wait_Tx();
         Uart1_Write_Char(abs(left_motor));
         Uart1_Wait_Tx();
     }
     else if(left_motor<0)
     {
         left_motor=0;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_motor);
         Uart1_Wait_Tx();
     }
     else if(left_motor<14)
     {
         left_motor+=7;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_motor);
         Uart1_Wait_Tx();
     }
     else if(left_motor<21)
     {
         left_motor=21;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_motor);
         Uart1_Wait_Tx();
     }
     else
     {
         left_motor=0;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_motor);
         Uart1_Wait_Tx();
     }
}
void decrease_left()
{
     if(left_motor>7)
     {
         left_motor-=7;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(abs(left_motor));
         Uart1_Wait_Tx();
     }
     else if(left_motor>0)
     {
         left_motor=0;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_motor);
         Uart1_Wait_Tx();
     }
     else if(left_motor>-14)
     {
         left_motor-=7;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_reverse);
         Uart1_Wait_Tx();
         Uart1_Write_Char(abs(left_motor));
         Uart1_Wait_Tx();
     }
     else if(left_motor>-21)
     {
         left_motor=-21;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_reverse);
         Uart1_Wait_Tx();
         Uart1_Write_Char(abs(left_motor));
         Uart1_Wait_Tx();
     }
     else
     {
         left_motor=0;
         Uart1_Write_Char(0x80);
         Uart1_Wait_Tx();
         Uart1_Write_Char(0x00);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_forward);
         Uart1_Wait_Tx();
         Uart1_Write_Char(left_motor);
         Uart1_Wait_Tx();
     }
}

void stop()
{
	while(right_motor<0)
	{
		increase_right();
		Delay_ms(motor_delay);
	}
	while(right_motor>0)
	{
		decrease_right();
		Delay_ms(motor_delay);
	}
	while(left_motor<0)
	{
		increase_left();
		Delay_ms(motor_delay);
	}
	while(left_motor>0)
	{
		decrease_left();
		Delay_ms(motor_delay);
	}
}
void turn_right()
{
	while(right_motor<21)
	{
		increase_right();
		Delay_ms(motor_delay);
	}
        while(left_motor<21)
	{
		increase_left();
		Delay_ms(motor_delay);
	}
}
void turn_left()
{
	while(left_motor>-21)
	{
		decrease_left();
		Delay_ms(motor_delay);
	}
        while(right_motor>-21)
	{
		decrease_right();
		Delay_ms(motor_delay);
	}
}

void go_straight()
{
	while(left_motor<21)
	{
		increase_left();
		Delay_ms(motor_delay);
	}
        while(right_motor>-21)
	{
		decrease_right();
		Delay_ms(motor_delay);
	}
}

void reverse()
{
	while(right_motor<21)
	{
		increase_right();
		Delay_ms(motor_delay);
	}
        while(left_motor>-21)
	{
		decrease_left();
		Delay_ms(motor_delay);
	}
}
void print_LCD(int val,int pos)
{
    /*IntToStr(val,txt);
    switch(pos)
    {
        case 1: Lcd_Custom_Out(1,6,txt);break;
        case 2: Lcd_Custom_Out(2,3,txt);break;
        case 3: Lcd_Custom_Out(2,8,txt);break;
        case 4: Lcd_Custom_Out(1,3,txt);break;
        case 5: Lcd_Custom_Out(1,8,txt);break;
        default: Lcd_Custom_Out(1,6,"ERROR");break;
    }*/
}
int display_Sensor(unsigned int ANX,unsigned int pos)
{
    if(ANX>1&&ANX<13)
    {
        adcRes=Adc_Read(ANX);
        adcRes=((255-(adcRes/16))/10);
        print_LCD(adcRes,pos);
        return adcRes;
    }
    else if(ANX==0||ANX==1)
    {
        adcRes = Adc_Read(ANX);
        adcRes=adcRes/8;
        print_LCD(adcRes,pos);
        return adcRes;
    }
    else{return 0;}
}

// determines whether the path in front is clear and adjusts motors
// accordingly
void behavior_obstacle_avoid()
{
     right =  display_Sensor(12,3);
     left = display_Sensor(11,2);
     if(right>IR_threshold&&left>IR_threshold)
     { reverse(); }
     else if(right<=IR_threshold&&left<=IR_threshold)
     { go_straight(); }
     else if(left<=IR_threshold)
     { turn_right(); }
     else if(right<=IR_threshold)
     { turn_left(); }
     else
     {
         Lcd_Custom_Out(1,6,"ERROR");
         while(1);
     }
     Delay_ms(motor_delay);
     if(left_motor<0&&right_motor<0)
     {
         if(display_Sensor(0,1)<10)
         {
             stop();
             i=rand()+1;
	     i=i%6;
	     turn_left();
	     for(j=0;j<i;j++)
             {
		 Delay_1sec();
             }
	     stop();
	     Delay_1sec();
         }
     }
     Delay_ms(500);
}

void behavior_wall_following()
{
     int a= display_Sensor(6,4);
     int b= display_Sensor(5,5) ;
     right =  display_Sensor(12,3);
     left = display_Sensor(11,2);
     if((a>=IR_wall_threshold&&a<=16)&&(b>=IR_wall_threshold&&b<=16))
     {
         if(right>=IR_threshold&&left>=IR_threshold)
         { reverse(); }
         if(right<IR_threshold||left<IR_threshold)
         { turn_right(); }
         Delay_ms(500);
     }
     else if((a>=16&&(b>=IR_wall_threshold&&b<=16))||(b<=10&&(a>=IR_wall_threshold&&a<=16)))
     {
         if(right>=IR_threshold&&left>=IR_threshold)
         { turn_left(); }
         if(right<IR_threshold||left<IR_threshold)
         { turn_right(); }
         Delay_ms(500);
     }
     else if((b>=16&&(a>=IR_wall_threshold&&a<=16))||(a<=10&&(b>=IR_wall_threshold&&b<=16)))
     {
         turn_right();
         Delay_ms(500);
     }
     else
     {
         if(right>=IR_threshold&&left>=IR_threshold)
         { reverse(); }
         if(right<IR_threshold||left<IR_threshold)
         { turn_right(); }
         Delay_ms(500);
     }
}
void behavior_go()
{
     if(behavior!=last_behavior)
     {
        k=rand()+1;
	k=k%6;
	go_straight();
	for(j=0;j<k;j++)
	{
		Delay_1sec();
	}
	stop();
	Delay_1sec();
     }
}

void behavior_right()
{
     if(behavior!=last_behavior)
     {
        k=rand()+1;
	k=k%6;
	turn_right();
	for(j=0;j<k;j++)
	{
		Delay_1sec();
	}
	stop();
	Delay_1sec();
     }
}

void behavior_left()
{
     if(behavior!=last_behavior)
     {
        k=rand()+1;
	k=k%6;
	turn_left();
	for(j=0;j<k;j++)
	{
		Delay_1sec();
	}
	stop();
	Delay_1sec();
     }
}

void behavior_stop()
{
     if(behavior!=last_behavior)
     {
	stop();
	Delay_1sec();
     }
}

void print_behavior()
{
    switch(behavior)
    {
        case 0: Lcd_Custom_Out(1,5,"avoid");break;
        case 1: Lcd_Custom_Out(1,5,"follow");break;
        case 2: Lcd_Custom_Out(1,5,"go");break;
        case 3: Lcd_Custom_Out(1,5,"right");break;
        case 4: Lcd_Custom_Out(1,5,"left");break;
        case 5: Lcd_Custom_Out(1,5,"stop");break;
        default: Lcd_Custom_Out(1,5,"ERROR");break;
    }
    /*switch(behavior)
    {
        case 0: Lcd_Custom_Out(2,5,"avoid");break;
        case 1: Lcd_Custom_Out(2,5,"follow");break;
        case 2: Lcd_Custom_Out(2,5,"go");break;
        case 3: Lcd_Custom_Out(2,5,"right");break;
        case 4: Lcd_Custom_Out(2,5,"left");break;
        case 5: Lcd_Custom_Out(2,5,"stop");break;
        default: Lcd_Custom_Out(2,5,"ERROR");break;
    }*/
}

void check_behavior()
{
    //Lcd_Custom_Out(1,6,"Start");
    k=0;
    counter1=0;
    counter2=0;
    counter3=0;
    if(PORTCbits.RC13==1)
    {
        while(PORTCbits.RC13==1 && counter3<320) {counter3++;}
        while(PORTCbits.RC13==0 && counter1<320)  {counter1++;}
        while(PORTCbits.RC13==1 && counter2<320) {counter2++;}
        k=(counter1+counter2)  ;
    }
    else
    {
        while(PORTCbits.RC13==0 && counter3<320) {counter3++;}
        while(PORTCbits.RC13==1 && counter2<320)  {counter2++;}
        while(PORTCbits.RC13==0 && counter1<320) {counter1++;}
        k=(counter1+counter2)  ;
    }
    if(k==320){;}
    else if(k==41)
    {
        last_behavior=behavior;
        behavior=2;
        Lcd_Custom_Out(2,2, "GO");
    }
    else if(k==61)
    {
        last_behavior=behavior;
        behavior=1;
        Lcd_Custom_Out(2,2, "WF");
    }
    else if(k==81)
    {
        last_behavior=behavior;
        behavior=0;
        Lcd_Custom_Out(2,2, "OA");
    }
    else if(k==101)
    {
        last_behavior=behavior;
        behavior=4;
        Lcd_Custom_Out(2,2, "TL");
    }
    else if(k==121)
    {
        last_behavior=behavior;
        behavior=3;
        Lcd_Custom_Out(2,2, "TR");
    }
    else if(k==141)
    {
        last_behavior=behavior;
        behavior=5;
        Lcd_Custom_Out(2,2, "ST");
    }
    else
    { Delay_10ms(); }
    IntToStr(k,txt);
    Lcd_Custom_Out(2,8, txt);
}

void Main_Init()
{
    PORTB = 0x0000;
    TRISB = 0xFFFF;       // set pin as input - needed for ADC to work
    TRISF=0;
    TRISD=0;
    TRISC=0xFFFF;
    last_behavior=0;
    behavior=rand()%2;           //code for obstacle avoidance
    Lcd_Custom_Config(&PORTF, 5,4,1,0, &PORTD, 0,2,1);
    Lcd_Custom_Out(1,5,"START") ;
    Uart1_Init(19200);                 // initialize USART module
    Delay_100ms();
    motor_init();                      //stops motors and initializes internal counters
    Delay_ms(motor_delay);                     // pause for usart lines stabilization
    rx1 = Uart1_Read_Char();           // perform dummy read to clear the register
}

void main() {
     Main_Init();
     while(1)
     {
         check_behavior();
         if(behavior==0)
         { behavior_obstacle_avoid(); }
         else if(behavior==1)
         { behavior_wall_following(); }
         else if(behavior==2)
         { behavior_go(); }
         else if(behavior==3)
         { behavior_right(); }
         else if(behavior==4)
         { behavior_left(); }
         else if(behavior==5)
         { behavior_stop(); }
         else
         { Lcd_Custom_Out(2,1,"error"); }
         //Lcd_Custom_Out(2,5,"Cont") ;
         print_behavior();
     }
}//~!