 
 #define arraySize(a) (sizeof(a) / sizeof(a[0]))
 
-const uint8_t buttonPin = 2;
+const uint8_t buttonPin = 6;
 // const uint8_t id_address = 0;
 const double threshold = 0.01;
-// unsigned long start = 0;
 double distanceTo;
 double course;
 uint8_t waypoint_index = 0;
 
 const double waypoint[][2] = {
-  { 36.139670, -96.004777 },
-  { 36.139777, -96.003538 },
-  { 36.140135, -96.004070 },
+  { 36.049521, -95.921004 },
+  { 36.048774, -95.921697 },
+  // { 36.140135, -96.004070 },
 };
+
+Button button = Button(buttonPin);
 
 // U8GLIB_SSD1351_128X128_HICOLOR oled(OLED_CS, OLED_DC, OLED_RESET);
 Ucglib_SSD1351_18x128x128_HWSPI oled(OLED_DC, OLED_CS, OLED_RESET);
 
+TEENSY3_LP LP = TEENSY3_LP();
+
 TinyGPSPlus gps;
 
-// TinyGPSCustom ack_command(gps, "PMTK001", 1);
-// TinyGPSCustom ack_response(gps, "PMTK001", 2);
-// TinyGPSCustom awake(gps, "PMTK010", 1);
+TinyGPSCustom ack_command(gps, "PMTK001", 1);
+TinyGPSCustom ack_response(gps, "PMTK001", 2);
+TinyGPSCustom awake(gps, "PMTK010", 1);
 
 State GetFix       = State(enterGetFix, updateGetFix, exitGetFix);
 State ReadLocation = State(enterReadLocation, updateReadLocation, exitReadLocation);
 State SleepModules = State(enterSleepModules, updateSleepModules, NULL);
 State SleepNow     = State(updateSleepNow);
 State WakeModules  = State(enterWakeModules, updateWakeModules, NULL);
-State NextWaypoint = State(enterNextWaypoint, updateNextWaypoint, NULL);
-State LastWaypoint = State(updateLastWaypoint);
+State NextWaypoint = State(enterNextWaypoint, NULL, NULL);
+State LastWaypoint = State(enterLastWaypoint, NULL, NULL);
 State GPSFailure   = State(enterGPSFailure, NULL, NULL);
 
 FSM stateMachine = FSM(GetFix);
 
-// void pictureLoop(void (draw)()) {
-//   oled.firstPage();
-//   do {
-//     draw();
-//   } while(oled.nextPage());
-// }
-
-void onDown(ButtonInformation* sender) { /* do nothing here */ }
-
-void onUp(ButtonInformation* sender) { /* do nothing here */ }
-
-void onDouble(ButtonInformation* sender) { /* do nothing here */ }
-
-void onHold(ButtonInformation* sender) {
-  if(millis() < 6000) {
-    // reset waypoint index
+void onClick(Button& button) {
+  if(stateMachine.isInState(NextWaypoint)) {
+    if(waypoint_index >= arraySize(waypoint)) {
+      stateMachine.transitionTo(LastWaypoint);
+    } else {
+      stateMachine.transitionTo(ReadLocation);
+    }
+  }
+}
+
+void onHold(Button& button) {
+  if(millis() < 6000 || stateMachine.isInState(LastWaypoint)) {
+    waypoint_index = 0;
   } else {
-    stateMachine.transitionTo(NextWaypoint);
+    if(gps.location.isValid())
+      stateMachine.transitionTo(NextWaypoint);
   }
 }
 

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

  int width = oled.getStrWidth("0 satellites");
  oled.setClipRange(0, oled.getHeight() - 12, width, 12);

  TimedEvent.start(GET_FIX_TIMER);
}

  oled << datetime;

  oled.setFont(ucg_font_7x13Br);
  oled.setColor(0, 255, 0);

  oled.setPrintPos(0, oled.getHeight() / 2);
  oled << distanceTo << F(" miles");

  oled.setPrintPos(0, (oled.getHeight() / 2) + (oled.getFontAscent() - oled.getFontDescent()));
  oled << TinyGPSPlus::cardinal(course);

  oled.setFont(ucg_font_fixed_v0r);
  oled.setColor(255, 255, 255);

  oled.setPrintPos(0, oled.getHeight() + oled.getFontDescent());
  oled << status;

  oled.setPrintPos(0, oled.getHeight() + (oled.getFontDescent() + 12));
  oled << F("hdop: ") << gps.hdop.value() << F(" WP: ") << (waypoint_index + 1) << '/' << arraySize(waypoint) << "    ";
}

void setup() {
  button.clickHandler(onClick);
  button.holdHandler(onHold, 5000);

  TimedEvent.addTimer(GET_FIX_TIMER, 5000, gpsFailure);
  TimedEvent.addTimer(READ_LOCATION_TIMER, 60000, initiateSleep);
