#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define __DEBUG__

#define MAX_OPTIONS 10      //Maximum number of options for each menu
#define MAX_MENUS 3
#define MAX_GRAPH 3
#define DISP_WIDTH 128      // OLED display width
#define DISP_HEIGHT 64      // OLED display height
#define REFRESH 10          //Refresh time in ms

Adafruit_SSD1306 display(DISP_WIDTH, DISP_HEIGHT, &Wire, -1);
int i = 0;

class Option{
  private:

  int sizex;
  int sizey;
  String content;
  int pos;
  int textSpacing;
  bool fill = false;
  bool disp = false;
  int destinationType;
  int destinationIndex;

  public:

  //Option(){}
  
  void configure(String content, int sizex, int sizey, int pos, int destinationType, int destinationIndex){
    this->sizex = sizex;
    this->sizey = sizey;
    this->content = content;
    this->pos = pos;
    this->destinationType = destinationType;
    this->destinationIndex = destinationIndex;
    this->disp = true;
    this->textSpacing = ((sizey - 7)/2) + 7;
  }

  int getDestinationType(){
    int destinationType = this->destinationType;
    return destinationType;
  }

  int getDestinationIndex(){
    int destinationIndex = this->destinationIndex;
    return destinationIndex;
  }

  void drawopt(int page, int pos, int optPPage){
    if(this->disp){
      if(this->pos == pos){
        display.fillRect(0, (this->sizey)*(this->pos) + 1 - (page*optPPage*this->sizey), this->sizex, this->sizey, WHITE);
        display.setTextColor(SSD1306_BLACK);
        display.setCursor(5, (this->sizey)*(this->pos + 1) - (page*optPPage*this->sizey) - this->textSpacing);
        display.print(this->content);
        display.setTextColor(SSD1306_WHITE);
      }
      else{
        display.drawRect(0, (this->sizey)*(this->pos) + 1 - (page*optPPage*this->sizey), this->sizex, this->sizey, WHITE);
        display.setCursor(5, (this->sizey)*(this->pos + 1) - (page*optPPage*this->sizey) - this->textSpacing);
        display.print(this->content);
      }
    }
  }
};

class Menu{    //ContentTypeMenu true, it is a menu
  private:

  int sizex;
  int sizey;                  //Y size of each option in the menu
  int options = 0;            //This indicates the number of options created
  int pos = 0;                //This indicates the position of the cursor
  int page = 0;               //If the menu is too long, this indicates the page that is being displayed
  Option opt[MAX_OPTIONS];
  int optPPage;

  int previousScreen = 0;
  int previousContentType = 0;
  
  public:

  void configure(int sizex, int sizey){
    this->sizex = sizex;
    this->sizey = sizey;
    this->optPPage = DISP_HEIGHT / this->sizey;
  }

  void createOption(String content, bool destinationTypeMenu, int destinationIndex){
    this->opt[this->options].configure(content, this->sizex, this->sizey, this->options++, destinationTypeMenu, destinationIndex);
  }

  int extractDestinationType(){
    int destinationType = this->opt[this->pos].getDestinationType();
    return destinationType;
  }

  int extractDestinationIndex(){
    int destinationIndex = this->opt[this->pos].getDestinationIndex();
    return destinationIndex;
  }

  void drawMenu(){
    display.clearDisplay();
    this->page = pos/this->optPPage;
    for(int i = 0; i < options; i++){
      this->opt[i].drawopt(this->page, this->pos, this->optPPage);
    }
    display.display();
  }
  
  int extractPos(){
    return(this->pos);
  }

  int extractOptNumber(){
    return(this->options);
  }

  void increasePos(){
    this->pos++;
  }

  void decreasePos(){
    this->pos--;
  }

  void setPreviousScreen(int prev){
    this->previousScreen = prev;
  }

  void setPreviousContentType(int prev){
    this->previousContentType = prev;
  }

  int getPreviousScreen(){
    int prev = this->previousScreen;
    return prev;
  }

  int getPreviousContentType(){
    int prev = this->previousContentType;
    return prev;
  }
};

class Graph{    //ContentTypeMenu false, it is not a menu
  private:

  String title;
  char graphType;       //'a' Vertical Bar, 'b' Horizontal Bar, 'c' Cartesian Graph

  //Assign whatever value in "configure(..." if a parameter is not required for the specified graphType
  
