#define BACKWARD 0x2
#define LEFT 0x4
#define RIGHT 0x8

// Sensor Data
#define LEFT_FRONT_BUMPER 0x1
#define RIGHT_FRONT_BUMPER 0x2
#define REAR_BUMPER 0x4
#define DEBOUNCE_THRESHOLD 50

int sensorData = 0;
int lastSensorData = 0;
int lastLeftFrontBumperReading;
int lastRightFrontBumperReading;
int lastRearBumperReading;
int leftFrontBumperReadingTime;
int rightFrontBumperReadingTime;
int rearBumperReadingTime;
int command[2];

void setup() {
  Serial.begin(9600);

  pinMode(DRIVE_ENABLE_PIN, OUTPUT);
  pinMode(TURN_ENABLE_PIN, OUTPUT);
  pinMode(LEFT_PIN, OUTPUT);
  pinMode(RIGHT_PIN, OUTPUT);
  pinMode(BACKWARD_PIN, OUTPUT);
  pinMode(FORWARD_PIN, OUTPUT);

  pinMode(LEFT_FRONT_BUMPER_PIN, INPUT);
  digitalWrite(LEFT_FRONT_BUMPER_PIN, HIGH);
  pinMode(RIGHT_FRONT_BUMPER_PIN, INPUT);
  digitalWrite(RIGHT_FRONT_BUMPER_PIN, HIGH);
  pinMode(REAR_BUMPER_PIN, INPUT);
  digitalWrite(REAR_BUMPER_PIN, HIGH);

}

void loop() {

  // Read sensors and transmit
  readSensors();
  if (sensorData != lastSensorData) {
    Serial.write(sensorData);
    lastSensorData = sensorData;
  }

  // listen for wireless commands
  if (Serial.available() > 0) {
    if (readCommand() > 0) {
      executeCommand();
    }
  }
}

void readSensors() {
  int s;

  s = digitalRead(LEFT_FRONT_BUMPER_PIN);
  if (s != lastLeftFrontBumperReading) {
    leftFrontBumperReadingTime = millis();
  }
  if ((millis() - leftFrontBumperReadingTime) > DEBOUNCE_THRESHOLD) {
    if (s == LOW) {
      sensorData |= LEFT_FRONT_BUMPER;
    } else {
      sensorData &= ~LEFT_FRONT_BUMPER;
    }
  }
  lastLeftFrontBumperReading = s;

  s = digitalRead(RIGHT_FRONT_BUMPER_PIN);
  if (s != lastRightFrontBumperReading) {
    rightFrontBumperReadingTime = millis();
  }
  if ((millis() - rightFrontBumperReadingTime) > DEBOUNCE_THRESHOLD) {
    if (s == LOW) {
      sensorData |= RIGHT_FRONT_BUMPER;
    } else {
      sensorData &= ~RIGHT_FRONT_BUMPER;
    }
  }
  lastRightFrontBumperReading = s;

  s = digitalRead(REAR_BUMPER_PIN);
  if (s != lastRearBumperReading) {
    rearBumperReadingTime = millis();
  }
  if ((millis() - rearBumperReadingTime) > DEBOUNCE_THRESHOLD) {
    if (s == LOW) {
      sensorData |= REAR_BUMPER;
    } else {
      sensorData &= ~REAR_BUMPER;
    }
  }
  lastRearBumperReading = s;
}

int readCommand() {
  int b = Serial.read();
  if (b == BEGIN_COMMAND) {
    command[0] = readByte();
    command[1] = readByte();
    return 1;
  } else {
    return 0;
  }
}

// blocking read
int readByte() {
  while (true) {
    if (Serial.available() > 0) {
      return Serial.read();
    }
  }
}

void executeCommand() {
  int c = command[0];
  int speed = command[1];

  digitalWrite(DRIVE_ENABLE_PIN, LOW);
  if (c & FORWARD) {
    digitalWrite(BACKWARD_PIN, LOW);
    digitalWrite(FORWARD_PIN, HIGH);
  }
  if (c & BACKWARD) {
    digitalWrite(FORWARD_PIN, LOW);
    digitalWrite(BACKWARD_PIN, HIGH);
  }
  if (c & (FORWARD | BACKWARD)) {
    analogWrite(DRIVE_ENABLE_PIN, speed);
  }

  digitalWrite(TURN_ENABLE_PIN, LOW);
  if (c & LEFT) {
    digitalWrite(RIGHT_PIN, LOW);
    digitalWrite(LEFT_PIN, HIGH);
  }
  if (c & RIGHT) {
    digitalWrite(LEFT_PIN, LOW);
    digitalWrite(RIGHT_PIN, HIGH);
  }
  if (c & (LEFT | RIGHT)) {
    digitalWrite(TURN_ENABLE_PIN, HIGH);
  }
}