 uint8_t fifoBuffer[64];
 
 char rfidTag[TAG_LENGTH + 1];
+char lockTag[TAG_LENGTH + 1];
+bool locked = false;
+unsigned long lastRead = 0;
+
+char buffer[TAG_LENGTH + 1];
+int count = 0;
 
 // Software serial for XBee module
 SoftwareSerial xbeeSerial(2, 3); // RX, TX

  initMpu();
}

void setLockState(bool locked) {
  digitalWrite(A0, locked);
}

bool readRfid() {
  char buffer[TAG_LENGTH];
  int count = 0;
  bool tagChanged = false;

  if (rfidSerial.available()) {
    if (rfidSerial.read() != 2)
      return false;
    while (count < TAG_LENGTH && rfidSerial.available()) {
      buffer[count] = rfidSerial.read();
      count++;
    }

    if (count == TAG_LENGTH) {
      count = 0;
      if (rfidSerial.read() != 3)
        return false;
      for (int i = 0; i < TAG_LENGTH; i++) {
        tagChanged |= buffer[i] != rfidTag[i];
      }
    }
    if (tagChanged) {
      memcpy(rfidTag, buffer, TAG_LENGTH);
      rfidTag[TAG_LENGTH] = 0;
      Serial.print("Changed: "); Serial.println(rfidTag);
    }
  }
}

void loop() {
  readRfid();
  readLightsensor();
  readDataFromMpu();


      }
      break;
    case 'r':
      // FIXME: This is highly dangerous
      xbeeSerial.print("r=");xbeeSerial.println(lockTag);
      break;
    }
  }
