#include <IRremote.h>
int RECV_PIN1 = 13;
IRrecv irrecvmain(RECV_PIN1);
int RECV_PIN2 = 2;
int RECV_PIN3 = 26;
int RECV_PIN4 = 28;
int RECV_PIN5 = 30;
IRrecv irrecv2(RECV_PIN2);
IRrecv irrecv3(RECV_PIN3);
decode_results resultsmain;
decode_results results2;
decode_results results3;
IRrecv irrecv4(RECV_PIN4);
IRrecv irrecv5(RECV_PIN5);
decode_results results4;
decode_results results5;
int checkloop = 0;
int intright = 0;
int intleft = 0;
int intfor = 0;
int intback = 0;
int check1 = 0;
//driving
int slow = 0;
int LF = 6;
int LB = 9;
int RB = 11;
int RF = 10;
int echo1 = 5;
int trig1 = 12;
int echo2 = 3;
int trig2 = 7;
int echo3 = 22;
int trig3 = 23;
int echo4 = 24;
int trig4 = 25;
int echo5 = 41;
int trig5 = 40;
long duration1;
long duration2;
long duration3;
long duration4;
long duration5;
int distanceF1;
int distanceF2;
int distanceR1;
int distanceR2;
int distanceup;
int F1;
int F2;
int b1 = 8;
int b2 = 4;
bool forward = false;
bool backward = false;
bool left = false;
bool right = false;
bool stop = true;
bool sens2;
bool sens3;
bool sens4;
bool sens5;
bool stopping = false;
unsigned long s2millis;
unsigned long s3millis;
unsigned long s4millis;
unsigned long s5millis;
unsigned long current_time;
int s2l=36;
int s3l=37;
int s4l=38;
int s5l=39;

