#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Arduino.h>   // required by platformIO
#include <Fonts/Tiny3x3a2pt7b.h>
#include <LiquidCrystal.h>

#define SCREEN_WIDTH 128  // OLED display width, in pixels
#define SCREEN_HEIGHT 32  // OLED display height, in pixels
#define LED_BUILTIN 2     // Set the GPIO pin where you connected your test LED or comment this line out if your dev board has a built-in LED

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
// The pins for I2C are defined by the Wire-library.
// On an arduino UNO:       A4(SDA), A5(SCL)
// On an arduino MEGA 2560: 20(SDA), 21(SCL)
// On an arduino LEONARDO:   2(SDA),  3(SCL), ...
#define OLED_RESET -1        // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C  ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define XPOS 39
#define YPOS 36
#define BPOS 27

#define NUMFLAKES 10  // Number of snowflakes in the animation example
#define LOGO_HEIGHT 16
#define LOGO_WIDTH 16

static const unsigned char PROGMEM logo_bmp[] = { 0b00000000, 0b11000000,
                                                  0b00000001, 0b11000000,
                                                  0b00000001, 0b11000000,
                                                  0b00000011, 0b11100000,
                                                  0b11110011, 0b11100000,
                                                  0b11111110, 0b11111000,
                                                  0b01111110, 0b11111111,
                                                  0b00110011, 0b10011111,
                                                  0b00011111, 0b11111100,
                                                  0b00001101, 0b01110000,
                                                  0b00011011, 0b10100000,
                                                  0b00111111, 0b11100000,
                                                  0b00111111, 0b11110000,
                                                  0b01111100, 0b11110000,
                                                  0b01110000, 0b01110000,
                                                  0b00000000, 0b00110000 };


#define FONT1 1
#define FONT2 2
#define FONT3 3

bool serialDebug = 1;  // enable debugging info on serial port
int mainMenuIndex = 0;
int subMenuIndex = 0;
int largeMainMenu = 0;
int subMenuActive = 0;

String results[] = { "OEM", "OIL", "COOLANT", "TYRES", "CODES", "BOOST", "BATTERY" };
String resultsInfo[] = { "Return this screen to\nthe default SUBARU screen", "View engine oil pressure and temperature", "View engine coolant pressure and temperature", "View tyre pressure", "View OBD2 Codes", "View Turbo boost guage", "View battery information" };
int resultsLength = sizeof(results)/sizeof(String);
int resultsInfoLength = sizeof(resultsInfo)/sizeof(String);

void setup() {

  // setup pin mode for reading joystick x and y axsis and z button
  pinMode(XPOS, INPUT);
  pinMode(YPOS, INPUT);
  pinMode(BPOS, INPUT);

  Serial.begin(115200);  // start serial comms at speed 115200

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;  // Don't proceed, loop forever
  }

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  // display.setFont(&Tiny3x3a2pt7b);
  display.setRotation(0);
  delay(100);
  display.display();
  delay(200);  // Pause for 2 seconds
  display.clearDisplay();

  // show display boot animation
  displayBoot();

  // show menu with item 1 selected
  setCurrentPosition(0);
  display.clearDisplay();
  displayMenuDirection(0,0);
}

