int wait_pin(int pin, int state, unsigned long int timeout)
{
  unsigned long int starttime = millis();
  while ((digitalRead(pin) != state) && ((millis()-starttime) < timeout));
  if (digitalRead(pin) == state)
  {
    return 0; // OK
  }
  else
  {
    return -1; // NOK
  }
}