#include <Servo.h>
#include <IRremote.h>

const short rg = 5;
const short rb = 3;
const short lg = 6;
const short lb = 11;
const short TrigPin = 8;
const short EchoPin = 7;
const short ServoPin = 9;
const short LeasR = A2;
const short LeasL = A3;
const short RecvPin = A0;

bool fwd = false;
bool disableLeas = false;
bool leas[2];
bool remote = false;
int msg;
unsigned int distance[5];

Servo myservo;
IRrecv irrecv(RecvPin);
decode_results results;


float getDistance() {
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin, LOW);
  return 0.017 * pulseIn(EchoPin, HIGH);
}

int getMessage() {
  if (irrecv.decode(&results)) {
    int value = results.value;
    irrecv.resume();
    return value;
  }
  return 0;
}

void r(int left, int right, float duration = 10) {
  if (right > 0) {
    analogWrite(rg, abs(right));
  }
  else if (right < 0) {
    analogWrite(rb, abs(right));
  }
  if (left > 0) {
    analogWrite(lg, abs(left));
  }
  else if (left < 0) {
    analogWrite(lb, abs(left));
  }
  if (duration > 0) {
    delay(duration * 100);
    s();
  }
}

void s() {
  analogWrite(rg, 0);
  analogWrite(rb, 0);
  analogWrite(lg, 0);
  analogWrite(lb, 0);
}

void setup() {
  Serial.begin(9600);
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);
  pinMode(LeasR, INPUT);
  pinMode(LeasL, INPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  myservo.attach(ServoPin);
  myservo.write(90);
  delay(1000);
}


void loop() {

  if (!remote) {
    leas[0] = !digitalRead(LeasL);
    leas[1] = !digitalRead(LeasR);
    distance[3] = getDistance();
    delay(20);
    distance[4] = getDistance();
    if (distance[3] > distance[4]) {
      distance[0] = distance[3];
    } else {
      distance[0] = distance[4];
    }

    if (distance[0] > 50 || disableLeas && !leas[0] && !leas[1]) {
      if (!fwd) {
        r(250, 250, 0);
        fwd = true;
        delay(100);
      }
    }

    else {
      fwd = false;
      s();
      if (distance[0] < 20 ) {
        r(-120, -120, 5);
        delay(200);
      }
      myservo.write(10);
      delay(350);
      distance[1] = getDistance();
      myservo.write(170);
      delay(650);
      distance[2] = getDistance();
      myservo.write(90);
      delay(400);
      if (leas[0] < leas[1] || distance[1] < distance[2]) {
        r(0, 120, 5);
      }
      else if (leas[0] > leas[1] || distance[1] > distance[2]) {
        r(120, 0, 5);
      }
      delay(200);
    }
  }

  else {
    msg = getMessage();
    msg != 0 && Serial.println(msg);
  }
}