#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Arduino.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

enum menuState {
  MENU_NORMAL,
  MENU_CONFIRM,
  MENU_BACK
};

class AZMenuNode {
  public:
    AZMenuNode()
    {}

    AZMenuNode(const char* text, int fid, uint8_t layer) {
      _text = text;
      _fid = fid;
      _layer = layer;
    }

    void getInfo(const char*& text) {
        text = _text;
    }

    int getFID() {
      return _fid;
    };
    
    uint8_t getLayer(){
      return _layer;
    };

private:
    const char* _text;
    int _fid;
    uint8_t _layer;
};

template <uint8_t maxElements> class AZMenu
{
  public:
    AZMenu(uint8_t pageSize) {
        _nodeIndex = 0;
        _numUsedElements = 0;
        _pageSize = pageSize;
        _explicitConfirmation = true;
    }

    void addNode(int layer, const char* const text, const int fid) {
      if (_nodeIndex < maxElements) {
        _nodes[_nodeIndex] = AZMenuNode(text, fid, layer);
        ++_nodeIndex;
        ++_numUsedElements;
      }
    }

    void setDrawing(void (*func)(const char* text, uint8_t index), 
                    void (*selectedItemfunc)(const char* text, uint8_t index),
                    void (*confirmFunc)(),
                    void (*backFunc)())
    {
        drawItemFunc = func;
        drawSelectedItemFunc = selectedItemfunc;
        drawConfirmFunc = confirmFunc;
        drawBackFunc = backFunc;
    }

    void reset() {
      _layer = 0;
      _rowIndex = 0;
      _offset = 0;
      _nodeIndex = 0;
      _startIndex = 0;
      _state = MENU_NORMAL;
      _confirmed = false;    
    }

    void up() {
      upNavigation();
      show();
    }

    void down() {
      downNavigation();
      show();
    }

    void right() {
      rightNavigation();
      show();
    }

    void left() {
      leftNavigation();
      show();
    }

    bool confirmed() {
      return _confirmed;
    }

    int getFID() {
      return _nodes[_nodeIndex].getFID();
    }

    void show() {
      draw();
    }

    void explicitConfirmation(bool enabled) {
      _explicitConfirmation = enabled;
    }

private:
    AZMenuNode _nodes[maxElements];
    uint8_t _numUsedElements;
    uint8_t _rowIndex;
    uint8_t _nodeIndex;
    uint8_t _pageSize;
    uint8_t _layer;
    uint8_t _offset;
    uint8_t _startIndex;
    menuState _state;
    bool _confirmed;
    bool _explicitConfirmation;
    
    void (*drawItemFunc)(const char* text, uint8_t index);
    void (*drawSelectedItemFunc)(const char* text, uint8_t index);
    void (*drawConfirmFunc)();
    void (*drawBackFunc)();

    void draw() {
      if(_explicitConfirmation) {
        switch(_state) {
          case MENU_NORMAL : 
            drawSelectionMode(); 
            break;
          case MENU_CONFIRM : 
            drawConfirmFunc();
            break;
          case MENU_BACK : 
            drawBackFunc();
            break;
        }
      } else {
        drawSelectionMode();
      }
    }

    void drawSelectionMode() {
      uint8_t itemIdx = 0;
      uint8_t row = 0;
      for(uint8_t i= _startIndex; i < _numUsedElements; i++ ) {
        if( _nodes[i].getLayer() == _layer) {
          if(itemIdx >= _offset) {
            const char* info;
            _nodes[i].getInfo(info);
            //Serial.println(info);
            if(itemIdx == _rowIndex) {
              drawSelectedItemFunc(info, row);
              _nodeIndex = i;
            }else {
              drawItemFunc(info, row);
            }
            row++;
          }

          if(row >= _pageSize) {
            break;
          }
            
          itemIdx++;
        }else {
          if( _nodes[i].getLayer() < _layer && i > _nodeIndex && i > _startIndex) {
            break;
          }
        }
        
      }
      //Serial.println("ok");
    }

    void upNavigation() {
      _confirmed = false;
      if(_state != MENU_NORMAL) {
          confirmationUpDown();
         return;
      }

      if(_rowIndex - 1 >= 0) {
        _rowIndex--;
        if(_rowIndex < _offset) {
          _offset--;
        }
      }
    }

    void downNavigation() {
      _confirmed = false;
      if(_state != MENU_NORMAL) {
          confirmationUpDown();
         return;
      }

      if(_nodeIndex + 1 > _numUsedElements - 1) {
        return;
      }

       AZMenuNode * topNode = getTopNode();
       for(uint8_t i= _nodeIndex + 1; i < _numUsedElements; i++ ) {
         if(_nodes[i].getLayer() < topNode->getLayer()) {
           break;
         }
         if(_nodes[i].getLayer() == topNode->getLayer()) {
           _rowIndex++;
            if(_offset + _pageSize <= _rowIndex) {
              setOffset();
            }
            break;
         }
       }
    }

    void confirmationUpDown() {
      if(_state == MENU_CONFIRM) {
        _state = MENU_BACK ;
      } else {
        _state = MENU_CONFIRM;
      }
    }

    void leftNavigation() {
        _confirmed = false;
        if(_state != MENU_NORMAL) {
          _state = MENU_NORMAL;
          return;
        }

        AZMenuNode * topNode = getTopNode();
        if(topNode->getLayer() < 1) {
          return;
        }

        _rowIndex = 0;
        _offset = 0;
        _layer = topNode->getLayer() - 1;
        _nodeIndex = findParent(_nodeIndex);

        if(_layer == 0) {
          _startIndex = 0;
        } else {
          _startIndex = findParent(_nodeIndex);
        }
        
        evaluateRowAndOffset(_nodeIndex);
        //Serial.println(String(_startIndex) + " " + String(_nodeIndex));
    }

    void rightNavigation() {
      _confirmed = false;
      if(_nodeIndex + 1 < _numUsedElements) {
        AZMenuNode * topNode = getTopNode();
          //toto je iba test, ci ma submenu
        if( _nodes[_nodeIndex + 1].getLayer() == topNode->getLayer() + 1) {
          _rowIndex = 0;
          _offset = 0;
          _layer = topNode->getLayer() + 1;
          _startIndex = _nodeIndex;
        } else {
          confirmationRight();
        }
      } else {
        confirmationRight();
      }
    }

    void confirmationRight() {
      if(!_explicitConfirmation) {
        _confirmed = true;
        return;
      }

      if(_state == MENU_NORMAL) {
        _state = MENU_CONFIRM;
      } else {
        _confirmed = _state == MENU_CONFIRM;
        _state = MENU_NORMAL;
      }
    }

    AZMenuNode * getTopNode() {
      return &_nodes[_nodeIndex];
    }

    uint8_t findParent(uint8_t nodeIndex) {
      AZMenuNode * node = &_nodes[nodeIndex];
      for(uint8_t i = _nodeIndex - 1; i >= 0; i--) {
          if(_nodes[i].getLayer() == node->getLayer() - 1) {
            return i;
          }
      }
      return 0;
    }

    void setOffset() {
      if((_rowIndex - _pageSize) + 1 > 0) {
            _offset = (_rowIndex - _pageSize) + 1; 
        } else {
          _offset = 0;
        }
    }

    void evaluateRowAndOffset(uint8_t index) {
       for(uint8_t i= _startIndex; i < _numUsedElements; i++ ) {
          if(_nodes[i].getLayer() == _layer) {
              if(i == index) {
                break;
              } else {
                _rowIndex++;
                setOffset();
              }
          }
       }
    }

};
/////////////////////

