begin with gps, can drive now

lib/Menu/menu.cpp

@@ -16,23 +16,37 @@ Menu::Menu() {
 
 void Menu::addEntry(MenuAction* entry) {
     this->entrys.push_back(entry);
-        
-    // TODO: Same shit here. Why I get the first element after an increment.
-    static uint8_t inc = 0;
-    inc++;
-    if (inc == 1) {
+
+    if (this->entrys.size() == 2)
         this->it = this->entrys.begin();
-    }
 }
 
 void Menu::printMenu() {
-    std::cout << std::endl;
-    for (std::list<MenuAction*>::iterator iter = this->entrys.begin(); iter != this->entrys.end(); iter++) {
-        MenuAction* selectedEntry = *(iter);
-        if (this->it == iter)
-            std::cout << "  " << selectedEntry->getName() << std::endl;
-        else
-            std::cout << selectedEntry->getName() << std::endl;
+    if (this->lcd) {
+        MenuAction* selectedEntry = *(this->it);
+        lcd->clear();
+        lcd->setCursor(0, 0);
+        lcd->print("-> ");
+        lcd->print(selectedEntry->getName());
+        lcd->setCursor(0, 1);
+        std::list<MenuAction*>::iterator iter = this->it;
+        iter++;
+        if (iter != this->entrys.end()) {
+            selectedEntry = *iter;
+            lcd->print(selectedEntry->getName());
+        } else {
+            selectedEntry = *this->entrys.begin();
+            lcd->print(selectedEntry->getName());
+        }
+    } else {
+        std::cout << std::endl;
+        for (std::list<MenuAction*>::iterator iter = this->entrys.begin(); iter != this->entrys.end(); iter++) {
+            MenuAction* selectedEntry = *(iter);
+            if (this->it == iter)
+                std::cout << "  " << selectedEntry->getName() << std::endl;
+            else
+                std::cout << selectedEntry->getName() << std::endl;
+        }
     }
     this->inSubmenu = false;
 }
@@ -85,8 +99,6 @@ void Menu::right() {
         selectedEntry->getMenu()->setParentMenu(this);
     } 
     selectedEntry->runAction();
-    if (selectedEntry->getIsMenu())
-        selectedEntry->getMenu()->down();
 }
 
 void Menu::left() {

lib/Menu/menuControl.h

@@ -13,6 +13,7 @@
 #define MENU_CONTROLL_H
 
 #include <iostream>
+#include <LiquidCrystal_I2C.h>
 
 class MenuControl {
     public:
@@ -27,9 +28,10 @@ class MenuControl {
 
         virtual void printMenu();
         void setParentMenu(MenuControl* menu) {this->parentMenu = menu;}
+        void setLcd(LiquidCrystal_I2C* lcd) {this->lcd = lcd;}
 
     protected:
         MenuControl* parentMenu = nullptr;
+        LiquidCrystal_I2C* lcd = nullptr;
 };
-
 #endif // MENU_CONTROLL_H