  double value;         //For: Vertical Bar  Horizontal Bar  Cartesian
  double xpos;          //For: Vertical Bar  Horizontal Bar  Cartesian
  double ypos;          //For: Vertical Bar  Horizontal Bar  Cartesian
  double height;        //For: Vertical Bar  Horizontal Bar  Cartesian
  double width;         //For: Vertical Bar  Horizontal Bar  Cartesian
  double yminimum;      //For: Vertical Bar                  Cartesian
  double ymaximum;      //For: Vertical Bar                  Cartesian
  double xminimum;      //For:               Horizontal Bar  Cartesian
  double xmaximum;      //For:               Horizontal Bar  Cartesian
  double yStepSize;     //For: Vertical Bar                  Cartesian
  double xStepSize;     //For:               Horizontal Bar  Cartesian
  double digit;         //For: Vertical Bar  Horizontal Bar  Cartesian
  double x;
  double yrange;
  double xrange;
  double ox;
  double oy;
  double count;
  double graphScale;
  bool redraw = true;
  
  int previousScreen = 0;
  int previousContentType = 0;
  
  public:

  void configure(String title, char graphType, double xpos, double ypos, double width, double height,
  double yminimum, double ymaximum, double xminimum, double xmaximum, double yStepSize, double xStepSize, double digit){
    this->title = title;
    this->graphType = graphType;
    this->yminimum = yminimum;
    this->ymaximum = ymaximum;
    this->xminimum = xminimum;
    this->count = xminimum;
    this->xmaximum = xmaximum;
    this->height = height;
    this->width = width;
    this->yStepSize = yStepSize;
    this->xStepSize = xStepSize;
    this->digit = digit;
    this->xpos = xpos;
    this->ypos = ypos;
    switch(graphType){
      case 'a':
      this->yrange = ymaximum - yminimum;
      this->graphScale = (yStepSize) * (height / this->yrange) - .001; //Adjusts the scale of the graph, according to the range and the size of the step
      break;

      case 'b':
      this->xrange = xmaximum - xminimum;
      this->graphScale = (xStepSize) * (width / this->xrange) - .001; //Adjusts the scale of the graph, according to the range and the size of the step
      break;

      case 'c':
      this->yrange = ymaximum - yminimum;
      this->xrange = xmaximum - xminimum;
      break;
    }
  }

