 
 uint8_t gsmSendCommand(const char* cmd, uint16_t timeout, const char* expected) {
   gsmSerial.listen();
-  
+
   if (cmd) {
     gsmPurgeSerialBuffer();
     gsmSerial.println(cmd);
   }
-  
+
   uint32_t t = millis();
   uint8_t n = 0;
-  
+
   do {
     if (gsmSerial.available()) {
       char c = gsmSerial.read();
       Serial.write(c);
-      
+
       if (n >= sizeof(gsmSerialBuffer) - 1) {
         // buffer full, discard first half
         n = sizeof(gsmSerialBuffer) / 2 - 1;
         memcpy(gsmSerialBuffer, gsmSerialBuffer + sizeof(gsmSerialBuffer) / 2, n);
       }
-      
+
       gsmSerialBuffer[n++] = c;
       gsmSerialBuffer[n] = 0;
-      
+
       if (strstr(gsmSerialBuffer, expected ? expected : "OK\r")) {
-       return n;
+        return n;
       }
     }
   } while (millis() - t < timeout);
+
+  return 0;
+}
+
+uint8_t gsmSendCommand_P(const __FlashStringHelper* cmd, uint16_t timeout, const char* expected) {
+  gsmSerial.listen();
+
+  if (cmd) {
+    gsmPurgeSerialBuffer();
+    gsmSerial.println(cmd);
+  }
+
+  uint32_t t = millis();
+  uint8_t n = 0;
+
+  do {
+    if (gsmSerial.available()) {
+      char c = gsmSerial.read();
+      Serial.write(c);
+
+      if (n >= sizeof(gsmSerialBuffer) - 1) {
+        // buffer full, discard first half
+        n = sizeof(gsmSerialBuffer) / 2 - 1;
+        memcpy(gsmSerialBuffer, gsmSerialBuffer + sizeof(gsmSerialBuffer) / 2, n);
+      }
+
+      gsmSerialBuffer[n++] = c;
+      gsmSerialBuffer[n] = 0;
+
+      if (strstr(gsmSerialBuffer, expected ? expected : "OK\r")) {
+        return n;
+      }
+    }
+  } while (millis() - t < timeout);
+
+  return 0;
+}
+
+void reboot() {
+  digitalWrite(ARDUINO_RESET_PIN, LOW);
+}
+
+void setup() {
+  digitalWrite(ARDUINO_RESET_PIN, HIGH);
+  pinMode(ARDUINO_RESET_PIN, OUTPUT);
   
-  return 0;
-}
-
-void setup() {
   Serial.begin(9600);
   gpsSerial.begin(9600);
   gsmSerial.begin(9600);
 
   for (;;) {
-    Serial.print(F("Initializing GSM... "));
-    while (!gsmInit());
-    Serial.println(F("OK"));
-  
-    Serial.print(F("Setting up network... "));
-    uint8_t ret = gsmSetup(APN);
-    if (ret == 0) break;
+    Serial.println(F("Initializing GSM... "));
+    uint8_t ret = gsmInit();
+    
+    if (ret == 0) {
+      Serial.println(F("OK"));
+      break;
+    }
+    
     Serial.print(F("Error code: "));
     Serial.println(ret);
   }
-  
-  Serial.println(F("OK"));
+
+  for (;;) {
+    Serial.println(F("Setting up network... "));
+    uint8_t ret = gsmSetup(APN);
+    
+    if (ret == 0) {
+      Serial.println(F("OK"));
+      break;
+    }
+    
+    Serial.print(F("Error code: "));
+    Serial.println(ret);
+  }
 }
 
 void loop() {
   gpsSerial.listen();
-  
+
   while (gpsSerial.available()) {
     gps.encode(gpsSerial.read());
     gpsUpdateValues();

#include <SoftwareSerial.h>
#include <TinyGPS++.h>

#define ID    1
#define APN   "internet"

SoftwareSerial gpsSerial(11, 12); // RX, TX
SoftwareSerial gsmSerial(9, 10); // RX, TX

}

uint8_t gsmInit() {
  pinMode(SIM800_RESET_PIN, OUTPUT);
  digitalWrite(SIM800_RESET_PIN, HIGH);
  delay(1000);
  digitalWrite(SIM800_RESET_PIN, LOW);
  delay(1000);
  digitalWrite(SIM800_RESET_PIN, HIGH);
  delay(3000);

  if (!gsmSendCommand_P(F("AT"))) {
    return 1;
  }
  
  if (!gsmSendCommand_P(F("AT+IPR=9600"))) {
    return 2;
  }
  
  if (!gsmSendCommand_P(F("AT+CFUN=1"))) {
    return 3;
  }

  return 0;
}

uint8_t gsmSetup(const char* apn) {
  bool success = false;

  for (uint8_t n = 0; n < 30; n++) {
    if (gsmSendCommand_P(F("AT+CREG?"))) {
      char *p = strstr(gsmSerialBuffer, "0,");

      if (p) {
        char mode = *(p + 2);

        if (mode == '1' || mode == '5') {
          success = true;
          break;
        }
      }
    }

    delay(1000);
  }

  if (!success) {
    return 1;
  }

  if (!gsmSendCommand_P(F("AT+CGATT?"))) {
    return 2;
  }

  if (!gsmSendCommand_P(F("AT+SAPBR=3,1,\"CONTYPE\",\"GPRS\""))) {
    return 3;
  }

  gsmSerial.listen();
  gsmSerial.print(F("AT+SAPBR=3,1,\"APN\",\""));
  gsmSerial.print(apn);
  gsmSerial.println(F("\""));

  if (!gsmSendCommand(0)) {
    return 4;
  }

  if (!gsmSendCommand_P(F("ATS0=1"))) {
    return 5;
  }

  if (!gsmSendCommand_P(F("AT+CMGF=1"))) {
    return 6;
  }

  if (!gsmSendCommand_P(F("AT+CPMS=\"SM\",\"SM\",\"SM\""))) {
    return 7;
  }

  return 0;
}

void gsmProcessSMS(uint16_t id, const char* sender, const char* text) {
  Serial.println(id);
  Serial.println(sender);
  Serial.println(text);

  if (strcmp(text, "reboot") == 0) reboot();

  char buffer[12];
  sprintf(buffer, "AT+CMGD=%u", id);
  gsmSendCommand(buffer);
}

void gsmCheckSMS() {
  gsmSendCommand_P(F("AT+CMGL=\"ALL\""));

  char* p = strstr(gsmSerialBuffer, "CMGL: ");

  while (p) {
    p += 6;

    uint8_t i;
    char index[3];
    char sender[20];
    char text[161];

    i = 0;
    memset(index, 0, sizeof(index));

    while (*p != ',') {
      index[i++] = *p++;
    }

    while (*p != '"') ++p; ++p;
    while (*p != '"') ++p; ++p;
    while (*p != '"') ++p; ++p;

    i = 0;
    memset(sender, 0, sizeof(sender));

    while (*p != '"') {
      sender[i++] = *p++;
    }

    while (*p != '\n') ++p; ++p;

    i = 0;
    memset(text, 0, sizeof(text));

    while (*p != '\r') {
      text[i++] = *p++;
    }

    uint16_t id = atoi(index);
    gsmProcessSMS(id, sender, text);

    p = strstr(p, "CMGL: ");
  };
}

int16_t gsmGetSignalQuality() {
  gsmSendCommand_P(F("AT+CSQ"));

  char* p = strstr(gsmSerialBuffer, "CSQ: ");

  if (p) {
    p += 5;
    uint8_t i = 0;
