#include <Wire.h>
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"

#define pushButton 12

#define i2c_Address 0x3c
SSD1306AsciiWire oled;

int startTime, endTime, option = 1;
int planNo = 0, speedNo = 0;
char plans[3] = {'R', 'L', 'F'};
int speeds[5] = {100, 125, 150, 175, 200};

void GO();
void CAL();
void PLAN();
void SPEED();
void PID();
void ABILITY();

void (*operations[4])() = {GO, CAL, PLAN, SPEED};

void setup() {
  pinMode(pushButton, INPUT_PULLUP);
  initDisplay();
}


void loop() {
  OS();
}

void initDisplay() {
  Wire.begin();
  Wire.setClock(400000L);
  oled.begin(&SH1106_128x64, i2c_Address);
  oled.setFont(Stang5x7);
}

void OS()
{ 
  start:
  menu(option);
  int press = button();
  if(press == 1) {
    option += 1;
    if(option > 6){
      option = 1;
    }
  } else if(press == 2)
  {
    operations[option - 1]();
    goto start;
  }
}

int button() {
  start:
  while (1) {
    if (digitalRead(pushButton) == 0) {
      startTime = millis();
      while (digitalRead(pushButton) == 0) {}
      break;
    }
  }
  endTime = millis();
  if (endTime - startTime > 500) {
    return 2;
  } else if(endTime - startTime > 25)
  {
    return 1;
  }
  else{
    goto start;
  }
}


void menu(int i) {
  int x, y;

  if(i < 4){
    x = 0;
    y = (2 * i);
    }
  else{
    x = 60;
    y = (2 * i) - 6;
    }

  oled.clear();
  oled.setCursor(x, y);
  oled.print("|         |");
  oled.setCursor(x, y - 1);
  oled.print("*---------*");
  oled.setCursor(x, y + 1);
  oled.print("*---------*");

  oled.setCursor(7, 2);
  oled.print("GO");
  oled.setCursor(7, 4);
  oled.print("CAL");
  oled.setCursor(7, 6);
  oled.print("PLAN :");
  oled.setCursor(50, 6);
  oled.print(plans[planNo]);


  oled.setCursor(65, 2);
  oled.print("SPEED:");
  oled.setCursor(103, 2);
  oled.print(speeds[speedNo]);
  oled.setCursor(65, 4);
  oled.print("PID");
  oled.setCursor(65, 6);
  oled.print("ABILITY");
}


void GO()
{
  oled.clear();
  oled.setCursor(40, 3);
  oled.print("Ready?");  
  int press = button();
  if(press == 1) 
  {
    return;
  } 
  else if(press == 2)
  {
    oled.clear();
    oled.setCursor(50, 3);
    oled.print("GO...");  
    if(button())
    {
      return;
    }
    //run();
  }
}

void CAL()
{
  oled.clear();
  oled.setCursor(5, 3);
  oled.print("Start Calibration...");  
  int press = button();
  if(press == 1) 
  {
    oled.clear();
    oled.setCursor(10, 3);
    oled.print("Calibrating...");  
    delay(2000);
    oled.clear();
    oled.setCursor(5, 3);
    oled.print("Calibration done!"); 
    if(button())
    {
      return;
    }
  } 
  else if(press == 2)
  {
    return;
  }
}

void PLAN()
{
  start:
  oled.clear();
  oled.setCursor(25, 3);
  oled.print("PlLAN :"); 
  oled.setCursor(75, 3); 
  oled.print(plans[planNo]);
  int press = button();
  if(press == 1) 
  {
    planNo++;
    if(planNo > 2)
    {
      planNo = 0;
    }
    goto start;
  } 
  else if(press == 2)
  {
    return;
  }
}

void SPEED()
{
  start:
  oled.clear();
  oled.setCursor(25, 3);
  oled.print("SPEED :"); 
  oled.setCursor(75, 3); 
  oled.print(speeds[speedNo]); 
  int press = button();
  if(press == 1) 
  {
    speedNo++;
    if(speedNo > 4)
    {
      speedNo = 0;
    }
    goto start;
  } 
  else if(press == 2)
  {
    return;
  }
}

void PID()
{

}

void ABILITY()
{

}