  void drawGraph(){
    double level, data, i;
    switch(graphType){
      case 'a':
      double my;
  
      if (this->redraw) {
        display.clearDisplay();
        this->redraw = false;
        display.fillRect(0, 0,  127 , 14, SSD1306_WHITE);
        display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
        display.setTextSize(1);
        display.setCursor(2, 4);
        display.println(this->title);
  
        for (i = 0; i <= this->height; i += this->graphScale) {
          my =  this->ypos - this->height + i;
          display.drawFastHLine(this->xpos + this->width + 1, my,  5, SSD1306_WHITE);
          // draw lables
          display.setTextSize(1);
          display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
          display.setCursor(this->xpos + this->width + 12, my - 3 );
          data = this->ymaximum - ( i * (this->yStepSize / this->graphScale));
          display.print(data, this->digit);
        }
      }
      // compute level of bar graph that is scaled to the  height and the hi and low vals
      // this is needed to accompdate for +/- range
      level = (this->height * (((this->value - this->yminimum) / (this->yrange))));
      // draw the bar graph
      // write a upper and lower bar to minimize flicker cause by blanking out bar and redraw on update
      display.drawRect(this->xpos, this->ypos - this->height, this->width, this->height, SSD1306_WHITE);
      display.fillRect(this->xpos, this->ypos - this->height, this->width, this->height - level,  SSD1306_BLACK);
      display.drawRect(this->xpos, this->ypos - this->height, this->width, this->height, SSD1306_WHITE);
      display.fillRect(this->xpos, this->ypos - level, this->width,  level, SSD1306_WHITE);
      // up until now print sends data to a video buffer NOT the screen
      // this call sends the data to the screen
      display.display();
      break;

      case 'b':

      if (this->redraw) {
        display.clearDisplay();
        this->redraw = false;
        display.fillRect(0, 0,  127 , 16, SSD1306_WHITE);
        display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
        display.setTextSize(1);
        display.setCursor(2, 4);
        display.println(this->title);
        // draw the text
        
        for (i = 0; i <= this->width; i += this->graphScale) {
          display.drawFastVLine(i + this->xpos , this->ypos ,  5, SSD1306_WHITE);
          // draw lables
          display.setTextSize(1);
          display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
          display.setCursor(i + this->xpos , this->ypos + 10);
          // addling a small value to eliminate round off errors
          // this val may need to be adjusted
          data =  ( i * (this->xStepSize / this->graphScale)) + this->xminimum + 0.00001;
          display.print(data, this->digit);
        }
      }
      // compute level of bar graph that is scaled to the width and the hi and low vals
      // this is needed to accompdate for +/- range capability
      // draw the bar graph
      // write a upper and lower bar to minimize flicker cause by blanking out bar and redraw on update
      level = (this->width * (((this->value - this->xminimum) / (this->xmaximum - this->xminimum))));
      display.fillRect(this->xpos + level, this->ypos - this->height, this->width - level, this->height,  SSD1306_BLACK);
      display.drawRect(this->xpos, this->ypos - this->height, this->width,  this->height, SSD1306_WHITE);
      display.fillRect(this->xpos, this->ypos - this->height, level,  this->height, SSD1306_WHITE);
      // up until now print sends data to a video buffer NOT the screen
      // this call sends the data to the screen
      display.display();
      break;

      case 'c':
      double temp;
    
      if (this->redraw == true) {
        this->redraw = false;
        display.clearDisplay();
        display.fillRect(0, 0,  127 , 16, SSD1306_WHITE);
        display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
        display.setTextSize(1);
        display.setCursor(2, 4);
        display.println(title);
        this->ox = (this->count - this->xminimum) * (this->width) / (this->xrange) + this->xpos;
        this->oy = (this->value - this->yminimum) * (- this->height) / (this->yrange) + this->ypos;
        // draw y scale
        display.setTextSize(1);
        display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
        for ( i = this->yminimum; i <= this->ymaximum; i += this->yStepSize) {
          // compute the transform
          // note my transform funcition is the same as the map function, except the map uses long and we need doubles
          temp =  (i - this->yminimum) * (- this->height) / (this->ymaximum - this->yminimum) + this->ypos;
          if (i == 0) {
            display.drawFastHLine(this->xpos - 3, temp, this->width + 3, SSD1306_WHITE);
          }
          else {
            display.drawFastHLine(this->xpos - 3, temp, 3, SSD1306_WHITE);
          }
          display.setCursor(this->xpos - 27, temp - 3);
          display.println(i, this->digit);
        }
        // draw x scale
        for (i = this->xminimum; i <= this->xmaximum; i += this->xStepSize) {
          // compute the transform
          display.setTextSize(1);
          display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
          temp =  (i - this->xminimum) * (this->width) / (this->xrange) + this->xpos;
          if (i == 0) {
            display.drawFastVLine(temp, this->ypos - this->height, this->height + 3, SSD1306_WHITE);
          }
          else {
            display.drawFastVLine(temp, this->ypos, 3, SSD1306_WHITE);
          }
          display.setCursor(temp, this->ypos + 6);
          display.println(i, this->digit);
        }
      }
    
      // graph drawn now plot the data
      // the entire plotting code are these few lines...
    
      this->x =  (this->count - this->xminimum) * (this->width) / (this->xrange) + this->xpos;
      this->value =  (this->value - this->yminimum) * (- this->height) / (this->yrange) + this->ypos;
      display.drawLine(this->ox, this->oy, this->x, this->value, SSD1306_WHITE);
      display.drawLine(this->ox, this->oy - 1, this->x, this->value - 1, SSD1306_WHITE);
      this->ox = this->x;
      this->oy = this->value;
    
      // up until now print sends data to a video buffer NOT the screen
      // this call sends the data to the screen
      display.display();
      this->count += 1;
      if(this->ox >= (this->xpos + this->width)){
        this->redraw = true;
        this->count = xminimum;
      }
    }
  }

  void redrawFlag(){
    this->redraw = true;
  }

  void setPreviousScreen(int prev){
    this->previousScreen = prev;
  }

  void setPreviousContentType(int prev){
    this->previousContentType = prev;
  }

  int getPreviousScreen(){
    int prev = this->previousScreen;
    return prev;
  }

  int getPreviousContentType(){
    int prev = this->previousContentType;
    return prev;
  }

  void assignValue(double value){
    this->value = value;
  }

  void reset(){
    this->x = 0;
  }
  
};


class Screen{
  private:
  