void loop() {


  int x,y,z;
  x=analogRead(XPOS);
  y=analogRead(YPOS);
  z=digitalRead(BPOS);

  // Get current menu item
  int dir = getCurrentMenuPosition();

  // hide information and go back to main menu
  if(y > 3800){
    Serial.println("JOYSTICK LEFT");
    if(getSubMenu() == 1){ // if inside sub menu
      setSubMenu(0);
      display.clearDisplay();

      if(getlargeMainMenu() == 1){
        displayLargeMenu(dir,dir);
      } else {
        displayMenuDirection(dir,dir);
      }
    } else { // if not in sub menu
      // do nothing
    }

    delay(200);
  }

  // show more information on the item
  if(y < 100){
    Serial.println("JOYSTICK RIGHT");
    setSubMenu(1);
    display.clearDisplay();
    displaySubMenu(dir);
    delay(200);
  }

  // select next item (down)
  if(x < 100){
    display.stopscroll();
    Serial.println("JOYSTICK DOWN");
    if(dir >= resultsLength-1){
      Serial.println("At last item");
      // do nothing
    } else {
      setCurrentPosition(dir + 1);
      display.clearDisplay();

      if(getSubMenu() == 1){
        Serial.println("Navigating Sub Menu");
        displaySubMenu(dir + 1);
      } else if(getlargeMainMenu() == 1){
        Serial.println("Navigating Large Main Menu");
        displayLargeMenu(dir + 1,dir);
      } else {
        Serial.println("Navigating Normal Main Menu");
        displayMenuDirection(dir + 1,dir);
      }
    }
    delay(200);
  }

  // select prev item (up)
  if(x > 3800){
    Serial.println("JOYSTICK UP");
    if(dir <= 0){
      Serial.println("At first item");
      // do nothing
    } else {
      setCurrentPosition(dir - 1);
      display.clearDisplay();

      if(getSubMenu() == 1){
        Serial.println("Navigating Sub Menu");
        displaySubMenu(dir - 1);
      } else if(getlargeMainMenu() == 1){
        Serial.println("Navigating Large Main Menu");
        displayLargeMenu(dir - 1,dir);
      } else {
        Serial.println("Navigating Normal Main Menu");
        displayMenuDirection(dir - 1,dir);
      }
    }
    delay(200);
  }

  // if button pressed
  if(z == 1){
    if(getSubMenu() == 1){ // if in sub menu
      // do nothing
    } else {
      if(getlargeMainMenu() == 1){ // if main menu is large, set to small
        Serial.println("Button Press - Small Menu");
        hideLargeMenu(dir,dir);
        setlargeMainMenu(0);
        display.clearDisplay();
        displayMenuDirection(dir,dir);
        delay(200);
      } else {                     // if main menu is small, set to large
        Serial.println("Button Press - Large Menu");
        setlargeMainMenu(1);
        display.clearDisplay();
        displayLargeMenu(dir,dir);
        delay(200);
      }
    }
  }
}  // loop

int getSubMenuIndex(){ // get sub menu index
  return subMenuIndex;
}

void setSubMenuIndex(int n){ // set sub menu index
  subMenuIndex = n;
}

int getlargeMainMenu(){ // check if main menu enlarged
  return largeMainMenu;
}

void setlargeMainMenu(int n){ // set main menu large (1) or small (0)
  largeMainMenu = n;
}

int getSubMenu(){ // check if in sub menu
  return subMenuActive;
}

void setSubMenu(int n){ // enable (1) or disable (0) sub menu
  subMenuActive = n;
}

void displayMenuDirection(int direction, int prevItem){
  Serial.println("__displayMenuDirection(int direction)");

  for(int i = 0; i <= 4; i++){
    display.clearDisplay();
    displayMiddleOption(direction,prevItem,i);
    displayNextOption(direction,prevItem,i);
    displayPrevOption(direction,prevItem,i);
    display.display();
    delay(10);
  }

  /*

  void displayMenuDirection(int direction, int prevItem){
  Serial.println("__displayMenuDirection(int direction)");

  for(int i = 0; i < 8; i++){
    display.clearDisplay();
    displayMiddleOption(direction,prevItem,i);
    displayNextOption(direction,prevItem,i);
    displayPrevOption(direction,prevItem,i);
    delay(20);
  }
}

*/
}

void setCurrentPosition(int curPos){
  mainMenuIndex = curPos;
}

int getCurrentMenuPosition(){
  return mainMenuIndex;
}

void displayBoot(){
  display.clearDisplay();
  display.drawLine(0,0,128,0, SSD1306_WHITE);
  display.drawLine(0,30,128,30, SSD1306_WHITE);
  for(int i = 0; i < 128; i+=4){
  	display.fillRect(0,3,i,25,SSD1306_WHITE);
  	display.display();
  	delay(5);
  }
}

void displayMiddleOption(int menuItem, int prevItem, int loopIndex){
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(2);  // Draw 2X-scale text

  if(menuItem == prevItem){
    display.setCursor(2, 8);
    display.println(results[menuItem]);
  }

  if(menuItem < prevItem){
    display.setCursor(2, 4+loopIndex);
    display.println(results[menuItem]);
  }

  if(menuItem > prevItem){
    display.setCursor(2, 12-loopIndex);
    display.println(results[menuItem]);
  }


  
  //display.display();

  /*

  void displayMiddleOption(int menuItem, int prevItem, int loopIndex){
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(2);  // Draw 2X-scale text
  display.setCursor(0, 8);
  display.println(results[menuItem]);
  //display.display();
}

*/
}

