#include <IRremote.hpp>

// pin definitions
const int RX_PIN = 12;
const int TRIG_PIN = 10;
const int ECHO_PIN = 9;
const int MTR_LED_PINS[] = {5, 4, 3, 2};
const int WARN_LED_PIN = 11;
// misc constants
const int RANGE = 100;
const unsigned long ONE_SEC = 1000;
// IR command constants
const int FWD   = 2;      // +
const int RIGHT = 144;    // >>|
const int REV   = 152;    // -
const int LEFT  = 224;    // |<<
const int STOP  = 168;    // >

unsigned long prevTime = 0;

void driveMotors(int command)  {
  //reset control
  for (int i = 0; i < 4; i++) {
    digitalWrite(MTR_LED_PINS[i], LOW);
  }
  if (command == FWD) {                   // forward
    digitalWrite(MTR_LED_PINS[0], HIGH);
    digitalWrite(MTR_LED_PINS[2], HIGH);
  } else if (command == RIGHT)  {         // right
    digitalWrite(MTR_LED_PINS[0], HIGH);
    digitalWrite(MTR_LED_PINS[3], HIGH);
  } else if (command == REV)  {           // back
    digitalWrite(MTR_LED_PINS[1], HIGH);
    digitalWrite(MTR_LED_PINS[3], HIGH);
  } else if (command == LEFT)  {          // left
    digitalWrite(MTR_LED_PINS[1], HIGH);
    digitalWrite(MTR_LED_PINS[2], HIGH);
  } else if (command == STOP)  {          // stop
    // nothing to do
  }
}

int getDistance()  {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  unsigned long duration = pulseIn(ECHO_PIN, HIGH);
  int distance = (int)(duration * .0343) / 2;
  //Serial.print("Distance: ");
  //Serial.println(distance);
  return distance;
}

void setup() {
  Serial.begin(115200);
  IrReceiver.begin(RX_PIN, ENABLE_LED_FEEDBACK);
  for (int i = 0; i < 4; i++) {
    pinMode(MTR_LED_PINS[i], OUTPUT);
  }
  pinMode(WARN_LED_PIN, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
}

void loop() {
  if (IrReceiver.decode()) {      // we received IR
    int command = IrReceiver.decodedIRData.command;
    Serial.println(command);      // print command number
    driveMotors(command);
    IrReceiver.resume();          // prepare for the next IR
  }
  if (millis() - prevTime >= ONE_SEC) {
    prevTime = millis();
    IrReceiver.stopTimer();     // needed for pulseIn
    int value = getDistance();
    IrReceiver.restartTimer();  // sense done
    if (value <= RANGE)  {
      digitalWrite(WARN_LED_PIN, HIGH);
      driveMotors(STOP);
    } else {
      digitalWrite(WARN_LED_PIN, LOW);
    }
  }
}