  Menu menu[MAX_MENUS];
  Graph graph[MAX_GRAPH];
  int counterM = 0;
  int counterG = 0;
  bool redraw = true;
  int currentScreen = 0;
  int contentType = 0;

  public:

  void configure(bool fullsetting){
    if(fullsetting){
      //Adafruit_SSD1306 display(DISP_WIDTH, DISP_HEIGHT, &Wire, -1);
      Serial.begin(115200);
      if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
  #ifdef __DEBUG__
        Serial.println("Display not found!");
  #endif
        while (true);
      }
    }
    display.clearDisplay();
  
    // Text size
    display.setTextSize(2);
    // Text color
    display.setTextColor(SSD1306_WHITE);
    // Text position
  
    display.setCursor(0, 20);
    display.println("Bienvenido");
    
    
    display.setTextSize(1);
  
    display.display();
    delay(5000);
  }

  void createMenu(int sizex, int sizey){
    this->menu[counterM].configure(sizex, sizey);
    this->counterM++;
  }

  void createOption(int menuIndex, String content, bool destinationTypeMenu, int destinationIndex){
    this->menu[menuIndex].createOption(content, destinationTypeMenu, destinationIndex);
  }

  void createVGraph(String title, double xpos, double ypos, double width, double height,
  double yminimum, double ymaximum, double yStepSize, double digit){
    this->graph[counterG].configure(title, 'a', xpos, ypos, width, height, yminimum, ymaximum, 0, 0, yStepSize, 0, digit);
    this->counterG++;
  }

  void createHGraph(String title, double xpos, double ypos, double width, double height, 
  double xminimum, double xmaximum, double xStepSize, double digit){
    this->graph[counterG].configure(title, 'b', xpos, ypos, width, height, 0, 0, xminimum, xmaximum, 0, xStepSize, digit);
    counterG++;
  }

  void createCGraph(String title, double xpos, double ypos, double width, double height,
  double yminimum, double ymaximum, double xminimum, double xmaximum, double yStepSize, double xStepSize, double digit){
    this->graph[counterG].configure(title, 'c', xpos, ypos, width, height, yminimum, ymaximum, xminimum, xmaximum, yStepSize, xStepSize, digit);
    counterG++;
  }

/*  
  void redrawFlag(){
    this->redraw = true;
  }
*/

  void graphAssignValue(int graphIndex, double value){
    this->graph[graphIndex].assignValue(value);
    this->redraw = true;
  }

  void control(){
    if (redraw){
      if (contentType == 0){
        menu[currentScreen].drawMenu();
      }
      else if (contentType == 1){
        graph[currentScreen].drawGraph();
      }
      this->redraw = false;
    }
  }

  void increasePos(){
    if(this->menu[this->currentScreen].extractPos() < this->menu[this->currentScreen].extractOptNumber() - 1)
      this->menu[this->currentScreen].increasePos();
  }

  void decreasePos(){
    if(this->menu[this->currentScreen].extractPos() > 0)
      this->menu[this->currentScreen].decreasePos();
  }
  
  void goTo(){
    if(this->contentType == 0){
      int newScreen = this->menu[this->currentScreen].extractDestinationIndex();
      int newContentType = this->menu[this->currentScreen].extractDestinationType();
      if (newContentType == 0){
        this->menu[newScreen].setPreviousScreen(this->currentScreen);
        this->menu[newScreen].setPreviousContentType(this->contentType);
      }
      else if(newContentType == 1){
        this->graph[newScreen].setPreviousScreen(this->currentScreen);
        this->graph[newScreen].setPreviousContentType(this->contentType);
        this->graph[newScreen].reset();
        this->graph[newScreen].redrawFlag();
      }
      else if(newContentType == 2){

      }
      this->contentType = newContentType;
      this->currentScreen = newScreen;
      this->redraw = true;
    }
  }

  void goBack(){
    if(contentType == 0){
      //Gets indexes from previous screen saved in actual screen if it is a menu, and sets them as the current indexes
      this->currentScreen = this->menu[this->currentScreen].getPreviousScreen();
      this->contentType = this->menu[this->currentScreen].getPreviousContentType();
    }
    else if(contentType == 1){
      //Gets indexes from previous screen saved in actual screen if it is a graph, and sets them as the current indexes
      this->currentScreen = this->graph[this->currentScreen].getPreviousScreen();
      this->contentType = this->graph[this->currentScreen].getPreviousContentType();
    }
  }

  void plusAction(){
    if(contentType == 0){
      increasePos();
    }
  }

  void minusAction(){
    if(contentType == 0){
      decreasePos();
    }
  }
  
};

