#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Constantes para definir los pines
const int subir = 2;
const int bajar = 3;
const int seleccionar = 4;
const int volver = 5;
const int ledPin = 10;

int valorPulsador = 0;
int valBtn2 = 0;
int enterVal = 0;
int previo = LOW;
int previo2 = LOW;
int enterValPrev = 0;

LiquidCrystal_I2C lcd(0x27, 20, 4);

int indice = 0;
int inAction = false;

float km = 0.0;

class Boton {
  private:
    int pin;
    int value;
    int alt;
  public:
    Boton(int p);
    int getValue();
    void setAlt();
    int getAlt();

    boolean changed();
    int read();
};

Boton::Boton(int p) {
  pin = p;
  alt = 0;
  pinMode(pin, INPUT);
}

void Boton::setAlt() {
  alt = value;
}

int Boton::getValue() {
  return value;
}

int Boton::read() {
  value = digitalRead(pin);
  return getValue();
}

boolean Boton::changed() {
  return value != alt;
}

class Menu {
  private:
    int toShow = 0;
    String title;
    Menu *submenu[10];
    Menu *parent;
    int nextItem = 0;

  public:
    Menu(String t);
    void showMenu();
    String getTitle();
    void up();
    void down();
    void addItem(Menu *m);
    Menu* getSubMenu();
    bool hatSubmenu();
    void setParent(Menu *m);
    Menu* getParent();
    bool hatParent();
};

Menu::Menu(String t) {
  title = t;
  parent = NULL;
}

void Menu::setParent(Menu *m) {
  parent = m;
}

boolean Menu::hatParent() {
  return (parent != NULL);
}

Menu* Menu::getParent() {
  Serial.println("-------");
  Serial.println(parent->getTitle());
  Serial.println("-------");
  return parent;
}

void Menu::addItem(Menu *m) {
  submenu[nextItem] = m;
  nextItem++;
}

String Menu::getTitle() {
  return title;
}

Menu* Menu::getSubMenu() {
  return submenu[toShow];
}

void Menu::up() {
  Serial.println(getTitle());
  toShow--;
  if ( toShow < 0) {
    toShow = 0;
  }
}

void Menu::down() {
  Serial.println(getTitle());
  toShow++;
  if ( toShow >= nextItem) {
    toShow = nextItem - 1;
  }
}

boolean Menu::hatSubmenu() {
  return nextItem > 0;
}


void Menu::showMenu() {
  digitalWrite(ledPin, HIGH);
  lcd.clear();

  int desde = 0;
  int hasta = 4;
  int cursor = 0;

  Serial.println(toShow);
  if ( toShow > 3) {
    Serial.println("Desde " + String(toShow - 3) + " hasta " + String(toShow));
    desde = toShow - 3;
    hasta = toShow + 1;
  } else {
    Serial.println("Desde " + String(0) + " hasta " + String(3));
  }

  for (int r = desde; r < hasta; r ++) {
    lcd.setCursor(1, r - desde);
    lcd.print(submenu[r]->getTitle());
  }
  if ( toShow > 3) {
    cursor = 3;
  } else {
    cursor = toShow;
  }
  lcd.setCursor(0, cursor);
  lcd.print((char)126);

}

Boton *up;
Boton *down;
Boton *enter;
Boton *back;
Menu *menu;
Menu *menuToShow;
Menu *tmp;

void ReadEntries() {

}

void startMenu() {

  menu = new Menu("Principal");
  Menu *op1 = new Menu("Op 1");
  Menu *op2 = new Menu("Op 2");
  Menu *op21 = new Menu("Sub Menu 2 1");
  Menu *op22 = new Menu("Sub Menu 2 2");
  Menu *op23 = new Menu("Sub Menu 2 3");
  Menu *op3 = new Menu("Op 3");
  Menu *op4 = new Menu("Op 4");
  Menu *op41 = new Menu("Sub Menu 4 1");
  Menu *op42 = new Menu("Sub Menu 4 2");
  Menu *op43 = new Menu("Sub Menu 4 3");
  Menu *op5 = new Menu("Op 5");
  Menu *op6 = new Menu("Op 6");

  op1->setParent(menu);
  menu->addItem(op1);

  op2->setParent(menu);

  op21->setParent(op2);
  op22->setParent(op2);
  op23->setParent(op2);

  op2->addItem(op21);
  op2->addItem(op22);
  op2->addItem(op23);

  menu->addItem(op2);

  op3->setParent(menu);
  menu->addItem(op3);

  op4->setParent(menu);

  op41->setParent(op4);
  op42->setParent(op4);
  op43->setParent(op4);

  op4->addItem(op41);
  op4->addItem(op42);
  op4->addItem(op43);

  menu->addItem(op4);

  op5->setParent(menu);
  menu->addItem(op5);

  op6->setParent(menu);
  menu->addItem(op6);

  pinMode(ledPin, OUTPUT);
  menu->showMenu();

  menuToShow = menu;
}

void setup() {
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();
  up = new Boton(subir);
  down = new Boton(bajar);
  enter = new Boton(seleccionar);
  back = new Boton(volver);

  startMenu();
}

void accion1() {
  do {
    back->read();
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Km:"); //--> Posicion 4
    lcd.setCursor(0, 1);
    lcd.print("Rumbo:"); //--> Posicion 6
    do {
      back->read();
      lcd.setCursor(10, 0);
      km = km + 0.05;
      lcd.print((float)km / 10, 2); // Distancia recorrida + corrección del Trip
      lcd.setCursor(7, 1);
      lcd.print("0.0"); // Distancia recorrida + corrección del Trip
      delay(100);
    } while (!back->getValue());
    delay(100);
  } while (!back->getValue());
}

void loop() {
  // Leemos el valor del pin
  up->read();
  down->read();
  enter->read();
  back->read();

  if (!inAction) {
    if (up->getValue()) {
      if (up->changed()) {
        menuToShow->up();
        up->setAlt();
        menuToShow->showMenu();
      }
    }
    else {
      //digitalWrite(ledPin, LOW);
      up->setAlt();
    }

    if (down->getValue()) {
      if (down->changed()) {
        menuToShow->down();
        down->setAlt();
        menuToShow->showMenu();
      }
    }
    else {
      //digitalWrite(ledPin, LOW);
      down->setAlt();
    }
  } else {

  }

  if (back->getValue()) {
    if (back->changed()) {
      back->setAlt();
      if ( menuToShow->hatParent()) {
        menuToShow = menuToShow->getParent();
      }
      menuToShow->showMenu();

      inAction = false;
    }
  }
  else {
    //digitalWrite(ledPin, LOW);
    back->setAlt();
  }

  if (enter->getValue()) {
    if (enter->changed()) {
      enter->setAlt();

      if (menuToShow->getSubMenu()->hatSubmenu()) {
        menuToShow = menuToShow->getSubMenu();
        menuToShow->showMenu();
      } else {
        menuToShow = menuToShow->getSubMenu();
        accion1();
        Serial.println("Se acabó la ación");
        if ( menuToShow->hatParent()) {
          menuToShow = menuToShow->getParent();
        }
        menuToShow->showMenu();
        inAction = false;
      }
    }
  }
  else {
    //digitalWrite(ledPin, LOW);
    enter->setAlt();
  }

  delay(100);
}