void initSerial(){

  Serial1.begin(SERIAL_BAUD_VEL);

  if(debugMode){
    Serial.begin(SERIAL_BAUD_VEL);
  }
}

void receiveBluetoothSerial(){
  while(Serial.available()>0){ 
    Serial1.write(Serial.read());// input from Serial to Serial1
  }
  while(Serial1.available()>0){
    int ascii = Serial1.read();
    Serial.print(char(ascii)); //input from Serial1 to Serial
  }
}

boolean findInResponse(const char *toMatch, unsigned int timeOut = 1000) {
  int byteRead;

  unsigned long timeOutTarget; // in milliseconds

  for (unsigned int offset = 0; offset < strlen(toMatch); offset++) {
    timeOutTarget = millis() + timeOut; // Doesn't handle timer wrapping
    while (!Serial1.available()) {
      // Wait, with optional time out.
      if (timeOut > 0) {
        if (millis() > timeOutTarget) {
          return false;
        }
      }
      delay(1); // This seems to improve reliability slightly
    }
    byteRead = Serial1.read();
    //Serial.print((char)byteRead);
    delay(1); // Removing logging may affect timing slightly

    if (byteRead != toMatch[offset]) {
      offset = 0;
      // Ignore character read if it's not a match for the start of the string
      if (byteRead != toMatch[offset]) {
        offset = -1;
      }
      continue;
    }
  }
  return true;
}

boolean sendRN4020(const char* command, const char* expectedResponse){
    Serial1.print(command);
    delay(20);
    Serial1.flush();
    Serial1.println();
  
    if (!findInResponse(expectedResponse, 3000)) {
      return false;
    }
    
    return true;
}

int getSensorPosition(){

  // HybridPlay sensor positions, max. 20
  int actualPos = 0;

  Serial1.print("SUR,E3EF00906B094F8AA5C9C0A7E9DDAD86");
  delay(20);
  Serial1.flush();
  Serial1.println();

  if(findInResponse("00")){
    actualPos = 0;
    return actualPos;
  }else if(findInResponse("01")){
    actualPos = 1;
    return actualPos;
  }else if(findInResponse("02")){
    actualPos = 2;
    return actualPos;
  }else if(findInResponse("03")){
    actualPos = 3;
    return actualPos;
  }else if(findInResponse("04")){
    actualPos = 4;
    return actualPos;
  }else if(findInResponse("05")){
    actualPos = 5;
    return actualPos;
  }else if(findInResponse("06")){
    actualPos = 6;
    return actualPos;
  }else if(findInResponse("07")){
    actualPos = 7;
    return actualPos;
  }else if(findInResponse("08")){
    actualPos = 8;
    return actualPos;
  }else if(findInResponse("09")){
    actualPos = 9;
    return actualPos;
  }else if(findInResponse("0A")){
    actualPos = 10;
    return actualPos;
  }else if(findInResponse("0B")){
    actualPos = 11;
    return actualPos;
  }else if(findInResponse("0C")){
    actualPos = 12;
    return actualPos;
  }else if(findInResponse("0D")){
    actualPos = 13;
    return actualPos;
  }else if(findInResponse("0E")){
    actualPos = 14;
    return actualPos;
  }else if(findInResponse("0F")){
    actualPos = 15;
    return actualPos;
  }else if(findInResponse("10")){
    actualPos = 16;
    return actualPos;
  }else if(findInResponse("11")){
    actualPos = 17;
    return actualPos;
  }else if(findInResponse("12")){
    actualPos = 18;
    return actualPos;
  }else if(findInResponse("13")){
    actualPos = 19;
    return actualPos;
  }else if(findInResponse("14")){
    actualPos = 20;
    return actualPos;
  }

  return actualPos;
}

int getSensorState(){

  int actualState = 0;

  Serial1.print("SUR,E3EF00906B094F8AA5C9C0A7E9DDAD88");
  delay(20);
  Serial1.flush();
  Serial1.println();

  if(findInResponse("00")){
    actualState = 0;
    return actualState;
  }else if(findInResponse("01")){
    actualState = 1;
    return actualState;
  }else if(findInResponse("02")){
    actualState = 2;
    return actualState;
  }else if(findInResponse("03")){
    actualState = 3;
    return actualState;
  }else if(findInResponse("04")){
    actualState = 4;
    return actualState;
  }else if(findInResponse("05")){
    actualState = 5;
    return actualState;
  }else if(findInResponse("06")){
    actualState = 6;
    return actualState;
  }else if(findInResponse("07")){
    actualState = 7;
    return actualState;
  }else if(findInResponse("08")){
    actualState = 8;
    return actualState;
  }else if(findInResponse("09")){
    actualState = 9;
    return actualState;
  }else if(findInResponse("0A")){
    actualState = 10;
    return actualState;
  }else if(findInResponse("0B")){
    actualState = 11;
    return actualState;
  }else if(findInResponse("0C")){
    actualState = 12;
    return actualState;
  }else if(findInResponse("0D")){
    actualState = 13;
    return actualState;
  }else if(findInResponse("0E")){
    actualState = 14;
    return actualState;
  }else if(findInResponse("0F")){
    actualState = 15;
    return actualState;
  }

  return actualState;

}