void setup()
{
    Serial.begin(9600);
    irrecvmain.enableIRIn(); // Start the reciever
    irrecv2.enableIRIn();
    irrecv3.enableIRIn();
    irrecv4.enableIRIn();
    irrecv5.enableIRIn();
    pinMode(RB, OUTPUT);
    pinMode(RF, OUTPUT);
    pinMode(LB, OUTPUT);
    pinMode(LF, OUTPUT);
    pinMode(trig1, OUTPUT); 
    pinMode(echo1, INPUT);				
    pinMode(trig2, OUTPUT); 
    pinMode(echo2, INPUT);
    pinMode(trig3, OUTPUT); 
    pinMode(echo3, INPUT);
    pinMode(trig4, OUTPUT); 
    pinMode(echo4, INPUT); 
    pinMode(trig5, OUTPUT); 
    pinMode(echo5, INPUT); 
    pinMode(b1, INPUT);
    pinMode(b2, INPUT);
    pinMode(s2l, OUTPUT);
    pinMode(s3l, OUTPUT);
    pinMode(s4l, OUTPUT);
    pinMode(s5l, OUTPUT);
}
void loop() {
	//Serial.println(distanceup);
	delay(5);
	if (stop == true) {
		left = false;
        right = false;
        forward = false;
        backward = false;
	}
    mainloop();
    check();
    if (irrecvmain.decode(&resultsmain)) {
        //Serial.println("Control Sensor ");
        Serial.println(resultsmain.value);
        irrecvmain.resume(); // Receive the next value
        if (resultsmain.value == 4294967295) {
        	stop = true;
        	forward = false;
        }
        else {
        	if (resultsmain.value == 3937909453) {
        		stop = false;
        		forward = true;
        	}
        }
    } 
    if (irrecv2.decode(&results2)) {
        //Serial.println("Sensor 2 ");
        //Serial.println(results2.value);
        irrecv2.resume(); // Receive the next value
        s2millis = millis();
        sens2=true;
    	//Serial.println("2");
    	digitalWrite(s2l, HIGH);
    	stop = false;
    	stopping = false;
    }
    
    if (irrecv3.decode(&results3)) {
        //Serial.println("Sensor 3 ");
        //Serial.println(results3.value);
        irrecv3.resume(); // Receive the next value
        s3millis = millis();
    	sens3=true;
    	//Serial.println("3");
    	digitalWrite(s3l, HIGH);
    	stop = false;
    	stopping = false;
    }
    if (irrecv4.decode(&results4)) {
        //Serial.println("Sensor 4 ");
        //Serial.println(results4.value);
        irrecv4.resume(); // Receive the next value
        s4millis = millis();
    	sens4=true;
    	//Serial.println("4");
    	digitalWrite(s4l, HIGH);
    	stop = false;
    	stopping = false;
    }
    if (irrecv5.decode(&results5)) {
        //Serial.println("Sensor 5 ");
        //Serial.println(results5.value);
        irrecv5.resume(); // Receive the next value
        s5millis = millis();
    	sens5=true;
    	//Serial.println("5");
    	digitalWrite(s5l, HIGH);
    	stop = false;
    	stopping = false;
    }
}
void ir1() {
    
    
    if (resultsmain.value == 3772778233) {
        //forward = true;
        left = false;
        right = false;
        backward = false;
        intfor=intfor+1;
        resultsmain.value = 0;
    }
    if (resultsmain.value == 3772810873) {
        //backward = true;
        left = false;
        right = false;
        forward = false;
        intback=intback+1;
        resultsmain.value = 0;
    }
    if (resultsmain.value == 3772819033) {
        backward = false;
        //left = true;
        right = false;
        forward = false;
        intleft=intleft+1;
        resultsmain.value = 0;
    }
    if (resultsmain.value == 3772794553) {
        backward = false;
        left = false;
        //right = true;
        forward = false;
        intright=intright+1;
        resultsmain.value = 0;
    }
    if (resultsmain.value == 3772793023) {
    	stop = true;
    }
}
void mainloop()
{
    //Serial.println(digitalRead(b1));
    //Serial.println(digitalRead(b2));
    if (digitalRead(b1) == 1) {
        if (stop == true) {
            forward = true;
            stop=false;
            stopping=false;
            delay(250);
        }
        else {
           stop=true;
           delay(250);
        }
    }
    if (digitalRead(b2) == 1) {
        if (backward == false) {
            backward = true;
            forward = false;
            left = false;
            right = false;
            delay(250);
        }
        else {
            backward = false;
            delay(250);
        }
    }


    if (stop==false) {
        digitalWrite(trig1, LOW);
        delayMicroseconds(2);
        digitalWrite(trig1, HIGH);
        delayMicroseconds(10);
        digitalWrite(trig1, LOW);
        duration1 = pulseIn(echo1, HIGH);
        distanceF1 = duration1*0.034/2;
     
        digitalWrite(trig2, LOW);
        delayMicroseconds(2);
        digitalWrite(trig2, HIGH);
        delayMicroseconds(10);
        digitalWrite(trig2, LOW);
        duration2 = pulseIn(echo2, HIGH);
        distanceF2 = duration2*0.034/2;
        F1=distanceF1+2;
        F2=distanceF2+2;
        //Serial.print("Distance F1: ");
        Serial.print(distanceF1);
        Serial.print(" ");
        Serial.println(distanceF2);
        
    }
    
    //else {
      //  distanceF1 = 100;
        //distanceF2 = 100;
    //}
    if (backward == true) {
        digitalWrite(trig3, LOW);
        delayMicroseconds(2);
        digitalWrite(trig3, HIGH);
        delayMicroseconds(10);
        digitalWrite(trig3, LOW);
        duration3 = pulseIn(echo3, HIGH);
        distanceR1 = duration3*0.034/2;
     
        digitalWrite(trig4, LOW);
        delayMicroseconds(2);
        digitalWrite(trig4, HIGH);
        delayMicroseconds(10);
        digitalWrite(trig4, LOW);
        duration4 = pulseIn(echo4, HIGH);
        distanceR2 = duration4*0.034/2;
        //Serial.print("Distance R1: ");
        //Serial.println(distanceR1);
        //Serial.print("Distance R2: ");
        //Serial.println(distanceR2);
        //Serial.println("");
    }
    else {
        distanceR1 = 100;
        distanceR2 = 100;
    }
}
void check()
{
//GO ---------------------------------------------------------------------------
if (stop == false) {
	forward=true;
    if (distanceF1<20) {
    	stopping=true;
    	forward = false;
    	Serial.print("f1<40");
        if (distanceF1>distanceF2) {
        	Serial.print("f1?");
            if (distanceF1>F2) {
            	Serial.print("f1");
                right=true;
                left=false;
                forward=false;
            }
            else {
                stop=true;
                Serial.print("stop1");
            }
        }
        else {
        	right=false;
            Serial.print("no2");
        if (distanceF2>distanceF1) {
        	Serial.print("f2?");
            if (distanceF2>F1) {
                left=true;
                right=false;
                Serial.print("f2");
                forward=false;
            }
            else {
            	stop=true;
            	Serial.print("stop3");
            }
        }
        else {
            left=false;
            Serial.println("no4");
        }
        }
    }
    else {
        if (distanceF2<20) {
        	forward = false;
        	stopping=true;
        	Serial.print("f2<40");
            if (distanceF1>distanceF2) {
            	Serial.print("f1?");
                if (distanceF1>F2) {
                    right=true;
                    left=false;
                    Serial.print("f1");
                    forward=false;
                }
                else {
                    stop=true;
                    Serial.print("stop4");
                }
            }
            else {
 
                right=false;
                Serial.print("no5");
            if (distanceF2>distanceF1) {
            	Serial.print("f2?");
                if (distanceF2>F1) {
                    left=true;
                    right=false;
                    Serial.print("f2");
                    forward=false;
                }
                else {
            		stop=true;
            		Serial.print("stop6");
        		}
            }
            else {
            	left=false;
        	    Serial.print("no7");
        	}
            }
        }
        else {
        	if (stopping = false) {
        		Serial.print("no8");
        		forward = true;
        		left=false;
        		right=false;
        	}
        	else {
            	Serial.print("stopping");
        	}
        }
    }
}
if (distanceF1<10) {
	right=false;
	left=false;
	forward=false;
	backward=true;
}
else {
	if (distanceF2<10) {
	right=false;
	left=false;
	forward=false;
	backward=true;
}
else {
	backward=false;
}
}
if (distanceup<15) {
	stop = true;
}
if (stop == true) {
		left = false;
        right = false;
        forward = false;
        backward = false;
        Serial.print("stop");
        stopping = false;
}
else {
	Serial.print("go");
}
	if (distanceF1 > 25 && distanceF2 > 25 && forward == true) {
        analogWrite(RF, 255);
        analogWrite(LF, 255);
        analogWrite(RB, 0);
        analogWrite(LB, 0);
        Serial.print("forward");
    }
    else {
        if (distanceR1 > 35 && distanceR2 > 35 && backward == true) {
            analogWrite(RF, 0);
            analogWrite(LF, 0);
            delay(200);
            analogWrite(RB, 255);
            analogWrite(LB, 255);
            analogWrite(RF, 0);
            analogWrite(LF, 0);
            Serial.print("back");
        }
        else {
            if (left == true) {
            analogWrite(RF, 255);
            analogWrite(LB, 255);
            analogWrite(RB, 0);
            analogWrite(LF, 0);
            Serial.print("left");
            }
            else {
                if (right == true) {
                    analogWrite(RB, 255);
                    analogWrite(LF, 255);
                    analogWrite(RF, 0);
                    analogWrite(LB, 0);
                    Serial.print("right");
                }
                else {
                    analogWrite(RF, 0);
                    analogWrite(LF, 0);
                    analogWrite(RB, 0);
                    analogWrite(LB, 0);
                    
                }
            }
        }
    }
	delay(100);
    if (millis()-s2millis>=750) {
    	sens2=false;
    	digitalWrite(s2l, LOW);
    }
    if (millis()-s3millis>=750) {
    	sens3=false;
    	digitalWrite(s3l, LOW);
    }
    if (millis()-s4millis>=750) {
    	sens4=false;
    	digitalWrite(s4l, LOW);
    }
    if (millis()-s5millis>=750) {
    	sens5=false;
    	digitalWrite(s5l, LOW);
    }
}