void drawItem(const char* text, uint8_t index) {
  display.setCursor(5, index * 20);
  display.print(text);  
}

void drawSelectedItem(const char* text, uint8_t index) {
  display.fillRect(1, index * 20, 125, 18, SSD1306_WHITE);
  display.setTextColor(SSD1306_BLACK);
  display.setCursor(5, index * 20 + 2);
  display.print(text);
  display.setTextColor(SSD1306_WHITE);
}

void drawConfirm() {
  drawSelectedItem("OK", 0);
  drawItem("Back", 1);
  /*
  display.setCursor(15, 12);
  display.setTextSize(3); 
  display.print("OK");  
  display.setCursor(60, 12);
  display.print("NO");  
  display.setTextSize(2); 
  */
}

void drawBack() {
  /*
  display.setCursor(5, 5);
  display.print("NO");  
  */
  drawItem("OK", 0);
  drawSelectedItem("Back", 1);
}

AZMenu<20> g_Menu(3);

void setup() {
  Serial.begin(9600);

  pinMode(4, INPUT);
  pinMode(5, INPUT);
  pinMode(7, INPUT);
  pinMode(8, INPUT);

  g_Menu.setDrawing(drawItem, drawSelectedItem, drawConfirm, drawBack);
  g_Menu.addNode(0, "ONE" , 120);
  g_Menu.addNode(0, "wifi" , 0);
  g_Menu.addNode(1, "Freq", 0);
  g_Menu.addNode(2, "2.4 GHz", 1);
  g_Menu.addNode(2, "5 GHz", 2);
  g_Menu.addNode(1, "setup wifi", 0);
  g_Menu.addNode(2, "s1", 3);
  g_Menu.addNode(2, "s2", 4);
  g_Menu.addNode(2, "s3", 5);

  g_Menu.addNode(0, "gas" , 0);
  g_Menu.addNode(1, "gas on", 6);
  g_Menu.addNode(1, "gas off", 7);

  g_Menu.addNode(0, "CO2" , 0);
  g_Menu.addNode(1, "co2 on", 8);
  g_Menu.addNode(1, "co2 off", 9);

  g_Menu.addNode(0, "pokus" , 0);
  g_Menu.addNode(1, "pokus 2", 10);

  g_Menu.addNode(0, "LEN tak" , 0);
  g_Menu.addNode(1, "len tak 2", 11);
/*
  g_Menu.addNode(0, "psik" , 0);
  g_Menu.addNode(1, "psik 2", 5);

  g_Menu.addNode(0, "kvietok" , 0);
  g_Menu.addNode(1, "kvietok 2", 6);
  */
  g_Menu.reset();
  g_Menu.explicitConfirmation(false);
  //Serial.println("Start");
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { //Ive changed the address //already chill
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  display.clearDisplay();
  display.setTextSize(2);  // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);

  //display.clearDisplay();
  //display.setCursor(1, 1);
  //display.setTextColor(WHITE);
  //display.setTextSize(1);
 // display.print("Enabled: No");
  //display.setCursor(80, 1);
  //display.print("CB");
  //display.display();

  g_Menu.show();
  display.display();
  //delay(1500);

  /*
  AddText("line 1");
  AddText("line 2");
  AddText("line 3");
  AddText("line 4");
  AddText("line 5");
  AddText("line 6");
  AddText("line 7");
  AddText("line 8");
  AddText("line 9");
  AddText("line 10");
  */
  Serial.println("end");
}


void MenuDown() {
  display.clearDisplay();
  g_Menu.down();
  //g_Menu.show();
  display.display();
}

void MenuUp() {
  display.clearDisplay();
  g_Menu.up();
  //g_Menu.show();
  display.display();
}

void MenuRight() {
  display.clearDisplay();
  g_Menu.right();
  //g_Menu.show();
  display.display();

  if(g_Menu.confirmed()) {
    Serial.println(g_Menu.getFID());
  }
}

void MenuLeft() {
  display.clearDisplay();
  g_Menu.left();
  //g_Menu.show();
  display.display();  
}

bool b4 = false;
bool b5 = false;

bool b7 = false;
bool b8 = false;

void loop() {

  if(digitalRead(4) == LOW && !b4) {
    b4 = true;
    MenuDown();
  } else {
    b4 = false;
  }

  if(digitalRead(5) == LOW && !b5) {
    b4 = true;
    MenuUp();
  } else {
    b4 = false;
  }

  if(digitalRead(7) == LOW && !b7) {
    b7 = true;
    MenuRight();
  } else {
    b7 = false;
  }


  if(digitalRead(8) == LOW && !b8) {
    b8 = true;
    MenuLeft();
  } else {
    b8 = false;
  }

  // put your main code here, to run repeatedly:
  //delay(200);
}

/*
const byte initMessagesLimit = 5;
char* initMessages[initMessagesLimit];
byte currMessageIdx = 0;
void AddText(char* text) {
  
  if(currMessageIdx < initMessagesLimit) {
    display.setCursor(0, 1 + currMessageIdx * 11);
    initMessages[currMessageIdx] = text;
    display.print(text);
    currMessageIdx++;
  } else {
    display.clearDisplay();
    for (int i = 0; i < initMessagesLimit - 1; ++i) {
       initMessages[i] = initMessages[i + 1];
       display.setCursor(0, 1 + i * 11);  
       display.print(initMessages[i]);        
    }
    initMessages[initMessagesLimit - 1] = text;
    display.setCursor(0, 1 + (initMessagesLimit - 1) * 11); 
    display.print(text); 
  }

  display.display();
  delay(300);
}
*/