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

//The following constants are used to configure the behaviour of the interface
// MAX_OPTIONS defines the maximum number of options a menu can hold
// MAX_MENUS declares the maximum number of menu screens in the interface
// MAX_GRAPHS is the maximum number of graphs to create
// DISP_WIDTH and DISP_HEIGHT are hardware specific (SSD1306)
// REFRESH: is the time in miliseconds the interface will take in refreshing (this time affects the loop, keep that in mind)

#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;    //Defines the size it will occupy in the x axis (width), this value is gotten from the menu
  int sizey;    //Defines the height of the option (this value is gotten from the menu it belongs to)
  String content;   //Text of the option
  int pos;      //Defines the position it has in the menu
  int textSpacing;   //According to the height, defines the space for the text, so that it's vertically centered
  bool fill = false;  //In case an option is not selected this should be false
  bool disp = false;   //In case an option is not configured, it should be false and, thus hidden
  int destinationType;   //Defines what the option leads to (another menu, graph, something else)
  int destinationIndex;  //Defines where the option leads to (index of the destination)

  public:

  //Option(){}
  
  //Method to configure an option, all attributes are assigned, and disp is true, so the option can be displayed
  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;
  }

//This method draws each option

  void drawopt(int page, int pos, int optPPage){
    if(this->disp){    //Checks if the option was configured and, as a result, is displayable
      if(this->pos == pos){   //If the position of the option corresponds to the position passed to the function, then it should be selected
        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{                  //If the option is not selected, the rectangle containing it shouldn't be filled
        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;                  //X size for each option in the menu
  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 method configures the menu created from Screen
    this->sizex = sizex;
    this->sizey = sizey;
    this->optPPage = DISP_HEIGHT / this->sizey;
  }

  //The following method is used to created an option for the menu
  void createOption(String content, bool destinationTypeMenu, int destinationIndex){
    //The option takes the place in the array defined by the options number variable (options), which is later increased.
    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(25, 20);
    display.println("Welcome");
    
    
    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++;
    }
    else
      cont = 0;
    if(cont == debounceTime/REFRESH){
      cont = 0;
      this->screen->goBack();
      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;
Keyboard keyboard(13, 12, 14, 27, 30, &screen);

void setup(){
  screen.configure(true);
  
  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, 1000, 25, 250, 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)
}