void displaySubMenu(int menuItem){
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);  // Draw 1X-scale text
  display.setCursor(0, 0);
  display.println(results[menuItem]);

  if(menuItem >= resultsInfoLength){
    // do nothing
  } else {
    display.setCursor(0,8);
    display.println(resultsInfo[menuItem]);
  }
  display.display();
}


void displayPrevOption(int menuItem, int prevItem, int loopIndex){
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);  // Draw 1X-scale text

  if(menuItem == prevItem){
    display.setCursor(0, 0);
  }

  if(menuItem < prevItem){
    display.setCursor(0, -4+loopIndex);
  }

  if(menuItem > prevItem){
    display.setCursor(0, 4-loopIndex);
  }

  
  if(menuItem == 0){
    // show last item in array
    //display.println(results[resultsLength-1]);
  } else if(menuItem == resultsLength-1) {
    // show first item in array
      display.println(results[menuItem-1]);
  } else {
    // show PREV item in array
    display.println(results[menuItem-1]);
  }

  //display.display();

  /*

  void displayPrevOption(int menuItem, int prevItem, int loopIndex){
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);  // Draw 2X-scale text
  display.setCursor(0, 0);

  
  if(menuItem == 0){
    // show last item in array
    //display.println(results[resultsLength-1]);
  } else if(menuItem == resultsLength-1) {
    // show first item in array
      display.println(results[menuItem-1]);
  } else {
    // show PREV item in array
    display.println(results[menuItem-1]);
  }

  //display.display();

  }

  */

  }

void displayNextOption(int menuItem, int prevItem, int loopIndex){
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);  // Draw 1X-scale text
  

  if(menuItem == prevItem){
    display.setCursor(0, 24);
  }

  if(menuItem < prevItem){
    display.setCursor(0, 20+loopIndex);
  }

  if(menuItem > prevItem){
    display.setCursor(0, 28-loopIndex);
  }

  
  if(menuItem == 0){
    // show first item in array
    display.println(results[menuItem+1]);
  } else if(menuItem == resultsLength-1) {
    // show first item in array
    //  display.println(results[0]);
  } else {
    // show first item in array
    display.println(results[menuItem+1]);
  }

  //display.display();

  /*

  void displayNextOption(int menuItem, int prevItem, int loopIndex){
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);  // Draw 2X-scale text
  display.setCursor(0, 24);

  
  if(menuItem == 0){
    // show first item in array
    display.println(results[menuItem+1]);
  } else if(menuItem == resultsLength-1) {
    // show first item in array
    //  display.println(results[0]);
  } else {
    // show first item in array
    display.println(results[menuItem+1]);
  }

  //display.display();
}

*/
}

// display main menu in larger text
void displayLargeMenu(int menuItem, int prevItem){
  Serial.println(prevItem);
  Serial.println(menuItem);
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(3);  // Draw 2X-scale text

  if(menuItem == prevItem){
    display.clearDisplay();
    display.setCursor(-8, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(-4, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(-2, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);
  }

  // moving down
  if(menuItem > prevItem){
    display.clearDisplay();
    display.setCursor(0, 20);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 14);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 10);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);
  }

  // moving up
  if(menuItem < prevItem){
    display.clearDisplay();
    display.setCursor(0, -2);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 2);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 6);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);
  }
}

  // display main menu in larger text
void hideLargeMenu(int menuItem, int prevItem){
  Serial.println(prevItem);
  Serial.println(menuItem);
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(3);  // Draw 2X-scale text

  // SLIDE OUT FROM LEFT
  if(menuItem == prevItem){
    display.clearDisplay();
    display.setCursor(0, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(-4, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(-6, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(-8, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);
  }

  // moving down
  if(menuItem > prevItem){
    display.clearDisplay();
    display.setCursor(0, 20);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 14);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 10);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);
  }

  // moving up
  if(menuItem < prevItem){
    display.clearDisplay();
    display.setCursor(0, -2);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 2);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 6);
    display.println(results[menuItem]);
    display.display();
    delay(20);

    display.clearDisplay();
    display.setCursor(0, 8);
    display.println(results[menuItem]);
    display.display();
    delay(20);
  }
}