   oled.setColor(255, 255, 0);
   oled.setPrintPos(0, oled.getHeight() / 2);
   oled << F("Please wait...");
-}
+
+  Serial3.println(F("$PMTK161,0*28")); // Sleep GPS
+}
+
 
 void updateSleepModules() {
+  if(ack_command.isUpdated())
+    if(strcmp(ack_command.value(), "161") + strcmp(ack_response.value(), "3") == 0)
+      stateMachine.transitionTo(SleepNow);
 }
 
 void updateSleepNow() {
+  oled.clearScreen();
+
+  LP.DeepSleep(GPIO_WAKE, PIN_6);
+
+  stateMachine.transitionTo(WakeModules);
 }
 
 void enterWakeModules() {
-  ;
+  drawTitle(F("Waking"));
+  oled.setFont(ucg_font_fixed_v0r);
+
+  oled.setColor(255, 255, 0);
+  oled.setPrintPos(0, oled.getHeight() / 2);
+  oled << F("Please wait...");
+
+  Serial3.println(' ');
 }
 
 void updateWakeModules() {
-  drawWakeModules();
+  if(awake.isUpdated()) {
+    awake.value();
+    delay(1000);
+    stateMachine.transitionTo(GetFix);
+  }
 }
 
 void enterNextWaypoint() {
-  ;
-}
-
-void updateNextWaypoint() {
-  drawNextWaypoint();
-}
-
-void updateLastWaypoint() {
-  drawLastWaypoint();
+  drawTitle(F("Waypoint Reached"));
+  oled.setFont(ucg_font_fixed_v0r);
+
+  oled.setColor(255, 255, 0);
+  oled.setPrintPos(0, oled.getHeight() / 2);
+  oled << F("Push the button to");
+  oled.setPrintPos(0, (oled.getHeight() / 2) + (oled.getFontAscent() - oled.getFontDescent()));
+  oled << F("continue.");
+
+  waypoint_index++;
+}
+
+void enterLastWaypoint() {
+  drawTitle(F("Open the Box"));
 }
 
 void enterGPSFailure() {
-  drawGPSFailure();
+  drawTitle(F("GPS Failure"));
+  oled.setFont(ucg_font_fixed_v0r);
+
+  oled.setColor(255, 255, 0);
+  oled.setPrintPos(0, oled.getHeight() / 2);
+  oled << F("No data from GPS!");
 }
 
 void drawReadLocation() {

// #include <avr/sleep.h>
// #include <pitches.h>

#include <SPI.h>
#include <Time.h>
// #include <EEPROM.h>
#include <Ucglib.h>
#include <TinyGPS++.h>
#include <Streaming.h>
#include <TimedEvent.h>
#include <ButtonEvent.h>
#include <FiniteStateMachine.h>

#define OLED_MOSI 11

  oled.setColor(255, 255, 0);
  oled.setPrintPos(0, oled.getHeight() / 2);
  oled << F("Please wait...");

  TimedEvent.start(GET_FIX_TIMER);
}