class Keyboard{
  private:

  byte goTo;
  byte goBack;
  byte plus;
  byte minus;
  byte debounceTime;

  Screen * screen;

  public:

  Keyboard(byte goTo, byte goBack, byte plus, byte minus, byte debounceTime, Screen * screen){
    this->goTo = goTo;
    this->goBack = goBack;
    this->plus = plus;
    this->minus = minus;
    this->debounceTime = debounceTime;

    this->screen = screen;

    pinMode(goTo, INPUT_PULLUP);
    pinMode(goBack, INPUT_PULLUP);
    pinMode(plus, INPUT_PULLUP);
    pinMode(minus, INPUT_PULLUP);
  }

  void checkGoTo(){
    static char cont;
    if(digitalRead(this->goTo) == LOW)
      cont++;
    else
      cont = 0;
    if(cont == debounceTime/REFRESH){
      cont = 0;
      this->screen->goTo();
      while(digitalRead(this->goTo) == LOW){
      }
    }
  }
  
  void checkGoBack(){
    static char cont;
    if(digitalRead(this->goBack) == LOW){
      cont++;
      Serial.println("Ayudaaaaa");
    }
    else
      cont = 0;
    if(cont == debounceTime/REFRESH){
      cont = 0;
      this->screen->goBack();
      Serial.println("Ayudaaaaant");
      while(digitalRead(this->goBack) == LOW){
      }
    }
  }

  void checkPlus(){
    static char cont;
    if(digitalRead(this->plus) == LOW)
      cont++;
    else
      cont = 0;
    if(cont == debounceTime/REFRESH){
      cont = 0;
      this->screen->plusAction();
      while(digitalRead(this->plus) == LOW){
      }
    }
  }

  void checkMinus(){
    static char cont;
    if(digitalRead(this->minus) == LOW)
      cont++;
    else
      cont = 0;
    if(cont == debounceTime/REFRESH){
      cont = 0;
      this->screen->minusAction();
      while(digitalRead(this->minus) == LOW){
      }
    }
  }

  void control(){
    this->checkGoTo();
    this->checkGoBack();
    this->checkPlus();
    this->checkMinus();
  }
};

Screen screen, * disp = &screen;
Keyboard keyboard(14, 4, 12, 13, 40, disp);

void setup(){
  screen.configure(true);
  Serial.println("Screen created");
  
  screen.createMenu(128, 13);             //Menu 0
  screen.createMenu(128, 13);             //Menu 1

  /*String title, char graphType, double xpos, double ypos, double width, double height,
  double yminimum, double ymaximum, double xminimum, double xmaximum, double yStepSize, double xStepSize, double digit*/
  
  screen.createVGraph("Grafica 1", 25, 60, 40, 40, 0, 100, 10, 0);               //Graph 0
  screen.createHGraph("Grafica 2", 10, 40, 100, 20, 0, 100, 10, 0);              //Graph 1
  screen.createCGraph("Grafica 3", 30, 50, 75, 30, 0, 100, 0, 100, 25, 25, 0);   //Graph 2
  
  screen.createOption(0, "Vertical graph", 1, 0);
  //Creates the first option in Menu 0, directing to a graph (contentType = 1 (Graph)), 0 (Graph 0)
  screen.createOption(0, "Horizontal graph", 1, 1);       
  screen.createOption(0, "Cartesian graph", 1, 2);
  screen.createOption(0, "Extra option", 0, 1);

  screen.createOption(1, "Test", 1, 3);
  screen.createOption(1, "Working?", 1, 4);

  screen.increasePos();
  screen.increasePos();
  screen.goTo();
//  screen.graphAssignValue(2, 50);
  screen.goBack();
  screen.increasePos();
  screen.goTo();
//  screen.goBack();
//  screen.decreasePos();
}

void loop(){
  screen.control();            //Controls the screen and redraws if needed
  keyboard.control();

  if(i <= 100){
    screen.graphAssignValue(1, i);    //Assigning a demo value to Graph 1
    screen.graphAssignValue(2, i);    //Assigning a demo value to Graph 2
    i++;
  }
  else
    i = 0;

  delay(REFRESH);                   //Refresh time (approx)
}